intrinsic void EndTimeSlice()                                 asm(opcdSystemFunctions, byte(sysFuncSwitchContext));
intrinsic void abortTimeslice()     asm(opcdSystemFunctions, byte(sysFuncSwitchContext));

#if defined(_Some_Defines_Unique_to_Arduino_Platform)
#define EndTimeSlice abortTimeslice
#endif

#pragma config(CircuitBoardType, typeCktBoard2560)
#pragma config(Sensor, dgtl3,  red,            sensorDigitalOut)
#pragma config(Sensor, dgtl8,  yellow,         sensorDigitalOut)
#pragma config(Sensor, dgtl9,  green,          sensorDigitalOut)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "SerialInterfaceCode_maxSpeedCortex.h";
//
// uses strings to send and receive data (Sep 2012)
// added binary mode & method to switch (Oct 2012)
//

#define STRING_PAUSE 60 // 60
//
int RxDelay = 2;
task main() {

initSerialPort ();
VexDataInit( myVexDataRx );
VexDataInit( myVexDataTx );
writeDebugStreamLine("Starting...");


StartTask(serialRxTask);
StartTask(sendVexData);
#ifdef FINEPRINT
writeDebugStreamLine("FINEPRINT on");
#endif

while (true) { // begin main program loop
  //
      wait10Msec(RxDelay);

  }// end main program loop

} // end task main

 /*
Post XBee I/F Troubleshooting
This Library demonstrates Serial Communicastions
Ascii or Binary Transmission
sets up a sample framework for datatransfer frames
with chesksums
For Vex Robotics Cortex Alternative Serial Communications
*/
// Storage for the vex communications data
#define  VEXDATAOFFSET          5
#define  VEX_DATA_BUFFER_SIZE  44
#define  MESSAGE_LENGTH 19
#define  INPUT_STRING_SIZE 19

//#define  TX_STEP 1500 // ms pause between transmissions
//#define  RX_PAUSE 14   // 10 60  This is the RX loop pause
int RX_PAUSE = 1;
int TX_STEP=1250;
int remoteWheelLeft=0,remoteWheelRight=0;
//
//#define FINEPRINT 1
#define FINE_PRINT_TX 1
#define DEBUG 1

void VexDataChecksumTx(void);

long time = 0;
int hours,minutes,seconds;
//long myLeftOSE=0,myRightOSE=0;
bool TxFlag=true;

#ifdef FINEPRINT
long millisec = 0;
#endif

static  long timeout     =0;


struct { // <40 Bytes
      unsigned char  header_56;
      unsigned char  header_A5;
      unsigned char  deviceId;
      unsigned char  messageId;
      unsigned char  messageLength;
      //5 bytes
      unsigned char  hours;
      unsigned char  minutes;
      unsigned char  seconds;
      unsigned char  milli_secH;
      unsigned char  milli_secL;
        unsigned char  WheelL_H;
      unsigned char  WheelL_L;
      unsigned char  WheelR_H;
      unsigned char  WheelR_L;
      unsigned char  MotorL;
      unsigned char  MotorR;
      unsigned char  Spare_H;
      unsigned char  Spare_L;
      unsigned char  checksum;
      //19 bytes
     } datas;

union  {
   datas   data;
   unsigned char    buffer[VEX_DATA_BUFFER_SIZE];
  } vexdata;

vexdata myVexDataRx,myVexDataTx;

// Functions
void SendString(string sOutgoing) // should convert this to a task as well
   {
  while(strlen(sOutgoing))
     {
    sendChar(UART2, StringGetChar(sOutgoing, 0));
    StringDelete(sOutgoing, 0, 1);
    wait1Msec(1);
    while(!bXmitComplete(UART2)) {abortTimeslice();}
     }
   }
//

void timeTag() {
      int temp=0;
    time = time1[T1];
    temp = floor(time/1000.0); // Format Time into HH:MM:SS
    minutes = floor(temp/60.0);
    hours   = floor(minutes/60.0);
    seconds = floor(temp - (minutes*60) - (hours*3600));
    return;
  }

void initSerialPort () {
  // initialize the serial port
  configureSerialPort(UART2, uartUserControl);
  setBaudRate(UART2, baudRate115200);
  return;
  }

