#include <iostream>
#include <fstream>
#include <utility>
#include <iomanip>
#include <sstream>
#include <fcntl.h>
#include <assert.h>
#include "gluex_newCanlib_debug.h"
#include <sys/time.h>
#include <time.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <vector>
#include <map>
#include <stdint.h>

#include "includeAnaGate/AnaGateDllCan.h"



//---------------------------------------------------
// Global variables needed to handle messages from AnaGate
// device.
unsigned char wAddr = 0;
unsigned char rAddr = 0;
unsigned int jz_timeout_count=0;

int m_MessageBuffer[266][11]; //0 is ID, 1 is Message Length, 2-9 is 8(max) integers of data.
//---------------------------------------------------

void clearBuffer()
{
	rAddr = 0;
	wAddr = 0;
	for(int i = 0; i < 256; i++)
	{
		m_MessageBuffer[i][0] = 0;
		m_MessageBuffer[i][1] = 0;
		m_MessageBuffer[i][2] = 0;
		m_MessageBuffer[i][3] = 0;
		m_MessageBuffer[i][4] = 0;
	}
}

//------ DEBUG GLOGBAL VARIABLES -----

int TOcount = 0;  //Number of Timeouts programming bases
int NACKcount = 0;  //Number of NACKS (Bad Responses) programming bases

int FailureFlag = 0;
int badID = 0;

//-------------------

void WriteMessage( Message& mess, Device& dev, fstream* outFile ){
  /*
    Send a message over the bus.
    mess = message to send
    dev  = handle for device
  */

#ifdef ANAGATE

  int nFlag = 0x01;  // bit 0 => extended ID, bit 1 => remote telegram
  int id = mess.getID();
  const int nData = (const int)mess.getNData();
  int *data = mess.getData();
  //char dataChar[nData];
  char *dataChar = (char*)malloc(nData*sizeof(char));
  for(int i=0; i<nData; i++ ){
    dataChar[i] = (char)data[i];
  }

  if (outFile!=NULL){
    if (wAddr > 100){
        //time_t rawtime;
        //time (&rawtime);
        //*outFile << "ExtraTime:\t" << ctime (&rawtime);
      for(int i = 101; i < wAddr+1; i++){
        time_t rawtime;
        time (&rawtime);
        *outFile<<"Extra:\t" <<m_MessageBuffer[i][0]<<" | ADC byte 1: "<<m_MessageBuffer[i][2]<<" | ADC byte 2: "<<m_MessageBuffer[i][3]<<endl;
    } }
  }


  clearBuffer();
  CANWrite( dev.getHandle(), id, dataChar, nData, nFlag );

  free(dataChar);

#else

  int *data = mess.getData();
  int nData = mess.getNData();
  int id = mess.getID();
  TPCANMsg LMsg;
  LMsg.ID = (u32_t)id;
  LMsg.LEN = (u8_t)nData;
  LMsg.MSGTYPE = MSGTYPE_EXTENDED;
  for( int i=0; i<nData; i++ ){
    LMsg.DATA[i] = data[i];
  }

  CAN_Write( dev.getHandle(), &LMsg );

#endif

}

//---------------------------------------------------
Message* ReadMessage( Device& dev , fstream* outFile){
  /*
    Reads a CAN message from device dev and returns
    an Message object.
  */

  Message* recMess = new Message( 0x0, NULL, 0 );

#ifdef ANAGATE

  // Set up a function to read CAN message.  When a CAN message
  // is received, ReadCANMessage will be called...
  //int nRC = CANSetCallback( dev.getHandle(), ReadCANMessage );

  int data[8];
  int toFlag = 0; //timeoutflag

  struct timeval start, current;

  gettimeofday(&start, NULL);
  while(wAddr==rAddr)
  {
    gettimeofday(&current, NULL);
    if((current.tv_sec - start.tv_sec) > 2)
    {
      cout<<"TIMEOUT"<<endl;
	    toFlag = 1;
	    FailureFlag = 1;
	    TOcount++;
	    wAddr++;
		jz_timeout_count++;
	    break;
    }
  }

  for( int i=0; i< m_MessageBuffer[rAddr][1]; i++ ){
      data[i] = (int)(m_MessageBuffer[rAddr][i+2] & 0x000000FF );
      if (toFlag==1)
	    {
	      data[i]=0;
	      m_MessageBuffer[rAddr][0] = 0x00000000;
	    }
    }

  recMess->setID( m_MessageBuffer[rAddr][0] );
  recMess->setNData( m_MessageBuffer[rAddr][1] );
  recMess->setData( &data[0] );

  //cout<<hex<< m_MessageBuffer[rAddr][0]<<dec <<" | "<<m_MessageBuffer[rAddr][1]<<" | "<<data[0]<<" | "<<data[1]<<" | "<<data[2]<<" | r: "<<(int)rAddr<<" | w: "<<(int)wAddr<<"\n";

  if (outFile!=NULL)
    *outFile<< m_MessageBuffer[rAddr][0] <<" | "<<m_MessageBuffer[rAddr][1]<<" | "<<data[0]<<" | "<<data[1]<<" | "<<data[2]<<" | r: "<<(int)rAddr<<" | w: "<<(int)wAddr<< endl;

  //cout<< m_MessageBuffer[rAddr][0] <<" | "<<m_MessageBuffer[rAddr][1]<<" | "<<data[0]<<" | "<<data[1]<<" | "<<data[2]<<" | "<<data[3]<<" | "<<data[4]<<" | "<<data[5]<<" | "<<data[6]<<" | "<<data[7]<<" | r: "<<(int)rAddr<<" | w: "<<(int)wAddr<<"\n";

  rAddr++;

#else

    TPCANMsg readMsg;
    if (!(CAN_Read( dev.getHandle(), &readMsg ) & CAN_ERR_QRCVEMPTY)){

      int data[8];
      for ( int i=0; i<(int)readMsg.LEN; i++ ){
	data[i] = (int)( readMsg.DATA[i] & 0x000000FF );
      }
      }
      recMess->setID( (int)readMsg.ID );
      recMess->setNData( (int)( readMsg.LEN & 0x000000FF ));
      recMess->setData( &data[0] );

    }

