// easy compile command:
//       mex aoi.c -lcomedi
//       OR
//       gcc aoi.c -lcomedi -lpthread

/*
 * aoi.c
 *
 * 0.992b
 *
 * PURPOSE:
 *	To send an arbitrary waveform to the NIDAC while concurrently reading in an arbitrary
 *	signal on multiple input channels
 *
 *
 * INPUT:
 * 	1:  An input data matrix consisted of the data to be outputted
 * 	2:  A row vector of channel listings for data to be received on
 * 	3:  Frequency of the signal to be sent/acquired
 *
 *
 * OUTPUT:
 * 	A data matrix consisting of the data acquired.  Channels are aligned by columns
 *
 *
 * CURRENT ISSUES:
 * 	1:	need to get this function working on the command line
 *
 *
 * RESOLVED ISSUES:
 * 	1:	Combined device bandwidth for the NI-DAC is 333Khz, so any combo of number
 * 		of devices sampled and the sampling rate over that number would kill it
 * 		thus resolving issues #0001 and #0002.
 *
 * 	2:	Giving is a STOP signal instead of a KILL/INT/etc.. works. Resolves #0003
 *
 *
 * PAST ISSUES:
 *	#0001:  When the sampling rate is too high it fails sometimes.  One hypothesis is
 *		that when the cpu needs to do other things the buffer overflows and 
 *		causes a bad read
 *
 *	#0002:  When the sampling rate is too high and/or there are too many channels to be
 *		sampled, the comedi will cause an error. 
 *
 * 	#0003:  When Ctrl-C was pressed, it always killed the process in an unpleasant way
 * 		in matlab. this no longer occurs.
 *
 *
 * REVISION:
 *
 * 	08/20/2002	:	Release of Beta version
 * 	08/21/2002	:	Fixed a bug that was shows up on Comedi version 0.7.65 or less
 * 				BUG : Comedi likes the buffer being filled completely
 * 					otherwise the FIFO does not push data out.
 * 					Therefore, extra data must be sent to the device so
 * 					that the real data mass be written.
 *
 */


// possibilities are MEX and EXE.  Changed to support compilation of EXE and MEX files
#define MEX 


#include <stdio.h>
#include <comedilib.h>
#include <signal.h>
#include <pthread.h>


#ifdef MEX

#include "matrix.h"
#include "mex.h"

#endif


#define BUF_LEN 8192
#define DELAY 2000
#define RANGE 0
#define DFLT_DAC_SUB_DEV 1
#define DFLT_ADC_SUB_DEV 0
#define MAX_ADC_CHAN_NUM 16
#define MAX_DAC_CHAN_NUM 2
#define DFLT_COMEDI_DEVICE_NAME "/dev/comedi0"
#define MAX_BANDWIDTH 333000
#define DFLT_FREQUENCY 10000

struct dataProperties {
	
	// global signal properties
	// signal length = the length of the input/output signal
	// frequency = the frequency of the signals
	int signalLength;
	double frequency;

	// variables pertaining to teh ADC and DAC
	// length = the signal length * the number of active channels in teh ADC/DAC
	// channelNum = the number of channels in teh ADC/DAC
	// signal = signal
	// sample = sample
	// channelList = which channels pertain to which element in the channel array
	// cppacklist = modified channelList for the hardware command interface
	int dacLength;
	int dacChannelNum;
	double *dacSignal;
	sampl_t *dacSample;
	unsigned int dacChannelList[MAX_DAC_CHAN_NUM];
	unsigned int dacCrPackList[MAX_DAC_CHAN_NUM];

	int adcLength;
	int adcChannelNum;
	double *adcSignal;
	sampl_t *adcSample;
	unsigned int adcChannelList[MAX_ADC_CHAN_NUM];
	unsigned int adcCrPackList[MAX_ADC_CHAN_NUM];

	// Extra data padding to fill the buffer
	sampl_t *more;
};


struct comediProperties {
	
	// dev is the device handle
	// cmd is the hardware command structure
	comedi_t *dev;
	comedi_cmd *cmd[2];
};


// our two global variables

