// https://forum.arduino.cc/t/i-am-building-an-arduino-rc-car-with-2-ultra-sonic-sensors-hc-sr04/1214989/33

int RenaPin = 13;
int Rin1Pin = 22;
int Rin2Pin = 24;
int Rin3Pin = 26;
int Rin4Pin = 28;
int RenbPin = 12;

int FenaPin = 11;
int Fin1Pin = 30;
int Fin2Pin = 32;
int Fin3Pin = 34;
int Fin4Pin = 36;
int FenbPin = 10;

char command;

//Initializing all variables

//Variables where distances measured are stored.
int distFront;
int distLeft;
int distRight;
int distBack;
//Minimum distance of 20cm from any obstacle.
int distLimit = 20;

//HCSR04 Variables
int trig = 6;
int trig2 = 2;
int echo =  7;
int echo2 = 3;
unsigned long pulsetime = 0;

//Calculate Battery Level1. Arduino
const float maxBattery1 = 8.0;
int perVolt1;                 // Percentage variable
float voltage1 = 0.0;         // Read battery voltage
int level1;
//Calculate Battery Level2. Motor
const float maxBattery2 = 6.0;
int perVolt2;                 // Percentage variable
float voltage2 = 0.0;         // Read battery voltage
int level2;

long previousMillis = -1000 * 10;
long interval = 1000 * 10;
unsigned long currentMillis;

// --------------------------------------------------------------------------- Setup
void setup() {
  Serial.begin(9600);
  pinMode(trig, OUTPUT);
  pinMode(trig2, OUTPUT);
  pinMode(echo, INPUT);
  pinMode(echo2, INPUT);

  // Setup motors

  pinMode(RenaPin, OUTPUT);
  pinMode(Rin1Pin, OUTPUT);
  pinMode(Rin2Pin, OUTPUT);
  pinMode(Rin3Pin, OUTPUT);
  pinMode(Rin4Pin, OUTPUT);
  pinMode(RenbPin, OUTPUT);

  pinMode(FenaPin, OUTPUT);
  pinMode(Fin1Pin, OUTPUT);
  pinMode(Fin2Pin, OUTPUT);
  pinMode(Fin3Pin, OUTPUT);
  pinMode(Fin4Pin, OUTPUT);
  pinMode(FenbPin, OUTPUT);

}

