//Left Motor => Channel A
//Right Motor => Channel B

/*

  Default pins
  
  ------Channel A------
  PinIN1 ....12 
  PinIN2...9 
  pinPwmA.....3
  
  ------Channel B------
  PinIN3.....13 
  PinIN4...8  
  pinPwmB.....11
  
  Pines de control, se declaran en la biblioteca MotorShieldR3.
*/

// #include <MotorShieldR3.h>

MotorShield Car;

#define pinfrontLights    7    //Pin that activates the Front lights.
#define pinbackLights     4    //Pin that activates the Back lights.   
#define pinpreventLights  10   //Pin that activates the Prevent lights.
#define pinbuzzer         6    //Pin that activates the Buzzer.

unsigned long milisegundos = 0;
unsigned long tiempo1 = 0;
unsigned long tiempoSegundos = 0;
int ledState = LOW;         // the current state of the output pin

char command = 'S';
char prevCommand = 'A';
int velocity = 0;          
int frecuencia = 1000;
boolean flag = false;

unsigned long timer0 = 2000;  //Stores the time (in millis since execution started) 
unsigned long timer1 = 0;  //Stores the time when the last command was received from the phone

void setup() 
{       
  Serial.begin(9600);  //Set the baud rate to that of your Bluetooth module.
  pinMode(pinfrontLights, OUTPUT);
  pinMode(pinbackLights, OUTPUT);
  pinMode(pinpreventLights, OUTPUT);
  pinMode(pinbuzzer, OUTPUT);
}

void loop(){
  if(Serial.available() > 0){ 
    timer1 = millis();   
    prevCommand = command;
    command = Serial.read(); 
    //Change pin mode only if new command is different from previous.   
    if(command!=prevCommand){
      Serial.println(command);
      switch(command){
      case 'F':  
        Car.Forward_2W(velocity, velocity);
        break;
      case 'B':  
        Car.Back_2W(velocity, velocity);
        break;
      case 'L':  
        Car.RotateLeft_2W(velocity, velocity);
        break;
      case 'R':
        Car.RotateRight_2W(velocity, velocity);  
        break;
      case 'S':  
        Car.Stopped_2W();
        break;        
      case 'I':  //FR  
        Car.ForwardRight_2W(velocity, velocity);
        break; 
      case 'J':  //BR  
        Car.BackRight_2W(velocity, velocity);
        break;        
      case 'G':  //FL  
        Car.ForwardLeft_2W(velocity, velocity);
        break; 
      case 'H':  //BL
        Car.BackLeft_2W(velocity, velocity);
        break;
      case 'W':  //Font ON 
        digitalWrite(pinfrontLights, HIGH);
        break;
      case 'w':  //Font OFF
        digitalWrite(pinfrontLights, LOW);
        break;
      case 'U':  //Back ON 
        digitalWrite(pinbackLights, HIGH);
        break;
      case 'u':  //Back OFF 
        digitalWrite(pinbackLights, LOW);
        break; 
      case 'X':  //Prevent ON 
        flag = true;   
        break;
      case 'x':  //Prevent OFF 
        flag = false;
        digitalWrite(pinpreventLights, LOW);
        break;       
      case 'V':  //Buzzer ON     
        tone(pinbuzzer,frecuencia);        
        break;
      case 'v':  //Buzzer OFF 
        noTone(pinbuzzer);
        break;       
      case 'D':  //Everything OFF 
        digitalWrite(pinfrontLights, LOW);
        digitalWrite(pinbackLights, LOW);
        Car.Stopped_2W();
        break;              
      default:  //Get velocity
        if(command=='q'){
          velocity = 255;  //Full velocity
          Car.SetSpeed_2W(velocity, velocity);
        }
        else{ 
          //Chars '0' - '9' have an integer equivalence of 48 - 57, accordingly.
          if((command >= 48) && (command <= 57)){ 
            //Subtracting 48 changes the range from 48-57 to 0-9.
            //Multiplying by 28 changes the range from 0-9 to 0-252.

            if((command >= 48) && (command <= 51)){    //Me aseguro que la velocidad no sea tan baja para que el motor se pueda mover
              command = 51;
            }
            
            velocity = (command - 48)*28;       
            Car.SetSpeed_2W(velocity, velocity);
          }
        }
      }
    }
  }
  else{

    milisegundos = millis();
    if(milisegundos > (tiempo1+250)){          //Si ha pasado 250 milisegundos ejecuta el IF
     if(flag == true){
      tiempo1 = millis();                      //Actualiza el tiempo actual
      ledState = !ledState;  
      digitalWrite(pinpreventLights, ledState);  
     }
     if(flag == false){
      digitalWrite(pinpreventLights, LOW); 
     }
    }
 
    timer0 = millis();  //Get the current time (millis since execution started).
    //Check if it has been 500ms since we received last command.
    if((timer0 - timer1)>500){  
      //More tan 500ms have passed since last command received, car is out of range.
      //Therefore stop the car and turn lights off.
      digitalWrite(pinfrontLights, LOW);
      digitalWrite(pinbackLights, LOW);
      Car.Stopped_2W();
    }
  }  
}