#include <ESP32Servo.h>

const int echoPin = 17; 
const int trigPin = 5; 
const int ledPins[] = {12, 14, 27, 26, 25, 33, 32}; 
Servo servoMotor;

void setup() {

  Serial.begin(9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  
  for (int i = 0; i < 7; i++) {
    pinMode(ledPins[i], OUTPUT);
  }
  
  servoMotor.attach(35); 
}

void loop() {
  long duration, distance;
  
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH);

  // Calculate distance in centimeters
  distance = (duration / 2) / 29.1;

  Serial.print("Distance: ");
  Serial.println(distance);

  if (distance <= 350) {
    servoMotor.write(45); 
    digitalWrite(ledPins[0], HIGH); 
  } else {
    digitalWrite(ledPins[0], LOW);
  }

  if (distance <= 300) {
    digitalWrite(ledPins[1], HIGH);
  } else {
    digitalWrite(ledPins[1], LOW);
  }

  if (distance <= 250) {
    servoMotor.write(90); 
    digitalWrite(ledPins[2], HIGH); 
  } else {
    digitalWrite(ledPins[2], LOW);
  }

  if (distance <= 200) {
    digitalWrite(ledPins[3], HIGH); 
  } else {
    digitalWrite(ledPins[3], LOW);
  }

  if (distance <= 150) {
    servoMotor.write(135); 
    digitalWrite(ledPins[4], HIGH); 
  } else {
    digitalWrite(ledPins[4], LOW);
  }

  if (distance <= 100) {
    digitalWrite(ledPins[5], HIGH); 
  } else {
    digitalWrite(ledPins[5], LOW);
  }

  if (distance <= 50) {
    servoMotor.write(180); 
    digitalWrite(ledPins[6], HIGH);
  } else {
    digitalWrite(ledPins[6], LOW);
  }

  delay(100);
}