// tasks
task sendVexData()   {

   long timeCheck=0,timeCheckOld=0;

   while (true) {
  timeCheck = time1[T1];
  // translate timetag
  timeTag();
  // Send Data every time step
  // Send data in Binary
  if ((timeCheck >= timeCheckOld + TX_STEP)&&(TxFlag)){
//  Timestamp
    myVexDataTx.data.hours = hours;
    myVexDataTx.data.minutes = minutes;
    myVexDataTx.data.seconds = seconds;
    myVexDataTx.data.MotorL = 20;
    myVexDataTx.data.MotorR = 20;
//

      VexDataChecksumTx( );

      for (int i=0; i < MESSAGE_LENGTH; i++)
        {

      #ifdef FINE_PRINT_TX
                           long millisec =0;
                     millisec = time1[T1];
                     writeDebugStream(" %d ms ",millisec);
                     writeDebugStreamLine("Tx %d",myVexDataTx.buffer[i]);
      #endif
          sendChar(UART2, myVexDataTx.buffer[i]);
          wait1Msec(1);
          while(!bXmitComplete(UART2)) {abortTimeslice();}
         }

#ifdef FINE_PRINT_TX
   writeDebugStream("\n%d:%d:%d ",myVexDataTx.data.hours,myVexDataTx.data.minutes,myVexDataTx.data.seconds);
   writeDebugStream(" Tx good data %X\n",myVexDataTx.data.checksum);
#endif
    timeCheckOld = timeCheck;
      } // end timestep Xmit
   } // end while
} // end task TxVexData
//
/*-----------------------------------------------------------------------------*/
/*                                                                             */
/*  Timeout code                                                               */
/*                                                                             */
/*-----------------------------------------------------------------------------*/

void StartTimeout( )
{
    // Start timeout
    timeout = time1[T1];
}

void StopTimeout( )
{
    // Stop timeout
    timeout = 0;
}

int CheckTimeout( )
{
    // Is timeout running ?
    if( timeout > 0 )
        {
        if((time1[T1] - timeout) < 20)
            return(0);
        else
            return(1);
        }
    return(0);
}


void VexDataChecksumTx(  )
{
    int  i;
    int  cs = 0;

    for(i=VEXDATAOFFSET;i<abs(myVexDataTx.data.messageLength-1);i++)
      cs = cs ^ myVexDataTx.buffer[i];
    myVexDataTx.data.checksum = (cs & 0xFF);
    return;
}

void VexDataChecksumRx(  )
{
    int  i;
    int  cs = 0;

    for(i=VEXDATAOFFSET;i<abs(myVexDataRx.data.messageLength-1);i++) //myVexDataRx.data.messageLength);i++)
      cs = cs ^ myVexDataRx.buffer[i];
    myVexDataRx.data.checksum = (cs & 0xFF);
    return;
}


void VexDataInit( vexdata &v )
{
    int  i;

    // clear all
    for(i=0;i<VEX_DATA_BUFFER_SIZE;i++)
      v.buffer[i] = 0;
      // Initialize packet
      v.data.header_56    = 0x56;
      v.data.header_A5    = 0xA5;
      v.data.deviceId       = 0x02;
      v.data.messageId    = 0x02;
      v.data.messageLength= MESSAGE_LENGTH;
    //writeDebugStreamLine("init rx/tx length : %d",v.data.messageLength);
}

