/****************************
  Connections:
  -----------------------------
  [Arduino Uno] --> [L298N Red Board]:
     11         -->   ENA (Black)/(أسود)
     9          -->   IN1 (White)/(أبيض)
     8          -->   IN2 (Gray)/(رمادي)
     7          -->   IN3 (Purple)/(أرجواني)
     6          -->   IN4 (Blue)/(أزرق)
     10         -->   ENB (Green)/(أخضر)
  [Arduino Uno] --> [Ultrasonic Sensor]:
     5V         -->   Vcc  (Purple)/(أرجواني)
     2          -->   Trig (Gray)/(رمادي)
     3          -->   Echo (White)/(أبيض)
     GND        -->   GND  (Black)/(أسود)
  [Arduino Uno] --> [IR Sensor (Left)]:
     GND        -->   GND (Brown)/(بني)
     5V         -->   VCC (Red)/(أحمر)
     A5         -->   OUT (Orange)/(برتقالي)
  [Arduino Uno] --> [IR Sensor (Center)]:
     GND        -->   GND (Gray)/(رمادي)
     5V         -->   VCC (White)/(أبيض)
     A2         -->   OUT (Black)/(أسود)
  [Arduino Uno] --> [IR Sensor (Right)]:
     GND        -->   GND (Brown)/(بني)
     5V         -->   VCC (Red)/(أحمر)
     A0         -->   OUT (Orange)/(برتقالي)
  [Arduino Uno] --> [Buzzer]:
     GND        -->   - (Green)/(أخضر)
     13         -->   + (Yellow)/(أصفر)
  -----------------------------
*****************************/
const int trigPin = 2;
const int echoPin = 3;
// defines variables
long duration;
int distance;

#define ESP32_pin     12  //input for arduino from ESP32 to STOP Car for collecting POST
#define SensorLeft    A5   //input pin of left sensor
#define SensorMiddle  A2   //input pin of middle sensor
#define SensorRight   A0   //input pin of right sensor
unsigned char SL;        //state of left sensor
unsigned char SM;        //state of middle sensor
unsigned char SR;        //state of right sensor


int ESP32_Command = 0;
#define Lpwm_pin  10  //ENB
#define Rpwm_pin  11  //ENA
int pinLB = 6;        //IN4
int pinLF = 7;        //IN3
int pinRB = 9;        //IN2
int pinRF = 8;        //IN1
unsigned char Lpwm_val = 120;//255 = max 110
unsigned char Rpwm_val = 120;//255 = max

void M_Control_IO_config(void)
{
  pinMode(ESP32_pin, INPUT);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(SensorLeft, INPUT);
  pinMode(SensorMiddle, INPUT);
  pinMode(SensorRight, INPUT);
  pinMode(pinLB, OUTPUT);
  pinMode(pinLF, OUTPUT);
  pinMode(pinRB, OUTPUT);
  pinMode(pinRF, OUTPUT);
  pinMode(Lpwm_pin, OUTPUT); // pin 11 (PWM)
  pinMode(Rpwm_pin, OUTPUT); // pin 10 (PWM)
}
void Set_Speed(unsigned char Left, unsigned char Right)
{
  analogWrite(Lpwm_pin, Left);
  analogWrite(Rpwm_pin, Right);
}

void turnL()
{
  Serial.println("L");
  digitalWrite(pinRB, LOW);
  digitalWrite(pinRF, HIGH);
  digitalWrite(pinLB, LOW);
  digitalWrite(pinLF, HIGH);
  delay(10);
}
void back()
{
  Serial.println("B");
  digitalWrite(pinRB, LOW);
  digitalWrite(pinRF, HIGH);
  digitalWrite(pinLB, HIGH);
  digitalWrite(pinLF, LOW);
  delay(10);
}
void advance()
{
  Serial.println("F");
  digitalWrite(pinRB, HIGH);
  digitalWrite(pinRF, LOW );
  digitalWrite(pinLB, LOW);
  digitalWrite(pinLF, HIGH);
  delay(10);
}
void stopp()
{
  Serial.println("STOP!!");
  digitalWrite(pinRB, HIGH);
  digitalWrite(pinRF, HIGH);
  digitalWrite(pinLB, HIGH);
  digitalWrite(pinLF, HIGH);
  delay(10);
}
void turnR()
{
  Serial.println("R");
  digitalWrite(pinRB, HIGH);
  digitalWrite(pinRF, LOW);
  digitalWrite(pinLB, HIGH);
  digitalWrite(pinLF, LOW);
  delay(10);
}

void hornON() {
  digitalWrite(13, 1);
}
void hornOFF() {
  digitalWrite(13, 0);
}

void setup()
{
  Serial.begin(9600);
  pinMode(13, OUTPUT);
  M_Control_IO_config();
  Set_Speed(Lpwm_val, Rpwm_val);
  stopp();
  delay(5000);

}

void Sensor_Scan(void)
{
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);

  distance = duration * 0.034 / 2;
  SL = digitalRead(SensorLeft);
  SM = digitalRead(SensorMiddle);
  SR = digitalRead(SensorRight);
  Serial.print(SL);
  Serial.print(SM);
  Serial.println(SR);
}

int flagallblack = 0;
void loop()
{
  ESP32_Command = digitalRead(ESP32_pin);// if [0 = Move] / if [1 = STOP]
  Sensor_Scan();

  if (distance > 10 && ESP32_Command == 0) {
    hornOFF();
    ///////////////////////////////////////////////////////////////////////////////
    if (SM == HIGH)// middle sensor in black area
    {
      if (SL == LOW & SR == HIGH) // black on left, white on right, turn left
      {
        turnR();
      }
      else if (SR == LOW & SL == HIGH) // white on left, black on right, turn right
      {
        turnL();
      }
      else if (SR == HIGH & SL == HIGH) { // all Black
        if(flagallblack==0){
        stopp() ;
        delay(5000);
        flagallblack=1;
        }
        else{
          advance();
          delay(500);
          flagallblack=0;
        }
        //turnL();
        //delay(100);
      }
      else // white on both sides, going forward
      {
        advance();

      }
    }
    ///////////////////////////////////////////////////////////////////////////////
    else // middle sensor on white area
    {
      if (SL == LOW & SR == HIGH) // black on left, white on right, turn left
      {
        turnR();

      }
      else if (SR == LOW & SL == HIGH) // white on left, black on right, turn right
      {
        turnL();


      }

      else // all white, stop
      {
        turnL();
        // back();
        delay(100);
        stopp() ;
      }

    }
    ///////////////////////////////////////////////////////////////////////////////
  }

  else if (distance > 10 && ESP32_Command == 1) {
    stopp();
    hornON();
    delay(500);
    hornOFF();
    delay(500);
  }
  else if (distance < 10 && ESP32_Command == 1) {
    stopp();
    hornON();
    delay(500);
    hornOFF();
    delay(500);
  }
  else if (distance < 10 && ESP32_Command == 0) {
    stopp();
    hornON();
    delay(500);
    hornOFF();
    delay(500);
  }

}//end of loop