// System Includes
#include <sys/name.h>
#include <sys/proxy.h>
#include <stdio.h>
#include <sys/irqinfo.h>
#include <conio.h>
#include <sys/kernel.h>
#include <math.h>
#include <errno.h>
#include <time.h>

// DT2821 Port Addresses.
#define ADCSR   0x0240   // A/D control & status register.
#define CHANCSR 0x0242   // CHANNEL control & status register.
#define ADDAT   0x0244   // A/D DATA port.
#define DACSR   0x0246   // D/A control & status register.
#define DADAT   0x0248   // D/A data port.
#define DIODAT  0x024a   // Digital IO data port.
#define SUPCSR  0x024c   // Board control & status register.
#define TMRCTR  0x024e   // Pacer clock register.

#define TRUE 1

// Global variables
pid_t proxy;
volatile unsigned counter;

int debug = 0;

#pragma off(check_stack);
pid_t far timer_int_handler()
{
     int N;
     N = 1;
     return proxy;
	 if( (++counter % N) == 0 ) return(proxy);
     return(0);
}
#pragma on(check_stack);

// Set the sampling (pacing) frequency
void set_pacer_freq(double pacer_freq)
{
     unsigned short int M, N, count;

     if (pacer_freq > 4000000) pacer_freq = 4000000;
     if (pacer_freq < (4000000/pow(2,23))) pacer_freq = 4000000/pow(2,23);
     N = ceil( log(4000000/(256*pacer_freq))/log(2) );
     N = N & 0x000F;
     if (N == 1) N = 0;
     M = 256 - ( 4000000/(pacer_freq * pow(2,N)) );
     M = M & 0x00FF;
     count = (N * 256) + M;
     outpw(TMRCTR, count);
     printf("Pacer clock frequency = %f \n", 4000000/((pow(2,N)*(256-M))));
     printf("COUNT=%x, freq=%f, M=%d, N=%d \n", count, pacer_freq, M, N);
} 

// Initialize the ADC board
void init_DT2821(double freq)
{
     outpw(SUPCSR, 0x0001);					// DT2821 Board Reset.  
     set_pacer_freq(2000);					// Set pacer clock frequency.
     outpw(DACSR, 0x0002);					// Selects dual mode DAC output and
											// DIO port 0 as input and DIO port 1
											// as output. 
     while((inpw(DACSR) & 0x0080) == 0);	// Wait for DAC READY = 1.
     outpw(DADAT, 0x0800);					// Set DAC outputs to zero.
     outpw(DADAT, 0x0800);
     while((inpw(DACSR) & 0x0080) != 0);	// Wait for DAC READY = 1.
     outpw(SUPCSR, 0x0080);					// Software trigger for D/A.
     printf("DT2821 has been initilized with DAC outputs =0.0V \n");
}

// Scan all 16 of the ADC channels for a possible input
void adc_scan(int *ptr_addata)
{
     int k;	
     for (k = 0; k < 16; ++k)
     {
          outpw(SUPCSR, 0x2240);
          outpw(CHANCSR, 0x8000);					// Activate "Load List Enable",
													// # of entries in ch. list = 1.
          outpw(ADCSR, 0x0200 + k);					// Next entry = ch. k, gain = 1.
          outpw(CHANCSR, 0x0000);					// Deactivate "Load List Enable".
          outpw(SUPCSR, 0x0010);					// Preload multiplexer.
          while ( (inpw(ADCSR) & 0x0100) != 0 );	// Wait for MUXBUSY = 0.
          outpw(SUPCSR, 0x0008);					// Software trigger pacer clock -
													// which starts A/D conversions.
          while ( (inpw(ADCSR) & 0x0080) == 0);		// Wait for A/D DONE.
          *ptr_addata++ = inpw(ADDAT);
     }
}

// Sends a word to the dac 
void put_word_dac(int x, int y)
{
     while ((inpw(DACSR) & 0x080) == 0);   // Wait for DAC READY = 1.
     outpw(DADAT, x);
     outpw(DADAT, y);	
     while((inpw(DACSR) & 0x0080) != 0);   // Wait for DAC READY = 0.
     outpw(SUPCSR, 0x0080);                // S/W trigger for D/A.
}

// Reader A process  - sends a request to get data from the collecting process
void readerA(int recieveNumber)
{
	pid_t clientPID;
	
	char messageID;
	char dio_data;

	int adcData[16];
	int adcCounter;

	float runningTime;
	clock_t startTime;
	clock_t endTime;


	printf("Starting A\n");
	
	// Name this process readerA and attach 
	if (qnx_name_attach(0, "/READERA") == -1)
	{
		if (errno == EAGAIN)
			printf("Name in use! READERA\n");
		else if (errno == EBUSY)
			printf("Name is busy! READERA\n");
		else
			printf("Error attaching readerA\n");
		exit(1);
	}

	delay(1500);

	// Locate the collection server to get messages from 
	while ((clientPID = qnx_name_locate(0, "/DATA", 80, 0)) == -1) {}

	messageID = 'A';

	// While we still have to recieve from the counter  (recieveNumber), 
	// Send the message to the clientPID (the message server), then put that
	// data to the DAC. (Reply is automatically read by send)
	
	startTime = clock();
	
	for (adcCounter=0; adcCounter<=recieveNumber; adcCounter++)
	{
		if (Send(clientPID, &messageID, &adcData, sizeof(messageID), sizeof(adcData)) == -1)
		{
			//printf("readerA: Invalid reply\n");
		}

		if (debug)
		{
			printf("readerA sending to DAC.\n");
			printf("readerA has sent: %x\n", adcData[0]);     
		}

		put_word_dac(adcData[0], adcData[1]);

		delay(1000);
	}

	endTime = clock();

	runningTime = ((endTime - startTime - 50*recieveNumber)  / (float)(recieveNumber*100));

	printf("Reader A throughput: %f ms / read cycle\n", runningTime);

	delay(500);
}

