#include <ESP32Servo.h>

const int trigPin = 23;    // Ultrasonic sensor trigger pin
const int echoPin = 22;    // Ultrasonic sensor echo pin
const int servoPin = 25;   // Servo motor pin
const int ledPin = 14;    // LED pin

Servo servo;

void setup() {
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(ledPin, OUTPUT);

  servo.attach(servoPin);
  servo.write(90);  // Set initial position to 90 degrees
}

void loop() {
  long duration, distance;
  
  // Trigger ultrasonic sensor
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  // Read echo pin
  duration = pulseIn(echoPin, HIGH);
  
  // Calculate distance in cm
  distance = (duration / 2) / 29.1;

  // Control servo motor based on distance
  if (distance >= 0 && distance <= 150) {
    servo.write(0);     // Rotate servo to 0 degrees
    blinkLed(1, 100);   // Blink LED once every 100ms
  } else if (distance > 150 && distance <= 300) {
    servo.write(180);   // Rotate servo to 180 degrees
    blinkLed(3, 200);   // Blink LED three times every 200ms
  } else {
    servo.write(90);    // Set servo to 90 degrees (no movement)
    digitalWrite(ledPin, LOW);  // Turn off LED
  }

  delay(500);  // Wait for 500ms before next reading
}

void blinkLed(int count, int interval) {
  for (int i = 0; i < count; i++) {
    digitalWrite(ledPin, HIGH);  // Turn on LED
    delay(interval / 2);
    digitalWrite(ledPin, LOW);   // Turn off LED
    delay(interval / 2);
  }
}