//******servo*****
#include <Servo.h> 
#define autoOFF 2000
#define LEFT 0
#define RIGHT 1
int servoPins[2] = {5, 6}; 
Servo servoLeft, servoRight;
Servo servo[2] ={servoLeft, servoRight};
//********

//*****BLADE***
int bladePin = 3;
Servo servoBlade;
//*************

//****fire****
#define GAS 0
#define FIRE 1
int firePins[2] = {10,11};
Servo servoGas, servo_fire;
Servo servoFire[2] ={servoGas, servo_fire};
unsigned int value_gasOff = 90;
unsigned int value_gasOn = 115;
unsigned int value_fireOff = 110;
unsigned int value_fireOn = 70;
boolean isFireOn = false;
//**************

unsigned int value_stop = 90; 
unsigned int value_forward = 160;
unsigned int value_previous[2] = {90,90};

 
#define cmdL 'L'      // UART-command for left motor
#define cmdR 'R'      // UART-command for right motor
#define cmdB 'B'      // UART-command for BLADE
#define cmdF 'F'      // UART-command for FIRE
 
char incomingByte;    // incoming data
 
char L_Data[4];       // array data for left motor
byte L_index = 0;     // index of array L
char R_Data[4];       // array data for right motor
byte R_index = 0;     // index of array R
char B_Data[1];       // array data for BLADE
byte B_index = 0;     // index of array H
char F_Data[1];       // array data for FIRE
byte F_index = 0;     // index of array F
char command;         // command
 
unsigned long currentTime, lastTimeCommand;
 
void setup() {
  Serial.begin(9600);       // initialization UART
  
  for (int i = 0; i < 2; i++) //servos init
    servo[i].attach(servoPins[i]);
  
  //blade init
  servoBlade.attach(bladePin);
  
  for (int i = 0; i < 2; i++) //FIRE init
    servoFire[i].attach(firePins[i]);

}

void loop() {
  if (Serial.available() > 0) {          // if received UART data
    incomingByte = Serial.read();        // raed byte
    
    if(incomingByte == cmdL) {           // if received data for left motor L
      command = cmdL;                    // current command
      memset(L_Data,0,sizeof(L_Data));   // clear array
      L_index = 0;                       // resetting array index
    }
    else if(incomingByte == cmdR) {      // if received data for left motor R
      command = cmdR;
      memset(R_Data,0,sizeof(R_Data));
      R_index = 0;
    }
    else if(incomingByte == cmdB) {      // if received data for BLADE
      command = cmdB;
      memset(B_Data,0,sizeof(B_Data));
      B_index = 0;
    }    
    else if(incomingByte == cmdF) {      // if received data for FIRE
      command = cmdF;
      memset(F_Data,0,sizeof(F_Data));
      F_index = 0;
    }
    else if(incomingByte == '\r') command = 'e';   // end of line
     
    if(command == cmdL && incomingByte != cmdL){
      L_Data[L_index] = incomingByte;              // store each byte in the array
      L_index++;                                   // increment array index
    }
    else if(command == cmdR && incomingByte != cmdR){
      R_Data[R_index] = incomingByte;
      R_index++;
    }
    else if(command == cmdB && incomingByte != cmdB){
      B_Data[B_index] = incomingByte;
      B_index++;
    }    
    else if(command == cmdF && incomingByte != cmdF){
      F_Data[F_index] = incomingByte;
      F_index++;
    }    
    else if(command == 'e'){                       // if we take the line end
      //Serial.println(command);
      changeOutputs(atoi(L_Data),atoi(R_Data),atoi(B_Data),atoi(F_Data));
      delay(10);
    }
    lastTimeCommand = millis();                    // read the time elapsed since application start
  }
  //check for autoOFF
  if(millis() >= (lastTimeCommand + autoOFF)){     // compare the current timer with variable lastTimeCommand + autoOFF
    changeOutputs(value_stop,value_stop,0,0);                             // stop the car
  }
}
 
void changeOutputs(int mLeft, int mRight, unsigned int bladeState, unsigned int fireState){
  if(value_previous[LEFT] > value_stop && mLeft < value_stop){
    servo[LEFT].write(mLeft); 
    delay(200);
    servo[LEFT].write(value_stop); 
    delay(200);
  }
  servo[LEFT].write(mLeft);  
  
  if(value_previous[RIGHT] > value_stop && mRight < value_stop){
    servo[RIGHT].write(mRight); 
    delay(200);
    servo[RIGHT].write(value_stop); 
    delay(200);
  }
  servo[RIGHT].write(mRight); 
  
  value_previous[LEFT] = mLeft;
  value_previous[RIGHT] = mRight;
  
  runBlade(bladeState);
  openFire(fireState);

}
 
void runBlade(unsigned int state){
  //blade is off
  if(state == 0){
    servoBlade.write(value_stop);
  }
  //blade is on
  else if(state == 1){
    servoBlade.write(value_forward);
  }
}

void openFire(unsigned int state){
  //fire is off
  if(state == 0){
    servoFire[GAS].write(value_gasOff);
    servoFire[FIRE].write(value_fireOff);
    isFireOn = false;
  }
  //fire is on
  else if(state == 1 && isFireOn == false){
    servoFire[FIRE].write(value_fireOn);
    delay(200);
    servoFire[GAS].write(value_gasOn);
    delay(200);
    isFireOn = true;
  }
}