#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;

const float GAMMA = 0.7;
const float RL10 = 50;
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Fuzzy Logic Variables
float beratPakaian;         // Input: Berat Pakaian
float tingkatKekotoran;     // Input: Tingkat Kekotoran
float airCuci;              // Output: Jumlah Air Cuci
float lamaCuci;             // Output: Lama Waktu Cuci

void setup() {
  // Setup servo motors, LCD, and pins
  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);

  lcd.init();
  lcd.backlight();
  servoPembuangan.write(0);
  servoKranAir.write(0);
  servoSabun.write(0);
  servoPewangi.write(0);
  // ... (setup code for other components)

  // Setup fuzzy logic variables
  beratPakaian = 0.0;
  tingkatKekotoran = 0.0;
  airCuci = 0.0;
  lamaCuci = 0.0;
}

// ... (other functions like ultrasonic, etc.)
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;
}
// Fuzzy Logic Functions
float fuzzifikasiBeratPakaian(float nilaiBerat) {
  float ringan = 1.0 / (1.0 + exp(-0.1 * (nilaiBerat - 5)));
  float sedang = exp(-0.05 * pow((nilaiBerat - 5), 2));
  float berat = 1.0 / (1.0 + exp(-0.1 * (5 - nilaiBerat)));

  // Outputkan derajat keanggotaan untuk masing-masing kategori
  return ringan, sedang, berat;
}

float fuzzifikasiKekotoran(float nilaiKekotoran) {
  float biasa = 1.0 / (1.0 + exp(-0.1 * (nilaiKekotoran - 50)));
  float kotor = exp(-0.05 * pow((nilaiKekotoran - 50), 2));
  float sangatKotor = 1.0 / (1.0 + exp(-0.1 * (50 - nilaiKekotoran)));

  // Outputkan derajat keanggotaan untuk masing-masing kategori
  return biasa, kotor, sangatKotor;
}

void inferensi() {
  // Aturan fuzzy 1
  // Jika berat pakaian ringan dan tingkat kekotoran biasa, maka air cuci sedikit dan lama cuci cepat
  float rule1 = min(fuzzyBeratPakaian.ringan, fuzzyKekotoran.biasa);
  airCuci = rule1 * 20;   // Angka 20 adalah faktor skala, disesuaikan dengan kebutuhan
  lamaCuci = rule1 * 30;  // Angka 30 adalah faktor skala, disesuaikan dengan kebutuhan

  // Aturan fuzzy 2
  // Jika berat pakaian sedang dan tingkat kekotoran kotor, maka air cuci sedang dan lama cuci sedang
  float rule2 = min(fuzzyBeratPakaian.sedang, fuzzyKekotoran.kotor);
  airCuci += rule2 * 30;   // Angka 30 adalah faktor skala, disesuaikan dengan kebutuhan
  lamaCuci += rule2 * 60;  // Angka 60 adalah faktor skala, disesuaikan dengan kebutuhan

  // Aturan fuzzy 3
  // Jika berat pakaian berat dan tingkat kekotoran sangat kotor, maka air cuci banyak dan lama cuci lama
  float rule3 = min(fuzzyBeratPakaian.berat, fuzzyKekotoran.sangatKotor);
  airCuci += rule3 * 40;   // Angka 40 adalah faktor skala, disesuaikan dengan kebutuhan
  lamaCuci += rule3 * 90;  // Angka 90 adalah faktor skala, disesuaikan dengan kebutuhan

  // Aturan fuzzy 4
  // Jika berat pakaian sedang dan tingkat kekotoran biasa, maka air cuci sedang dan lama cuci cepat
  float rule4 = min(fuzzyBeratPakaian.sedang, fuzzyKekotoran.biasa);
  airCuci += rule4 * 25;   // Angka 25 adalah faktor skala, disesuaikan dengan kebutuhan
  lamaCuci += rule4 * 40;  // Angka 40 adalah faktor skala, disesuaikan dengan kebutuhan

  // Aturan fuzzy 5
  // Jika berat pakaian ringan dan tingkat kekotoran sangat kotor, maka air cuci banyak dan lama cuci lama
  float rule5 = min(fuzzyBeratPakaian.ringan, fuzzyKekotoran.sangatKotor);
  airCuci += rule5 * 35;   // Angka 35 adalah faktor skala, disesuaikan dengan kebutuhan
  lamaCuci += rule5 * 80;  // Angka 80 adalah faktor skala, disesuaikan dengan kebutuhan

  // Aturan fuzzy 6 (Tambahan)
  // Jika berat pakaian berat dan tingkat kekotoran biasa, maka air cuci banyak dan lama cuci sedang
  float rule6 = min(fuzzyBeratPakaian.berat, fuzzyKekotoran.biasa);
  airCuci += rule6 * 35;   // Angka 35 adalah faktor skala, disesuaikan dengan kebutuhan
  lamaCuci += rule6 * 60;  // Angka 60 adalah faktor skala, disesuaikan dengan kebutuhan

  
}



float defuzzifikasiAirCuci() {
  // Metode defuzzifikasi (Contoh: Centroid)
  float numerator = (airCuci * fuzzyBeratPakaian.ringan * fuzzyKekotoran.biasa) +
                    (airCuci * fuzzyBeratPakaian.sedang * fuzzyKekotoran.kotor) +
                    (airCuci * fuzzyBeratPakaian.berat * fuzzyKekotoran.sangatKotor);

  float denominator = fuzzyBeratPakaian.ringan * fuzzyKekotoran.biasa +
                      fuzzyBeratPakaian.sedang * fuzzyKekotoran.kotor +
                      fuzzyBeratPakaian.berat * fuzzyKekotoran.sangatKotor;

  return numerator / denominator;
}

float defuzzifikasiLamaCuci() {
  // Metode defuzzifikasi (Contoh: Centroid)
  float numerator = (lamaCuci * fuzzyBeratPakaian.ringan * fuzzyKekotoran.biasa) +
                    (lamaCuci * fuzzyBeratPakaian.sedang * fuzzyKekotoran.kotor) +
                    (lamaCuci * fuzzyBeratPakaian.berat * fuzzyKekotoran.sangatKotor);

  float denominator = fuzzyBeratPakaian.ringan * fuzzyKekotoran.biasa +
                      fuzzyBeratPakaian.sedang * fuzzyKekotoran.kotor +
                      fuzzyBeratPakaian.berat * fuzzyKekotoran.sangatKotor;

  return numerator / denominator;
}



void kontrolMesinCuci(float beratPakaian, float tingkatKekotoran) {
  // Fuzzifikasi
  float fuzzyBeratPakaian = fuzzifikasiBeratPakaian(beratPakaian);
  float fuzzyKekotoran = fuzzifikasiKekotoran(tingkatKekotoran);

  // Inferensi
  inferensi();

  // Defuzzifikasi
  airCuci = defuzzifikasiAirCuci();
  lamaCuci = defuzzifikasiLamaCuci();

  // ... (add washing machine control based on airCuci and lamaCuci)
}

void loop() {
  // ... (other loop code)

  // Simulasi input berat pakaian dan tingkat kekotoran (harus diganti dengan nilai sebenarnya)
  beratPakaian = 3.0;            // ringan, sedang, berat
  tingkatKekotoran = 70.0;       // biasa, kotor, sangat

  // Kontrol mesin cuci berdasarkan berat pakaian dan tingkat kekotoran
  kontrolMesinCuci(beratPakaian, tingkatKekotoran);

  // ... (other loop code)
}