#endif
    return recMess;

}

//-----------------------------------------------
#ifdef ANAGATE

void WINAPI ReadCANMessage(int nIdentifier, const char * pcBuffer,
			   int nBufferLen, int nFlags, int hHandle )
{
  /*
     Call back function needed to read messages from the AnaGate
     device.  When AnaGate device receives a message, this function
     should be called.
  */

  for( int i=0; i<nBufferLen; i++ )
    m_MessageBuffer[wAddr][i+2] = 0;

  for( int i = 0; i < nBufferLen; i++ ){
    m_MessageBuffer[wAddr][i+2] = pcBuffer[i];
  }

  m_MessageBuffer[wAddr][1] = nBufferLen;
  m_MessageBuffer[wAddr][0] = nIdentifier;

  wAddr++;

  return;
}

#endif

//----------------------------------------------------------

Device* OpenCANBus( char* port, char* address ){
  /*
    Function to connect to CAN bus via either PCAN-USB dongle
    or AnaGate TCP-IP gateway, and configure gateway device
    with appropriate baudarate etc.
    Function arguments are only used if opening AnaGate
    device, in which case:

    char *port = "A", "B", "C" or "D"
    char *address = IP address of gateway
  */


#ifdef ANAGATE

  Device *dev;

  int nPort = 0;  // Anagate port
  if( strcmp( port,"A" ) == 0 )nPort = 0;
  if( strcmp( port,"B" ) == 0 )nPort = 1;
  if( strcmp( port,"C" ) == 0 )nPort = 2;
  if( strcmp( port,"D" ) == 0 )nPort = 3;

  cout << "Trying to open CAN/TCP-IP gateway port " << port << "... " << endl;
  int h = 0;
  int nRC = 0;
  //tripled Timeout value, and turned off Data Confirmations, as per suggestion by AnaGate Support - dwb 7/11/12
  //nRC = CANOpenDevice( &h, TRUE, TRUE, nPort, address, 1000 );
  nRC = CANOpenDevice( &h, FALSE, TRUE, nPort, address, 3000 );

  if ( nRC != 0 ){
    cout << "Couldn't open device..... try again!" << endl;
    nRC = CANCloseDevice( h );
    return NULL;
  }

  cout << "Gateway is open... " << endl;

  // Set up CAN communication
  // baudrate = 50000, standard modus, No Termination, no Highspeed, no TimeStamp
  nRC = CANSetGlobals( h, 50000, 0, FALSE, FALSE, FALSE );

  dev = new Device( h );

  nRC = CANSetCallback( dev->getHandle(), ReadCANMessage );


  return dev;

#else

  HANDLE h;
  unsigned int wBTR0BTR1 = 0x472F;  // bit rate 50kB/s
  char *szDevNode = "/dev/pcan32";  // device mount point
  h = LINUX_CAN_Open( szDevNode, O_RDWR );
  if( h == NULL ){
    cout << "Couldn't open USB device..." << endl;
    cout << "\tFirst check the dongle is connected, then check " ;
    cout << "the device permissions." << endl;
    return NULL;
  }

  // A cout line is needed after opening the device
  // otherwise program hangs.
  cout << "Opened USB-Linux device... " << endl;

  CAN_Status( h );
  // initialise comm... bit rate 50kB/s, extended message id
  int error_no = CAN_Init( h, wBTR0BTR1, CAN_INIT_TYPE_EX );
  if( error_no ) {
    u32_t status = CAN_Status( h );
    cerr << "Status: "  << status <<dec;
    return NULL;
  }


  Device *dev = new Device( h );
  return dev;

#endif

}

//----------------------------------------------------------

void LEDSwitch(Device &dev,
	       int id, int color)
{
  /*
    Send a CAN message to change the LED.
    id    = base ID to address
    color = 0x00 => red
          = 0x01 => green
	  = 0x02 => off
  */

  // set up data field:
  int data[] = { RLED_INIT, color };

  Message mess( id, &data[0], 2 );
  WriteMessage( mess, dev );

}

//----------------------------------------------------------

void getID(Device &dev, int id, int nReplies){
  /*
    Sends a CAN message to all bases on the bus
    asking them to send back their ID.

    id     = base ID to address
    nBases = number of bases on the bus i.e. the
    number of replies that the program
    will wait for.
  */

  int read1 = 0;
  vector<int> id_vec;
  vector<int> repeat_ids;

  // set up data field:
  int data[] = { ID_INIT };
  int ndata = 1;
  Message mess( id, &data[0], ndata );
  WriteMessage( mess, dev );

  while( read1 < nReplies ){

    Message *recMess = ReadMessage( dev );
    //cout << "board id: "  << recMess->getID() << endl;

    //Check for any repeat IDs on device
    for(int it=0; it<id_vec.size(); ++it) {
      if(recMess->getID()==id_vec[it]){
        repeat_ids.push_back(recMess->getID());
       }
    }

    //Print any repeats
    if(repeat_ids.size()>=1){
      cout <<"WARNING!!!! The following CAN id's were seen multiple times, causing others to probably be missed!!!" << endl;
      for(int it=0; it<repeat_ids.size(); ++it) {
        cout << repeat_ids[it] << endl;
      }
    }

    delete recMess;
    read1++;

  }


  return;

}

