#include <ESP32Servo.h>

#define TRIGGER_PIN 2
#define ECHO_PIN 4
#define SERVO_PIN 14

#define LED_RED_PIN 27
#define LED_GREEN_PIN 26
#define LED_BLUE_PIN 25
#define LED_YELLOW_PIN 33
#define LED_ORANGE_PIN 32
#define LED_WHITE_PIN 5
#define LED_PURPLE_PIN 18

Servo servo;

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

  pinMode(TRIGGER_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(LED_RED_PIN, OUTPUT);
  pinMode(LED_GREEN_PIN, OUTPUT);
  pinMode(LED_BLUE_PIN, OUTPUT);
  pinMode(LED_YELLOW_PIN, OUTPUT);
  pinMode(LED_ORANGE_PIN, OUTPUT);
  pinMode(LED_WHITE_PIN, OUTPUT);
  pinMode(LED_PURPLE_PIN, OUTPUT);

  servo.attach(SERVO_PIN);
}

void loop() {
  long duration, distanceCm;

  digitalWrite(TRIGGER_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIGGER_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIGGER_PIN, LOW);

  duration = pulseIn(ECHO_PIN, HIGH);
  distanceCm = duration * 0.034 / 2;

  Serial.print("Distance: ");
  Serial.print(distanceCm);
  Serial.println(" cm");

  // Control LEDs based on distance
  digitalWrite(LED_RED_PIN, distanceCm < 10);
  digitalWrite(LED_GREEN_PIN, distanceCm >= 10 && distanceCm < 20);
  digitalWrite(LED_BLUE_PIN, distanceCm >= 20 && distanceCm < 30);
  digitalWrite(LED_YELLOW_PIN, distanceCm >= 30 && distanceCm < 40);
  digitalWrite(LED_ORANGE_PIN, distanceCm >= 40 && distanceCm < 50);
  digitalWrite(LED_WHITE_PIN, distanceCm >= 50 && distanceCm < 60);
  digitalWrite(LED_PURPLE_PIN, distanceCm >= 60);

  // Control servo motor based on distance
  if (distanceCm < 10) {
    servo.write(0); // Move servo to 0 degrees
  } else if (distanceCm >= 10 && distanceCm < 20) {
    servo.write(90); // Move servo to 90 degrees
  } else {
    servo.write(180); // Move servo to 180 degrees
  }

  delay(1000);
}