#include <Wire.h>           // Library untuk komunikasi I2C
#include <LiquidCrystal_I2C.h>  // Library untuk LCD I2C
#include <Servo.h>           // Library untuk Servo

// Definisi pin untuk sensor ultrasonik sampah
const int trigPinSampah = 9;
const int echoPinSampah = 10;

// Definisi pin untuk sensor ultrasonik manusia
const int trigPinManusia = 7;
const int echoPinManusia = 8;

// Definisi Servo motor
Servo servoMotor;
const int servoPin = 6;

// Definisi pin untuk buzzer
const int buzzerPin = 5;

// Variabel untuk menyimpan waktu dan jarak
long duration;
int distanceSampah;
int distanceManusia;

// Inisialisasi LCD I2C 16x2 (alamat I2C default 0x27)
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Fungsi untuk menghitung jarak menggunakan sensor ultrasonik
int getDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);

  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH);

  // Konversi waktu ke jarak (dalam cm)
  int distance = duration * 0.034 / 2;
  return distance;
}

// Fungsi logika fuzzy untuk menghitung derajat keanggotaan
int fuzzyLogic(int distance) {
  // Variabel derajat keanggotaan
  double sangatRendah = 0, rendah = 0, sedang = 0, tinggi = 0, kosong = 0;

  // Menentukan derajat keanggotaan fuzzy berdasarkan jarak
  if (distance <= 10) {
    sangatRendah = 1.0;
  } else if (distance <= 20) {
    sangatRendah = (20 - distance) / 10.0;
    rendah = (distance - 10) / 10.0;
  } else if (distance <= 30) {
    rendah = (30 - distance) / 10.0;
    sedang = (distance - 20) / 10.0;
  } else if (distance <= 40) {
    sedang = (40 - distance) / 10.0;
    tinggi = (distance - 30) / 10.0;
  } else {
    kosong = 1.0;
  }

  // Mengambil nilai derajat keanggotaan tertinggi sebagai hasil defuzzifikasi
  double maxVal = tinggi;
  if (sedang > maxVal) maxVal = sedang;
  if (rendah > maxVal) maxVal = rendah;
  if (sangatRendah > maxVal) maxVal = sangatRendah;
  if (kosong > maxVal) maxVal = kosong;

  return static_cast<int>(maxVal * 100); // Hasil defuzzifikasi diubah menjadi nilai integer 0-100
}

// Fungsi untuk memberikan output buzzer berdasarkan jarak sampah
void buzzerControl(int distanceSampah) {
  if (distanceSampah == 30) {
    // Buzzer menghasilkan nada 
    tone(buzzerPin, 1000); // Nada frekuensi 
    delay(200);
    noTone(buzzerPin);
  } else if (distanceSampah == 15) {
    // Buzzer berkedip 
    for (int i = 0; i < 5; i++) { // Buzzer berkedip 5 kali
      tone(buzzerPin, 1000); // Nada frekuensi 
      delay(500);            // Buzzer menyala selama 500ms
      noTone(buzzerPin);      // Matikan buzzer
      delay(500);             // Tunggu 500ms sebelum berkedip lagi
    }
  } else if (distanceSampah == 5) {
    // Buzzer berkedip
    for (int i = 0; i < 5; i++) { // Buzzer berkedip 5 kali
      tone(buzzerPin, 1000); // Nada frekuensi 
      delay(200);            // Buzzer menyala selama 200ms
      noTone(buzzerPin);      // Matikan buzzer
      delay(200);             // Tunggu 200ms sebelum berkedip lagi
    }
  }
}

// Fungsi logika fuzzy berdasarkan aturan yang telah diberikan
void fuzzyControl(int distanceSampah, int distanceManusia) {
  // Rule 1: Sampah 30 cm, Manusia 10 cm -> Servo bergerak 120 derajat
  if (distanceSampah == 30 && distanceManusia == 10) {
    Serial.println("Rule 1: Sampah Belum Penuh dan Servo bergerak 120 derajat");
    servoMotor.write(120); // Servo bergerak ke 120 derajat
    buzzerControl(distanceSampah); // Panggil output buzzer untuk jarak sampah
  }
  // Rule 2: Sampah 15 cm, Manusia 10 cm -> Servo bergerak 120 derajat
  else if (distanceSampah == 15 && distanceManusia == 10) {
    Serial.println("Rule 2: Sampah Belum Penuh dan Servo bergerak 120 derajat");
    servoMotor.write(120); // Servo bergerak ke 120 derajat
    buzzerControl(distanceSampah); // Panggil output buzzer untuk jarak sampah
  }
  // Rule 3: Sampah 5 cm, Manusia 10 cm -> Servo tidak bergerak
  else if (distanceSampah == 5 && distanceManusia == 10) {
    Serial.println("Rule 3: Sampah Penuh dan Servo tidak bergerak");
    servoMotor.write(0); // Servo tetap pada posisi tutup
    buzzerControl(distanceSampah); // Panggil output buzzer untuk jarak sampah
  }
  // Rule 4: Sampah 5 cm, Manusia 20 cm -> Servo tidak bergerak
  else if (distanceSampah == 5 && distanceManusia == 20) {
    Serial.println("Rule 4: Sampah Penuh dan Servo tidak bergerak");
    servoMotor.write(0); // Servo tetap pada posisi tutup
    buzzerControl(distanceSampah); // Panggil output buzzer untuk jarak sampah
  }
  // Jika tidak sesuai dengan aturan, servo kembali ke posisi awal (0 derajat)
  else {
    Serial.println("Tidak ada rule yang cocok, servo tidak bergerak");
    servoMotor.write(0);
  }
}

// Setup awal Arduino
void setup() {
  // Inisialisasi komunikasi serial
  Serial.begin(9600);

  // Inisialisasi pin sensor ultrasonik
  pinMode(trigPinSampah, OUTPUT);
  pinMode(echoPinSampah, INPUT);
  pinMode(trigPinManusia, OUTPUT);
  pinMode(echoPinManusia, INPUT);

  // Inisialisasi Servo
  servoMotor.attach(servoPin);
  servoMotor.write(0); // Tutup servo pada awal

  // Inisialisasi Buzzer
  pinMode(buzzerPin, OUTPUT);

  // Inisialisasi LCD
  lcd.init();
  lcd.backlight();  // Nyalakan backlight LCD
  lcd.setCursor(0, 0);
  lcd.print("Smart Trash Bin");

  // Cetak pesan awal
  Serial.println("Smart Trash Bin with Human Detection - Fuzzy Logic Rules");
}

// Loop utama
void loop() {
  // Mendapatkan jarak sampah dari sensor ultrasonik 1
  distanceSampah = getDistance(trigPinSampah, echoPinSampah);

  // Mendapatkan jarak manusia dari sensor ultrasonik 2
  distanceManusia = getDistance(trigPinManusia, echoPinManusia);

  // Cetak jarak ke serial monitor
  Serial.print("Jarak Sampah: ");
  Serial.print(distanceSampah);
  Serial.println(" cm");

  Serial.print("Jarak Manusia: ");
  Serial.print(distanceManusia);
  Serial.println(" cm");

  // Menampilkan jarak ke LCD
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Sampah: ");
  lcd.print(distanceSampah);
  lcd.print(" cm");

  lcd.setCursor(0, 1);
  lcd.print("Manusia: ");
  lcd.print(distanceManusia);
  lcd.print(" cm");

  // Panggil fungsi fuzzyControl untuk memeriksa aturan dan menggerakkan servo
  fuzzyControl(distanceSampah, distanceManusia);

  // Tunggu 1 detik sebelum melakukan pengukuran lagi
  delay(1000);
}