#include <ESP32Servo.h>
#include <LiquidCrystal_I2C.h>

const int buangPin = 18;
const int isiPin = 15;
const int sabunPin = 2;
const int pewangiPin = 4;
const int putarKiriPin = 13;
const int putarKananPin = 12;
const int buzzerPin = 14;
const int trig = 27;
const int echo = 26;

Servo servoPembuangan;
Servo servoKranAir;
Servo servoSabun;
Servo servoPewangi;
LiquidCrystal_I2C lcd(0x27, 16, 2);

int prosesAktif = 0;  // Variabel untuk menyimpan jenis proses yang sedang berjalan

// Fungsi keanggotaan untuk variabel input "TinggiAir"
float tinggiAirRendah(float x) {
  if (x <= 50) return 1;
  else if (x > 50 && x <= 100) return (100 - x) / 50;
  else return 0;
}

float tinggiAirSedang(float x) {
  if (x > 50 && x <= 100) return (x - 50) / 50;
  else if (x > 100 && x <= 150) return (150 - x) / 50;
  else return 0;
}

float tinggiAirTinggi(float x) {
  if (x > 100 && x <= 150) return (x - 100) / 50;
  else if (x > 150) return 1;
  else return 0;
}

// Fungsi keanggotaan untuk variabel output "AksiServo"
float aksiServoRendah(float x) {
  if (x <= 90) return 1;
  else if (x > 90 && x <= 135) return (135 - x) / 45;
  else return 0;
}

float aksiServoSedang(float x) {
  if (x > 90 && x <= 135) return (x - 90) / 45;
  else if (x > 135 && x <= 180) return (180 - x) / 45;
  else return 0;
}

float aksiServoTinggi(float x) {
  if (x > 135 && x <= 400) return (x - 135) / 45;
  else return 0;
}

// Fungsi untuk inferensi fuzzy dan defuzzifikasi
float inferensiFuzzy(float tinggiAir) {
  float tinggiAirRendahValue = tinggiAirRendah(tinggiAir);
  float tinggiAirSedangValue = tinggiAirSedang(tinggiAir);
  float tinggiAirTinggiValue = tinggiAirTinggi(tinggiAir);

  float aksiServoRendahValue = fmin(tinggiAirRendahValue, aksiServoRendah(tinggiAir));
  float aksiServoSedangValue = fmin(tinggiAirSedangValue, aksiServoSedang(tinggiAir));
  float aksiServoTinggiValue = fmin(tinggiAirTinggiValue, aksiServoTinggi(tinggiAir));

  float hasil = (aksiServoRendahValue * 90 + aksiServoSedangValue * 135 + aksiServoTinggiValue * 180) /
                (aksiServoRendahValue + aksiServoSedangValue + aksiServoTinggiValue);

  return hasil;
}

void setup() {
  servoPembuangan.attach(buangPin, 500, 2400);
  servoKranAir.attach(isiPin, 500, 2400);
  servoSabun.attach(sabunPin, 500, 2400);
  servoPewangi.attach(pewangiPin, 500, 2400);

  pinMode(putarKiriPin, OUTPUT);
  pinMode(putarKananPin, OUTPUT);
  pinMode(buzzerPin, OUTPUT);
  pinMode(echo, INPUT);
  pinMode(trig, OUTPUT);

  Serial.begin(9600);
  lcd.init();
  lcd.backlight();
  servoPembuangan.write(0);
  servoKranAir.write(0);
  servoSabun.write(0);
  servoPewangi.write(0);
}

float ultrasonik() {
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  
  int durasiPantulan = pulseIn(echo, HIGH);
  return durasiPantulan * 0.034 / 2;
}

void buangAir() {
  float sisaAir = ultrasonik();
  
  // Inferensi fuzzy dan defuzzifikasi
  float aksiServo = inferensiFuzzy(sisaAir);
  
  servoPembuangan.write(aksiServo);
  delay(5000);
  servoPembuangan.write(0);
  
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Buang Air");
  lcd.setCursor(0, 1);
  lcd.print("Tinggi Air:");
  lcd.print(sisaAir);

  Serial.println("FUZZYFIKASI MAMDANI");
  Serial.print("Keanggotaan Rendah: ");
  Serial.println(tinggiAirRendah(sisaAir));
  Serial.print("Keanggotaan Sedang: ");
  Serial.println(tinggiAirSedang(sisaAir));
  Serial.print("Keanggotaan Tinggi: ");
  Serial.println(tinggiAirTinggi(sisaAir));
  Serial.print("Posisi Servo: ");
  Serial.println(aksiServo);
}

void isiAir() {
  servoKranAir.write(180);
  delay(2000);
  servoKranAir.write(0);
}

void isiSabun() {
  servoSabun.write(180);
  delay(2000);
  servoSabun.write(0);
}

void isiPewangi() {
  servoPewangi.write(180);
  delay(2000);
  servoPewangi.write(0);
}

void mesin() {
  for (int i = 0; i < 10; i++) {
    digitalWrite(putarKiriPin, LOW);
    digitalWrite(putarKananPin, HIGH);
    delay(1000);
    digitalWrite(putarKananPin, LOW);
    digitalWrite(putarKiriPin, HIGH);
    delay(1000);
    digitalWrite(putarKiriPin, LOW);
  }
}


void prosesCuci() {
  isiAir();
  isiSabun();
  mesin();
  buangAir();
}

void bilas(){
  isiAir();
  mesin();
  buangAir();
}


void loop() {

  lcd.clear();
  lcd.setCursor(0, 1);
  lcd.println("Hilangkan Debu 1");
  bilas();  // Bilas debu dan kotoran pertama
  delay(2000);


  lcd.clear();
  lcd.setCursor(0, 1);
  lcd.print("Hilangkan Debu 2");
  bilas(); // Bilas debu dan kotoran kedua
  delay(2000);

  lcd.clear();
  lcd.setCursor(0, 1);
  lcd.print("Mencuci");
  prosesCuci(); // Bilas debu dan kotoran pertama
  delay(2000);

  lcd.clear();
  lcd.setCursor(0, 1);
  lcd.print("Selesai mencuci ");
  delay(2000); // Delay 2 detik setelah mencuci selesai

  lcd.clear();
  lcd.setCursor(0, 1);
  lcd.print("Buang Air bekas");
  buangAir();
  delay(2000);

  lcd.clear();
  lcd.setCursor(0, 1);
  lcd.print("Bilasan pertama ");
  bilas(); // Proses bilas pertama
  delay(2000);

  lcd.clear();
  lcd.setCursor(0, 1);
  lcd.print("Bilasan kedua   ");
  bilas(); // Proses bilas kedua
  delay(2000);

  lcd.clear();
  lcd.setCursor(0, 1);
  lcd.print("Bilasan Pewangi ");
  isiPewangi(); // Isi pelembut / pewangi pakaian
  mesin(); // Proses bilas ketiga
  delay(2000);

  lcd.clear();
  lcd.setCursor(0, 1);
  lcd.print("Buang Air bekas");
  buangAir();
  delay(2000);

  lcd.clear();
  lcd.setCursor(0, 1);
  lcd.print("Selesai mencuci ");
  tone(buzzerPin, 100, 1000);
  delay(2000); // Delay 2 detik setelah mencuci selesai
  servoPembuangan.write(0); // Mengembalikan posisi servo ke 0 setelah selesai mencuci
}