#include <Ultrasonic.h>
#include <ESP32Servo.h>

const int trigPin = 22;  // Trigger pin of the ultrasonic sensor
const int echoPin = 23;  // Echo pin of the ultrasonic sensor
const int ledPin1 = 21;    // LED pin
const int ledPin2 = 19;    // LED pin
const int ledPin3 = 18;    // LED pin
const int ledPin4 = 17;    // LED pin
const int ledPin5 = 4;    // LED pin
const int ledPin6 = 0;    // LED pin
const int ledPin7 = 2;    // LED pin
const int servoPin = 16; // Servo motor pin

Ultrasonic ultrasonic(trigPin, echoPin);
Servo myServo;

void setup() {
  Serial.begin(115200);
  pinMode(ledPin1, OUTPUT);
  pinMode(ledPin2, OUTPUT);
  pinMode(ledPin3, OUTPUT);
  pinMode(ledPin4, OUTPUT);
  pinMode(ledPin5, OUTPUT);
  pinMode(ledPin6, OUTPUT);
  pinMode(ledPin7, OUTPUT);
  myServo.attach(servoPin);
}

void loop() {
  // Measure distance using the ultrasonic sensor
  int distance = ultrasonic.read();

  // Print distance to Serial Monitor
  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" cm");

  // Check if distance is less than 50cm
  if (distance > 300 && distance <= 350) {
    // Trigger LED
    digitalWrite(ledPin1, HIGH);
    digitalWrite(ledPin2, LOW);
    digitalWrite(ledPin3, LOW);
    digitalWrite(ledPin4, LOW);
    digitalWrite(ledPin5, LOW);
    digitalWrite(ledPin6, LOW);
    digitalWrite(ledPin7, LOW);

    // Trigger Servo motor
    myServo.write(45); // Assuming 90 degrees is the desired position

  }

  else if (distance > 250 && distance <= 300) {
    // Trigger LED
    digitalWrite(ledPin1, HIGH);
    digitalWrite(ledPin2, HIGH);
    digitalWrite(ledPin3, LOW);
    digitalWrite(ledPin4, LOW);
    digitalWrite(ledPin5, LOW);
    digitalWrite(ledPin6, LOW);
    digitalWrite(ledPin7, LOW);

  }

  else if (distance > 200 && distance <= 250) {
    // Trigger LED
    digitalWrite(ledPin1, HIGH);
    digitalWrite(ledPin2, HIGH);
    digitalWrite(ledPin3, HIGH);
    digitalWrite(ledPin4, LOW);
    digitalWrite(ledPin5, LOW);
    digitalWrite(ledPin6, LOW);
    digitalWrite(ledPin7, LOW);

    // Trigger Servo motor
    myServo.write(90); // Assuming 90 degrees is the desired position

  }

  else if (distance > 150 && distance <= 200) {
    // Trigger LED
    digitalWrite(ledPin1, HIGH);
    digitalWrite(ledPin2, HIGH);
    digitalWrite(ledPin3, HIGH);
    digitalWrite(ledPin4, HIGH);
    digitalWrite(ledPin5, LOW);
    digitalWrite(ledPin6, LOW);
    digitalWrite(ledPin7, LOW);

  }

  else if (distance > 100 && distance <= 150) {
    // Trigger LED
    digitalWrite(ledPin1, HIGH);
    digitalWrite(ledPin2, HIGH);
    digitalWrite(ledPin3, HIGH);
    digitalWrite(ledPin4, HIGH);
    digitalWrite(ledPin5, HIGH);
    digitalWrite(ledPin6, LOW);
    digitalWrite(ledPin7, LOW);

    // Trigger Servo motor
    myServo.write(135); // Assuming 90 degrees is the desired position

  }

  else if (distance > 50 && distance <= 100) {
    // Trigger LED
    digitalWrite(ledPin1, HIGH);
    digitalWrite(ledPin2, HIGH);
    digitalWrite(ledPin3, HIGH);
    digitalWrite(ledPin4, HIGH);
    digitalWrite(ledPin5, HIGH);
    digitalWrite(ledPin6, HIGH);
    digitalWrite(ledPin7, LOW);

  }

  else if (distance <= 50) {
    // Trigger LED
    digitalWrite(ledPin1, HIGH);
    digitalWrite(ledPin2, HIGH);
    digitalWrite(ledPin3, HIGH);
    digitalWrite(ledPin4, HIGH);
    digitalWrite(ledPin5, HIGH);
    digitalWrite(ledPin6, HIGH);
    digitalWrite(ledPin7, HIGH);

    // Trigger Servo motor
    myServo.write(180); // Assuming 90 degrees is the desired position

  }

  else {
    digitalWrite(ledPin1, LOW);
    digitalWrite(ledPin2, LOW);
    digitalWrite(ledPin3, LOW);
    digitalWrite(ledPin4, LOW);
    digitalWrite(ledPin5, LOW);
    digitalWrite(ledPin6, LOW);
    digitalWrite(ledPin7, LOW);
  }

  delay(500);  // Add a small delay for stability
}