#include <ESP32Servo.h>

// Define the LED and button pins
const int ledPin1 = 2;
const int ledPin2 = 5;
const int ledPin3 = 4;
const int buttonPin1 = 15;
const int buttonPin2 = 16;
const int buttonPin3 = 17;

// Define the ultrasonic sensor pins (HC-SR05)
const int trigPin = 18;  // Connects to the trigger pin of HC-SR05
const int echoPin = 19;  // Connects to the echo pin of HC-SR05

// Define the servo pin
const int servoPin = 14;

bool ledState1 = false;
bool ledState2 = false;
bool ledState3 = false;
bool lastButtonState1 = HIGH;
bool lastButtonState2 = HIGH;
bool lastButtonState3 = HIGH;
bool currentButtonState1 = HIGH;
bool currentButtonState2 = HIGH;
bool currentButtonState3 = HIGH;

Servo myServo;

void setup() {
  // Initialize the LED pins as outputs
  pinMode(ledPin1, OUTPUT);
  pinMode(ledPin2, OUTPUT);
  pinMode(ledPin3, OUTPUT);
  // Initialize the button pins as inputs with pull-up resistors
  pinMode(buttonPin1, INPUT_PULLUP);
  pinMode(buttonPin2, INPUT_PULLUP);
  pinMode(buttonPin3, INPUT_PULLUP);
  // Initialize the ultrasonic sensor pins
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  
  // Attach the servo motor
  myServo.attach(servoPin);
  myServo.write(0); // Initial position

  // Set the LEDs to be off initially
  digitalWrite(ledPin1, LOW);
  digitalWrite(ledPin2, LOW);
  digitalWrite(ledPin3, LOW);

  Serial.begin(115200);
}

void loop() {
  // Read the state of the buttons
  currentButtonState1 = digitalRead(buttonPin1);
  currentButtonState2 = digitalRead(buttonPin2);
  currentButtonState3 = digitalRead(buttonPin3);

  // Check if button 1 has been pressed (transition from HIGH to LOW)
  if (currentButtonState1 == LOW && lastButtonState1 == HIGH) {
    // Toggle LED 1 state
    ledState1 = !ledState1;
    digitalWrite(ledPin1, ledState1 ? HIGH : LOW);
    Serial.println(ledState1 ? "LED 1 ON" : "LED 1 OFF");
    delay(200);
  }

  // Check if button 2 has been pressed (transition from HIGH to LOW)
  if (currentButtonState2 == LOW && lastButtonState2 == HIGH) {
    // Toggle LED 2 state
    ledState2 = !ledState2;
    digitalWrite(ledPin2, ledState2 ? HIGH : LOW);
    Serial.println(ledState2 ? "LED 2 ON" : "LED 2 OFF");
    delay(200);
  }

  // Check if button 3 has been pressed (transition from HIGH to LOW)
  if (currentButtonState3 == LOW && lastButtonState3 == HIGH) {
    // Toggle LED 3 state
    ledState3 = !ledState3;
    digitalWrite(ledPin3, ledState3 ? HIGH : LOW);
    Serial.println(ledState3 ? "LED 3 ON" : "LED 3 OFF");
    delay(200);
  }

  // Update the last button states
  lastButtonState1 = currentButtonState1;
  lastButtonState2 = currentButtonState2;
  lastButtonState3 = currentButtonState3;

  // Measure the distance using the ultrasonic sensor (HC-SR05)
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  long duration = pulseIn(echoPin, HIGH);
  // Speed of sound is approximately 343 meters per second (34300 cm/s)
  // Divide by 2 for the round trip, and then divide by 10000 to get centimeters
  long distance = duration * 34300 / 2 / 10000;

  // Control the servo motor based on distance
  if (distance < 10) { // Adjust the distance threshold as needed
    myServo.write(90); // Move servo to 90 degrees
    Serial.println("Servo Open");
  } else {
    myServo.write(0); // Move servo to 0 degrees
    Serial.println("Servo Close");
  }

  // Add a small delay to avoid overwhelming the serial output
  delay(100);
}