//task serialRxTask()
task serialRxTask()
{
    static  int serialRxState=0;
    static  int serialDataReceived = 0;
    short c;

    while (true) {
       // check for a received character
       c = getChar(UART2);
       if( c < 0 )
         {
         // no received char
         // Check for inter char timeout
         if( CheckTimeout() == 1 ){
           serialRxState = 0;
           StopTimeout();
           writeDebugStream("timeout %d \n",timeout);
           }
         //wait1Msec(1);
         }
       // A new character has been received so process it.

       // Start inter char timeout
       StartTimeout();

       // Where are we in the message parsing process ?
       switch( serialRxState )
         {
         case  0:
             // look for first header byte
             if( (unsigned char)c == myVexDataRx.data.header_56 ) {
#ifdef FINEPRINT
               millisec = time1[T1];
               writeDebugStream("\n%d ms ",millisec);
               writeDebugStream("RxState is %d ",serialRxState);
               writeDebugStream("%1X",c);
#endif
              serialRxState++;
             }
             else
               StopTimeout();

             break;

         case  1:
             // look for second header byte
             if( (unsigned char)c == myVexDataRx.data.header_A5 ) {

#ifdef FINEPRINT
               millisec = time1[T1];
               writeDebugStream("\n%d ms ",millisec);
               writeDebugStream("RxState is %d ",serialRxState);
               writeDebugStream("%1X",c);
#endif
               serialRxState++;
             }
             else
               {
               // Bad message
               StopTimeout();
               serialRxState = 0;
               writeDebugStreamLine("bombed");
               }
             break;


        case  2:
             // We have a good header so next is device ID High byte
             myVexDataRx.data.deviceId = c;

#ifdef FINEPRINT
               millisec = time1[T1];
               writeDebugStream("%d ms ",millisec);
               writeDebugStream("RxState is %d ",serialRxState);
               writeDebugStreamLine("%1X",c);
#endif
             serialRxState++;
             break;


         case  3:
             // message ID
             myVexDataRx.data.messageId=c;

#ifdef FINEPRINT
               millisec = time1[T1];
               writeDebugStream(" %d ms ",millisec);
               writeDebugStream("RxState is %d ",serialRxState);
               writeDebugStreamLine("%1X",c);
#endif
             serialRxState++;
             break;

         case  4:
             // next is message length (expect 11 or 0x0B)
             myVexDataRx.data.messageLength=c;
             if (myVexDataRx.data.messageLength > 11) {
               myVexDataRx.data.messageLength = 11;
             } // limit check message length
#ifdef FINEPRINT
               millisec = time1[T1];
               writeDebugStream(" %d ms ",millisec);
               writeDebugStream("RxState is %d ",serialRxState);
               writeDebugStreamLine("%1X",c);
#endif
             serialRxState++;
             serialDataReceived =8;
             break;

         case  5:
             // receive the data packet
             myVexDataRx.buffer[serialDataReceived ] = c;
#ifdef FINEPRINT
               millisec = time1[T1];
               writeDebugStream(" %d ms ",millisec);
               writeDebugStream("RxState is %d ",serialRxState);
               writeDebugStreamLine("%1X",c);
#endif
             if( ++serialDataReceived == MESSAGE_LENGTH-1){ // myVexDataRx.data.messageLength ){
               serialRxState++;
             }
             // check for buffer overflow - This check worked but the checksum failed
             // with buffer overflow when getting bad length data in
             if( serialDataReceived > (MESSAGE_LENGTH) )
               {
               // Error
               StopTimeout();
               serialRxState = 0;
               writeDebugStream("overflow data\n");
               }
             break;

         case  6:
             // Received checksum byte
#ifdef FINEPRINT
               millisec = time1[T1];
               writeDebugStream(" %d ms ",millisec);
               writeDebugStream("RxState is %d ",serialRxState);
               writeDebugStreamLine("%1X",c);
#endif
             // stop timeout
             StopTimeout();

             // calculate received checksum
             VexDataChecksumRx(  );
             // comapare checksums
#ifdef FINEPRINT
               millisec = time1[T1];
               writeDebugStream(" %d ms ",millisec);
               writeDebugStream("Calcd checksum is ",serialRxState);
               writeDebugStreamLine("%1X",myVexDataRx.data.checksum);
#endif

             if( (unsigned char)c == myVexDataRx.data.checksum)
               {
               // Good data
#ifdef  DEBUG
               remoteWheelLeft = myVexDataRx.data.WheelL_H << 8;
               remoteWheelLeft = myVexDataRx.data.WheelL_H & 0xFF00;
               remoteWheelLeft = remoteWheelLeft + myVexDataRx.data.WheelL_L;
               remoteWheelRight = myVexDataRx.data.WheelR_H << 8;
               remoteWheelRight = myVexDataRx.data.WheelR_H & 0xFF00;
               remoteWheelRight = remoteWheelRight + myVexDataRx.data.WheelR_L;
               writeDebugStream("\n%d:%d:%d ",myVexDataRx.data.hours,myVexDataRx.data.minutes,myVexDataRx.data.seconds);
               writeDebugStream(" PotValue = %d ",remoteWheelLeft);
   #endif
                     writeDebugStream(" good data\n");
               }
             else
               {
               writeDebugStream(" bad data\n");

               }

             // done
             serialRxState = 0;
             //wait1Msec(CHAR_PAUSE);
             break;

         default:
         } // end case
         wait1Msec(RX_PAUSE);
       } // end while
} // end task