struct dataProperties data;
struct comediProperties com;
pthread_t inThread, outThread;
void (*matlabHandler)();


int startUp();
void cleanUp();
void fromPhys();
void toPhys();
int makeMore();
void *dacThread();
void *adcThread();
int makeDacCommand();
int makeAdcCommand();
int comediIntTrigger();
void intHandler(int sig);
int start();

#ifdef MEX
void mexFunction( int nlhs, mxArray **plhs, int nrhs, const mxArray **prhs ); 
#else
int main();
#endif



int startUp() {

	// Allocate our dynamic memory
	#ifdef MEX
	com.cmd[0] = (comedi_cmd *) mxMalloc(sizeof(comedi_cmd));
	com.cmd[1] = (comedi_cmd *) mxMalloc(sizeof(comedi_cmd));
	data.dacSample = (sampl_t *) mxMalloc(sizeof(sampl_t) * data.dacLength);
	data.adcSample = (sampl_t *) mxMalloc(sizeof(sampl_t) * data.adcLength);
	data.more = (sampl_t *) mxMalloc(sizeof(sampl_t) * BUF_LEN);
	#else
	com.cmd[0] = (comedi_cmd *) malloc(sizeof(comedi_cmd));
	com.cmd[1] = (comedi_cmd *) malloc(sizeof(comedi_cmd));
	data.dacSample = (sampl_t *) malloc(sizeof(sampl_t) * data.dacLength);
	data.adcSample = (sampl_t *) malloc(sizeof(sampl_t) * data.adcLength);
	data.more = (sampl_t *) malloc(sizeof(sampl_t) * BUF_LEN);
	#endif


	// Open our comedi device
    	com.dev = comedi_open(DFLT_COMEDI_DEVICE_NAME);
    	if(com.dev == NULL){
		comedi_perror("error opening comedi device : startUp ");
		return -1;
	}

}


void cleanUp() {

	#ifdef MEX
	mxFree(com.cmd[0]);
	mxFree(com.cmd[1]);
	mxFree(data.dacSample);
	mxFree(data.adcSample);
	mxFree(data.more);
	#else
	free(com.cmd[0]);
	free(com.cmd[1]);
	free(data.dacSample);
	free(data.adcSample);
	free(data.more);
	#endif

	comedi_close(com.dev);

}


void fromPhys() {

    unsigned int maxdata[MAX_DAC_CHAN_NUM];
    comedi_range * cr[MAX_DAC_CHAN_NUM];
    int i,j,k=0;

	for ( i=0 ; i<data.dacChannelNum ; i++ ) {
    		maxdata[i] = comedi_get_maxdata(com.dev, DFLT_DAC_SUB_DEV, data.dacChannelList[i] );
		cr[i] = comedi_get_range(com.dev, DFLT_DAC_SUB_DEV, data.dacChannelList[i], RANGE);
	}

	for ( i=0 ; i<data.signalLength ; i++ ) {
		for ( j=0 ; j<data.dacChannelNum ; j++ ) {
			data.dacSample[k++] = (sampl_t) comedi_from_phys(data.dacSignal[j*data.signalLength+i],cr[j],maxdata[j]);
			//fprintf(stderr,"dac sample: %d\n",data.dacSample[k-1]);
		}
	}

}


void toPhys() {
	int current;
	unsigned int maxdata[MAX_ADC_CHAN_NUM];
	comedi_range *cr[MAX_ADC_CHAN_NUM];
	int i,j,k=0;

	// Get info needed for conversion from sampl_t to volts
	for ( i=0 ; i<data.adcChannelNum ; i++ ) {
		maxdata[i] = comedi_get_maxdata(com.dev, DFLT_ADC_SUB_DEV, data.adcChannelList[i]);
		cr[i] = comedi_get_range(com.dev, DFLT_ADC_SUB_DEV, data.adcChannelList[i], RANGE);
	}

	for ( i=0 ; i<data.adcChannelNum ; i++ ) {
		for ( j=0 ; j<data.signalLength ; j++ ) {
			data.adcSignal[k++] = comedi_to_phys(data.adcSample[j*data.adcChannelNum+i],cr[i], maxdata[i]);
			//fprintf(stderr,"adc sample: %d\n",data.adcSample[j*data.adcChannelNum+i]);
		}
	}
}