//----------------------------------------------------------------------

void TestPulser(Device &dev, int id, int command)
{
  /*
    Send a message to configure or start the test pulser
    id      = base ID to address
    command = TP_FIRE       start test pulser
	      TP_ENBSYNC    enable external sync of pulser
 	      TP_DISSYNC    disable external sync of pulser

  */

  // set up data field:
  int data[] = { PULSER_INIT, command };

  Message mess( id, &data[0], 2 );
  WriteMessage( mess, dev );
}

//----------------------------------------------------------

void HVControl(Device &dev, int id, int cmd1, int cmd2, bool HVread, int nReplies ){
  /*
     Sends CAN message which switches the HV on/off by
     enabling/disabling  MVB, MVT, HVB, HVT pins.

     id = base ID to address
     HVread = 0   no message returned by base
            = 1   base will return HV status
     nReplies = no of replies to wait for

     1st data byte enables/disables MVB, MVT, HVB, HVT pins

     2nd data byte enables/disables JAM pins

     To enable HV:
     cmd1 = 0xFF: enable HV
     cmd2 = 0x00

     To disable HV:
     cmd1 = 0xF0: disable HV
     cmd2 = 0x00

     Enabling of JAM pins should only be done by experts!

  */

  // set up data field:
  int data[] = { HV_INIT, cmd1, cmd2 };

  Message mess( id, &data[0], 3 );
  WriteMessage( mess, dev );

  if( cmd1 == 0xFF && cmd2 == 0x00 )
    cout << "Switching HV on by enabling MVB, MVT, HVB, HVT" << endl;
  if( cmd1 == 0xF0 && cmd2 == 0x00 )
    cout << "Switching HV off by disabling MVB, MVT, HVB, HVT" << endl;

  int read1 = 0;
  if( HVread == true ){

    while( read1 < nReplies ){

      Message *recMess = ReadMessage( dev );
      cout << recMess->getID() << endl;
      cout << endl << "Board id: " << recMess->getID() << endl;
      cout << dec;
      int *recData = recMess->getData();
      int HV_stat;
      HV_stat = recData[1];
      cout << "HV status: " << HV_stat << endl;
      if((HV_stat & 0x40)==0x40) cout<<"ENBMVB = ON\n";  // ENBMVB STATUS
      else cout<<"ENBMVB = OFF\n";

      if((HV_stat & 0x80)==0x80) cout<<"ENBMVT = ON\n";  // ENBMVT STATUS
      else cout<<"ENBMVT = OFF\n";

      if((HV_stat & 0x10)==0x10) cout<<"ENBHVB = ON\n";	// ENBHVB STATS
      else cout<<"ENBHVB = OFF\n";

      if((HV_stat & 0x04)==0x04) cout<<"ENBHVT = ON\n";	// ENBHVT STATUS
      else cout<<"ENBHVT = OFF\n";

      if((HV_stat & 0x20)==0x20) cout<<"JAMHVB = ON\n";	// JAMHVB STATUS
      else cout<<"JAMHVB = OFF\n";

      if((HV_stat & 0x08)==0x08) cout<<"JAMHVT = ON\n";  // JAMHVT STATUS
      else cout<<"JAMHVT = OFF\n";

      delete recMess;
      read1++;

    }

  }
}

//----------------------------------------------------------

void SetVoltage(Device &dev, int id, float Voltage){
  /*
    This sends a CAN message instructing the MCU to write
    a new value to the 12-bit DAC.

    id      = base id to address
    Voltage = new voltage to set

    Voltage must be a float between 0 and 2047.5
    12-bit DAC => takes input values 0-4096

    1. Multiply Voltage by 2 and cast as integer
    => integer from 0-4096 i.e. a 12-bit number
    2. Split this value into 8 most significant bits
    and 4 least significant bits, so that they
    can fit in the 8-bit CAN message data fields.
    3. Send CAN message.
  */

  int DAchannel;
  int value = (int)(Voltage*2.);
  DAchannel = value & 0x0000FFFF;

  // set up data field:
  int data[] = { DA_INIT, ((DAchannel>>4) & 0x0000FF), ((DAchannel<<4) & 0x0000F0) };

  Message mess( id, &data[0], 3 );
  WriteMessage( mess, dev );

  //Debug Output Bytes
  //int dummy1 = (DAchannel>>4) & 0x000000FF;
  //int dummy2 = (DAchannel<<4) & 0x000000F0;
  //cout << "1st CAN byte: " << DA_INIT << endl;
  //cout << "2nd CAN byte: " << dummy1 << endl;
  //cout << "3rd CAN byte: " << dummy2 << endl;




}

//----------------------------------------------------------

void PowerDown(Device &dev, int id){
  /*
    Function sends CAN message putting control board
    in low power mode

    id = id of base to address
  */

  // set up data field:
  int data[] = { PD_INIT };

  Message mess( id, &data[0], 1 );
  WriteMessage( mess, dev );


}

//----------------------------------------------------------

void PowerUp(Device &dev, int id){
  /*
    Send a CAN message to MCU - triggers an interrupt in
    firmware which will wake it up from halt mode.
  */

  // set up data field:
  int data[] = { PU_INIT };

  Message mess( id, &data[0], 1 );
  WriteMessage( mess, dev );


}

//----------------------------------------------------------

