#include "./int_to_hex.h"
#include "./io_poll.h"
#include "./telemetry.h"
//#include "../globalHeader.h"
#include <sys/types.h>
#include <sys/stat.h>
#include <signal.h>
#include <sys/shm.h>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <string>
#include <math.h>
#include "telemetry_string.h"

int globalStatus = 1;
int send_status = 1;
//extern int teleos_enable;

struct curr_adc current_adc[32];

//extern int ioFD;
int topDigitalChannel[73];	

void initialize()
{
   for (int i = 0; i <= 72; i ++)
      topDigitalChannel[i] = 1;
}


void* pollDig(void* args)
{

   // for digital IO
   int element_num_, group, port;
   unsigned char channel;
   int temp;
   extern int io_poll_digitals_thread_timer;

   initialize();

  while (1) {

	  if (globalStatus == 0)	break;
   // poll the digital values
   for (element_num_ = 1; element_num_ <= 72; element_num_++) {
      if (0 < element_num_ && element_num_ <= 24) {
         group = 0;
	 if (element_num_ <= 8) port = 0;
         else if (8 < element_num_ && element_num_ <= 16) port = 1;
         else if (16 < element_num_ && element_num_ <= 24) port = 2;
         }
      else if (24 < element_num_ && element_num_ <= 48) {
     	 group = 1;      
	 if (element_num_ <= 32) port = 0;
         else if (32 < element_num_ && element_num_ <= 40) port = 1;
         else if (40 < element_num_ && element_num_ <= 48) port = 2;
	 }
	 else if (48 <  element_num_ && element_num_ <= 72) {
	 group = 2;
	 if (element_num_ <= 56) port = 0;
         else if (56 < element_num_ && element_num_ <= 64) port = 1;
         else if (64 < element_num_ && element_num_ <= 72) port = 2;
      }

      channel = inttohex((int)pow(2.0, ((element_num_-1) % 8)));
      temp = readDigitalChannel(group, port, channel); //check for change
      if (temp != topDigitalChannel[element_num_])
      	 {
         topDigitalChannel[element_num_] = temp;       //update and transmitt to sub
         if (send_status == true) ioElementControl.send_output_down(1, 1, element_num_, !temp, -1.0);
	 }

   } // end for

   usleep(50000);	
   if(io_poll_digitals_thread_timer < 1000) io_poll_digitals_thread_timer++;

   } // end while



   return 0;

}


/***************************************************************
 * NOTE: There is no point in checking the errno number for
 *	   errors opening the previous devices as the system
 * 	   should not continue to function nor does it have
 *       any way of recovering since there would be no way
 *       to run the system without them.
 ****************************************************************/
