#include <NewPing.h>
#include <Servo.h>
int right1 = 2; // in1 of right tire
int rightEN = 3; // enA of right tire
int right2 = 4; // in2 of right tire
int left1 = 5; // in1 of left tire
int leftEN = 6; // enA of left tire
int left2 = 7; // in2 of right tire

int rightLED = 8;
int leftLED = 9;

int trig = 12;  // trigger PIN
int echo = 11;  // echo PIN
int max_dist = 300; // max distance of ultrasonic
int usReading; // reading of ultrasonic
int usReadingThreshold = 15; // max distance between car and walls
NewPing US (trig,echo,max_dist); // NewPing initialization
Servo servoMotor; // Servo initialization

int MAXSPD = 170; // maximum speed of motors
int leftDistance;
int rightDistance;


void printReading(){
  Serial.print("Distance is currently ");
  Serial.println(usReading);
}

void moveRightTire(int pwm){
  digitalWrite(right1, HIGH);
  digitalWrite(right2, LOW);
  analogWrite(rightEN, pwm); // moves the right tire forward at pwm speed
}

void reverseRightTire(int pwm){
  digitalWrite(right1, LOW);
  digitalWrite(right2, HIGH);
  analogWrite(rightEN, pwm); // moves the right tire forward at pwm speed in opposite direction
}

void moveLeftTire(int pwm){
  digitalWrite(left1, HIGH);
  digitalWrite(left2, LOW);
  analogWrite(leftEN, pwm); // moves the left tire forward at pwm speed
}

void reverseLeftTire(int pwm){
  digitalWrite(left1, LOW);
  digitalWrite(left2, HIGH);
  analogWrite(leftEN, pwm); // moves the left tire forward at pwm speed in opposite direction
}

void moveForward(){
  moveRightTire(MAXSPD); // moves right tire at max speed
  moveLeftTire(MAXSPD); // moves left tire at max speed
}

void moveStop(){
  moveRightTire(0); // stops right tire
  moveLeftTire(0); // stops left tire
}

char checkDirections(){
  servoMotor.write(15); //looks left
  delay(150);
  leftDistance = US.ping_cm(); //sets the left distance to the current reading
  servoMotor.write(165); //looks right
  delay(275);
  rightDistance = US.ping_cm(); //sets the right distance to the current reading
  servoMotor.write(90);
  delay(150);
  if(rightDistance>leftDistance){
    return 'R';
  }else{
    return 'L';
  }
}

void carTurn(char directn){
  if(directn == 'R'){
    digitalWrite(rightLED,HIGH);
    reverseRightTire(MAXSPD);
    moveLeftTire(MAXSPD);
    delay(500);
    moveForward();
    digitalWrite(rightLED,LOW);
  }else if(directn == 'L'){
    digitalWrite(leftLED,HIGH);
    moveRightTire(MAXSPD);
    reverseLeftTire(MAXSPD);
    delay(500);
    moveForward();
    digitalWrite(leftLED,LOW);
  }
}

void setup() {
  pinMode(rightLED,OUTPUT);
  pinMode(leftLED,OUTPUT);
  pinMode(right1,OUTPUT);
  pinMode(right2,OUTPUT);
  pinMode(rightEN,OUTPUT);
  pinMode(left1,OUTPUT);
  pinMode(left2,OUTPUT);
  pinMode(leftEN,OUTPUT);
  Serial.begin(115200);
  servoMotor.attach(10); // Servo PIN
  servoMotor.write(90); // sets initial servo angle
  delay(200);

  moveForward(); // moves the car forward
}

void loop() {
  delay(50);
  usReading = US.ping_cm();
  printReading();
  if(usReading < usReadingThreshold){ // checks if the reading is below the threshold
    moveStop();
    char vacantDirection = checkDirections(); // checks vacant directions left & right
    carTurn(vacantDirection); // turns the car in the vacant direction
  }
}