#include <ESP32Servo.h>

// Define pin connections
#define TRIG_PIN 26 // ESP32 pin connected to Ultrasonic Sensor's TRIG pin
#define ECHO_PIN 25 // ESP32 pin connected to Ultrasonic Sensor's ECHO pin
#define LED1_PIN 17 // ESP32 pin connected to the first LED's pin
#define LED2_PIN 16 // ESP32 pin connected to the second LED's pin
#define SERVO_PIN 5 // 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(25, OUTPUT); // Set ESP32 pin to output mode for the ultrasonic sensor
  pinMode(33, INPUT);  // Set ESP32 pin to input mode for the ultrasonic sensor
  pinMode(32, OUTPUT); // Set ESP32 pin to output mode for the first LED
  pinMode(13, OUTPUT); // Set ESP32 pin to output mode for the second LED
  servo.attach(12);   // Attaches the servo on the specified pin to the servo object
  servo.write(90);           // Initially set the servo to 90 degrees (stopped position)
}

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

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

  // Calculate the distance
  distance_cm = 0.017 * duration_us;

  if (distance_cm < DISTANCE_THRESHOLD) {
    // If the object is closer than the threshold, light up LED1 and stop the servo
    digitalWrite(32, HIGH); // Turn on the first LED
    delay(1000);
    digitalWrite(13, LOW);  // Turn off the second LED
    servo.write(90);              // Stop the servo motor
  } else {
    // If the object is farther than the threshold, light up LED2 and run the servo clockwise
    digitalWrite(32, LOW);  // Turn off the first LED
    delay(1000);
    digitalWrite(13, HIGH); // Turn on the second LED
    servo.write(180);             // Move servo to 180 degrees (clockwise)
  }

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

  delay(500); // Delay between measurements
}