void ADC( Device &dev, int id, int command, int nReplies ){
  /*
    Read one of the ADCs.

    id       = id of base to address
    command  = ADC_MVB   medium voltage (bottom)
	       ADC_MVT   medium voltage (top)
	       ADC_DYN   first dynode voltage
	       ADC_CAT   photocathod voltage
	       ADC_DAC   DAC voltage
	       ADC_TEM   temperature monitor
	       ADC_CUR   current monitor
     nReplies = number of replies to wait for

     The message sent back by the base contains the ADC value in
     channels, this function also calibrates that value into volts
     C or A.
  */



  int i;

  // set up data field:
  int data[] = { ADC_INIT, command };

  Message mess( id, &data[0], 3 );
  WriteMessage( mess, dev );


  uint8_t value0;
  uint8_t value1;
  uint8_t value2;
  uint8_t value3;
  uint8_t value4;
  uint8_t value5;
  uint8_t value6;



  int command_check;

  int read1 = 0;
  while ( read1 < nReplies ){
    Message *recMess = ReadMessage( dev );
    read1++;

    int *recData = recMess->getData();

    value0 = recData[0];
    value1 = recData[1];
    value2 = recData[2];
    value3 = recData[3];
    value4 = recData[4];
    value5 = recData[5];
    value6 = recData[6];


    uint16_t my_variance = (value4 << 8) | (value5) ;
	double HV_correction   = ((double)value6/(32.*1023.)) * 3.00 * 1000. ;
	
    command_check = recData[3];


    int ADCchannels =  (value1 << 8) | value2 ;
    float ADCvoltage = (float)ADCchannels;

    cout << "board id: " <<  recMess->getID() << endl;
    cout << dec;

    //cout << "Command base sent back was: 0x" <<hex<< command_check << endl;
    //cout << "Command was supposed to be: 0x" <<hex<<command<<endl;
    cout << dec;

    //cout << "DEBUG: First  byte returned: " << (int)value1 << endl;
    //cout << "DEBUG: Second byte returned: " << (int)value2 << endl;

    if( command == ADC_CAT){
      ADCvoltage = (float)(ADCchannels/1023.) * 3.00 * 1000.;
	  double ADCvoltage_better = ADCvoltage + HV_correction;
      cout<<"Photocathode voltage is " << ADCvoltage << " volts.\n";
      cout<<"The photocathode voltage in ADC channels (0 - 1023) is:  " << ADCchannels << endl;
	  cout<<"16 bit photocathode is " << ADCvoltage_better << " volts.\n";
      cout<<"Variance is: " << my_variance << endl;
    }
    else if(command == ADC_DYN){
      ADCvoltage = (float)(ADCchannels/1023.) * 3.00 * 1000.;
      cout<<"1st dynode voltage is " << ADCvoltage << " volts.\n";
      cout<<"The 1st dynode voltage in ADC channels (0 - 1023) is:  " << ADCchannels << endl;
    }
    else if(command == ADC_MVT){
      ADCvoltage = (float)(ADCchannels/1023.)*3.00;
      cout<<"Medium voltage (top) is " << ADCvoltage << " volts.\n";
      cout<<"The medium voltage (top) in ADC channels (0 - 1023) is:  " << ADCchannels << endl;
    }
    else if(command == ADC_MVB){
      ADCvoltage = (float)(ADCchannels/1023.)*3.00;
      cout<<"Medium voltage (bottom) is " << ADCvoltage << " volts.\n";
      cout<<"The medium voltage (bottom) in ADC channels (0 - 1023) is:  " << ADCchannels << endl;
    }
    else if(command == ADC_TEM){
      ADCvoltage = (float)(ADCchannels/1023.)*3.00*100;
      cout<<"Temperature is " << ADCvoltage << " C.\n";
      cout<<"The temperature monitor in ADC channels (0 - 1023) is:  " << ADCchannels << endl;
    }
    else if(command == ADC_CUR){
      ADCvoltage = (float)(ADCchannels/1023.)*3.00/200.;
      cout<<"Current is " << ADCvoltage << " A.\n";
      cout<<"The current monitor in ADC channels (0 - 1023) is:  " << ADCchannels << endl;
    }
    else if(command == ADC_DAC){
      ADCvoltage = (float)(ADCchannels/1023.)*3.00;
      cout<<"DAC voltage is " << ADCvoltage << " volts.\n";
      cout<<"The DAC voltage in ADC channels (0 - 1023) is:  " << ADCchannels << endl;
    }
    else if(command == READ_HV){
      int HV_set = value1*8 + value2/32.;
      cout<<"HV last sent to base was " << HV_set << " volts.\n";
      //cout<<"The HV Set on base in ADC channels (0 - 1023) is:  " << HV_set << endl;
    }

	
    else{
      cout<<"The voltage in ADC channels (0 - 1023) is:  " << ADCchannels << endl;
    }

    cout << endl;

    delete recMess;
  }

}

