#include <OneWire.h>
#include <DallasTemperature.h>
#include <LiquidCrystal_I2C.h>
#include <ESP32Servo.h>
#include <DHT.h>
#include <Fuzzy.h>
#include <Wire.h>

#define oneWireBus 4
// Define pins
#define DHTPIN 33 // Pin where the DHT22 is connected
#define DHTTYPE DHT22
// Define relay pins
#define RELAY1_PIN 27
#define RELAY2_PIN 26

// Setup a oneWire instance to communicate with any OneWire devices
OneWire oneWire(oneWireBus);
// Pass our oneWire reference to Dallas Temperature sensor
DallasTemperature sensor(&oneWire);
// Initialize DHT sensor
DHT dht(DHTPIN, DHTTYPE);
// Setup LCD 16x2
LiquidCrystal_I2C LCD(0x27, 16, 2);
// Initialize Fuzzy Logic
Fuzzy *fuzzy = new Fuzzy();
// Timer
unsigned long relay1LastActivated = 0;
unsigned long relay2LastActivated = 0;
// Setup Servo
Servo myServo;
int servoPin = 25; // Connect the servo signal pin to this pin

void setup() {
  Serial.begin(115200);
  sensor.begin();
  dht.begin();

  LCD.init();
  LCD.backlight();
  LCD.begin(16, 2);
  
  // Display initialization message
  LCD.setCursor(0, 0);
  LCD.print("Sistem IoT Kolam LAT");
  
  delay(3000); // 3-second delay to show the message
  
  // Clear the LCD after showing the message
  LCD.clear();
  delay(100); // Short delay to ensure clear

  // Attach the servo on pin 25
  myServo.attach(servoPin);

  // Initialize relay pins
  pinMode(RELAY1_PIN, OUTPUT);
  pinMode(RELAY2_PIN, OUTPUT);

  // Start with relays off
  digitalWrite(RELAY1_PIN, LOW);
  digitalWrite(RELAY2_PIN, LOW);

  // Setup fuzzy logic input for temperature
  FuzzyInput *suhu = new FuzzyInput(1);
  FuzzySet *sangatPanas = new FuzzySet(-6, -5, 5, 6);
  FuzzySet *panas = new FuzzySet(5, 6, 6, 7);
  FuzzySet *normal = new FuzzySet(6, 7, 8, 9);
  FuzzySet *dingin = new FuzzySet(8, 9, 9, 10);
  FuzzySet *sangatDingin = new FuzzySet(9, 10, 14, 14);
  suhu->addFuzzySet(sangatPanas);
  suhu->addFuzzySet(panas);
  suhu->addFuzzySet(normal);
  suhu->addFuzzySet(dingin);
  suhu->addFuzzySet(sangatDingin);
  fuzzy->addFuzzyInput(suhu);

  // Setup fuzzy logic output for Relay 1
  FuzzyOutput *relay1 = new FuzzyOutput(1);
  FuzzySet *relay1Mati = new FuzzySet(0, 0, 0, 0);
  FuzzySet *relay1Sedang = new FuzzySet(5, 10, 10, 15);
  FuzzySet *relay1Lama = new FuzzySet(10, 15, 25, 25);
  relay1->addFuzzySet(relay1Mati);
  relay1->addFuzzySet(relay1Sedang);
  relay1->addFuzzySet(relay1Lama);
  fuzzy->addFuzzyOutput(relay1);

  // Setup fuzzy logic output for Relay 2
  FuzzyOutput *relay2 = new FuzzyOutput(2);
  FuzzySet *relay2Mati = new FuzzySet(0, 0, 0, 0);
  FuzzySet *relay2Sedang = new FuzzySet(5, 10, 10, 15);
  FuzzySet *relay2Lama = new FuzzySet(10, 15, 25, 25);
  relay2->addFuzzySet(relay2Mati);
  relay2->addFuzzySet(relay2Sedang);
  relay2->addFuzzySet(relay2Lama);
  fuzzy->addFuzzyOutput(relay2);

  // Fuzzy rules
  // Rule 1: if suhu is sangat panas then relay 1 lama and relay 2 mati
  FuzzyRuleAntecedent *ifSuhuSangatPanas = new FuzzyRuleAntecedent();
  ifSuhuSangatPanas->joinSingle(sangatPanas);
  FuzzyRuleConsequent *thenRelay1LamaRelay2Mati = new FuzzyRuleConsequent();
  thenRelay1LamaRelay2Mati->addOutput(relay1Lama);
  thenRelay1LamaRelay2Mati->addOutput(relay2Mati);
  FuzzyRule *fuzzyRule1 = new FuzzyRule(1, ifSuhuSangatPanas, thenRelay1LamaRelay2Mati);
  fuzzy->addFuzzyRule(fuzzyRule1);

  // Rule 2: if suhu is panas then relay 1 sedang and relay 2 mati
  FuzzyRuleAntecedent *ifSuhuPanas = new FuzzyRuleAntecedent();
  ifSuhuPanas->joinSingle(panas);
  FuzzyRuleConsequent *thenRelay1SedangRelay2Mati = new FuzzyRuleConsequent();
  thenRelay1SedangRelay2Mati->addOutput(relay1Sedang);
  thenRelay1SedangRelay2Mati->addOutput(relay2Mati);
  FuzzyRule *fuzzyRule2 = new FuzzyRule(2, ifSuhuPanas, thenRelay1SedangRelay2Mati);
  fuzzy->addFuzzyRule(fuzzyRule2);

  // Rule 3: if suhu is normal then relay 1 mati and relay 2 mati
  FuzzyRuleAntecedent *ifSuhuNormal = new FuzzyRuleAntecedent();
  ifSuhuNormal->joinSingle(normal);
  FuzzyRuleConsequent *thenRelay1MatiRelay2Mati = new FuzzyRuleConsequent();
  thenRelay1MatiRelay2Mati->addOutput(relay1Mati);
  thenRelay1MatiRelay2Mati->addOutput(relay2Mati);
  FuzzyRule *fuzzyRule3 = new FuzzyRule(3, ifSuhuNormal, thenRelay1MatiRelay2Mati);
  fuzzy->addFuzzyRule(fuzzyRule3);

  // Rule 4: if suhu is dingin then relay 1 mati and relay 2 sedang
  FuzzyRuleAntecedent *ifSuhuDingin = new FuzzyRuleAntecedent();
  ifSuhuDingin->joinSingle(dingin);
  FuzzyRuleConsequent *thenRelay1MatiRelay2Sedang = new FuzzyRuleConsequent();
  thenRelay1MatiRelay2Sedang->addOutput(relay1Mati);
  thenRelay1MatiRelay2Sedang->addOutput(relay2Sedang);
  FuzzyRule *fuzzyRule4 = new FuzzyRule(4, ifSuhuDingin, thenRelay1MatiRelay2Sedang);
  fuzzy->addFuzzyRule(fuzzyRule4);

  // Rule 5: if suhu is sangat dingin then relay 1 mati and relay 2 lama
  FuzzyRuleAntecedent *ifSuhuSangatDingin = new FuzzyRuleAntecedent();
  ifSuhuSangatDingin->joinSingle(sangatDingin);
  FuzzyRuleConsequent *thenRelay1MatiRelay2Lama = new FuzzyRuleConsequent();
  thenRelay1MatiRelay2Lama->addOutput(relay1Mati);
  thenRelay1MatiRelay2Lama->addOutput(relay2Lama);
  FuzzyRule *fuzzyRule5 = new FuzzyRule(5, ifSuhuSangatDingin, thenRelay1MatiRelay2Lama);
  fuzzy->addFuzzyRule(fuzzyRule5);
}

