/*
  Exceed RC RockCrawler Driver, on MEGA board for Android
  
*/

#include <Servo.h>

// if debug is enabled then prgm assumes its on a MEGA board with bt module on serial1 lines and serial0 for USB serial
#define DEBUG false

/* MACROS, if degub is enabled, serial1 is the bluetooth port, serial0 is the USB port */
#if DEBUG == true
#define SERIAL_AVAIL Serial1.available  
#define SERIAL_READ Serial1.read
#else /* if debug is NOT enabled, serial0 is the bluetooth port, no USB exist */
#define SERIAL_AVAIL Serial.available
#define SERIAL_READ Serial.read
#endif

/* flags for specific indicators in the incomming serial buffer */
const byte S_FLAG = 0x61;  // start flag, signifies the beginning of the data in the buffer
const byte E_FLAG = 0x13;  // end flag, signifies the end of the data in the buffer
const byte B_FLAG = 0x3B;  // separator flag, signifies the separation betweeen bytes of data

/* counters and indexes */
int i,j, n, mstart;

byte inBuf[7];    // the buffer of bytes to store the incoming serial data

/* pin locations */
const int strSvrF = 10; // front steering servo
const int strSvrR = 3;  // rear steering servo
const int driveS = 6;   // throttle controller

/* Steering and throttle servo objects */
Servo frontS;  //90 is center, higher for right, lower for left, ms, 30-150
Servo rearS;  //90 is center, 30-150
Servo thrtl;  // 80 is centered signal, <80: forward, >80: backward

/*-----------------------------------------------------------------------------------------------------*/
// setup gets called once at the start of the program, will not be called again untill the board is reset
void setup(){
  
  /* Pin Modes, input or output */
  pinMode(strSvrF, OUTPUT);
  pinMode(strSvrR, OUTPUT);
  pinMode(driveS, OUTPUT);
  
  /* Steering and Throttle Servo init's */
  frontS.attach(strSvrF);
  rearS.attach(strSvrR);
  thrtl.attach(driveS);
  
  /* Integer Var's, Used for counters and indexes, init to 0 */
  i = j = n = mstart = 0;
  
  /* Center the steering servos */
  center_str();
  
  /* if in debug mode, init serial1(MEGA) to 9600 baud for receiving Bluetooth data */
  if(DEBUG)
    Serial1.begin(9600);  // set baud to 9600
    
  /* init default serial to 9600, if debug enabled this is the com port for the USB connection
     else if debug is disabled then this is the bluetooth serial connection */
  Serial.begin(9600);
  
  /* If debug is enabled then print a message to USB serial port */
  if(DEBUG){
    Serial.println("DEBUG ENABLED"); 
  }
}// end setup
/*-----------------------------------------------------------------------------------------------------*/

/*-----------------------------------------------------------------------------------------------------*/
// loop is a continual iteration, will loop forever
void loop(){
    /* if there is any serial data in the defined serial port */  
    if(SERIAL_AVAIL()>0){
      
      delay(20);        // delay a small amount to allow the serial buffer to receive all the bytes
           
      n=0;              // index for the buffer array
      
      /* read the serial data as a byte and store it in the array at the index n, iterate until no more data is available */
      while(SERIAL_AVAIL()>0){
        inBuf[n++] = SERIAL_READ();  // read and store the serial data, increment the counter
      }//end while
      
      /* if debug is enabled, 1st check the buffer for the correct flags and separators, 2nd print the raw data to the USB serial port
          3r print the interpreted commands from the data in the buffer
         if debug is NOT enabled(false) then process the buffer and execute the commands it specifies*/
      if(DEBUG){
        check_buf( mstart, (n-1), inBuf );  // check the buffer
        print_r( mstart, (n-1), inBuf );    // print the raw data
        print_d( mstart, (n-1), inBuf );    // print the interpreted commands
        proc_cmd( mstart, (n-1), inBuf );  // process the commands in the buffer
      }
      else
        proc_cmd( mstart, (n-1), inBuf );  // process the commands in the buffer
      
      /* if debug is enabled, flush the serial1(bluetooth) buffer */
      if(DEBUG)
        Serial1.flush();	// flush the serial1 buffer
      
      /* flush the default serial buffer, if debug is enabled then this is the USB serial port, if NOT enabled then this is the bluetooth port*/
      Serial.flush();
      
            
    }//end if

}// end loop
/*-----------------------------------------------------------------------------------------------------*/

