#include <ESP32Servo.h>
Servo servo;  // create servo object to control a servo
#define trigger_pin 5
#define echo_pin 18
#define SERVO_PIN 19 //steering servo pin 
// #define SERVO_PIN 12 //seeting servo pin
// Setting Motor A
int enable1Pin = 14;
int motor1Pin1 = 27;
int motor1Pin2 = 26;
//Settin Motor B
int enable2Pin = 32;
int motor2Pin3 = 25;
int motor2Pin4 = 33;
// Setting PWM properties
const int frequency = 3000;
const int pwm_channel = 0;
const int resolution = 8;
long t;    //var time to calculate distance
int distance; //var to store diatnce
void setup() {
  servo.attach(SERVO_PIN);  // attaches the servo on pin 12 to the servo objectư
  pinMode(trigger_pin, OUTPUT); // attaches the trigger pin to 5 pin
  pinMode(echo_pin, INPUT); // attaches echo pin to 18 pin
  //modding motor A
  pinMode(enable1Pin, OUTPUT);
  pinMode(motor1Pin1, OUTPUT);
  pinMode(motor1Pin2, OUTPUT);
  //modding Motor B
  pinMode(enable2Pin, OUTPUT);
  pinMode(motor2Pin3, OUTPUT);
  pinMode(motor2Pin4, OUTPUT);
  // configure LEDC PWM
  ledcAttachChannel(enable1Pin, frequency, resolution, pwm_channel);
  ledcAttachChannel(enable2Pin, frequency, resolution, pwm_channel);
}
void loop() {
  //setting hc-sro4
  digitalWrite(trigger_pin, LOW);
  delay(5);
  digitalWrite(trigger_pin, HIGH);
  delay(10);
  digitalWrite(trigger_pin, LOW);
  //getting distance from hc-sr04
  t = pulseIn(echo_pin, HIGH);
  distance = (t * 0.034) / 2;
  //default moving forward
  MoveForward();
  delay(200);
  //centering servo
  servo.write(90);
  delay(500);
  if (distance < 20) {
    int distanceLeft = 0; //setting var for left and right
    int distanceRight = 0;
    delay(200);
    distanceLeft = ServoLookLeft(distance);
    delay(200);
    distanceRight = ServoLookRight(distance);
    delay(200);
    StopCar();
    delay(200);
    MoveBackward();
    delay(200);
    StopCar();
    delay(200);
    //calcauting shortest distance
    if (distanceRight <= distanceLeft) {
      TurnLeft();
    }
    else {
      TurnRight();
    }
  }
}
//servo seek clear path
//servo turn left
int ServoLookLeft(int distance) {
  int ditanceLeft = 0;
  delay(500);
  servo.write(180); //lefting servo and taking value
  delay(2000);
  return distance;
}
//servo turn right
int ServoLookRight(int distance) {
  int ditanceRight = 0;
  delay(500);
  servo.write(0); //lefting servo and taking value
  delay(2000);
  return distance;
}
int SeekClearPath() {
  int ditanceRight = 0;
  int ditanceLeft = 0;
  delay(500);
  servo.write(0); //righting servo
  delay(2000);
  ditanceRight = distance;
  servo.write(180); //lefting servo
  delay(2000);
  ditanceLeft = distance;
  //lcd.clear();
  //centering  servo back
  servo.write(90);
  delay(1000);
}
//car movement functions
//move forward
void MoveForward() {
  //Set speed motor A and B
  analogWrite(enable1Pin, 255);
  analogWrite(enable2Pin, 255);
  digitalWrite(motor1Pin1, LOW);
  digitalWrite(motor1Pin2, HIGH);
  digitalWrite(motor2Pin3, HIGH);
  digitalWrite(motor2Pin4, LOW);
  //lcd.clear();
}
//move backward
void MoveBackward() {
  analogWrite(enable1Pin, 255);
  analogWrite(enable2Pin, 255);
  digitalWrite(motor1Pin1, HIGH);
  digitalWrite(motor1Pin2, LOW);
  digitalWrite(motor2Pin3, HIGH);
  digitalWrite(motor2Pin4, LOW);
  //lcd.clear();
  delay(700);
}
//turn car left
void TurnLeft() {
  //Set speed motor A and B
  analogWrite(enable1Pin, 255);
  analogWrite(enable2Pin, 255);
  digitalWrite(motor1Pin1, LOW);
  digitalWrite(motor1Pin2, HIGH);
  digitalWrite(motor2Pin3, LOW);
  digitalWrite(motor2Pin4, LOW);
  //lcd.clear();
  delay(850);
  //Stop motor for half second
  analogWrite(enable1Pin, 0);
  analogWrite(enable2Pin, 0);
  digitalWrite(motor1Pin1, LOW);
  digitalWrite(motor1Pin2, LOW);
  digitalWrite(motor2Pin3, LOW);
  digitalWrite(motor2Pin4, LOW);
  delay(500);
}
//turn car  right
void TurnRight() {
  //turn right
  analogWrite(enable1Pin, 255);
  analogWrite(enable2Pin, 255);
  digitalWrite(motor1Pin1, LOW);
  digitalWrite(motor1Pin2, LOW);
  digitalWrite(motor2Pin3, LOW);
  digitalWrite(motor2Pin4, HIGH);
  //lcd.clear();
  delay(850);
  //Stop motor for half second
  analogWrite(enable1Pin, 0);
  analogWrite(enable2Pin, 0);
  digitalWrite(motor1Pin1, LOW);
  digitalWrite(motor1Pin2, LOW);
  digitalWrite(motor2Pin3, LOW);
  digitalWrite(motor2Pin4, LOW);
  delay(500);
}
// stop car
void StopCar() {
  analogWrite(enable1Pin, 0);
  analogWrite(enable2Pin, 0);
  digitalWrite(motor1Pin1, LOW);
  digitalWrite(motor1Pin2, LOW);
  digitalWrite(motor2Pin3, LOW);
  digitalWrite(motor2Pin4, LOW);
  delay(700);
}