void* 
pollIO(void* args)
{
   static int fd_AIO, fd_AIO2;
   unsigned char buff[4];
   unsigned int analogChannel;
   int intVolt, intHiByte;
   int i;
   char str_dummy[2];
   extern int io_poll_analogs_thread_timer;

   /* open the AIO16 device */
   if((fd_AIO = open("/dev/AIO16", O_RDWR)) < 0) {
		perror("**ERROR**: /dev/AIO16");
		exit(errno);
   }

   /* open the 2nd AIO16 device */
   if((fd_AIO2 = open("/dev/AIO16B", O_RDWR)) < 0) {
		perror("**ERROR**: /dev/AIO16B");
		exit(errno);
   }

   /* initialize the digital channel - logic ground */
   initialize();



  /* This is a forever loop with its own context of execution.
   * It is always needed for our system to work correctly.
   * If any of the devices were not working, we should not
   * have gotten here in the first place as the system would
   * have quit with error messages as previously declared.
   */ 
  while(1) {
	if (globalStatus == 0)	break;

	/* polling the first analog card. There will be some changes made where
 	 * we will use the device driver to poll all 16 channels at once.
	 */
   	
   	for (i = 0; i < 16; i++) {
                if (read(fd_AIO, buff, i) < 0) {
			 //alarm->AIO_BOARD_NOT_WORKING
			break;
		}
		
		current_adc[i].loByte = buff[0];
		current_adc[i].hiByte = buff[1];
		current_adc[i].loByte >>= 4;	
		intVolt = current_adc[i].loByte;
		intHiByte = current_adc[i].hiByte;
		intHiByte <<= 4;
		intVolt |= intHiByte;	

		current_adc[i].volt_old[0] = ((float)intVolt) * 5.00 / 4095.00;
		current_adc[i].intVolt = intVolt;
		
		//Box filter removed June 17 2005.  No noticable noise.
		current_adc[i].volt =  current_adc[i].volt_old[0]; //new line June 17 2005 without filter
				
		// copying  the old history	
		// To put filter back in uncomment the folloing section
		// and take out the line above. June 2005
		/*
		current_adc[i].volt_old[4] = current_adc[i].volt_old[3];
		current_adc[i].volt_old[3] = current_adc[i].volt_old[2]; 
		current_adc[i].volt_old[2] = current_adc[i].volt_old[1]; 
		current_adc[i].volt_old[1] = current_adc[i].volt_old[0]; 
				
		current_adc[i].volt =  (current_adc[i].volt_old[0]+	
					current_adc[i].volt_old[1]+		
					current_adc[i].volt_old[2]+		
					current_adc[i].volt_old[3]+		
					current_adc[i].volt_old[4])/5;
		*/
		if(send_status == 1) {
			// i+1 is the element number consisting [1:16] for
			// the first analog board.  tx the analogs to the sub
        		ioElementControl.send_output_down(1, 0, i+1, -1, current_adc[i].volt);
   		}
   	
	}

        // repeat for the 2nd analog card
	
  	for (i = 0; i < 16; i++) {
		if (read(fd_AIO2, buff, i) < 0) {
			// alarm->AIO_BOARD_NOT_WORKING
			break;
		}
		
		current_adc[i+16].loByte = buff[0];
		current_adc[i+16].hiByte = buff[1];
		current_adc[i+16].loByte >>= 4;	
		intVolt = current_adc[i+16].loByte;
		intHiByte = current_adc[i+16].hiByte;
		intHiByte <<= 4;
		intVolt |= intHiByte;	
		
		current_adc[i+16].volt_old[0] = ((float)intVolt) * 5.00 / 4095.00;
		current_adc[i+16].intVolt = intVolt;
		//printf("volt is: %f\n", current_adc[i+16].volt);
	
		// copying  the old history	
		current_adc[i+16].volt_old[4] = current_adc[i+16].volt_old[3];
		current_adc[i+16].volt_old[3] = current_adc[i+16].volt_old[2];
		current_adc[i+16].volt_old[2] = current_adc[i+16].volt_old[1];
		current_adc[i+16].volt_old[1] = current_adc[i+16].volt_old[0];

		current_adc[i+16].volt =  (current_adc[i+16].volt_old[0]+	
					current_adc[i+16].volt_old[1]+		
					current_adc[i+16].volt_old[2]+		
					current_adc[i+16].volt_old[3]+		
					current_adc[i+16].volt_old[4])/5;
	
		if (send_status == 1) {
			// i+17 is the element number consisting [17:32] for
			// the second analog board.
         		ioElementControl.send_output_down(1, 0, i+17, -1, current_adc[i+16].volt);
   		}
	}

	
	// analog output on the two boards
	
	float raw, gain, offset, analog, max, min;
 	for (int i = 1; i <= 2; i++) {
 	 	ioElementControl.readVoltage(1, 3, i, analog, raw, gain, offset, max, min);
 		if (i == 1)    	str_dummy[0] = '0';     //0 is analog output channel 1
		else           	str_dummy[0] = '1';     //1 is analog output channel 2
		
		str_dummy[1] = 0;

		if (analog < 0)	analog = 0.0;
          	
		intVolt = (int)((analog * 4095)/ 5.0);
	  	write(fd_AIO, str_dummy, intVolt);
	  	
	  	//The following outputs onto a digital channel the detritus and tools interlocks
	  	//This is onto the analog board but we use 2 of the 4 digital outputs
	  	//The second analog board is not implemented.	  	
	  	extern int detritus_interlock, tool_interlock;
	  	str_dummy[0] = '4';                     //4 is digital output 1 on the fisrt board
	  	write(fd_AIO, str_dummy, (detritus_interlock + (tool_interlock * 2)));
	  	
	        }

      for (int i = 3; i <= 4; i++) {
		ioElementControl.readVoltage(1, 3, i, analog, raw, gain, offset, max, min);
		if (i == 3) str_dummy[0] = '0';
		else        str_dummy[0] = '1';
		
		str_dummy[1] = 0;
          	
		if (analog < 0)	analog = 0.0;

          	intVolt = (int)((analog * 4095)/ 5.0);
		if (intVolt < 0)         intVolt = 0;
		else if (intVolt > 4095) intVolt = 4095;
	    	write(fd_AIO2, str_dummy, intVolt); 
                }

      usleep(50000);
      if(io_poll_analogs_thread_timer < 100000 ) io_poll_analogs_thread_timer++;
   } // end while



// disable periodic interrupts
//if ((retval = ioctl(fd_rtc, RTC_PIE_OFF, 0)) < 0) {
//	perror("ioctl");
//	exit(errno);
  // }

   close(fd_AIO);
   close(fd_AIO2);
   //close(fd_rtc);

   return 0;
}