#include <ESP32Servo.h>

Servo myservo;  // create servo object to control a servo
int servo_angle = 0;    // variable to store the servo position
#define SERVOPIN 16     // Recommended PWM GPIO pins on the ESP32 include 2,4,12-19,21-23,25-27,32-33 
#define SERVO_LEFT 45
#define SERVO_RIGHT 135
#define SERVO_FORWARD 90


#define US_TRIG 12
#define US_ECHO 14
#define US_STOP_DISTANCE 10
long distance_forward_cm,distance_left_cm,distance_right_cm;


void setup() {
    //Serial port
    Serial.begin(115200);

    //Ultra sonic pins
    pinMode(US_TRIG, OUTPUT);
    pinMode(US_ECHO, INPUT);
    //servo init
    myservo.attach(SERVOPIN);         // attaches the servo on pin 18 to the servo object
                                      // using default min/max of 1000us and 2000us
                                      // different servos may require different min/max settings
                                      // for an accurate 0 to 180 sweep
    delay(1000);
    myservo.write(SERVO_FORWARD);     // start looking forward
    delay(2000);
    myservo.write(SERVO_LEFT);     // start looking forward
    delay(2000);
    myservo.write(SERVO_FORWARD);     // start looking forward
    delay(2000);
    myservo.write(SERVO_RIGHT);     // start looking forward
    delay(2000);
    myservo.write(SERVO_FORWARD);     // start looking forward
    
}


void loop() {

  distance_forward_cm = measure_distance_cm();      // measure the distance in front of us

  if(distance_forward_cm<US_STOP_DISTANCE){
    //there is something in front of us
    myservo.write(SERVO_LEFT);                      // look the left and meaasure distance
    delay(1000);                                     // wait 1s
    distance_left_cm = measure_distance_cm();       // measure the distance in left of us
    delay(1000);                                     // wait 1s
    myservo.write(SERVO_RIGHT);                      // look the left and meaasure distance
    delay(1000);                                     // wait 1s
    distance_right_cm = measure_distance_cm();       // measure the distance in left of us
    delay(1000);                                     // wait 1s

    if(distance_left_cm > distance_right_cm){
      // goto to the left
      for(int pos = SERVO_FORWARD;pos>SERVO_LEFT;pos--){
        myservo.write(pos);
        delay(15);
      }
    }
    else{
      // goto to the right
      for(int pos = SERVO_FORWARD;pos<SERVO_RIGHT;pos++){
        myservo.write(pos);
        delay(15);
      }
    }

    delay(1000);
    myservo.write(SERVO_FORWARD);
    delay(1000);
  }
  
  delay(250);//ms
}

long measure_distance_cm(){
  long distance_cm,echo_duration;
  // start measurment by a short high pulse of 10 microseconds length at the trig pin ...
  digitalWrite(US_TRIG,LOW);
  delayMicroseconds(2);
  digitalWrite(US_TRIG,HIGH);
  delayMicroseconds(10);
  digitalWrite(US_TRIG,LOW);

  // measure the duration in microseconds of the echo pin HIGH - this is the time the echo needs to be back at the sensor
  pinMode(US_ECHO,INPUT);
  echo_duration = pulseIn(US_ECHO,HIGH);

  // convert into cm ... 344m/sec is the speed of noise - thus 34400cm/sec ... or 34,400cm/milisec ... or 0,0344cm/microsec
  // the echo has to go the distance twice - forth and back - so the duration has to be the half of the measured one
  // distance_cm = echo_duration/2 * 0,0344   or   distance_cm = echo_duration/2 / 29,1
  distance_cm = (echo_duration/2) / 29.1;

  //print this result at the screen
  Serial.print("Distance in cm: ");
  Serial.println(distance_cm);
  return(distance_cm);

}