#include <DHT.h>
#include <ESP32Servo.h> // Use ESP32Servo instead of Servo.h

// Define pins for sensors and devices
#define DHTPIN 18         // DHT22 connected to D2
#define DHTTYPE DHT22     // DHT22 sensor
#define TRIGPIN 21        // Ultrasonic 1 Trig pin
#define ECHOPIN 4         // Ultrasonic 1 Echo pin
#define TRIGPIN2 22       // Ultrasonic 2 Trig pin
#define ECHOPIN2 23       // Ultrasonic 2 Echo pin
#define SERVOPIN 17       // Servo motor pin

// Initialize DHT sensor and servo
DHT dht(DHTPIN, DHTTYPE);
Servo myServo;

void setup() {
  Serial.begin(9600);

  // Initialize DHT sensor
  dht.begin();

  // Attach the servo motor
  myServo.attach(SERVOPIN);

  // Initialize pins for ultrasonic sensors
  pinMode(TRIGPIN, OUTPUT);
  pinMode(ECHOPIN, INPUT);
  pinMode(TRIGPIN2, OUTPUT);
  pinMode(ECHOPIN2, INPUT);

  Serial.println("System Initialized!");
}

float measureDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH);
  return (duration * 0.034) / 2; // Convert to cm
}

void loop() {
  Serial.println("Reading sensors...");

  // Read DHT22 Temperature and Humidity
  float temperature = dht.readTemperature();
  float humidity = dht.readHumidity();

  if (isnan(temperature) || isnan(humidity)) {
    Serial.println("Failed to read from DHT sensor!");
  } else {
    Serial.print("Temperature: "); Serial.print(temperature); Serial.println(" °C");
    Serial.print("Humidity: "); Serial.print(humidity); Serial.println(" %");
  }

  // Measure distances with ultrasonic sensors
  float distance1 = measureDistance(TRIGPIN, ECHOPIN);
  Serial.print("Distance 1: "); Serial.print(distance1); Serial.println(" cm");

  float distance2 = measureDistance(TRIGPIN2, ECHOPIN2);
  Serial.print("Distance 2: "); Serial.print(distance2); Serial.println(" cm");

  // Control servo based on distance2
  if (distance2 < 50.0) { // Example condition
    Serial.println("Activating Servo...");
    myServo.write(90); // Rotate servo to 90 degrees
  } else {
    Serial.println("Servo at Rest...");
    myServo.write(0); // Reset servo to 0 degrees
  }

  Serial.println("Waiting for next cycle...\n");
  delay(2000); // Wait for 2 seconds before repeating loop
}