/*__________ Funcions______________*/
/*-------------------------------------------------------------------*/
// processes the commands taken from the input buffer
void proc_cmd( int start, int m_end, byte inB[]){
  switch(inB[start+1]){
    case 0x30:
       steer_l((int)inB[start+3]);
       break;
    case 0x40:
       center_str();
       break;
    case 0x50:
       steer_r((int)inB[start+3]);
       break;
    default:
       center_str(); 
  }// end steering dir. and mag. switch
  
  drive_m((int)inB[start+5]);
  
}// end process command
/*-------------------------------------------------------------------*/

/*-------------------------------------------------------------------*/
// center the steering servos
void center_str(){
  frontS.write(90);
  rearS.write(90);
}// end center
/*-------------------------------------------------------------------*/


/*-------------------------------------------------------------------*/
// drive the bot with velocity val, 80->180: backwards | 80->0: forwards
void drive_m( int val){
  if((val<84)&&(val>76))
    val = 80;   
  if(val<0)
     val = 0;
  if(val>180)
     val = 180;    
  thrtl.write(val); 
}// end drive motors
/*-------------------------------------------------------------------*/


/*-------------------------------------------------------------------*/
// steer the bot left with val, map 0-100% to 30-150
void steer_l(int val){  
  if(val>100)
    val = 100;
  if(val<0)
    val = 0;

  val = map(val, 0, 100, 0, 60);

  frontS.write(90-val);
  rearS.write(90+val);
  
}// end steer left
/*-------------------------------------------------------------------*/

/*-------------------------------------------------------------------*/
// steer bot right with same map
void steer_r(int val){  
  if(val>100)
    val = 100;
  if(val<0)
    val = 0;
    
  val = map(val, 0, 100, 0, 60);

  frontS.write(90+val);
  rearS.write(90-val);
  
}// end steer right
/*-------------------------------------------------------------------*/

/*-------------------------------------------------------------------*/
//prints thef raw buffer on one line for easy debuging
void print_d(int start, int m_end, byte inB[]){
  Serial.println("-----------------------------------------------------");
  Serial.print("Bytes: ");
  Serial.print((m_end+1)-start);
  Serial.print("  - data: ");
  for(int i=start; i<=m_end; i++){    
    Serial.print(i);
    Serial.print(": 0x");
    Serial.print(inB[i], HEX);
    Serial.print(" | ");
  }
  Serial.println(":: END");
  Serial.println("-----------------------------------------------------");
}//end print data
/*-------------------------------------------------------------------*/


/*-------------------------------------------------------------------*/
// interprets and prints the data in the buffer
void print_r( int start, int m_end, byte inB[]){
  Serial.println("----------------------------------\nData Received->");
  Serial.print("Steering: ");
  switch(inB[start+1]){
    case 0x30:
      Serial.println( "LEFT");
      break;
    case 0x40:
      Serial.println("CENTER");
      break;
    case 0x50:
      Serial.println("RIGHT");
      break;
    default:
      Serial.println( "ERROR");
  }//end str dir switch
  
  Serial.print("Steering Magn.: ");
  if((inB[start+3]>=0x00) && (inB[start+3]<=0x64)){
    Serial.println(((int)inB[start+3]), DEC);
  }
  else
    Serial.println("ERROR");
    
  Serial.print("Velocity: ");
  if((inB[start+5]>=0x00) && (inB[start+5]<=0xB4)){
    Serial.println(((int)inB[start+5]), DEC); 
  }
  else
    Serial.println("ERROR");
    
  Serial.println("---------------------------------");  
  
}//end print Relative
/*-------------------------------------------------------------------*/

/*-------------------------------------------------------------------*/
// checks the buffer for the flags and separators, print an error if they are not what they should be
void check_buf(int start, int m_end, byte inB[]){
  if(inB[start] != S_FLAG)
    Serial.println("ERROR: invalid start flag");
    
  if(inB[start+2] != B_FLAG)
    Serial.println("ERROR: invalid separator at inBuff[2]");
    
  if(inB[start+4] != B_FLAG)
    Serial.println("ERROR: invalid separator at inBuff[4]");
    
  if(inB[start+6] != E_FLAG)
    Serial.println("ERROR: invalid end flag");
}// end check buffer