#include <DHT.h>
#include <ESP32Servo.h>

// Pin Configuration
#define DHTPIN 2
#define DHTTYPE DHT22
#define TRIG_PIN 4
#define ECHO_PIN 16
#define RELAY_LAMPU 27     // Lampu
#define RELAY_KIPAS 26     // Kipas
#define SERVO_PIN 15

// Objects
DHT dht(DHTPIN, DHTTYPE);
Servo myServo;

// Global Variables
float temperature = 0.0;
float humidity = 0.0;
long distance = 0;
bool relayLampuStatus = false;
bool relayKipasStatus = false;
bool servoOpen = false;
int counter = 0;

// Timing variables
unsigned long lastSensorRead = 0;
const unsigned long sensorInterval = 2000; // Read sensors every 2 seconds

void setup() {
  Serial.begin(9600); // Match baud rate with reference code
  delay(1000); // Give time for Serial Monitor to initialize
  
  // Initialize pins
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(RELAY_LAMPU, OUTPUT);
  pinMode(RELAY_KIPAS, OUTPUT);
  
  // Set relays OFF initially
  digitalWrite(RELAY_LAMPU, LOW);
  digitalWrite(RELAY_KIPAS, LOW);
  
  // Initialize servo
  myServo.attach(SERVO_PIN);
  myServo.write(0); // Closed position
  
  // Initialize DHT22
  Serial.println("Initializing DHT22 sensor...");
  dht.begin();
  delay(5000); // Wait 5 seconds for DHT22 stabilization
  
  Serial.println("ESP32 System Ready!");
}

void loop() {
  // Read sensors periodically
  if (millis() - lastSensorRead >= sensorInterval) {
    readSensors();
    controlAutomatic();
    lastSensorRead = millis();
  }
  delay(100); // Small delay to prevent excessive looping
}

void readSensors() {
  // Read DHT22
  Serial.println("Data: " + String(counter));
  Serial.printf("DHT pin status before reading: %d\n", digitalRead(DHTPIN));
  temperature = dht.readTemperature();
  humidity = dht.readHumidity();
  
  // Print temperature in Celsius
  Serial.print("Temperature:\t");
  Serial.print(temperature);
  Serial.println("C");
  
  // Print temperature in Fahrenheit
  Serial.print("Temperature:\t");
  Serial.print(dht.readTemperature(true));
  Serial.println("F");
  
  // Print humidity
  Serial.println("Humidity:\t" + String(humidity) + "%");
  
  // Print heat index
  Serial.print("Heat Index In Celsius: ");
  Serial.println(dht.computeHeatIndex(temperature, humidity, false));
  Serial.print("Heat Index In Fahrenheit: ");
  Serial.println(dht.computeHeatIndex(temperature, humidity, true));
  Serial.println(" ");
  
  // Check for invalid readings
  if (isnan(temperature) || isnan(humidity)) {
    Serial.println("Failed to read from DHT22 sensor!");
  }
  
  // Read HC-SR04 ultrasonic sensor
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  
  long duration = pulseIn(ECHO_PIN, HIGH, 30000); // 30ms timeout
  if (duration > 0) {
    distance = duration * 0.034 / 2; // Convert to cm
    if (distance > 400) distance = 400; // Limit to reasonable range
    Serial.printf("HC-SR04 - Distance: %ld cm\n", distance);
  } else {
    Serial.println("Ultrasonic sensor timeout!");
  }
  
  counter++;
}

void controlAutomatic() {
  // Servo control based on distance
  bool shouldOpen = (distance < 20 && distance > 0);
  if (shouldOpen && !servoOpen) {
    myServo.write(90); // Open servo
    servoOpen = true;
    Serial.println("Servo opened automatically (distance < 20cm)");
  } else if (!shouldOpen && servoOpen) {
    myServo.write(0); // Close servo
    servoOpen = false;
    Serial.println("Servo closed automatically (distance >= 20cm)");
  }
  
  // Automatic fan control based on temperature and humidity
  bool shouldTurnOnFan = (temperature > 30.0 || humidity > 70.0);
  bool shouldTurnOffFan = (temperature < 28.0 && humidity < 65.0);
  
  if (shouldTurnOnFan && !relayKipasStatus && !isnan(temperature) && !isnan(humidity)) {
    digitalWrite(RELAY_KIPAS, HIGH);
    relayKipasStatus = true;
    Serial.println("Kipas turned ON automatically (temp > 30°C or humidity > 70%)");
  } else if (shouldTurnOffFan && relayKipasStatus && !isnan(temperature) && !isnan(humidity)) {
    digitalWrite(RELAY_KIPAS, LOW);
    relayKipasStatus = false;
    Serial.println("Kipas turned OFF automatically (temp < 28°C and humidity < 65%)");
  }
  
  // Automatic lamp control (example: turn on if temperature > 32°C)
  bool shouldTurnOnLamp = (temperature > 32.0);
  bool shouldTurnOffLamp = (temperature < 30.0);
  
  if (shouldTurnOnLamp && !relayLampuStatus && !isnan(temperature)) {
    digitalWrite(RELAY_LAMPU, HIGH);
    relayLampuStatus = true;
    Serial.println("Lampu turned ON automatically (temp > 32°C)");
  } else if (shouldTurnOffLamp && relayLampuStatus && !isnan(temperature)) {
    digitalWrite(RELAY_LAMPU, LOW);
    relayLampuStatus = false;
    Serial.println("Lampu turned OFF automatically (temp < 30°C)");
  }
}