#include <ESP32Servo.h>

// Pin definitions
const int trigPin = 25; // Ultrasonic sensor TRIG pin
const int echoPin = 26; // Ultrasonic sensor ECHO pin
const int servoPin = 27; // Servo motor signal pin

Servo myServo; // Create a Servo object

// Constants
const int detectionRange = 20; // Range in cm for detection
const int delayTime = 5000;    // Delay in milliseconds (5 seconds)

// Variables
bool isPersonDetected = false; // Tracks if a person is in range

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

  // Configure ultrasonic sensor pins
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  // Attach servo to the designated pin
  myServo.attach(servoPin);
  myServo.write(0); // Set initial position to 0 degrees

  Serial.println("System ready. Waiting for person detection...");
}

void loop() {
  long duration;
  int distance;

  // Trigger the ultrasonic sensor
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Measure the echo time
  duration = pulseIn(echoPin, HIGH);

  // Calculate the distance (in cm)
  distance = duration * 0.034 / 2;

  // Check if the person is within the detection range
  if (distance > 0 && distance <= detectionRange) {
    if (!isPersonDetected) { // If not already detected
      Serial.println("Person detected! Rotating servo to 90 degrees...");
      myServo.write(90); // Rotate to 90 degrees
      isPersonDetected = true;
    }
  } else {
    if (isPersonDetected) { // If previously detected but now out of range
      Serial.println("Person moved out of range. Waiting 5 seconds...");
      delay(delayTime); // Wait for 5 seconds
      myServo.write(0); // Return servo to 0 degrees
      Serial.println("Servo returned to 0 degrees.");
      isPersonDetected = false;
    }
  }

  delay(100); // Small delay to stabilize sensor readings
}