void getADC( Device &dev, int id, int command, int nReplies , fstream* outFile){
  /*
    Read one of the ADCs.

    id       = id of base to address
    command  = ADC_MVB   medium voltage (bottom)
	       ADC_MVT   medium voltage (top)
	       ADC_DYN   first dynode voltage
	       ADC_CAT   photocathod voltage
	       ADC_DAC   DAC voltage
	       ADC_TEM   temperature monitor
	       ADC_CUR   current monitor
     nReplies = number of replies to wait for

     The message sent back by the base contains the ADC value in
     channels, this function also calibrates that value into volts
     C or A.
  */

  int i;

  // set up data field:
  int data[] = { ADC_INIT, command };

  Message mess( id, &data[0], 3 );
  WriteMessage( mess, dev, outFile );

  string adc_type = "";

  int value0;
  int value1;
  int command_check;
  int value6;
  int read1 = 0;
  while ( read1 < nReplies ){
    Message *recMess = ReadMessage( dev , NULL);
    read1++;

    int *recData = recMess->getData();

    value0 = recData[1];
    value1 = recData[2];
    value6 = recData[6];
    command_check = recData[3];
    

    int ADCchannels =  (value0 << 8) | value1 ;
    float ADCvoltage = (float)ADCchannels;


    //cout << "board id 0x" << hex << recMess->getID() << endl;
    //cout << dec;

    if(command == command_check) {
      if( command == ADC_CAT){
        adc_type = "CAT";
        ADCvoltage = (float)(ADCchannels/1023.) * 3.00 * 1000. +  ((double)value6/(32.*1023.)) * 3.00 * 1000.;
        //cout<<"Photocathode voltage is " << ADCvoltage << " volts.\n";
        //cout<<"The photocathode voltage in ADC channels (0 - 1023) is:  " << ADCchannels << endl;
      }
      else if(command == ADC_DYN){
        adc_type = "DYN";
        ADCvoltage = (float)(ADCchannels/1023.) * 3.00 * 1000.;
        //cout<<"1st dynode voltage is " << ADCvoltage << " volts.\n";
        //cout<<"The 1st dynode voltage in ADC channels (0 - 1023) is:  " << ADCchannels << endl;
      }
      else if(command == ADC_MVT){
        adc_type = "MVT";
        ADCvoltage = (float)(ADCchannels/1023.)*3.00;
        //cout<<"Medium voltage (top) is " << ADCvoltage << " volts.\n";
        //cout<<"The medium voltage (top) in ADC channels (0 - 1023) is:  " << ADCchannels << endl;
      }
      else if(command == ADC_MVB){
        adc_type = "MVB";
        ADCvoltage = (float)(ADCchannels/1023.)*3.00;
        //cout<<"Medium voltage (bottom) is " << ADCvoltage << " volts.\n";
        //cout<<"The medium voltage (bottom) in ADC channels (0 - 1023) is:  " << ADCchannels << endl;
      }
      else if(command == ADC_TEM){
        adc_type = "TEM";
        ADCvoltage = (float)(ADCchannels/1023.)*3.00*100;
        //cout<<"Temperature is " << ADCvoltage << " C.\n";
        //cout<<"The temperature monitor in ADC channels (0 - 1023) is:  " << ADCchannels << endl;
      }
      else if(command == ADC_CUR){
        adc_type = "CUR";
        ADCvoltage = (float)(ADCchannels/1023.)*3.00/200.;
        //cout<<"Current is " << ADCvoltage << " A.\n";
        //cout<<"The current monitor in ADC channels (0 - 1023) is:  " << ADCchannels << endl;
      }
      else if(command == ADC_DAC){
        adc_type = "DAC";
        ADCvoltage = (float)(ADCchannels/1023.)*3.00;
        //cout<<"DAC voltage is " << ADCvoltage << " volts.\n";
        //cout<<"The DAC voltage in ADC channels (0 - 1023) is:  " << ADCchannels << endl;
      }

      else{
        adc_type = "NoType";
        //cout<<"The voltage in ADC channels (0 - 1023) is:  " << ADCchannels << endl;
      }

    }

    else cout << "WARNING: Found a mismatch!!! Skipping... " << endl;

    //std::stringstream stream;
    //stream << "0x" << hex << recMess->getID();
    //std::string result( stream.str() );
    int my_id = (int) recMess->getID();
    if (my_id==0) {cout << "Manny Found a TimeOut" << endl;}

    if (outFile!=NULL) *outFile << my_id << "\t" << adc_type << "\t" << ADCvoltage << endl;

    delete recMess;
  }
}

//----------------------------------------------------------

void CheckVersion( Device &dev, int id, int nReplies, string version_num ){

  /*
   If version number passed to function does not match the version on the board
   then gives a cout statement
  */

  // set up data field:
  int data[] = { VER_INIT };

  Message mess( id, &data[0], 3 );
  WriteMessage( mess, dev );

  int read1 = 0;

  while( read1 < nReplies ){

    Message *recMess = ReadMessage( dev );
    int read_id = recMess->getID();
    int *recData = recMess->getData();
    string base_firm = recData[1]+"."+recData[2];

    if(base_firm != version_num) {
        cout << "Error: Base " << dec << read_id << " is running firmware " << base_firm << endl;
    }

    delete recMess;
    read1++;

  }

  return;


}

void ReadVersion( Device &dev, int id, int nReplies ){

  /*
    Sends a CAN message out with identifier id.

    id = id of base to address; 0x00 to address all bases on bus
    nBases = number of bases on the bus i.e. the number of replies
    that the program will wait for.


  */

  // set up data field:
  int data[] = { VER_INIT };

  Message mess( id, &data[0], 3 );
  WriteMessage( mess, dev );

  int read1 = 0;

  while( read1 < nReplies ){

    Message *recMess = ReadMessage( dev );
    int read_id = recMess->getID();
    int *recData = recMess->getData();

    cout << "board id " << read_id;
    cout << dec;
    cout << " is running firmware version " << recData[1];
    cout << "." << recData[2] << endl;
    //cout << "Third bit is: " << hex << recData[3] << endl;
      for(int i = 0; i < 8; i++)
	cout<<recData[i]<<" ";
      cout<<"\n";
    delete recMess;
    read1++;

  }

  return;


}