void loop() {
  if (Serial.available() > 0) {
    command = Serial.read();
    motor_stop();   //initialize with motors stopped
    //Change pin mode only if new command is different from previous.
    switch (command) {
      case 'F':
        //Checks for obstacle since forward command is received.
        distFront = readDistanceF();
        if (distFront < distLimit)
        {
          //Override the controller since obstacle is detected.
          override();
          distFront = readDistanceF();
          Serial.print("A");
          Serial.print(";");
          Serial.print(distFront); //send distance to MIT App
          Serial.print(";");
          Serial.print(0, DEC); //2nd value...dummy variable
          Serial.println(";");
        }
        else
        {
          //No obstacle----> move forward.
          drive_forward();
        }
        break;
      case 'B':
        distBack = readDistanceB();
        if (distBack < distLimit)
        {
          motor_stop();
          delay(500);
          drive_forward();
          delay(300);
          motor_stop();
          distBack = readDistanceB();
          Serial.print("R");
          Serial.print(";");
          Serial.print(distBack); //send distance to MIT App
          Serial.print(";");
          Serial.print(0, DEC); //2nd value...dummy variable
          Serial.println(";");
        }
        else
          drive_backward();
        break;
      case 'L':
        turn_left();
        break;
      case 'R':
        turn_right();
        break;
      case 'O' :
        drive_open();
        break;
      case 'C'  :
        drive_close();
        break;

      case 'T':     //reads distance
        {
          distFront = readDistanceF();
          Serial.print("A");
          Serial.print(";");
          Serial.print(distFront); //send distance to MIT App
          Serial.print(";");
          Serial.print(0, DEC); //2nd value...dummy variable
          Serial.println(";");
        }
        break;
    }
    //Read battery voltage every 10sec.
    currentMillis = millis();
    if (currentMillis - (previousMillis) > (interval)) {
      previousMillis = currentMillis;
      voltage1 = (analogRead(A4) * 5.0 / 1024.0) * 2.0;
      perVolt1 = (voltage1 * 100) / maxBattery1;
      if      (perVolt1 <= 75)                {
        level1 = 0;
      }
      else if (perVolt1 > 75 && perVolt1 <= 80) {
        level1 = 1;
      }
      else if (perVolt1 > 80 && perVolt1 <= 85) {
        level1 = 2;
      }
      else if (perVolt1 > 85 && perVolt1 <= 90) {
        level1 = 3;
      }
      else if (perVolt1 > 90 && perVolt1 <= 95) {
        level1 = 4;
      }
      else if (perVolt1 > 95)                 {
        level1 = 5;
      }

      voltage2 = (analogRead(A6) * 5.0 / 1024.0) * 2.0; // 6.0V
      perVolt2 = (voltage2 * 100) / maxBattery2;
      if      (perVolt2 <= 75)                {
        level2 = 0;
      }
      else if (perVolt2 > 75 && perVolt2 <= 80) {
        level2 = 1;
      }
      else if (perVolt2 > 80 && perVolt2 <= 85) {
        level2 = 2;
      }
      else if (perVolt2 > 85 && perVolt2 <= 90) {
        level2 = 3;
      }
      else if (perVolt2 > 90 && perVolt2 <= 95) {
        level2 = 4;
      }
      else if (perVolt2 > 95)                 {
        level2 = 5;
      }

      Serial.print("B");
      Serial.print(";");
      Serial.print(level1);
      Serial.print(";");
      Serial.print(level2);
      Serial.println(";");
    }
  }//serial avail ends here..

  //Checks for obstacle since forward command is received.
  distFront = readDistanceF();
  if (distFront < distLimit)
  {
    //Override the controller since obstacle is detected.
    override();
    distFront = readDistanceF();
    Serial.print("A");
    Serial.print(";");
    Serial.print(distFront); //send distance to MIT App
    Serial.print(";");
    Serial.print(0, DEC); //2nd value...dummy variable
    Serial.println(";");
  }
  //check backwards..
  distBack = readDistanceB();
  if (distBack < distLimit)
  {
    motor_stop();
    delay(500);
    drive_forward();
    delay(300);
    motor_stop();
    distBack = readDistanceB();
    Serial.print("R");
    Serial.print(";");
    Serial.print(distBack); //send distance to MIT App
    Serial.print(";");
    Serial.print(0, DEC); //2nd value...dummy variable
    Serial.println(";");
  }




}

// --------------------------------------------------------------------------- Drive

void motor_stop()
{
  analogWrite(FenaPin, 0);
  analogWrite(FenbPin, 0);
  analogWrite(RenaPin, 0);
  analogWrite(RenbPin, 0);
  digitalWrite(Rin1Pin, LOW);
  digitalWrite(Rin2Pin, LOW);
  digitalWrite(Rin3Pin, LOW);
  digitalWrite(Rin4Pin, LOW);

  digitalWrite(Fin1Pin, LOW);
  digitalWrite(Fin2Pin, LOW);
  digitalWrite(Fin3Pin, LOW);
  digitalWrite(Fin4Pin, LOW);
  delay(25);
}

void drive_forward()
{
  analogWrite(FenaPin, 255);
  analogWrite(FenbPin, 255);
  analogWrite(RenaPin, 255);
  analogWrite(RenbPin, 255);
  digitalWrite(Rin1Pin, HIGH);
  digitalWrite(Rin2Pin, LOW);
  digitalWrite(Rin3Pin, LOW);
  digitalWrite(Rin4Pin, HIGH);

  digitalWrite(Fin1Pin, HIGH);
  digitalWrite(Fin2Pin, LOW);
  digitalWrite(Fin3Pin, LOW);
  digitalWrite(Fin4Pin, HIGH);

}

void drive_backward()
{
  analogWrite(FenaPin, 255);
  analogWrite(FenbPin, 255);
  analogWrite(RenaPin, 255);
  analogWrite(RenbPin, 255);
  digitalWrite(Rin1Pin, LOW);
  digitalWrite(Rin2Pin, HIGH);
  digitalWrite(Rin3Pin, HIGH);
  digitalWrite(Rin4Pin, LOW);

  digitalWrite(Fin1Pin, LOW);
  digitalWrite(Fin2Pin, HIGH);
  digitalWrite(Fin3Pin, HIGH);
  digitalWrite(Fin4Pin, LOW);

}

