// Servo Sweep example for the ESP32
// https://wokwi.com/arduino/projects/323706614646309460

#include <ESP32Servo.h>
#include <NewPing.h>

#define trig_pin 18 //analog input 1
#define echo_pin 5 //analog input 2
#define maximum_distance 200

const int servoPin = 18;
int distance = 40;

Servo servo_motor;
NewPing sonar(trig_pin, echo_pin, maximum_distance);

void setup() {
  servo_motor.attach(servoPin, 500, 2400);
}

void loop() {
  obstacleAvoiding();
}

int readPing(){
  delay(70);
  int cm = sonar.ping_cm();
  if (cm==0){
    cm=250;
  }
  Serial.print("Ping: ");
  Serial.print(cm);
  return cm;
}

int lookRight(){
  servo_motor.write(0);
  delay(500);
  int distance = readPing();
  delay(100);
  servo_motor.write(90);
  return distance;
}

int lookLeft(){
  servo_motor.write(180);
  delay(500);
  int distance = readPing();
  delay(100);
  servo_motor.write(90);
  return distance;
}

void obstacleAvoiding() {
  int distanceRight = 0;
  int distanceLeft = 0;
  delay(50);
  if (distance <= 55){
    // moveStop(); uneccesary for testing servo only purpose
    delay(2000);
    // moveBackward();
    delay(150);
    // moveStop();
    delay(1000);
    distanceRight = lookRight();
    delay(300);
    distanceLeft = lookLeft();
    delay(300);
    if (distance >= distanceLeft){
      // turnRight();
      // moveStop();
      Serial.println("Turning right and stopping...");
    }
    else{
      // turnLeft();
      // moveStop();
      Serial.println("Turning left and stopping...");
    }
  }
  else{
    // moveForward();
    Serial.println("Moving forward...");
  }
    distance = readPing();
}