#include <Servo.h>
#include <NewPing.h>
#define TRIGGER_PINF 2
#define ECHO_PINF A1
#define TRIGGER_PINR 4
#define ECHO_PINR A2
#define TRIGGER_PINL 10
#define ECHO_PINL A4
#define MAX_DISTANCE 400
NewPing uF(TRIGGER_PINF, ECHO_PINF, MAX_DISTANCE);
NewPing uR(TRIGGER_PINR, ECHO_PINR, MAX_DISTANCE);
NewPing uL(TRIGGER_PINL, ECHO_PINL, MAX_DISTANCE);
Servo s;
int uF1;
int uR1;
int uL1;
int sumF;
int sumR;
int sumL;
int in1=7;
int in2=8;
int in3=13;
int in4=12;
int en1=6;
int en2=5;
int f=A3;
int fD=9;
int value;
int i;  //servo angle
int I; //counter
//counter
int r=11;    //relay
unsigned long timea;
unsigned long timeb;
void setup() {
  pinMode(in1,OUTPUT);
  pinMode(in2,OUTPUT);
  pinMode(in3,OUTPUT);
  pinMode(in4,OUTPUT);
  pinMode(en1, OUTPUT);
  pinMode(en2, OUTPUT);
  pinMode(f,INPUT);    //flame
  pinMode(fD,INPUT);
  pinMode(r,OUTPUT);
  s.attach(3);
  Serial.begin(9600);
  delay(50);
}
void walk_right(){
analogWrite(en1, 255);
analogWrite(en2, 255);
digitalWrite(in1,HIGH);
digitalWrite(in2,LOW);
digitalWrite(in3,HIGH);
digitalWrite(in4,LOW);

}
void walk_backward_fast(){
analogWrite(en1, 255);
analogWrite(en2, 255);
digitalWrite(in1,HIGH);
digitalWrite(in2,LOW);
digitalWrite(in3,HIGH);
digitalWrite(in4,LOW);

}
void walk_left(){
  analogWrite(en1, 255); //low speed
  analogWrite(en2, 255);
digitalWrite(in1,LOW);
digitalWrite(in2,HIGH);
digitalWrite(in3,LOW);
digitalWrite(in4,HIGH);

}
void walk_forward_fast(){
  analogWrite(en1, 255);
  analogWrite(en2, 255);
digitalWrite(in1,LOW);
digitalWrite(in2,HIGH);
digitalWrite(in3,LOW);
digitalWrite(in4,HIGH);
}
void walk_backward(){
          //changes according to the position of the robot
analogWrite(en1, 100);
analogWrite(en2, 100);
digitalWrite(in1,LOW);
digitalWrite(in2,HIGH);
digitalWrite(in3,HIGH);
digitalWrite(in4,LOW);
}
void walk_forward(){
          //changes according to the position of the robot
analogWrite(en1, 100);
analogWrite(en2, 100);
digitalWrite(in1,HIGH);
digitalWrite(in2,LOW);
digitalWrite(in3,LOW);
digitalWrite(in4,HIGH);
}
void stop1(){
analogWrite(en1, 255);
analogWrite(en2, 255);
digitalWrite(in1,LOW);
digitalWrite(in2,LOW);
digitalWrite(in3,LOW);
digitalWrite(in4,LOW);

  }
void detect(){
  s.write(0);
  delay(500);   //34an mydtect4 7aga whwa rag3 (performance)
for (i=0;i < 180; i+=1){
s.write(i);
delay(4);

if(digitalRead(fD) == 0){

  if (i <= 94){
   walk_right();
    delay((94-i)*(700/90));
    stop1();
    delay(100);
    s.write(90);
    delay(200);

    if (digitalRead(fD) == 0) {

    timeb = millis();
    walk_forward_fast();
    digitalWrite(r, HIGH);
    while(digitalRead(fD) == 0){
      delay(1);
    }
    stop1();
    timea= millis() - timeb;
    delay(500);
    digitalWrite(r,LOW);
  walk_backward_fast();
  delay(timea);
  stop1(); }
  walk_left();
  delay((94-i)*(600/90));
  stop1();

 }
 else
 {
  walk_left();
    delay((i-94)*(600/90));
    stop1();
    delay(100);
    s.write(90);
 delay(200);
 if (digitalRead(fD) == 0) {
  timeb = millis();
    walk_forward_fast();
    digitalWrite(r, HIGH);
    while(digitalRead(fD) == 0){
      delay(1);
    }
    stop1();
    timea= millis() - timeb;
    delay(500);
    digitalWrite(r,LOW);
  walk_backward_fast();
  delay(timea);
  stop1();}
  walk_right();
  delay((i-90)*(700/90));
  stop1();
  }
 }
}
}
void readUF(int x){
  sumF = 0; sumR=0; sumL=0;
   for(i=1 ; i < (x+1) ;i++){
    sumF = sumF + uF.ping_cm();
    delay(20);
    }
    uF1 = sumF/x;
  }
  void readUL(int y){
   sumL=0;
   for(i=1 ; i < (y+1) ;i++){
    sumL= sumL+ uL.ping_cm();
    delay(20);
    }
    uL1 = sumL/y;


  }
  void readUR(int z){
   sumR=0;
   for(i=1 ; i < (z+1) ;i++){
    sumR= sumR+ uR.ping_cm();
    delay(20);
    }
    uR1 = sumR/z;

  }


void loop(){
// detect();
walk_forward();
 readUF(30);
Serial.print("front ");
Serial.println(uF1);
Serial.print("right ");
Serial.println(uR1);
Serial.print("left ");
Serial.println(uL1);
if (uF1 <= 15){
  stop1();
  readUR(30);
  readUL(30);
  if (uR1 > uL1){

    walk_right();
    delay(390);
    stop1();
    delay(1000);
    }
    else{
      walk_left();
      delay(400);
      stop1();
      delay(1000);
      }
  }
}