void turn_left()
{
  analogWrite(FenaPin, 180);
  analogWrite(FenbPin, 180);
  analogWrite(RenaPin, 120);
  analogWrite(RenbPin, 120);
  digitalWrite(Rin1Pin, LOW);
  digitalWrite(Rin2Pin, HIGH);
  digitalWrite(Rin3Pin, LOW);
  digitalWrite(Rin4Pin, HIGH);

  digitalWrite(Fin1Pin, LOW);
  digitalWrite(Fin2Pin, HIGH);
  digitalWrite(Fin3Pin, LOW);
  digitalWrite(Fin4Pin, HIGH);

}

void turn_right()
{
  analogWrite(FenaPin, 180);
  analogWrite(FenbPin, 180);
  analogWrite(RenaPin, 120);
  analogWrite(RenbPin, 120);
  digitalWrite(Rin1Pin, HIGH);
  digitalWrite(Rin2Pin, LOW);
  digitalWrite(Rin3Pin, HIGH);
  digitalWrite(Rin4Pin, LOW);

  digitalWrite(Fin1Pin, HIGH);
  digitalWrite(Fin2Pin, LOW);
  digitalWrite(Fin3Pin, HIGH);
  digitalWrite(Fin4Pin, LOW);
}
void drive_open()
{
  analogWrite(FenaPin, 255);
  analogWrite(FenbPin, 255);
  analogWrite(RenaPin, 255);
  analogWrite(RenbPin, 255);
  digitalWrite(Rin1Pin, LOW);
  digitalWrite(Rin2Pin, HIGH);
  digitalWrite(Rin3Pin, HIGH);
  digitalWrite(Rin4Pin, LOW);

  digitalWrite(Fin1Pin, HIGH);
  digitalWrite(Fin2Pin, LOW);
  digitalWrite(Fin3Pin, LOW);
  digitalWrite(Fin4Pin, HIGH);
}
void drive_close()
{
  analogWrite(FenaPin, 255);
  analogWrite(FenbPin, 255);
  analogWrite(RenaPin, 255);
  analogWrite(RenbPin, 255);
  digitalWrite(Rin1Pin, HIGH);
  digitalWrite(Rin2Pin, LOW);
  digitalWrite(Rin3Pin, LOW);
  digitalWrite(Rin4Pin, HIGH);

  digitalWrite(Fin1Pin, LOW);
  digitalWrite(Fin2Pin, HIGH);
  digitalWrite(Fin3Pin, HIGH);
  digitalWrite(Fin4Pin, LOW);
}


//Returns distance value from ultrasonic sensor
int readDistanceF()
{
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  pulsetime = pulseIn(echo, HIGH);
  return pulsetime / 58.2; //magic
}
int readDistanceB()
{
  digitalWrite(trig2, LOW);
  delayMicroseconds(2);
  digitalWrite(trig2, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig2, LOW);
  pulsetime = pulseIn(echo2, HIGH);
  return pulsetime / 58.2; //magic
}



void override()
{
  /*Note: The time it takes for the rover to turn depends in the
    motors you use...so that will be fixed during debugging.*/

  //Stop receiving data from the user.
  Serial.end();
  delay(10);

  //Obstacle avoidance maneuver and all that jazz..
  motor_stop();
  delay(500);
  drive_backward();
  delay(300);
  motor_stop();
  delay(10);
  scanLeft();
  delay(100);
  scanRight();
  if (distLeft > distRight && distLeft > distLimit)
  {
    turn_left();
    delay(500);
    motor_stop();
  }
  else if (distRight > distLeft && distRight > distLimit)
  {
    turn_right();
    delay(500);
    motor_stop();
  }
  else
  {
    drive_backward();
    delay(300);
    motor_stop();
  }

  //Restart the receiver.
  Serial.begin(9600);
}

void scanLeft()
{
  Serial.println("Scanning Left....");
  delay(10);
  turn_left();  //turns left to scan
  delay(300);
  motor_stop();
  delay(10);
  distLeft = readDistanceF();
  //Serial.print("Left is ");
  //Serial.println(distLeft); //scan
  delay(100);
  turn_right();   //turns to the centre again
  delay(300);
  motor_stop();
}

void scanRight()
{
  Serial.println("Scanning Right....");
  delay(10);
  turn_right();
  delay(300);
  motor_stop();
  delay(10);
  distRight = readDistanceF();
  //Serial.print("Right is ");
  //Serial.println(distRight);
  delay(100);
  turn_left();
  delay(300);
  motor_stop();
}