#include <Servo.h>
#include <Fuzzy.h>
#include <FuzzyComposition.h>
#include <FuzzyInput.h>
#include <FuzzyIO.h>
#include <FuzzyOutput.h>
#include <FuzzyRule.h>
#include <FuzzyRuleAntecedent.h>
#include <FuzzyRuleConsequent.h>
#include <FuzzySet.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

// Pin yang digunakan
#define LDR A2 // LDR sensor
#define TRIGGER 7 // Ultrasonic trigger
#define ECHO 6 // Ultrasonic echo
#define LED 3 // LED output
#define SERVO_PIN 9 // Servo pin

// Inisialisasi LCD I2C
LiquidCrystal_I2C lcd(0x27, 16, 2);  // Alamat I2C untuk LCD 16x2

// Objek servo
Servo myservo;

// Objek fuzzy
Fuzzy *fuzzy = new Fuzzy();

void setup() {
  Serial.begin(9600);
  pinMode(LDR, INPUT);
  pinMode(TRIGGER, OUTPUT);
  pinMode(ECHO, INPUT);
  pinMode(LED, OUTPUT);

  // Inisialisasi servo
  myservo.attach(SERVO_PIN);
  myservo.write(90); // Servo awalnya tertutup

  // Inisialisasi LCD
  lcd.init();
  lcd.backlight();

  // Fuzzy sets untuk LDR (0-1023)
  FuzzySet *lowldr = new FuzzySet(0, 0, 0, 300);
  FuzzySet *midldr = new FuzzySet(200, 500, 800, 900);
  FuzzySet *highldr = new FuzzySet(800, 900, 1023, 1023);

  // Fuzzy sets untuk Ultrasonic (jarak dalam cm)
  FuzzySet *near = new FuzzySet(0, 0, 10, 30);
  FuzzySet *medium = new FuzzySet(20, 40, 50, 90);
  FuzzySet *far = new FuzzySet(80, 90, 100, 120);

  // LED brightness (0-255)
  FuzzySet *off = new FuzzySet(0, 0, 0, 0);
  FuzzySet *midb = new FuzzySet(10, 20, 30, 40);
  FuzzySet *highb = new FuzzySet(200, 200, 255, 255);

  // Input untuk LDR
  FuzzyInput *ldr = new FuzzyInput(1);
  ldr->addFuzzySet(lowldr);
  ldr->addFuzzySet(midldr);
  ldr->addFuzzySet(highldr);
  fuzzy->addFuzzyInput(ldr);

  // Input untuk Ultrasonic
  FuzzyInput *distance = new FuzzyInput(2);
  distance->addFuzzySet(near);
  distance->addFuzzySet(medium);
  distance->addFuzzySet(far);
  fuzzy->addFuzzyInput(distance);

  // Output untuk brightness LED
  FuzzyOutput *brightness = new FuzzyOutput(1);
  brightness->addFuzzySet(off);
  brightness->addFuzzySet(midb);
  brightness->addFuzzySet(highb);
  fuzzy->addFuzzyOutput(brightness);

  // Rule fuzzy untuk mengatur kecerahan LED

  // LDR rendah & jarak dekat -> LED terang
  FuzzyRuleAntecedent *ifLdrLowAndDistNear = new FuzzyRuleAntecedent();
  ifLdrLowAndDistNear->joinWithAND(lowldr, near);
  FuzzyRuleConsequent *thenBrightHigh = new FuzzyRuleConsequent();
  thenBrightHigh->addOutput(highb);
  FuzzyRule *fuzzyRule1 = new FuzzyRule(1, ifLdrLowAndDistNear, thenBrightHigh);
  fuzzy->addFuzzyRule(fuzzyRule1);

  // LDR sedang & jarak sedang -> LED sedang
  FuzzyRuleAntecedent *ifLdrMidAndDistMedium = new FuzzyRuleAntecedent();
  ifLdrMidAndDistMedium->joinWithAND(midldr, medium);
  FuzzyRuleConsequent *thenBrightMid = new FuzzyRuleConsequent();
  thenBrightMid->addOutput(midb);
  FuzzyRule *fuzzyRule2 = new FuzzyRule(2, ifLdrMidAndDistMedium, thenBrightMid);
  fuzzy->addFuzzyRule(fuzzyRule2);

  // LDR tinggi & jarak jauh -> LED mati
  FuzzyRuleAntecedent *ifLdrHighAndDistFar = new FuzzyRuleAntecedent();
  ifLdrHighAndDistFar->joinWithAND(highldr, far);
  FuzzyRuleConsequent *thenBrightOff = new FuzzyRuleConsequent();
  thenBrightOff->addOutput(off);
  FuzzyRule *fuzzyRule3 = new FuzzyRule(3, ifLdrHighAndDistFar, thenBrightOff);
  fuzzy->addFuzzyRule(fuzzyRule3);
}

// Mengukur jarak menggunakan sensor ultrasonic
int getDistance() {
  digitalWrite(TRIGGER, LOW);
  delayMicroseconds(5);
  digitalWrite(TRIGGER, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIGGER, LOW);
  long pulse = pulseIn(ECHO, HIGH) / 2;
  return pulse * 10 / 292; // Menghitung jarak dalam cm
}

// Mengukur nilai dari LDR
int getLdrValue() {
  return analogRead(LDR); // Membaca nilai analog dari LDR
}

void loop() {
  // Dapatkan nilai dari sensor ultrasonic
  int dist = getDistance();
  // Dapatkan nilai dari LDR
  int light = getLdrValue();

  // Fuzzy logic untuk mengontrol brightness LED
  fuzzy->setInput(1, light);
  fuzzy->setInput(2, dist);
  fuzzy->fuzzify();
  int outputLed = fuzzy->defuzzify(1);

  // Mengatur brightness LED berdasarkan output fuzzy
  analogWrite(LED, outputLed);

  // Mengatur posisi Servo berdasarkan jarak menggunakan metode biasa
  if (dist < 30) {
    myservo.write(0); // Buka servo jika jarak dekat
  } else {
    myservo.write(90); // Tutup servo jika jarak jauh
  }

  // Variabel untuk menyimpan status LED dan Servo
  String ledStatus;
  String servoStatus;

  // Menentukan kondisi LED berdasarkan output fuzzy
  if (outputLed == 0) {
    ledStatus = "LED: Mati";
  } else if (outputLed > 10 && outputLed <= 200) {
    ledStatus = "LED: Redup";
  } else {
    ledStatus = "LED: Terang";
  }

  // Menentukan kondisi Servo berdasarkan jarak
  if (dist < 30) {
    servoStatus = "Servo: Terbuka";
  } else {
    servoStatus = "Servo: Tertutup";
  }

  // Menampilkan status Servo dan LED pada LCD
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(servoStatus);
  lcd.setCursor(0, 1);
  lcd.print(ledStatus);

  // Menampilkan hasil di Serial Monitor
  Serial.print("Jarak: ");
  Serial.print(dist);
  Serial.print(" cm. LDR: ");
  Serial.print(light);
  Serial.print(" => LED Brightness: ");
  Serial.print(outputLed);
  Serial.print(", Servo Pos: ");
  Serial.println(servoStatus);

  delay(500); // Delay untuk memperbarui setiap setengah detik
}