// Reader B process  - sends a request to get data from the collecting process
void readerB(int recieveNumber)
{
	pid_t clientPID;
	
	char messageID;
	char dio_data;
	
	int adcData[16];
	int adcCounter;
	int error;

	float runningTime;
	clock_t startTime;
	clock_t endTime;

	printf("Starting B\n");

	// Name this process readerA and attach 
	if (qnx_name_attach(0, "/READERC") == -1)
	{
		if (errno == EAGAIN)
			printf("Name in use! READERB\n");
		else if (errno == EBUSY)
			printf("Name is busy! READERB\n");
		else
			printf("Error attaching readerB\n");
		exit(1);
	}

	delay(1500);

	// Locate the collection server to get messages from 
	while ((clientPID = qnx_name_locate(0, "/DATA", 80, 0)) == -1) {}
	
	messageID = 'B';

	// While we still have to recieve from the counter  (recieveNumber), 
	// Send the message to the clientPID (the message server), then put that
	// data to the DAC. (Reply is automatically read by send)

	startTime = clock();

	for(adcCounter=0; adcCounter<=recieveNumber; adcCounter++)
	{
		if (Send(clientPID, &messageID, &adcData, sizeof(messageID), sizeof(adcData)) == -1)
		{	
			//printf("readerB: Invalid reply\n");
		}

		if (debug)
		{
			printf("readerB sending to DAC.\n");
			printf("readerB has sent: %x\n", adcData[0]);     
		}

		put_word_dac(adcData[0], adcData[1]);
		delay(1000);
	}

	endTime = clock();

	runningTime = ((endTime - startTime - 50*recieveNumber)  / (float)(recieveNumber*100));

	printf("Reader B throughput: %f ms / read cycle\n", runningTime);

	delay(500);

}

void Collect(int numberSamples)
{
	char messageID;
	char dio_data;

	int adcData[16];
	int i, id;

	float runningTime;
	clock_t startTime;
	clock_t endTime;
	
	pid_t serverPID;
	pid_t clientPID;
	
	init_DT2821(2000);

	messageID = 'P';

	printf("Starting Collect\n");

	// Attach the timing proxy 
	if((proxy = qnx_proxy_attach(0, &messageID, 1, 0) ) == -1 ) 
	{
		printf("Error attaching collect to proxy\n");
		exit(1);
	}
	
	// Attach this process and assign a label "DATA"
	if (qnx_name_attach(0, "/DATA") == -1)
	{
		printf("Error attaching /DATA\n");
		exit(1);
	}	

	// Attach the interrupt handler to QNX
	if((id = qnx_hint_attach(-1, &timer_int_handler, FP_SEG(&counter)))== -1) 
	{
		printf("Error attaching interrupt in 'Collect'\n");
		exit(1);
	}


	printf("Starting message handler\n");

	// Until we run out of samples, wait for a client request, check to see what
	// reader (a or b) requested the information and then post a reply.
  	
	startTime = clock();

	for (i=0; i <= numberSamples; i++)
	{
		if((serverPID = Receive(0, &messageID, sizeof(messageID) )) == -1)
		{
 			printf("Error recieving message\n");
		}

		switch(messageID)
		{			
			case 'P': 
				adc_scan(&adcData);
				dio_data = inpw(DIODAT); 
				break;

			case 'A':  
				Reply(serverPID, &adcData, sizeof(adcData));
				break;
			
			case 'B':   
				Reply(serverPID, &adcData, sizeof(adcData));
				break;
			
			default:   
				printf("Problem with the latest receive message.\n");
		}
	}

	endTime = clock();

	runningTime = ((endTime - startTime )  / (float)(numberSamples));

	printf("Collector throughput: %f ms / read cycle\n", runningTime);

	init_DT2821(10000);
	qnx_hint_detach(id);

	printf("Collection routine has completed\n");
}

void main()
{
	pid_t pidReaderA;
	pid_t pidReaderB;

	int process;
	unsigned dummy = 0;
	unsigned dummy2 = 0;

	int readA = 25;
	int readB = 25;
	int numberSamples = 400;



	// Create reader A
	if((pidReaderA = fork()) == -1)
	{
		printf ("Forking error with readerA.\n");
	}

	if(pidReaderA == 0)	
	{
		process = qnx_name_locate(0, "/READERB", dummy, &dummy2);
		qnx_name_detach(0, process);

		printf("Spawning B\n");
		readerB(readB);
		delay(1000);

		return;
	}

	// Create reader B
	if((pidReaderB = fork()) == -1)
	{
		printf ("Forking error with readerB.\n");
	}

	if(pidReaderB == 0)	
	{
		process = qnx_name_locate(0, "/READERA", dummy, &dummy2);
		qnx_name_detach(0, process);

		printf("Spawning A\n");	
		readerA(readA);
		delay(1000);
		
		return;
	}
	
	// Start the collection process (this thread)
	printf("Clocks per sec: %f", (float)CLOCKS_PER_SEC);
	process = qnx_name_locate(0, "/DATA", dummy, &dummy2);
	qnx_name_detach(0, process);
	Collect(numberSamples);

	delay(1000);
}