#include <AFMotor.h> // includes adafruit motor shield library
#include <SharpDistSensor.h> // includes sharp distance sensor library
#include <Servo.h> // includes the servo motor library


#define MAX_DISTANCE 240 //The compiler will replace any mention of MAX_DISTANCE with the value 240 at compile time.
#define MAX_SPEED 230 //The compiler will replace any mention of MAX_SPEED with the value 230 at compile time,it also sets speed of DC  motors
#define MAX_SPEED_OFFSET 20// The compiler will replace any mention of MAXX_SPEED_OFFSET with the value 20 atcompile time.



AF_DCMotor motor1(1, MOTOR12_1KHZ); // creates motor one selects motor pin 1, sets the frequency of the motor to one kilo Hertz
AF_DCMotor motor2(3, MOTOR12_1KHZ); //creates motor two and selects motor pin 3, sets the frequency of the motor to one kilo Hertz



const byte sensorPin = A0; // The const keyword stands for constant. It is a variable qualifier that modifies the behavior of the variable, making a variable "read-only".
//This means that the variable can be used just as any other variable of its type, but its value cannot be changed. A byte stores an 8-bit unsigned number, from 0 to 255. in this case the code selects analog pin A0 for the sensor.

const byte medianFilterWindowSize = 5;// this code selects a window size for the sensor
SharpDistSensor sensor(sensorPin, medianFilterWindowSize);// creates a sensor object and sets the sensorpin and the window size which were given a value in the previous lines of codes


Servo myservo;   // creates a servo motor object named as "myservo"

boolean goesForward=false;// boolean is a non-standard type alias for bool defined by Arduino. it si the same as int and it sets goesForward to fasle=0.
int distance = 100;// give // it gives the variable "distance" a value "100".
int speedSet = 0;// it gives the variable speedSet the value "0"


void setup() { // starts void setup to be executed only once

Serial.begin(9600); // we all know what this does
  myservo.attach(9);  // pin 9 for the servo motor
  myservo.write(115); // set the angle of the servo to 115 degrees
  delay(2000);// delay for two seconds
  distance = readPing();//distance is equal to the ping that was read from the sensor
  delay(100);// delay by 100 milliseconds
  distance = readPing(); // repeat
  delay(100);//repeat
  distance = readPing();// repeat
  delay(100);//repeat
  distance = readPing();// repeat
  delay(100);// repeat. it is repeated in order to take a more accurate reading from the sensor
Serial.print(distance); // overall serial print to see what the reading is on the serial monitor
}

void loop() {// starts the loop function
  unsigned int distance = sensor.getDist();
 int distanceR = 0;
 int distanceL =  0;
 Serial.println(distance);
 delay(40);

 if(distance<=500)
 {
  moveStop();
  delay(100);
  moveBackward();
  delay(600);
  moveStop();
  delay(200);
  distanceR = lookRight();
  delay(200);
  distanceL = lookLeft();
  delay(200);

  if(distanceR>=distanceL)
  {
    turnRight();
    moveStop();
  }else
  {
    turnLeft();
    moveStop();
  }
 }else
 {
  moveForward();
 }
 distance = readPing();
}

int lookRight()
{
    myservo.write(50);
    delay(500);
    int distance = readPing();
    delay(100);
    myservo.write(115);
    Serial.print(distance + "right");// serial print for right
    return distance;

}

int lookLeft()
{
    myservo.write(170);
    delay(500);
    int distance = readPing();
    delay(100);
    myservo.write(115);
     Serial.print(distance + "left"); // serial print for left
    return distance;
    delay(100);

}

int readPing() {
  delay(70);
  unsigned int cm = sensor.getDist();
  if(cm==0)
  {
    cm = 250;
  }
  return cm;
}

void moveStop() {
  motor1.run(RELEASE);
  motor2.run(RELEASE);
  }

void moveForward() {

 if(!goesForward)
  {
    goesForward=true;
    motor1.run(FORWARD);
    motor2.run(FORWARD);
   for (speedSet = 150; speedSet < MAX_SPEED; speedSet +=2) // slowly bring the speed up to avoid loading down the batteries too quickly
   {
    motor1.setSpeed(speedSet);
    motor2.setSpeed(speedSet+MAX_SPEED_OFFSET);
    delay(5);
   }
  }
}

void moveBackward() {
    goesForward=false;
    motor1.run(BACKWARD);
    motor2.run(BACKWARD);
  for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2) // slowly bring the speed up to avoid loading down the batteries too quickly
  {
    motor1.setSpeed(speedSet);
    motor2.setSpeed(speedSet+MAX_SPEED_OFFSET);
    delay(5);
  }
}

void turnRight() {
  motor1.run(FORWARD);
  motor2.run(BACKWARD);
  delay(500);
  motor1.run(FORWARD);
  motor2.run(FORWARD);
}

void turnLeft() {
  motor1.run(BACKWARD);
  motor2.run(FORWARD);
  delay(500);
  motor1.run(FORWARD);
  motor2.run(FORWARD);
}