//----------------------------------------------------------
int BootloaderAct(Device &dev, int id, int nReplies, int linecount, std::vector<int>& retVals,bool break_on_error){

  const int timedelay = 10000;
  int badStrand = 999;
  bool upload_success=true;

  int data[] = { IAP_INIT, 0x7F };
  FILE* file1;
  file1 = fopen("BASEFIRMWARE.HEX","r");
  char dummy1[50];
  char dummy2[50];

  vector<int> StartingIDs = GetBaseIDs(dev,id,nReplies);
  vector<int> BadIDs;

  Message mess( id, &data[0], 2);

  WriteMessage( mess, dev);
  ReceiveMsg(dev, id, nReplies);

  //If upload timed out, and you are re-entering with a non-zero linecount, you want to start
  //at line = linecount of BASEFIRMWARE.HEX, so do fscanf, linecount times, to get to the
  //line you were interrupted on
  if(linecount!=0)
  {

  	for(int j = 0; j < linecount; j++)
  	{
        	fscanf(file1, "%s", &dummy1);
  	}
  }

  for(int j = linecount; j < 10000; j++)
  {
	  
	if(jz_timeout_count>=100) {
	cout << "Too many timeouts!!!! Quitting upload..." << endl;
	return 1;
	}	
	  
	badStrand = 999;
        FailureFlag = 0;
	if(j%100==0) cout<<"i: "<<j<<endl;
	//cout<<"\ni: "<<j<<"\n";

	fscanf(file1, "%s", &dummy1);
	usleep(timedelay);
    string sdummy1 = dummy1;


        //Skip lines if they refer to status address (which cannot be written to via bootloader)
        //or EOF line
        if(sdummy1.find(":0F4800")!=string::npos){
            cout << "Skipping out of bound address..." << endl;
            continue;
        }

        if(feof(file1) || sdummy1==":00000001FF") {
            cout << "End of File. Done." << endl;
            break;
        }

	    data[1] = 0x31;
	    mess.setData(&data[0]);
	    WriteMessage( mess, dev);
	    usleep(timedelay);
	    data[1] = 0xCE;
	    mess.setData(&data[0]);
	    WriteMessage( mess, dev);
	    usleep(timedelay);
	    ReceiveMsg(dev, id, nReplies);

	    //cout<<sdummy1<<"\n";

	    int size;
	    string s_size;
	    s_size.assign(sdummy1,1,2);
	    size = strtoul(s_size.c_str(),NULL,16);

	    string address;
	    address.assign(sdummy1,3,4);
	    //cout<<"Address: "<<address<<" ";
	    //cout<<"Size: "<<size<<" ";
	    //cout<<"Bytes: ";

	    string bytes[17];
	    string checksumstr;
	    int checksum;
	    int sum;
	    int addrsum;
	    string addr1;
	    string addr2;
	    addr1.assign(sdummy1,3,2);
	    addr2.assign(sdummy1,5,2);
	    sum = size;
	    sum += strtoul(addr1.c_str(),NULL,16);
	    sum += strtoul(addr2.c_str(),NULL,16);

	    for(int i = 0; i < size; i++)
	      {
		bytes[i].assign(sdummy1,9+i*2,2);
		//cout<<bytes[i]<<" ";
		sum += strtoul(bytes[i].c_str(),NULL,16);
	      }
	    checksumstr.assign(sdummy1,9+size*2,2);
	    int invsum = 0x100 - (sum % 0x100);
	    //cout<<"\n"<<hex<<"SUM: "<<sum<<" INVSUM: "<<invsum<<dec<<"\n";
	    //cout<<"CheckSumStr: "<<checksumstr<<"\n";


	    ////SEND ADDRESS
	    data[1] = 0x00;
	    mess.setData(&data[0]);
	    WriteMessage( mess, dev);
	    usleep(timedelay);
	    data[1] = 0x00;
	    mess.setData(&data[0]);
	    WriteMessage( mess, dev);
	    usleep(timedelay);
	    data[1] = strtoul(addr1.c_str(),NULL,16);
	    mess.setData(&data[0]);
	    WriteMessage( mess, dev);
	    usleep(timedelay);
	    data[1] = strtoul(addr2.c_str(),NULL,16);
	    mess.setData(&data[0]);
	    WriteMessage( mess, dev);
	    usleep(timedelay);

	    addrsum = strtoul(addr1.c_str(),NULL,16) ^ strtoul(addr2.c_str(),NULL,16);
	    //cout<<"\n"<<"Addr1: "<<addr1<<" Addr2: "<<addr2<<" AddrSum: "<<addrsum<<"\n";

	    ////SEND ADDRESS SUM
	    data[1] = addrsum;
	    mess.setData(&data[0]);
	    WriteMessage( mess, dev);
	    usleep(timedelay);

	    ////RECEIVE ADDRESS CONFIRM
	    ReceiveMsg(dev, id, nReplies);


	    ////SEND DATA "SIZE"
	    data[1] = size;  //size, 0 counts as 1, so 7 bytes = 0x06;
	    mess.setData(&data[0]);
	    WriteMessage( mess, dev);
	    usleep(timedelay);


	    for(int i = 0; i < size; i++)
	      {
		data[1] = strtoul(bytes[i].c_str(),NULL,16);
		mess.setData(&data[0]);
		WriteMessage( mess, dev);
		usleep(timedelay);
	      }

	    data[1] = strtoul(checksumstr.c_str(),NULL,16);;
	    mess.setData(&data[0]);
	    WriteMessage( mess, dev);

	    ReceiveMsg(dev, id, nReplies);

  	if(FailureFlag==1)
  	{
        BadIDs = GetBaseIDs(dev,id,nReplies);
        PrintMissingIDs(StartingIDs,BadIDs);
		retVals.push_back(j);
		retVals.push_back(badStrand);
		retVals.push_back(badID);
		upload_success = false;
		if(break_on_error) {
            cout << "ERROR ENCOUNTERED!!! STOPPING UPLOAD!!!" << endl;
            break;
		}
	}

  }

  if(!upload_success) {
    cout << "EXITING... ERRORS ENCOUNTERED DURING UPLOAD!!!!" << endl;
    return 1;
  }

  retVals.push_back(-1); //signals a completed clear if in retVals[0];
  return 0;
}


