#include <ESP32Servo.h>

// constants won't change
const int TRIG_PIN = 4;               // Arduino pin connected to Ultrasonic Sensor's TRIG pin
const int ECHO_PIN = 5;               // Arduino pin connected to Ultrasonic Sensor's ECHO pin
const int SERVO_PIN = 13;             // Arduino pin connected to Servo Motor's pin
const int DISTANCE_THRESHOLD = 50;    // centimeters
const int SERVO_MIN_ANGLE = 0;        // Minimum angle for servo
const int SERVO_MAX_ANGLE = 180;      // Maximum angle for servo
const int SERVO_SPEED = 5;            // Speed of servo movement

Servo servo; // create servo object to control a servo

// variables will change:
float duration_us, distance_cm;
unsigned long servoMovedTime = 0;
int currentServoAngle = 0;

void setup() {
  Serial.begin(115200);         // initialize serial port
  pinMode(TRIG_PIN, OUTPUT);    // set arduino pin to output mode
  pinMode(ECHO_PIN, INPUT);     // set arduino pin to input mode
  servo.attach(SERVO_PIN);      // attaches the servo on pin 13 to the servo object
  servo.write(currentServoAngle); // initialize servo position
}

void loop() {
  // generate 10-microsecond pulse to TRIG pin
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  // measure duration of pulse from ECHO pin
  duration_us = pulseIn(ECHO_PIN, HIGH);
  // calculate the distance
  distance_cm = 0.017 * duration_us;

  if (distance_cm < DISTANCE_THRESHOLD) {
    moveServo(180); // Rotate servo motor to 90 degrees
    servoMovedTime = millis();
  } else {
    if (millis() - servoMovedTime >= 10000) {
      moveServo(0); // Rotate servo motor to 0 degrees
    }
  }

  // print the value to Serial Monitor
  Serial.print("distance: ");
  Serial.print(distance_cm);
  Serial.println(" cm");

  delay(500);
}

void moveServo(int targetAngle) {
  if (currentServoAngle != targetAngle) {
    if (currentServoAngle < targetAngle) {
      for (int angle = currentServoAngle; angle <= targetAngle; angle += SERVO_SPEED) {
        servo.write(angle);
        delay(25); // Adjust the delay as per your servo motor speed requirement
        currentServoAngle = angle;
      }
    } else {
      for (int angle = currentServoAngle; angle >= targetAngle; angle -= SERVO_SPEED) {
        servo.write(angle);
     // Adjust the delay as per your servo motor speed requirement
        currentServoAngle = angle;
      }
    }
  }
}