#include <Servo.h>
#include <NewPing.h>

Servo servoMotor;

const int LeftMotorForward = 5;
const int LeftMotorBackward = 4 ;
const int RightMotorForward = 3 ;
const int RightMotorBackward = 2 ;

int  trigPin = A1 ;
int  echoPin = A2 ;


int distance ;
bool goesForward = false;
int max_distance = 200;
NewPing sonar(trigPin,echoPin,max_distance);

bool goesBackward = false ;


void setup() {
 Serial.begin(9600);

 servoMotor.attach(8);

  pinMode(RightMotorForward, OUTPUT);
  pinMode(LeftMotorForward,  OUTPUT);
  pinMode(LeftMotorBackward, OUTPUT);
  pinMode(RightMotorBackward,  OUTPUT);
  
  servoMotor.write(90);
  delay(1000);
  
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
 
}
void loop() {

  int distanceRight = 0;
  int distanceLeft = 0 ;
  delay(50);

if (distance <= 20){
    stopMove();
    delay(300);
    moveBackward();
    delay(400);
   stopMove();
    delay(300);
    distanceRight = lookRight();
    delay(300);
    distanceLeft = lookLeft();
    delay(300);

   if(distance>=distanceLeft){
     turnRight();
     stopMove();
     
   }
    else{
     turnleft();
     stopMove();
    }
  }
  else{
    moveForward();
  }
  distance = readPing();
  
}

int lookLeft(){
  servoMotor.write(180);
  delay(1000);
  int distance = readPing();
  delay(100);
  return distance;

}

int lookRight(){
  servoMotor.write(0);
  delay(1000);
  distance = readPing();
  delay(100);
  return distance ;
}
int readPing(){
  delay(70);
  int cm = sonar.ping_cm();
  if(cm==0){
    cm =250;
  }
  return cm ;
}
 
void turnleft() {
  
  digitalWrite(LeftMotorForward,  LOW);
  digitalWrite(LeftMotorBackward,  HIGH);

 digitalWrite(RightMotorForward, HIGH);
  digitalWrite(RightMotorBackward, LOW);

  delay(500);
}
void turnRight(){

  digitalWrite(LeftMotorForward,  HIGH);
  digitalWrite(RightMotorBackward, HIGH);
  
  digitalWrite(LeftMotorBackward,  LOW);
  digitalWrite(RightMotorForward, LOW);
  
  delay(500);
}
void moveForward(){
  if(!goesForward){
    goesForward = true ;
    goesBackward = false;
  digitalWrite(LeftMotorForward,  HIGH);
  digitalWrite(RightMotorBackward, LOW);

   digitalWrite(RightMotorForward, HIGH);
  digitalWrite(LeftMotorBackward,  LOW);
  }
}
void moveBackward() {
if(!goesBackward){
  goesForward = false ;
  goesBackward = true ;

  digitalWrite(LeftMotorBackward, HIGH);
  digitalWrite(RightMotorBackward,  HIGH);
  
  digitalWrite(LeftMotorForward, LOW);
  digitalWrite(RightMotorForward,  LOW);
} 
 }
void stopMove() {
  digitalWrite(RightMotorForward,  LOW);
  digitalWrite(LeftMotorForward, LOW);
  digitalWrite(RightMotorBackward,  LOW);
  digitalWrite(LeftMotorBackward, LOW);
}