//-----OPTIONS-----
#define BASE_TIMEOUT 50000       //time to wait for characters in us
#define BASE_WATCHDOG_COUNTS 2   //number of timeouts before kill mode takes effect
#define BASESTR_LENGTH 96

//-----BUFFER----
char basestr[BASESTR_LENGTH] = ""; //rs232 buffer for base stream

//-----FLAGS-----
int1 base_timeout_error=FALSE; //raised when timeout occurs waiting for incoming rs232
int  base_timeout_counts = 0;   //number of consecutive base timeouts - used for watchdog timer

/** 
  * This struct contains the most recent parameters recieved from base
  *
**/
struct
{
   char mode;  //the mode, M=manual, A=autonomous, K=kill (do nothing)
   int  pScale;   //power scale to apply to motors
} PARAMS;

/**
  * This function will wait for a incoming character a certain amount of time
  * until it times out and raises the timeout_error flag
**/
char base_timed_getc() {
   int32 timeout=0;
   base_timeout_error=FALSE;
   while(!kbhit(BASESTREAM)&&(timeout<BASE_TIMEOUT)) // wait up to .5ms for character
     {
          delay_us(1);
          timeout = timeout + 1;
     }
   if(kbhit(BASESTREAM))
          return(fgetc(BASESTREAM));
   else {
          base_timeout_error=TRUE;
          return(0);
   }
}

void parse_basepart(char * part)
{
   char header[3] = ""; //the parameter hear coming in (2 chars)
   char test[3] = ""; //used for header testing
   char temp[15] = ""; //temporary string for conversions, etc..
   int8 index  = 0;   //index used for lattitude and longitude for array positioning
   int8 i      = 0;    //variable used for looping
   //set header - 2 bytes
   header[0] = part[0];
   header[1] = part[1];
//!DEBUG output part
//fprintf(BASESTREAM,"PART:%sHEAD:%s\r\n",part,header);
   //test for mode
   strcpy(test,"MD");
   if(strcmp(header,test) == 0)
   {
      //set mode
      PARAMS.mode = part[2];
      //if Heading mode enable hCONTROL
      if(PARAMS.mode == 'H')
      {
         hCONTROL.enabled = 1;
         vCONTROL.enabled = 1;
      } else if (PARAMS.mode == 'A'){ 
         hCONTROL.enabled = 1;
         vCONTROL.enabled = 1;
      } else {
         hCONTROL.enabled = 0;
         vCONTROL.enabled = 0;
      }
      return;
   }

   //test for motor 1 command
   strcpy(test,"M1");
   if(strcmp(header,test) == 0)
   {
      //set motor 1 command
      //for roboteq
      strcpy(ROBO.M1,part+2);
      return;
   }
   //test for motor 2 command
   strcpy(test,"M2");
   if(strcmp(header,test) == 0)
   {
      //set motor 2 command
      //for roboteq
      strcpy(ROBO.M2,part+2);
      return;
   }
   //test for Latitude
   strcpy(test,"LT");
   if(strcmp(header,test) == 0)
   { 
     //first byte(part+2) is index
     index = *(part+2);
     //copy value to temp string
     strcpy(temp,part+3);
     //convert string to float and assign
     WAYPOINTS.point[index-1].lat = atof(temp);

//fprintf(BASESTREAM,"INDEX:%dPOINT:%f",index,WAYPOINTS.point[index-1].lat);

     return;
   }
   //test for Longitude
   strcpy(test,"LN");
   if(strcmp(header,test) == 0)
   { 
     //first byte(part+2) is index
     index = *(part+2);
     //copy value to temp string
     strcpy(temp,part+3);
     //convert string to float and assign
     WAYPOINTS.point[index-1].lon = atof(temp);
//fprintf(BASESTREAM,"INDEX:%dPOINT:%f",index,WAYPOINTS.point[index-1].lon);
     return;
   }
   //test for Power Scaling Value
   strcpy(test,"PS");
   if(strcmp(header,test) == 0)
   { 
     //copy value to temp string
     strcpy(temp,part+2);
     //convert string to float and assign
     PARAMS.pScale = atoi(temp);
     return;
   }
   //test for desired heading command
   strcpy(test,"DH");
   if(strcmp(header,test) == 0)
   { 
     //copy value to temp string
     strcpy(temp,part+2);
     //convert string to float and assign
     hCONTROL.desired = atof(temp);
     return;
   }
   //test for heading proportional gain value
   strcpy(test,"HP");
   if(strcmp(header,test) == 0)
   { 
     //copy value to temp string
     strcpy(temp,part+2);
     //convert string to float and assign
     hCONTROL.pGain = atoi(temp);
     return;
   }
   //test for heading integral gain value
   strcpy(test,"HI");
   if(strcmp(header,test) == 0)
   { 
     //copy value to temp string
     strcpy(temp,part+2);
     //convert string to float and assign
     hCONTROL.iGain = atoi(temp);
     return;
   }
   //test for heading derivitive gain value
   strcpy(test,"HD");
   if(strcmp(header,test) == 0)
   { 
     //copy value to temp string
     strcpy(temp,part+2);
     //convert string to float and assign
     hCONTROL.dGain = atoi(temp);
     return;
   }
   //test for Desired Velocity
   strcpy(test,"DV");
   if(strcmp(header,test) == 0)
   { 
     //copy value to temp string
     strcpy(temp,part+2);
     //convert string to float and assign
     vCONTROL.desired = atof(temp);
     return;
   }
   //test for velocity proportional gain value
   strcpy(test,"VP");
   if(strcmp(header,test) == 0)
   { 
     //copy value to temp string
     strcpy(temp,part+2);
     //convert string to float and assign
     vCONTROL.pGain = atoi(temp);
     return;
   }
   //test for velocity integral gain value
   strcpy(test,"VI");
   if(strcmp(header,test) == 0)
   { 
     //copy value to temp string
     strcpy(temp,part+2);
     //convert string to float and assign
     vCONTROL.iGain = atoi(temp);
     return;
   }
   //test for velocity derivitive gain value
   strcpy(test,"VD");
   if(strcmp(header,test) == 0)
   { 
     //copy value to temp string
     strcpy(temp,part+2);
     //convert string to float and assign
     vCONTROL.dGain = atoi(temp);
     return;
   }
   //test for clear waypoints
   strcpy(test,"CW");
   if(strcmp(header,test) == 0)
   { 
     //we need to clear all the waypoints
     for(i=0;i<NUM_WAYPOINTS;i++)
     {
     WAYPOINTS.point[i].lat = (float)0;
     WAYPOINTS.point[i].lon = (float)0;
     }
     //we need to set the current waypoint back to zero
     WAYPOINTS.current = 0;
     return;
   }
}

