#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_SPEED 250 //The compiler will replace any mention of MAX_SPEED with the value 230 at compile time,it also sets speed of DC  motors



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(90); // set the angle of the servo to 115 degrees
  delay(5000);// delay for two seconds
  distance = readPing();//distance is equal to the ping that was read from the sensor, evaluate whatever value or expression is on the right side of the equal sign, and store it in the variable to the left of the equal sign.
  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();// "unsigned int" is nearly the same as "int" unsigned means that it can store only positive values non-negative to be more precise this is done to save memory.
  //it is used for things like distance that will never be negative. this code was recommended to me by a friend
 int distanceR = 0;// gives the variable distanceR the value 0
 int distanceL =  0;// gives the variable distanceL the value 0
 Serial.println(distance);// prints the distance onto the serial monitor for 40 milliseconds
 delay(40);

 if(distance<=650)// if the distance is less than 500 then execute the following commands (this is where the coding will start to get very complex as i will need to add moving forward and backward here depending on the situation).
 {
  moveStop(); // moveStop is a declared function. more info towards the end
  delay(100);// delay for 100 milliseconds
  moveBackward();// Move backwards for 600 milliseconds.
  delay(500);
  moveStop();// stop the motors
  delay(200);// delay for 200 milliseconds.
  distanceR = lookRight();// make distanceR value equal to the lookRight value.
  delay(200);// delay by 200 milliseconds
  distanceL = lookLeft();// make distanceL value equal to the lookLeft value.
  delay(200);// delay by 200 milliseconds

  if(distanceR>=distanceL)//if the distanceR is bigger than distance left execute the following.
  {
    turnRight();// turn right
    moveStop();// stop
  }else // Corresponds to the second if structure code. Meaning if it is wrong then execute the following.
  {
    turnLeft(); // Turn left.
    moveStop(); // stop.
  }
 }else // corresponds to the first "if" structure command. meaning if it is false then execute the following
 {
  moveForward();// move forward
 }
 distance = readPing();// makes distance equal to the readPing, evaluate whatever value or expression is on the right side of the equal sign, and store it in the variable to the left of the equal sign.
}

int lookRight()//replace the variable with the number prduced after the next 7 lines of code are executed.
{
    myservo.write(0); // set the angle/direction of the servo to 50.
    delay(300);//wait for half a second
    int distance = readPing();// make distance equal to readPing
    delay(400);//delay for 100 milliseconds
    myservo.write(90);
    delay(300);// set the servo angle back to its initial position 115
    Serial.print("right:");
Serial.print(distance);
delay(10);
    return distance;// return the number where it was mentioned

}

int lookLeft()// variable lookLeft is stored as the following
{
    myservo.write(180); // set the servo angle to 170 degrees
    delay(300);// delay for 500 milliseconds
    int distance = readPing();// make distance equal to the readping
    delay(400);//delay for 100 milliseconds
    myservo.write(90);
    delay(300);// return to original position(115 degrees)
    Serial.print("left:");
Serial.print(distance);
delay(10);
    return distance;// return distance to where it was mentioned

}

int readPing() { // it will execute the code below if readPing is mentioned.
  delay(70);// delay by 70 milliseconds
  unsigned int kms = sensor.getDist();// set the variable on the left to the value of the variable on the right
if((kms >= 0 && kms <=200) || kms>1350){
    kms = 250;
}
  return kms;//Terminate the function and return a value from a function to the calling function, if desired.


}
void moveStop() {//is a function declaration. It indicates that the function is expected to return no information to the function from which it was called. so when movestop is mentioned it will only execute the following lines.
  motor1.run(RELEASE); //releases the motor ready to be used/stop  rotation
  motor2.run(RELEASE);//releases the motor ready to be used/stop rotation

  }

void moveForward() {// this is a function decleration

 if(!goesForward)// the "!" is a boolean operator.which means logical not results in a true if the operand is false and vice versa. and in this case the operator was set to false at the begginning.
  {// This code simply tells us that if "goesForward" is not a "true" operand( meaning if it is false) then execute the following commands.
    goesForward=true; // set goesForward to true so that this code is only executed once
    motor1.run(FORWARD); // make the motor run forward
    motor2.run(FORWARD); // make motor two run forward
   for (speedSet = 200; speedSet < MAX_SPEED; speedSet +=2) // set the speed to 150 if the speed set is below max speed then increase the speed by two.
   {
    motor1.setSpeed(speedSet);// set the speed of the motor one to the speed set in the previous code
    motor2.setSpeed(speedSet);// set the speed of the motor two to speedset and add the offset speed to it in order to take sharp turns.
    delay(5);// repeat with a delay of 5 miliseconds
   }
  }
}

void moveBackward() {// function decleration
    goesForward=false; // set goes forrward to false again for the motors to be able to move forward again.
    motor1.run(BACKWARD); // make motor 1 move backwards
    motor2.run(BACKWARD);  // make motor two move backwards
  for (speedSet = 200; speedSet < MAX_SPEED; speedSet +=2) // slowly bring the speed up untill the desireded speed is reached the "for" structure creates a loop
  {
    motor1.setSpeed(speedSet);// set the speed of motor one to the speedset
    motor2.setSpeed(speedSet);// set the speed of motor two to speedset+ max speed offset
    delay(5);// delay by 5 milliseconds and restart the for function untill the speedset reaches the max speed or untill the distance is less than 500.
  }

}
void turnRight() {// function decleration
  motor1.run(FORWARD);// run motor one forward
  motor2.run(BACKWARD); // run motor 2 backward
  delay(400);// delay by half a second. this also means execute the previous codes for half a second
  motor1.run(FORWARD);      // run motor one forward
  motor2.run(FORWARD);      // rotate motor 2 forward
}

void turnLeft() {// function decleration
  motor1.run(BACKWARD);// run motor one backwards
  motor2.run(FORWARD); // run motor 2 forwards
  delay(400);// execute the previous command for 500 milliseconds
  motor1.run(FORWARD); // run motor one forward
  motor2.run(FORWARD);//run motor two forward
}