int makeMore() {

	int i,j;
	sampl_t *last = data.dacSample + data.dacLength - data.dacChannelNum;

	for( i=0 ; i<BUF_LEN/2 ; i++ ) {
		for ( j=0 ; j<data.dacChannelNum ; j++ ) {
			data.more[data.dacChannelNum*i+j] = *(last+j);
                }
        }
	//fprintf(stderr,"last sample-2: %d\nlast: %d\nlast sample+1: %d\nlast+1: %d\n",data.dacSample[data.dacLength-2],*last,data.dacSample[data.dacLength-1],*(last+1));

	return 0;
}


void *dacThread() {

	int bytesWritten=0;
	int samplesWritten=0;
	int samplesLeft = data.dacLength;
	int count=2;

	while (count--) {
		while (samplesLeft > 0) {
			if (count==1) {
				bytesWritten = write(comedi_fileno(com.dev), (void *) (data.dacSample+samplesWritten), samplesLeft * sizeof(sampl_t));
			} else {
				bytesWritten = write(comedi_fileno(com.dev), (void *) data.more, BUF_LEN * sizeof(sampl_t));
			}
			samplesLeft -= (bytesWritten / sizeof(sampl_t));
			samplesWritten += (bytesWritten / sizeof(sampl_t));
			if (bytesWritten < 0) {
				perror("write error : dacThread ");
				pthread_exit ((void *) -1);
			}
		}
		samplesLeft=BUF_LEN;
		samplesWritten=0;
	}
}


void *adcThread() {

	int bytesRead=0;
	int samplesExpected = data.adcLength;
	int samplesRead = 0;

	//fprintf(stderr,"adc len: %d\ndac len: %d\n",data.adcLength,data.dacLength);
	while(samplesExpected) {
		bytesRead = read(comedi_fileno(com.dev), (void *) (data.adcSample+samplesRead), samplesExpected * sizeof(sampl_t));
		samplesExpected -= bytesRead / sizeof(sampl_t);
		samplesRead += bytesRead / sizeof(sampl_t);
		//fprintf(stderr,"bytes read: %d\n",bytesRead);
	     
		if ((bytesRead < 0) || (bytesRead % 2 != 0)) {
			perror("read error : adcThread ");
			pthread_exit((void *) -1);
		}
	}
}


int makeDacCommand() {

	int i;

	for ( i=0 ; i<data.dacChannelNum ; i++ ) {
		data.dacCrPackList[i] = CR_PACK(data.dacChannelList[i], RANGE, AREF_DIFF);  // channel, range, aref
	}

	memset(com.cmd[0], 0, sizeof(comedi_cmd));
	com.cmd[0]->subdev = DFLT_DAC_SUB_DEV;
	com.cmd[0]->flags = 0;
	com.cmd[0]->start_src = TRIG_INT;
	com.cmd[0]->start_arg = 0;
	com.cmd[0]->scan_begin_src = TRIG_TIMER;
	com.cmd[0]->scan_begin_arg = 1e9/data.frequency;
	com.cmd[0]->convert_src = TRIG_NOW;
	com.cmd[0]->convert_arg = 0;
	com.cmd[0]->scan_end_src = TRIG_COUNT;
	com.cmd[0]->scan_end_arg = data.dacChannelNum;
	com.cmd[0]->stop_src = TRIG_NONE;
	com.cmd[0]->stop_arg = 0;
	com.cmd[0]->chanlist = data.dacCrPackList;
	com.cmd[0]->chanlist_len = data.dacChannelNum;

	if ((comedi_command(com.dev, com.cmd[0])) < 0) {
		comedi_perror("command error : makeDacCommand ");
		return -1;
	}
}


