#include <SoftwareSerial.h> // Allows communication with Bluetooth
#include <HCSR04.h>
#include <L298N.h>
#include <Servo.h>

SoftwareSerial mySerial(0, 1); // RX, TX
int data;

const unsigned int IN1 = 7;
const unsigned int IN2 = 8;
const unsigned int EN = 6;
const unsigned int IN3 = 5;
const unsigned int IN4 = 4;
const unsigned int ENB = 3;
const unsigned int trigPin = 13;
const unsigned int echoPin = 12;

int mode = 0; //mode 0 is bluetooth mode, mode 1 is obstacle-avoiding mode

UltraSonicDistanceSensor distanceSensor(13, 12);  // Initialize sensor that uses digital pins 13 and 12.

L298N motor(EN, IN1, IN2);
L298N motor2(ENB, IN3, IN4);

Servo servo1;

void setup() {
  // Open serial communications and wait for port to open:
  mySerial.begin(9600);
  Serial.begin(9600);
  while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB port only
  }
  Serial.println("Initiated");
  motor.setSpeed(225); //set the speed of the motors, between 0-255
  motor2.setSpeed (225);
  pinMode(trigPin, OUTPUT);// set the trig pin to output (Send sound waves)
  pinMode(echoPin, INPUT);// set the echo pin to input (recieve sound waves)
  servo1.attach(11);
  servo1.write(115);
}
void loop() {

  while (mode == 0)    //changed if to while loop (RA)
  {
    if (mySerial.available()) {
    data = mySerial.read(); // define data as the num recieved from BT
    Serial.println(data);

  }
    if (data == 1) {
      forward_car();
    }
    else if (data == 2) {
      back_car();
    }
    else if (data == 3) {
      left_car();
    }
    else if (data == 4) {
      right_car();
    }
    else if (data == 5) {
      stop_car();
    }

    else if (data == 6) {
       mode = 1;
       break;
    }
  }

    while (mode == 1) {
    // Every 500 miliseconds, do a measurement using the sensor and print the distance in centimeters.
    Serial.println(distanceSensor.measureDistanceCm());
    delay(500);
    long duration, distance; // start the scan
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2); // delays are required for a succesful sensor operation.
    digitalWrite(trigPin, HIGH);

    delayMicroseconds(10); //this delay is required as well!
    digitalWrite(trigPin, LOW);
    duration = pulseIn(echoPin, HIGH);

   if (distanceSensor.measureDistanceCm() < 30) {
    digitalWrite(2, HIGH); // turn the LED on (HIGH is the voltage level)
    motor.stop();
    motor2.stop();
    delay(500);
    motor.backward();
    motor2.backward(); //actually backward
    delay(1000);
    motor.stop();
    motor2.stop();
    delay(500);


    if (lookLeft() > lookRight()) {
      motor.forward();
      motor2.backward();
      delay(90);
    }
    else {
      motor.backward(); //this means turn
      motor2.forward();
      delay(90);
    }

   }
  else
  { digitalWrite(2, LOW);
    Serial.println ("No obstacle detected. going forward");
    motor.forward(); //if there's no obstacle ahead, Go Forward!
    motor2.forward();
    delay (100);
  }


    if (mySerial.available()) {
      data = mySerial.read(); // define data as the num recieved from BT
      Serial.println(data);

    }

    if (data != 6) {
      mode = 0;
      break;
    }

  }
}


void forward_car()
{ motor.forward();
  motor2.forward();

}

void back_car()
{ motor.backward();
  motor2.backward();
}

void left_car()
{ motor.forward();
  motor2.backward();
}

void right_car()
{ motor.backward();
  motor2.forward();
}

void stop_car()
{ motor.stop();
  motor2.stop();

}

int lookRight()
    {
      servo1.write(50);
      delay(500);
      int distance = distanceSensor.measureDistanceCm();
      delay(100);
      servo1.write(115);
      return distance;
    }

    int lookLeft()
    {
      servo1.write(170);
      delay(500);
      int distance = distanceSensor.measureDistanceCm();
      delay(100);
      servo1.write(115);
      return distance;
      delay(100);
    }