#include <ESP32Servo.h>  // Include the ESP32Servo library

#define TRIG_PIN_1 4       // GPIO4 on ESP32 for the first sensor
#define ECHO_PIN_1 2       // GPIO2 on ESP32 for the first sensor
#define TRIG_PIN_2 12      // GPIO12 on ESP32 for the second sensor
#define ECHO_PIN_2 13      // GPIO13 on ESP32 for the second sensor
#define SERVO_PIN 18       // GPIO18 on ESP32 for the servo signal pin

long duration_1, duration_2;
int distance_1, distance_2;

Servo myServo;  // Create a Servo object

// Method to read the distance from an ultrasonic sensor
int readDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  long duration = pulseIn(echoPin, HIGH);
  int distance = duration * 0.034 / 2;
  return distance;
}

// Method to control the servo based on the distance
void controlServo(int distance) {
  if (distance < 40) {
    myServo.write(90); // Rotate servo to 90 degrees if distance is less than 40cm
  } else {
    myServo.write(0); // Rotate servo to 0 degrees if distance is 40cm or more
  }
}

// Function to read and display the first distance sensor and control servo
void readD1() {
  distance_1 = readDistance(TRIG_PIN_1, ECHO_PIN_1);
  Serial.print("Distance 1: ");
  Serial.println(distance_1);
  controlServo(distance_1); // Control the servo based on the first sensor's distance
}

// Function to read and display the second distance sensor
void readD2() {
  distance_2 = readDistance(TRIG_PIN_2, ECHO_PIN_2);
  Serial.print("Distance 2: ");
  Serial.println(distance_2);
}

void setup() {
  Serial.begin(9600); // Initialize serial communication
  Serial.println("Hello");
  
  // Set the pin modes for the ultrasonic sensors
  pinMode(TRIG_PIN_1, OUTPUT); // Sets the trigPin_1 as an Output
  pinMode(ECHO_PIN_1, INPUT);  // Sets the echoPin_1 as an Input
  pinMode(TRIG_PIN_2, OUTPUT); // Sets the trigPin_2 as an Output
  pinMode(ECHO_PIN_2, INPUT);  // Sets the echoPin_2 as an Input

  // Attach the servo to the specified GPIO pin and set initial position
  myServo.attach(SERVO_PIN); 
  myServo.write(0); // Initialize servo at 0 degrees
}

void loop() {
  readD1(); // Read the first sensor and control the servo
  readD2(); // Read the second sensor
  delay(100); // Add a short delay between readings
}