int makeAdcCommand() {

	int i;

	for ( i=0 ; i<data.adcChannelNum ; i++ ) {
		data.adcCrPackList[i] = CR_PACK(data.adcChannelList[i], RANGE, AREF_DIFF);  // channel, range, aref
	}

	memset(com.cmd[1], 0, sizeof(comedi_cmd));
	com.cmd[1]->subdev = DFLT_ADC_SUB_DEV;
	com.cmd[1]->flags = 0;
	com.cmd[1]->start_src = TRIG_INT;
	com.cmd[1]->start_arg = 0;
	com.cmd[1]->scan_begin_src = TRIG_TIMER;
	com.cmd[1]->scan_begin_arg = 1e9/data.frequency;
	com.cmd[1]->convert_src = TRIG_TIMER;
	com.cmd[1]->convert_arg = 0;
	com.cmd[1]->scan_end_src = TRIG_COUNT;
	com.cmd[1]->scan_end_arg = data.adcChannelNum;
	com.cmd[1]->stop_src = TRIG_NONE;
	com.cmd[1]->stop_arg = 0;
	com.cmd[1]->chanlist = data.adcCrPackList;
	com.cmd[1]->chanlist_len = data.adcChannelNum;

	comedi_command_test(com.dev, com.cmd[1]);
	if ((comedi_command(com.dev, com.cmd[1])) < 0) {
		comedi_perror("command error : makeAdcCommand ");
		return -1;
	}
}


int comediIntTrigger()
{
    static comedi_insnlist start_insns; // List of commands to start stuff
    static comedi_insn insn[3];	// instructions to start acquisition
    lsampl_t delays[] = {0, DELAY, 0};

    start_insns.n_insns = 3;
    start_insns.insns = insn;

    memset(insn, 0, 3 * sizeof(comedi_insn));

    insn[0].insn = INSN_INTTRIG;
    insn[0].subdev = DFLT_DAC_SUB_DEV;
    insn[0].n = 1;
    insn[0].data = &delays[0];

    insn[1].insn = INSN_WAIT;
    insn[1].n = 1;
    insn[1].data = &delays[1];

    insn[2].insn = INSN_INTTRIG;
    insn[2].subdev = DFLT_ADC_SUB_DEV;
    insn[2].n = 1;
    insn[2].data = &delays[2];
	
    return comedi_do_insnlist(com.dev, &start_insns);
}

void intHandler(int sig)
{
	pthread_kill(outThread,SIGSTOP);
	pthread_kill(inThread,SIGSTOP);
	cleanUp();
	signal(SIGINT,matlabHandler);

	#ifdef MEX
	mexErrMsgTxt("User Interrupt");
	#else
	exit (0);
	#endif

}



#ifdef MEX

