//kakubotics
//This program is used to control a robot car using a app that communicates with Arduino trough a bluetooth module

#include<AFMotor.h>

//creates three objects to control the terminal 2,3 and 4 of motor shield
AF_DCMotor motor1(3);
AF_DCMotor motor2(4);


char state;
  int vSpeed=200;     // Default speed, from 0 to 255
  //Bluetooth (HC-05 JY-MCU) State pin on pin 2 of Arduino
  const int BTState = 2;
//Leds connected to Arduino UNO Pin 7
  const int lights1  = 7;
//Leds connected to Arduino UNO Pin 9
  const int lights2  = 9;
   const int lights3  = 10;
    const int lights4  = 11;

  int i=0,j=0,k=0,m=0;



void setup()
{

    pinMode(lights1, OUTPUT);
    pinMode(lights2, OUTPUT);
    pinMode(lights3, OUTPUT);
    pinMode(lights4, OUTPUT);
    pinMode(BTState, INPUT);

    Serial.begin(9600);  //Set the baud rate to your Bluetooth module.

}

void loop(){

    //Stop car when connection lost or bluetooth disconnected
     if(digitalRead(BTState)==LOW) { Stop(); }

  if(Serial.available() > 0){
    state = Serial.read();  //Save income data to variable 'state'
    Stop(); //initialize with motors stoped
    //Change pin mode only if new command is different from previous.
    //Serial.println(command);

     //Change speed if state is equal from 0 to 9. Values must be from 0 to 255 (PWM)
    if (state == '0'){
      vSpeed=0;}
    else if (state == '1'){
      vSpeed=100;}
    else if (state == '2'){
      vSpeed=120;}
    else if (state == '3'){
      vSpeed=140;}
    else if (state == '4'){
      vSpeed=160;}
    else if (state == '5'){
      vSpeed=180;}
    else if (state == '6'){
      vSpeed=200;}
    else if (state == '7'){
      vSpeed=220;}
    else if (state == '8'){
      vSpeed=240;}
    else if (state == '9'){
      vSpeed=255;}

    switch(state){
    case 'F':
      forward();
      break;
    case 'B':
       back();
      break;
    case 'L':
      left();
      break;
    case 'R':
      right();
      break;
    case 'W':
      frontlightson();
      break;
    case 'w':
      frontlightsoff();
     break;
    case 'U':
      backlightson();
      break;
    case 'u':
      backlightsoff();
      break;
    case 'V':
      upon();
      break;
    case 'v':
      upoff();
      break;
    case 'X':
      dowon();
      break;
    case 'x':
      dowoff();
      break;
    }
  }
}

void forward()
{
  motor1.setSpeed(vSpeed); //Define vSpeed velocity
  motor1.run(FORWARD); //rotate the motor clockwise
  motor2.setSpeed(vSpeed); //Define vspeed velocity
  motor2.run(FORWARD); //rotate the motor clockwise
}

void back()
{
  motor1.setSpeed(vSpeed);
  motor1.run(BACKWARD); //rotate the motor counterclockwise
  motor2.setSpeed(vSpeed);
  motor2.run(BACKWARD); //rotate the motor counterclockwise
}

void left()
{
  motor1.setSpeed(vSpeed); //Define maximum velocity
  motor1.run(FORWARD); //rotate the motor clockwise
  motor2.setSpeed(0);
  motor2.run(RELEASE); //turn motor2 off
}

void right()
{
  motor1.setSpeed(0);
  motor1.run(RELEASE); //turn motor1 off
  motor2.setSpeed(vSpeed); //Define maximum velocity
  motor2.run(FORWARD); //rotate the motor clockwise
}

void Stop()
{
  motor1.setSpeed(0);
  motor2.run(RELEASE); //turn motor1 off
  motor2.setSpeed(0);
  motor2.run(RELEASE); //turn motor2 off

}

void upon()
{
          if (k==0){
         digitalWrite(lights3, HIGH);
         k=1;
      }
      else if (k==1){
         digitalWrite(lights3, LOW);
         k=0;
      }


}

void upoff()
{
            if (k==0){
         digitalWrite(lights3, HIGH);
         k=1;
      }
      else if (k==1){
         digitalWrite(lights3, LOW);
         k=0;
      }


}

void dowon()
{
            if (m==0){
         digitalWrite(lights4, HIGH);
         m=1;
      }
      else if (m==1){
         digitalWrite(lights4, LOW);
         m=0;
      }
}

void dowoff()
{
         if (m==0){
         digitalWrite(lights4, HIGH);
         m=1;
      }
      else if (m==1){
         digitalWrite(lights4, LOW);
         m=0;
      }
}

void frontlightson()
{
        if (i==0){
         digitalWrite(lights1, HIGH);
             i=1;
      }
      else if (i==1){
         digitalWrite(lights1, LOW);
         i=0;
      }


}

void frontlightsoff()
{
        if (i==0){
         digitalWrite(lights1, HIGH);
         i=1;
      }
      else if (i==1){
         digitalWrite(lights1, LOW);
         i=0;
      }

}

void backlightson()
{
    if (j==0){
         digitalWrite(lights2, HIGH);
         j=1;
      }
      else if (j==1){
         digitalWrite(lights2, LOW);
         j=0;
      }



}

void backlightsoff()
{
    if (j==0){
         digitalWrite(lights2, HIGH);
         j=1;
      }
      else if (j==1){
         digitalWrite(lights2, LOW);
         j=0;
      }

}