void BootloaderClear( Device &dev, int id, int nReplies ){



  const int timedelay = 5000;

  int data[] = { IAP_INIT, 0x7F };
  //FILE* file1;
  //file1 = fopen("BASEFIRMWARE.HEX","r");

  Message mess( id, &data[0], 2);

  WriteMessage( mess, dev);
  ReceiveMsg(dev, id, nReplies);

  for(int i = 0; i < 928; i++)
  {
	  
	if(jz_timeout_count>=100) {
	cout << "Too many timeouts!!!! Quitting clearing..." << endl;
	return;
	}	

	  
	  
	  
        FailureFlag = 0;
	if(i%100==0) cout<<"i: "<<i<<endl; 

	//fscanf(file1, "%s", &dummy1);
	usleep(timedelay);

	do
	  {

	    FailureFlag = 0;

	    data[1] = 0x31;
	    mess.setData(&data[0]);
	    WriteMessage( mess, dev);
	    usleep(timedelay);
	    data[1] = 0xCE;
	    mess.setData(&data[0]);
	    WriteMessage( mess, dev);
	    usleep(timedelay);
	    ReceiveMsg(dev, id, nReplies);

	    //string sdummy1 = dummy1;
	    //cout<<sdummy1<<"\n";

	    int size;
	    string s_size;
	    s_size = "10";
	    size = strtoul(s_size.c_str(),NULL,16);
	    //cout<<hex;
	    int address;
	    address = 0xC600 + i * 0x10;

	    stringstream addr_ss;
	    addr_ss<<hex<<address;
	    //address.assign(sdummy1,3,4);
	    //cout<<"Address: "<<address<<" ";
	    //cout<<" addr_ss: "<<addr_ss.str()<<" ";
	    //cout<<dec;
	    //cout<<"Size: "<<size<<" ";
	    //cout<<"Bytes: ";

	    int bytes[17];
	    string checksumstr;
	    int checksum;
	    int sum;
	    int addrsum;
	    string addr_str;
	    string addr1;
	    string addr2;

	    addr1.assign(addr_ss.str(),0,2);
	    addr2.assign(addr_ss.str(),2,2);
	    sum = size;
	    sum += strtoul(addr1.c_str(),NULL,16);
	    sum += strtoul(addr2.c_str(),NULL,16);
	    //cout<<" Addr1: "<<addr1<<" Addr2: "<<addr2<<"\n";



	    for(int i = 0; i < size; i++)
	      {
		bytes[i]=0x00;
		//cout<<bytes[i]<<" ";
		sum += bytes[i];
	      }
	    //checksumstr.assign(sdummy1,9+size*2,2);
	    int invsum = 0x100 - (sum % 0x100);
	    //cout<<"\n"<<hex<<"SUM: "<<sum<<" INVSUM: "<<invsum<<dec<<"\n";
	    //cout<<"CheckSumStr: "<<checksumstr<<"\n";

	    ////SEND ADDRESS
	    data[1] = 0x00;
	    mess.setData(&data[0]);
	    WriteMessage( mess, dev);
	    usleep(timedelay);
	    data[1] = 0x00;
	    mess.setData(&data[0]);
	    WriteMessage( mess, dev);
	    usleep(timedelay);
	    data[1] = strtoul(addr1.c_str(),NULL,16);
	    mess.setData(&data[0]);
	    WriteMessage( mess, dev);
	    usleep(timedelay);
	    data[1] = strtoul(addr2.c_str(),NULL,16);
	    mess.setData(&data[0]);
	    WriteMessage( mess, dev);
	    usleep(timedelay);


	    addrsum = strtoul(addr1.c_str(),NULL,16) ^ strtoul(addr2.c_str(),NULL,16);
	    //cout<<"\n"<<"Addr1: "<<addr1<<" Addr2: "<<addr2<<" AddrSum: "<<addrsum<<"\n";


	    ////SEND ADDRESS SUM
	    data[1] = addrsum;
	    mess.setData(&data[0]);
	    WriteMessage( mess, dev);
	    usleep(timedelay);

	    ////RECEIVE ADDRESS CONFIRM
	    ReceiveMsg(dev, id, nReplies);

	    ////SEND DATA "SIZE"
	    data[1] = size;  //size, 0 counts as 1, so 7 bytes = 0x06;
	    mess.setData(&data[0]);
	    WriteMessage( mess, dev);
	    usleep(timedelay);


	    for(int i = 0; i < size; i++)
	      {
		data[1] = bytes[i];
		mess.setData(&data[0]);
		WriteMessage( mess, dev);
		usleep(timedelay);
	      }

	    data[1] = invsum;
	    mess.setData(&data[0]);
	    WriteMessage( mess, dev);


	    ReceiveMsg(dev, id, nReplies);



	  }while(FailureFlag == 1);

  }

  //cout<<"\nTOCount: "<<TOcount<<" | NACKCount: "<<NACKcount<<"\n";
}

