#include <Servo.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

// Pin untuk sensor ultrasonik dan komponen lainnya
const int trigPin1 = 9; // Sensor 1: untuk deteksi objek
const int echoPin1 = 8;

const int trigPin2 = 5; // Sensor 2: untuk mengukur ketinggian sampah
const int echoPin2 = 4;

const int servoPin = 3;      // Pin untuk Servo
const int redLedPin = 7;     // Pin untuk LED merah
const int greenLedPin = 6;   // Pin untuk LED hijau
const int buzzerPin = 2;      // Pin untuk Buzzer

Servo servoMotor;
long duration1, duration2;
int distanceTrash, distanceToTop;
const int maxDistance = 100; // Ketinggian maksimal tong dalam cm
int trashPercentage;  // Persentase sampah di dalam tong
LiquidCrystal_I2C lcd(0x27, 16, 2);  // Inisialisasi LCD dengan alamat I2C 0x27 dan ukuran 16x2

void setup() {
  servoMotor.attach(servoPin);       // Menghubungkan servo ke pin kontrol
  pinMode(trigPin1, OUTPUT);         // Set pin Trig untuk sensor 1 sebagai output
  pinMode(echoPin1, INPUT);          // Set pin Echo untuk sensor 1 sebagai input
  pinMode(trigPin2, OUTPUT);         // Set pin Trig untuk sensor 2 sebagai output
  pinMode(echoPin2, INPUT);          // Set pin Echo untuk sensor 2 sebagai input
  pinMode(redLedPin, OUTPUT);        // Set pin LED merah sebagai output
  pinMode(greenLedPin, OUTPUT);      // Set pin LED hijau sebagai output
  pinMode(buzzerPin, OUTPUT);   // Set pin buzzer sebagai output
  
  lcd.begin(16, 2);                  // Memulai LCD dengan ukuran 16x2
  lcd.backlight();                   // Mengaktifkan lampu latar LCD
  Serial.begin(9600);                // Memulai komunikasi serial untuk debugging
}

void loop() {
  // Mengirimkan sinyal Trig untuk sensor 1 (deteksi objek)
  digitalWrite(trigPin1, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin1, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin1, LOW);

  // Membaca sinyal Echo untuk sensor 1
  duration1 = pulseIn(echoPin1, HIGH);
  distanceTrash = duration1 * 0.034 / 2;  // Menghitung jarak dalam cm untuk sensor 1

  // Mengirimkan sinyal Trig untuk sensor 2 (ketinggian sampah)
  digitalWrite(trigPin2, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin2, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin2, LOW);

  // Membaca sinyal Echo untuk sensor 2
  duration2 = pulseIn(echoPin2, HIGH);
  distanceToTop = duration2 * 0.034 / 2;  // Menghitung jarak dalam cm untuk sensor 2

  // Menghitung persentase sampah di dalam tong
  trashPercentage = ((maxDistance - distanceToTop) * 100) / maxDistance;

  // Pastikan persentase tidak melebihi 100% atau kurang dari 0%
  if (trashPercentage > 100) {
    trashPercentage = 100;
  } else if (trashPercentage < 0) {
    trashPercentage = 0;
  }

  // Menampilkan jarak dan persentase pada LCD
  lcd.clear();
  lcd.setCursor(0, 0);  // Baris pertama, kolom pertama
  lcd.print("Jarak: ");
  lcd.print(distanceTrash);
  lcd.print(" cm");

  lcd.setCursor(0, 1);  // Baris kedua
  lcd.print("Isi: ");
  lcd.print(trashPercentage);
  lcd.print("%");

  // Jika objek terdeteksi dalam jarak < 20 cm, buka tutup dan nyalakan LED hijau
  if (distanceTrash < 20) {
    servoMotor.write(90);  // Membuka tutup (servo bergerak 90 derajat)
    digitalWrite(greenLedPin, HIGH);  // Nyalakan LED hijau
    digitalWrite(redLedPin, LOW);     // Matikan LED merah
  } 
  // Jika jarak lebih dari 20 cm, tutup tutup dan nyalakan LED merah
  else {
    servoMotor.write(0);   // Menutup tutup (servo kembali ke posisi awal)
    digitalWrite(greenLedPin, LOW);   // Matikan LED hijau
    digitalWrite(redLedPin, HIGH);    // Nyalakan LED merah
  }

  // Aktifkan buzzer jika kapasitas sampah lebih dari 80%
  if (trashPercentage > 80) {
    for (int i = 0; i < 3; i++) {  // Loop untuk bunyi dengan jeda
      tone(buzzerPin, 500);   // Bunyi buzzer dengan frekuensi 1000 Hz
      delay(300);              // Durasi bunyi 200ms
      noTone(buzzerPin);       // Matikan buzzer
      delay(800);              // Jeda 300ms sebelum bunyi lagi
    }
  } else {
    noTone(buzzerPin);   // Matikan buzzer
  }

  delay(500);  // Jeda untuk pembacaan berikutnya
}