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

Servo servo1; // servo kiri
Servo servo2; // servo kanan
int potValue1; // sensor proximity induktif
int potValue2; // sensor proximity kapasitif
LiquidCrystal_I2C lcd(0x27, 16, 2);

const int trigPin1 = 2; // Pin trigger sensor ultrasonik 1
const int echoPin1 = 3; // Pin echo sensor ultrasonik 1
const int trigPin2 = 4; // Pin trigger sensor ultrasonik 2
const int echoPin2 = 5; // Pin echo sensor ultrasonik 2
int pirSensorValue = 6;

long duration1;
long duration2;
int distance1;
int distance2;
float volumeOrganik;
float volumeAnorganik;
float maxVolume = 300; // Kapasitas maksimum tempat sampah (ml)

bool isMoving = false;
bool isOrganikFullNotified = false; 
bool isAnorganikFullNotified = false; 
unsigned long moveStartTime;
unsigned long moveDuration = 1000; // Durasi pergerakan servo (ms)

void setup() {
  servo1.attach(9);
  servo2.attach(10);
  servo1.write(150); // Set posisi awal servo 1 ke sudut 150
  servo2.write(30);  // Set posisi awal servo 2 ke sudut 30
  lcd.init();
  lcd.backlight();
  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  pinMode(6, INPUT); // Pin sensor PIR
}

void loop() {
  // Membaca nilai potensiometer 1
  potValue1 = analogRead(A0);
  // Membaca nilai potensiometer 2
  potValue2 = analogRead(A1);
  // Membaca nilai sensor PIR
  pirSensorValue = digitalRead(6);

  // Membaca jarak dari sensor ultrasonik 1
  digitalWrite(trigPin1, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin1, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin1, LOW);
  duration1 = pulseIn(echoPin1, HIGH);
  distance1 = duration1 * 0.034 / 2;

  // Membaca jarak dari sensor ultrasonik 2
  digitalWrite(trigPin2, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin2, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin2, LOW);
  duration2 = pulseIn(echoPin2, HIGH);
  distance2 = duration2 * 0.034 / 2;

    // Menampilkan nilai potensiometer ke terminal
  Serial.print("Potensiometer 1: ");
  Serial.println(potValue1);
  Serial.print("Potensiometer 2: ");
  Serial.println(potValue2);

  // Menghitung volume tempat sampah dalam persen
  volumeOrganik = (maxVolume - distance1) / maxVolume * 100.0; // Volume organik dalam persen
  volumeAnorganik = (maxVolume - distance2) / maxVolume * 100.0; // Volume anorganik dalam persen

  if (potValue1 > 1 && potValue2 > 1) {
    servo1.write(120);
    isMoving = true; // Aktifkan perintah bergerak
    moveStartTime = millis(); // Catat waktu mulai pergerakan
  } else if (potValue1 == 0 && potValue2 > 1) {
    servo2.write(100); // Gerakkan servo2 ke sudut 100
    isMoving = true;
    moveStartTime = millis();
  } else if (potValue1 > 1 && potValue2 == 0) {
    servo1.write(120);
    isMoving = true;
    moveStartTime = millis();
  } else if (pirSensorValue == HIGH) {
    servo2.write(120);
    isMoving = true;
    moveStartTime = millis();
  } else {
    // Jika tidak ada perintah bergerak aktif
    servo1.write(150); // Set posisi awal servo 1 ke sudut 150
    servo2.write(30);  // Set posisi awal servo 2 ke sudut 30
  }
  
  delay(1000); // Tunggu 1 detik

  // Tampilkan "smarttrashku.com" di awal dan jeda 10 detik
  lcd.clear();
  lcd.setCursor(4, 0);
  lcd.print("WELCOME");
  lcd.setCursor(0, 1);
  lcd.print("KELOMPOK DUA :D");
  delay(5000); // Jeda selama 10 detik

  lcd.clear();
  lcd.setCursor(4, 0);
  lcd.print("INFORMASI");
  lcd.setCursor(3, 1);
  lcd.print("DATA SAMPAH");
  delay(500); // Jeda selama 10 detik

  lcd.clear();
  lcd.setCursor(2, 0);
  lcd.print("VOL. ORGANIK");
  lcd.setCursor(6, 1);
  lcd.print(volumeOrganik);
  lcd.print("%");
  delay(500); // Jeda selama 10 detik

  lcd.clear();
  lcd.setCursor(1, 0);
  lcd.print("VOL. ANORGANIK");
  lcd.setCursor(5, 1);
  lcd.print(volumeAnorganik);
  lcd.print("%");
  delay(500); // Jeda selama 10 detik

  // Cek apakah tempat sampah organik penuh
  if (volumeOrganik >= 90 && !isOrganikFullNotified) {
    // Tempat sampah organik sudah 100% penuh, tampilkan notifikasi
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("TEMPAT SAMPAH");
    lcd.setCursor(0, 1);
    lcd.print("ORGANIK PENUH!");
    isOrganikFullNotified = true; // Set flag notifikasi penuh untuk organik
    delay(500); // Tahan notifikasi selama 2 detik
  }

  // Cek apakah tempat sampah anorganik penuh
  if (volumeAnorganik >= 90 && !isAnorganikFullNotified) {
    // Tempat sampah anorganik sudah 100% penuh, tampilkan notifikasi
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("TEMPAT SAMPAH");
    lcd.setCursor(0, 1);
    lcd.print("ANORGANIK PENUH!");
    isAnorganikFullNotified = true; // Set flag notifikasi penuh untuk anorganik
    delay(500); // Tahan notifikasi selama 2 detik
  }

  


}