/**
  * This function parses the data from the base buffer
  *
**/
void parse_base()
{
   //----local variables----
   char separators[2] = "$";  //separators in the string
   char *part;                //Pointer Used while splitting string
   //---explode the buffer by $ and parse each part   
   part = strtok(basestr,separators);
   while(part!=0)
   {
      parse_basepart(part);
      part = strtok(0,separators);
      //parse out the part...confused?
   }

}
/**
  * This function requests data from the base, puts it in a buffer 
  * then calls parse_base to parse it
  *
**/
void query_base()
{
   //-----local variables-------
   int i=0;             //used for looping
   //------Clear existing buffer----
   for(i=0;i<BASESTR_LENGTH;i++)
   {
      basestr[i] = 0x00;
   }
   //-----Request data from base----
   fputs("$RQ\r\n",BASESTREAM);
 
   //------recieve data-----
   i=0;                    //reset counter
   base_timeout_error = 0; //reset timeout flag
   while(!base_timeout_error)
   {
      basestr[i] = base_timed_getc(); //fills basestr with data
      if(basestr[i-1] == 0x0D && basestr[i] == 0x0A)
         {
         //clean characters and break recieveing
         basestr[i-1] = 0x00;
         basestr[i] = 0x00;
         break;
         }
      i++;
   }
//did we time out?
if(base_timeout_error)
{  
   //we timed out so report it
   base_timeout_counts++;
   //do we need to kill the system?
   if(base_timeout_counts /*>= BASE_WATCHDOG_COUNTS*/)
   {
      //the shit has hit the fan, stop the boat
      PARAMS.mode = 'K'; //kill mode
      //set motors to 0!
      //set motor 1 command;
      //for roboteq 
      strcpy(ROBO.M1,"A00");
      //set motor 1 command
      //for roboteq
      strcpy(ROBO.M2,"B00");
      //OK, update motors
      update_robo();
      //let base know we watchdogged
      fprintf(BASESTREAM,"WATCHDOG KILLED");
   }
   fprintf(BASESTREAM,"BASE TIMEOUT ERROR\r\n");
} else
{
   base_timeout_counts = 0;
   //!debug output buffer
//fprintf(BASESTREAM,"BUFFER:%s\r\n",basestr);
   
   //parse data recieved from base
   parse_base();
}
   
}