#include <ESP32Servo.h>

// Define pin connections
#define TRIG_PIN 33 // ESP32 pin connected to Ultrasonic Sensor's TRIG pin
#define ECHO_PIN 32 // ESP32 pin connected to Ultrasonic Sensor's ECHO pin
#define LED1_PIN 12 // ESP32 pin connected to the first LED's pin
#define LED2_PIN 14 // ESP32 pin connected to the second LED's pin
#define SERVO_PIN 26 // ESP32 pin connected to Servo Motor's pin

// Define the distance threshold in centimeters
#define DISTANCE_THRESHOLD 10 

// Variables will change:
float duration_us, distance_cm;
Servo servo; // create servo object to control a servo motor

void setup() {
  Serial.begin(9600); // Initialize serial port
  pinMode(33, OUTPUT); // Set ESP32 pin to output mode for the ultrasonic sensor
  pinMode(32, INPUT);  // Set ESP32 pin to input mode for the ultrasonic sensor
  pinMode(12, OUTPUT); // Set ESP32 pin to output mode for the first LED
  pinMode(14, OUTPUT); // Set ESP32 pin to output mode for the second LED
  servo.attach(26);   // Attaches the servo on the specified pin to the servo object
}

void loop() {
  // Generate 10-microsecond pulse to TRIG pin
  digitalWrite(33, HIGH);
  delayMicroseconds(10);
  digitalWrite(33, LOW);

  // Measure duration of pulse from ECHO pin
  duration_us = pulseIn(32, HIGH);

  // Calculate the distance
  distance_cm = 0.017 * duration_us;

  if (distance_cm < DISTANCE_THRESHOLD) {
    // If the object is close, move servo anticlockwise and turn on LED1
    servo.write(0); // Move servo to 0 degrees (anticlockwise)
    digitalWrite(12, HIGH); // Turn on the first LED
    delay(1000);
    digitalWrite(14, LOW);  // Turn off the second LED
  } else {
    // If the object is not close, move servo clockwise and turn on LED2
    servo.write(180); // Move servo to 180 degrees (clockwise)
    digitalWrite(12, LOW);  // Turn off the first LED
    delay(1000);
    digitalWrite(14, HIGH); // Turn on the second LED
  }

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

  delay(500); // Delay between measurements
}