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

// Inisialisasi LCD
LiquidCrystal_I2C lcd(0x27, 20, 4);

// Inisialisasi pin
const int infraredPin = 27;
const int induksiPin = 26;
const int kapasitifPin = 25;
const int scanPin = 16;
const int resetPin = 17;
const int buzzerPin = 23;
const int led1Pin = 32;
const int led2Pin = 33;
const int OrganicTrigPin = 4;
const int OrganicEchoPin = 0;
const int AnorganicTrigPin = 19;
const int AnorganicEchoPin = 18;
const int HumanTrigPin = 2;
const int HumanEchoPin = 15;

Servo servo1;
Servo servo2;

// Fungsi membaca jarak dari sensor ultrasonik
float readUltrasonic(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH);
  float distance = duration * 0.0343 / 2;
  return distance;
}

// Fungsi menampilkan teks pada LCD
void lcdDisplay(String line1, String line2, String line3, String line4) {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(line1);
  lcd.setCursor(0, 1);
  lcd.print(line2);
  lcd.setCursor(0, 2);
  lcd.print(line3);
  lcd.setCursor(0, 3);
  lcd.print(line4);
}

void setup() {
  // Inisialisasi LCD
  lcd.init();
  lcd.backlight();

  // Inisialisasi pin
  pinMode(scanPin, INPUT_PULLDOWN);
  pinMode(resetPin, INPUT_PULLUP);
  pinMode(buzzerPin, OUTPUT);
  pinMode(led1Pin, OUTPUT);
  pinMode(led2Pin, OUTPUT);
  pinMode(OrganicTrigPin, OUTPUT);
  pinMode(OrganicEchoPin, INPUT);
  pinMode(AnorganicTrigPin, OUTPUT);
  pinMode(AnorganicEchoPin, INPUT);
  pinMode(HumanTrigPin, OUTPUT);
  pinMode(HumanEchoPin, INPUT);

  servo1.attach(13);
  servo2.attach(14);

  // Tampilan awal LCD
  lcdDisplay("TEMPAT SAMPAH", "OTOMATIS", "KKN 49 UMG", "KOTAKUSUMA");
}

void loop() {
  // Reset sistem
  if (digitalRead(resetPin) == LOW) {
    lcdDisplay("TEMPAT SAMPAH", "OTOMATIS", "KKN 49 UMG", "KOTAKUSUMA");
    digitalWrite(led1Pin, LOW);
    digitalWrite(led2Pin, LOW);
    delay(500);
    return;
  }

  // Deteksi orang di depan
  if (readUltrasonic(HumanTrigPin, HumanEchoPin) < 50) {
    lcdDisplay("TUNJUKKAN SAMPAH", "", "", "");
    delay(1000);

    // Mulai pembacaan jika tombol scan ditekan
    if (digitalRead(scanPin) == HIGH) {
      int infraredVal = analogRead(infraredPin);
      int kapasitifVal = analogRead(kapasitifPin);
      int induksiVal = analogRead(induksiPin);

      if (infraredVal < 200 && kapasitifVal > 200 && induksiVal == 0) {
        lcdDisplay("SAMPAH ORGANIK", "===============", "Masukkan Sampah", "Ke Tong Sampah");
        servo1.write(90);
        digitalWrite(buzzerPin, HIGH);
        delay(1000);
        digitalWrite(buzzerPin, LOW);
        servo1.write(0);
      } else if (infraredVal < 200 && kapasitifVal < 200 && induksiVal == 0) {
        lcdDisplay("SAMPAH ANORGANIK", "===============", "Masukkan Sampah", "Ke Tong Sampah");
        servo2.write(90);
        digitalWrite(buzzerPin, HIGH);
        delay(1000);
        digitalWrite(buzzerPin, LOW);
        servo2.write(0);
      } else if (infraredVal > 200 && kapasitifVal < 200 && induksiVal == 0) {
        lcdDisplay("SAMPAH ANORGANIK", "===============", "Masukkan Sampah", "Ke Tong Sampah");
        servo2.write(90);
        digitalWrite(buzzerPin, HIGH);
        delay(1000);
        digitalWrite(buzzerPin, LOW);
        servo2.write(0);
      } else if (infraredVal > 200 && kapasitifVal == 0 && induksiVal > 0) {
        lcdDisplay("SAMPAH ANORGANIK", "===============", "Masukkan Sampah", "Ke Tong Sampah");
        servo2.write(90);
        digitalWrite(buzzerPin, HIGH);
        delay(1000);
        digitalWrite(buzzerPin, LOW);
        servo2.write(0);
      }
    }
  }

  // Deteksi kapasitas tempat sampah
  if (readUltrasonic(OrganicTrigPin, OrganicEchoPin) < 10) {
    digitalWrite(led1Pin, HIGH);
  } else {
    digitalWrite(led1Pin, LOW);
  }

  if (readUltrasonic(AnorganicTrigPin, AnorganicEchoPin) < 10) {
    digitalWrite(led2Pin, HIGH);
  } else {
    digitalWrite(led2Pin, LOW);
  }

  delay(100);
}