void ReadStat( Device &dev, int id, int nReplies,std::vector<int>& retVals ){


  int data[] = { STAT_INIT, STAT_READ };

  Message mess( id, &data[0], 2 );
  WriteMessage( mess, dev );


  int status_value;
  int read1 = 0;
  int failCount = 0;
  vector<int> id_vec;
  vector<int> repeat_ids;
  bool repeat_flag=false;
  vector<pair<int, int> > bad_stat_ids;

  while ( read1 < nReplies ){
    Message *recMess = ReadMessage( dev );
    read1++;

    int *recData = recMess->getData();

    status_value = recData[0];

    cout << "board id "  << recMess->getID() << endl;
    if(recMess->getID()==0)
	failCount++;
    cout << dec;

    //check for bad status values
    if(status_value<0 || status_value>=3) bad_stat_ids.push_back(make_pair(recMess->getID(),status_value));

    //Check for any repeat IDs on device
    for(int it=0; it<id_vec.size(); ++it) {
      if((recMess->getID()==id_vec[it]) && (recMess->getID() != 0)) repeat_flag=true;
    }

    if(!repeat_flag) id_vec.push_back(recMess->getID());
    if(repeat_flag)  repeat_ids.push_back(recMess->getID());
    repeat_flag=false;
    //done with checking


    cout<<"Status Value is " << status_value << "\n";

    delete recMess;
  }

  //Print any repeated IDs
    if(repeat_ids.size()>=1){
      cout <<"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << endl;
      cout <<"WARNING!!!! The following CAN id's were seen multiple times, causing others to probably be missed!!!" << endl;
      cout <<"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << endl;

      for(int it=0; it<repeat_ids.size(); ++it) {
        cout << repeat_ids[it] << endl;
      }
    }

    if(bad_stat_ids.size()>=1){
      cout <<"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << endl;
      cout << "WARNING!!! Unexpected status value!!!!" << endl;
      cout <<"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << endl;
      for(int it=0; it<bad_stat_ids.size(); ++it)  {
        pair<int, int> my_pair = bad_stat_ids[it];
        cout <<"ID: " << my_pair.first << " status value " << my_pair.second << endl;
      }

    }



    retVals.push_back(failCount);
}

void LightBad( Device &dev, int id, int nReplies,std::vector<int>& retVals ){

  jz_timeout_count=0;
  int data[] = { STAT_INIT, STAT_READ };

  Message mess( id, &data[0], 2 );
  WriteMessage( mess, dev );


  int status_value;
  int read1 = 0;
  int failCount = 0;

  while ( read1 < nReplies ){
    Message *recMess = ReadMessage( dev );
    read1++;

    int *recData = recMess->getData();

    status_value = recData[0];

    //cout << read1 << " board id " << recMess->getID() << endl;
    if(recMess->getID()!=0)
      retVals.push_back(recMess->getID());
      //cout << dec;

    //cout<<"Status Value is " << status_value << "\n";

    delete recMess;
  }
}

vector<int> GetBaseIDs( Device &dev, int id, int nReplies){




  int data[] = { STAT_INIT, STAT_READ };
  std::vector<int> retVals;

  int status_value;
  int read1 = 0;
  int failCount = 0;

  Message mess( id, &data[0], 2 );
  WriteMessage( mess, dev );

  while ( read1 < nReplies ){
    Message *recMess = ReadMessage( dev );
    read1++;

    int *recData = recMess->getData();

    status_value = recData[0];

    //cout << "board id 0x" << hex << recMess->getID() << endl;
    if(recMess->getID()!=0)
	retVals.push_back(recMess->getID());
    cout << dec;

    //cout<<"Status Value is " << status_value << "\n";

    delete recMess;
  }


  return retVals;
}



void WriteStat( Device &dev, int id, int nReplies, int statByte){


  int data[] = { STAT_INIT, STAT_WRITE, statByte };

  Message mess( id, &data[0], 3 );
  WriteMessage( mess, dev );

}



void ReceiveMsg( Device &dev, int id, int nReplies){

  int read1 = 0;
  if(jz_timeout_count>=100) {
    cout << "Too many timeouts!!! Exiting?" << endl;
    return;
  }

     while( read1 < nReplies ){

    Message *recMess = ReadMessage( dev );
    int read_id = recMess->getID();
    int *recData = recMess->getData();

    

    //cout << "board id 0x" << hex << read_id<<endl;

    //cout << " data0: " << recData[0]<<endl;
    //cout << " data1: " << recData[1]<<endl;
    //cout << " data2: " << recData[2]<<endl;

    //cout<<"ID: "<<read_id<<" -> ";

    if (recData[0] == 0x79)
    {
      //cout<<"GOOD\n";
    }
    else
    {
      cout<<"ID: "<<read_id<<dec<<" -> ";
      cout<<"BAD: "<<recData[0]<<"\n";
      FailureFlag = 1;
      badID = read_id;
      NACKcount++;
    }

    delete recMess;
    read1++;

  }

}

void ReadDigital( Device &dev)
{

	unsigned long nInputs;
	unsigned long nOutputs;

	CANReadDigital(dev.getHandle(), &nInputs, &nOutputs);

	cout<<hex;
	cout<<"Inputs: "<<nInputs<<"\n";
	cout<<"Outputs: "<<nOutputs<<"\n";
	cout<<dec;
}
void WriteDigital( Device &dev, unsigned long nOutputs)
{
  CANWriteDigital(dev.getHandle(), nOutputs);
}

void PrintMissingIDs(vector<int> initial, vector<int> final){

  for(unsigned int ii =0; ii < initial.size(); ++ii) {
    int initial_id = initial[ii];
    bool found = false;
    for(unsigned int j=0; j<final.size(); ++j) {
      if(initial_id==final[ii]) found = true;
    }
    if(!found) cout << "MISSING BASE " << initial[ii] << endl;
  }
}