void loop() {
  sensor.requestTemperatures();
  float temperatureC = sensor.getTempCByIndex(0);
  float suhuValue = dht.readTemperature();

  // Display temperature from DS18B20
  LCD.setCursor(0, 0);
  LCD.print("Suhu:");
  LCD.print(temperatureC);
  LCD.print("C  ");

  // Display temperature from DHT22
  LCD.setCursor(0, 1);
  LCD.print("DHT22:");
  LCD.print(suhuValue);
  LCD.print("C  ");
  
  // Fuzzification and defuzzification
  fuzzy->setInput(1, suhuValue);
  fuzzy->fuzzify();

  float relay1Value = fuzzy->defuzzify(1);
  float relay2Value = fuzzy->defuzzify(2);

  // Serial print for defuzzification results
  Serial.print("Hasil Defuzzifikasi - Relay1: ");
  Serial.print(relay1Value);
  Serial.print(", Relay2: ");
  Serial.println(relay2Value);

  // Determine relay 1 status
  String relay1Status;
  if (relay1Value < 0.5) {
    relay1Status = "Mati";
  } else if (relay1Value >= 5 && relay1Value <= 15) {
    relay1Status = "Sedang";
  } else if (relay1Value > 15) {
    relay1Status = "Lama";
  }

  // Determine relay 2 status
  String relay2Status;
  if (relay2Value < 0.5) {
    relay2Status = "Mati";
  } else if (relay2Value >= 5 && relay2Value <= 15) {
    relay2Status = "Sedang";
  } else if (relay2Value > 15) {
    relay2Status = "Lama";
  }

  // Control relay 1
  if (relay1Value > 0.5) {
    digitalWrite(RELAY1_PIN, HIGH);
    delay(relay1Value * 1000); // Convert to milliseconds
    digitalWrite(RELAY1_PIN, LOW);
  } else {
    digitalWrite(RELAY1_PIN, LOW);
  }

  // Control relay 2
  if (relay2Value > 0.5) {
    digitalWrite(RELAY2_PIN, HIGH);
    delay(relay2Value * 1000); // Convert to milliseconds
    digitalWrite(RELAY2_PIN, LOW);
  } else {
    digitalWrite(RELAY2_PIN, LOW);
  }

  // Display relay statuses on LCD
  LCD.setCursor(12, 0); // Start at the fourth line for relay statuses
  LCD.print("R1: ");
  LCD.print(relay1Status);
  LCD.print("R2: ");
  LCD.print(relay2Status);
  LCD.print("    "); // Clear remaining characters

  // Servo movement: 0 to 180 degrees
  LCD.setCursor(12, 3); // Position to display servo status
  LCD.print("Servo: M");
  for (int posisi = 0; posisi <= 180; posisi++) {
    myServo.write(posisi);
    delay(15);
  }

  delay(6000);

  // Servo movement: 180 to 0 degrees
  LCD.setCursor(12, 3); // Position to display servo status
  LCD.print("Servo: B");
  for (int posisi = 180; posisi >= 0; posisi--) {
    myServo.write(posisi);
    delay(30);
  }

  delay(2000); // 2-second delay
}