#include <ESP32Servo.h>

Servo servo;

const int trigPin = 23; // Ultrasonic sensor trigger pin connected to GPIO4
const int echoPin = 22; // Ultrasonic sensor echo pin connected to GPIO5
const int servoPin = 14; // Servo motor connected to GPIO14

const int ledPins[] = {12, 13, 16, 17, 18, 19, 21}; // LEDs connected to GPIO pins

void setup() {
  Serial.begin(115200);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  servo.attach(servoPin);

  for (int i = 0; i < 7; i++) {
    pinMode(ledPins[i], OUTPUT);
  }
}

long readUltrasonicDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH);
  long distance = duration * 0.034 / 2;
  return distance;
}

void loop() {
  long distance = readUltrasonicDistance();
  Serial.println(distance);

  for (int i = 0; i < 7; i++) {
    digitalWrite(ledPins[i], distance <= (350 - 50 * i) ? HIGH : LOW);
  }

  if (distance <= 200) {
    servo.write(180);
  } else if (distance <= 250) {
    servo.write(135);
  } else if (distance <= 300) {
    servo.write(90);
  } else if (distance <= 350) {
    servo.write(45);
  } else {
    servo.write(14); // Default position if the object is out of range
  }

  delay(50);
}