void mexFunction( int nlhs, mxArray **plhs, int nrhs, const mxArray **prhs ) {

	int i;

	matlabHandler = signal(SIGINT,SIG_IGN);

	// foo(DAC (output) data matrix, ADC (input) channel list, frequency)

	/*
	 *	DO SOME DIMENSION CHECKING
	 */

	// IF
	// the number of input arguments > 3
	// OR 
	// the number of input arguments < 1
	// OR
	// the number of output arguments > 1 
	if (((nrhs > 3) || (nrhs < 1)) || (nlhs >1)) {
		mexErrMsgTxt("Invalid number of arguments");
	}

	// IF
	// the number of columns in the DAC data matrix is < 1 OR > 2 
	// OR
	// the number of rows is < 1
	if (  ( (mxGetN(prhs[0]) < 1) || (mxGetN(prhs[0]) > 2) ) || (mxGetM(prhs[0]) < 1)  ) {
		mexErrMsgTxt("Invalid size for DAC (output) data matrix");
	}

	// IF
	// the number of colums > 16 OR < 1 
	// OR
	// the number of rows is != 1
	if (nrhs > 1) {
		if (((mxGetN(prhs[1]) > 16) || (mxGetN(prhs[1]) < 1)) || (mxGetM(prhs[1]) != 1)) {
			mexErrMsgTxt("Invalid Channel list matrix");
		}
	}


	/*
	 * 	DO SOME VALUE CHECKING
	 */

	// **** ISSUES ****
	// Add more error checking later
	// 1: check for values of the channel list 0-15
	// 2: check for values of the data output (range dep. on channel #)  
	// 3: maximum bandwidth is 300k
	
	// IF
	// the frequency is < 100 OR > 150000
	if (nrhs > 2) {
		if (((*mxGetPr(prhs[2]) < 100) || (*mxGetPr(prhs[2]) > 150000))) {
			mexErrMsgTxt("Frequency is out of range (100-100000 Hz)");
		}
	}

	
	/*
	 * 	ASSIGN VALUES
	 */

	// Get the length of the input signal

	data.signalLength = mxGetM(prhs[0]);

	//
	// DAC Values
	// 1: get the number of channels the dac uses
	// 2: assign pointer to DAC signal 
	// 3: assign value of the channel list (which channels correspond to which columns)
	// 	by default it's just 0 and 1
	// 4: assign get length of DAC signal

	data.dacChannelNum = mxGetN(prhs[0]);
	data.dacSignal = mxGetPr(prhs[0]);
	data.dacChannelList[0] = 0; 
	data.dacChannelList[1] = 1; 
	data.dacLength = data.signalLength * data.dacChannelNum;


	// ADC Values
	// 1: get number of channels 
	// 2: assign value of the channel list 
	// 3: get length of ADC signal
	// 4: create a Matlab matrix 
	// 5: assign a pointer to the Matlab matrix 

	if (nrhs > 1) {
		data.adcChannelNum = mxGetN(prhs[1]);
		for ( i=0 ; i<data.adcChannelNum ; i++ ) {
			data.adcChannelList[i] = (int) mxGetPr(prhs[1])[i];
		}
	} else {
		data.adcChannelNum = 2;
		data.adcChannelList[0] = 0;
		data.adcChannelList[1] = 1;
	}
	data.adcLength = data.signalLength * data.adcChannelNum;
	plhs[0]=mxCreateDoubleMatrix(data.signalLength,data.adcChannelNum,mxREAL);
	data.adcSignal = mxGetPr(plhs[0]);



	// Get the frequency
	
	if (nrhs > 2) {
		data.frequency = *mxGetPr(prhs[2]);
	} else {
		data.frequency = DFLT_FREQUENCY;
	}
	 
	// Check for maximum bandwidth
	if (data.adcChannelNum * data.frequency > MAX_BANDWIDTH) {
		mexErrMsgTxt("Maximum bandwidth of the device exceeded");
	}

	// start the main routine
	
	if (start() < 0) {
		cleanUp();
		mexErrMsgTxt("Exiting Process due to error(s)");
	}

	// Clean up memory and device
	cleanUp();
}

#else

int main () {
	printf("This option hasn't been implemented yet\n");
}

#endif
    



int start() {

	int rv1, rv2;


	// allocate dynamic memory and open comedi device
	if (startUp() < 0){
		return -1;
	}
	matlabHandler = signal(SIGINT,intHandler);

	// convert signals to hardware samples and arrange
	// the samples so that the device knows how to read them
	fromPhys();

	// enter and execute triggering and channel info 
	// to the ADC subdevice
	if (makeAdcCommand() < 0 ) {
		return -1;
	}
    
	// enter and execute triggering and channel info 
	// to the DAC subdevice
	if (makeDacCommand() < 0 ) {
		return -1;
	}

	if (makeMore() < 0 ) {
		return -1;
	}

	// create thread that writes to the device
	pthread_create(&outThread, NULL, *dacThread, NULL); 

	// create a thread that reads the device
	pthread_create(&inThread, NULL, *adcThread, NULL); 

	usleep(10000);

	// Trigger
	if (comediIntTrigger() < 0) {
		perror("comediIntTrigger\n");
		return -1;
	}

	// wait for the write thread to finish
	pthread_join(outThread, (void *) &rv1);

	// wait for the read thread to finish
	pthread_join(inThread, (void *) &rv2);

	if (( rv2 < 0 ) || ( rv1 < 0 )) {
		return -1;
	}

	// convert hardware samples to signals
	toPhys();

return 0;
}