#include <SoftwareSerial.h> // Allows communication with Bluetooth
#include <HCSR04.h>
#include <L298N.h>
SoftwareSerial mySerial(0, 1); // RX, TX
int data;

const unsigned int IN1 = 7;
const unsigned int IN2 = 6;
const unsigned int EN = 9;
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);

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)
}
void loop() {
  if (mySerial.available()) {
    data = mySerial.read(); // define data as the num recieved from BT
    Serial.println(data);

  }
  if (mode == 0 && data == 7) {
    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) {
    obstacle_mode();
  }


}

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

}

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

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

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

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

}

void obstacle_mode()
{ if (mode == 0) {
    mode = 1;
  }
  else if (mode == 1) {
    mode = 1;
  }

  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.forward(); //actually backward
      delay(1000);

      // Turn as long as there's an obstacle ahead.
      motor.backward(); //this means turn
      motor2.backward();
      delay(300);
    }

    else
    { digitalWrite(2, LOW);
      delay (15);
      motor.forward(); //if there's no obstacle ahead, Go Forward!
      motor2.backward();
    }
    if (mySerial.available()) {
      data = mySerial.read(); // define data as the num recieved from BT
      Serial.println(data);

    }


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

  }

}