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

#define PIR_PIN 4             // Pin untuk PIR sensor (D4)
#define SERVO_PIN 32          // Pin untuk servo motor (D32)
#define TRIG_PIN 18           // Pin untuk trig sensor ultrasonik (D18)
#define ECHO_PIN 5            // Pin untuk echo sensor ultrasonik (D5)

// Pin untuk LED
#define LED_RED 25            // Pin LED merah (D25)
#define LED_YELLOW 26         // Pin LED kuning (D26)
#define LED_GREEN 27          // Pin LED hijau (D27)

Servo servo;                   // Objek servo (Use the standard Servo library here)

long duration;
int distance;
int maxDistance = 10;          // Batas jarak maksimum (misal 10 cm untuk kotak penuh)
int halfFullDistance = 50;     // Batas jarak untuk setengah penuh (misal 20 cm)
int emptyDistance = 100;        // Batas jarak untuk kosong (misal 30 cm)

void setup() {
  pinMode(PIR_PIN, INPUT);     // Set pin PIR sebagai input
  pinMode(TRIG_PIN, OUTPUT);   // Set pin trig sebagai output
  pinMode(ECHO_PIN, INPUT);    // Set pin echo sebagai input
  
  // Set pin untuk LED sebagai output
  pinMode(LED_RED, OUTPUT);    
  pinMode(LED_YELLOW, OUTPUT); 
  pinMode(LED_GREEN, OUTPUT);  
  
  servo.attach(SERVO_PIN);     // Hubungkan servo ke pin yang sesuai
  servo.write(0);              // Awal pintu dalam keadaan tertutup (0 derajat)
  Serial.begin(115200);        // Inisialisasi komunikasi serial
}

void loop() {
  bool objectDetected = digitalRead(PIR_PIN);  // Baca status dari PIR sensor
  int distance = measureDistance();            // Hitung jarak dari sensor ultrasonik
  
  // Update status LED berdasarkan jarak
  updateLEDStatus(distance);
  
  // Jika PIR mendeteksi object dan kotak tidak penuh
  if (objectDetected && distance > maxDistance) {
    openDoor();   // Buka pintu
  } 
  // Jika sampah penuh, jangan buka pintu meskipun ada deteksi objek
  else if (distance <= maxDistance) {
    Serial.println("Sampah penuh, pintu tidak akan terbuka");
    closeDoor();  // Tutup pintu
  } 
  else {
    closeDoor();  // Tutup pintu
  }

  delay(500);     // Delay untuk menstabilkan pembacaan sensor
}

// Fungsi untuk mengukur jarak menggunakan sensor ultrasonik
int measureDistance() {
  digitalWrite(TRIG_PIN, LOW);   // Pastikan trig dalam kondisi rendah
  delayMicroseconds(2);
  
  digitalWrite(TRIG_PIN, HIGH);  // Kirim sinyal trig
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  
  duration = pulseIn(ECHO_PIN, HIGH);  // Baca waktu yang dibutuhkan sinyal untuk kembali
  distance = duration * 0.034 / 2;     // Hitung jarak (dalam cm)
  
  Serial.print("Distance: ");
  Serial.println(distance);
  
  return distance;
}

// Fungsi untuk membuka pintu
void openDoor() {
  servo.write(90);   // Putar servo ke 90 derajat (buka pintu)
  Serial.println("Pintu terbuka");
}

// Fungsi untuk menutup pintu
void closeDoor() {
  servo.write(0);    // Putar servo kembali ke 0 derajat (tutup pintu)
  Serial.println("Pintu tertutup");
}

// Fungsi untuk mengupdate status LED berdasarkan jarak
void updateLEDStatus(int distance) {
  if (distance <= maxDistance) {
    // Sampah penuh
    digitalWrite(LED_RED, HIGH);
    digitalWrite(LED_YELLOW, LOW);
    digitalWrite(LED_GREEN, LOW);
    Serial.println("Sampah penuh (LED Merah menyala)");
  } else if (distance <= halfFullDistance) {
    // Sampah setengah penuh
    digitalWrite(LED_RED, LOW);
    digitalWrite(LED_YELLOW, HIGH);
    digitalWrite(LED_GREEN, LOW);
    Serial.println("Sampah setengah penuh (LED Kuning menyala)");
  } else if (distance <= emptyDistance) {
    // Sampah kosong
    digitalWrite(LED_RED, LOW);
    digitalWrite(LED_YELLOW, LOW);
    digitalWrite(LED_GREEN, HIGH);
    Serial.println("Sampah kosong (LED Hijau menyala)");
  } else {
    // Jika tidak ada status yang sesuai, matikan semua LED
    digitalWrite(LED_RED, LOW);
    digitalWrite(LED_YELLOW, LOW);
    digitalWrite(LED_GREEN, LOW);
  }
}