#include <WiFi.h>
#include "Adafruit_MQTT.h"
#include "Adafruit_MQTT_Client.h"
#include "DHT.h"
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <math.h>
#include <ESP32Servo.h>

// --- KONFIGURASI JARINGAN WIFI (UNTUK WOKWI) ---
#define WLAN_SSID       "Wokwi-GUEST"
#define WLAN_PASS       ""

// --- KONFIGURASI ADAFRUIT IO ---
#define AIO_SERVER      "io.adafruit.com"
#define AIO_SERVERPORT  1883
// Ganti dengan Username dan AIO Key Anda!
#define AIO_USERNAME    "ImeiOz" 
#define AIO_KEY         "aio_Ppza91yduG0pnoGGKoeeysZjlC4Q"

// --- Konfigurasi Pin ---
#define DHT_PIN             15
#define NTC_PIN             34
#define SERVO_TEMP_PIN      13 // Servo untuk Suhu (Ventilasi)
#define SERVO_HUMIDITY_PIN  12 // Servo untuk Kelembapan (Kipas)
#define DHT_TYPE            DHT22

// --- Konfigurasi Layar OLED ---
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// --- Inisialisasi Sensor, MQTT, dan Aktuator ---
DHT dht(DHT_PIN, DHT_TYPE);
Servo servoTemperature;
Servo servoHumidity;
WiFiClient client;
Adafruit_MQTT_Client mqtt(&client, AIO_SERVER, AIO_SERVERPORT, AIO_USERNAME, AIO_KEY);

// --- Siapkan "Feeds" untuk Publikasi Data ---
Adafruit_MQTT_Publish suhuFahrenheitFeed = Adafruit_MQTT_Publish(&mqtt, AIO_USERNAME "/feeds/suhu-fahrenheit");
Adafruit_MQTT_Publish kelembapanAbsolutFeed = Adafruit_MQTT_Publish(&mqtt, AIO_USERNAME "/feeds/kelembapan-absolut");
Adafruit_MQTT_Publish servoTempAngleFeed = Adafruit_MQTT_Publish(&mqtt, AIO_USERNAME "/feeds/servo-temp-angle");
Adafruit_MQTT_Publish servoHumidAngleFeed = Adafruit_MQTT_Publish(&mqtt, AIO_USERNAME "/feeds/servo-humid-angle");


// ==========================================================
// ========== BAGIAN FUZZY LOGIC IMPLEMENTATION =============
// ==========================================================

// Fungsi Keanggotaan Trapesium
float trapmf(float x, float a, float b, float c, float d) {
    if (x <= a) return 0.0;
    if (x < b) return (x - a) / (b - a);
    if (x <= c) return 1.0;
    if (x < d) return (d - x) / (d - c);
    return 0.0;
}

// Fungsi Keanggotaan Segitiga
float trimf(float x, float a, float b, float c) {
    if (x <= a || x >= c) return 0.0;
    if (x < b) return (x - a) / (b - a);
    if (x > b) return (c - x) / (c - b);
    return 1.0;
}

// Fungsi Defuzzifikasi Centroid of Area (COA)
// Menggunakan 100 titik sampel (discretization points) dari 0 hingga 180
float defuzzify_coa(float w1, float c1, float w2, float c2, float w3, float c3) {
    float numerator = 0.0;
    float denominator = 0.0;
    float step = 180.0 / 100.0; // Rentang 0-180

    for (int i = 0; i <= 100; i++) {
        float x = i * step; // Nilai output (sudut servo)

        // Hitung Output Membership Function (MF) yang dipotong (clipped)
        float mu1 = min(w1, trimf(x, c1 - 45, c1, c1 + 45)); // Output MF 1
        float mu2 = min(w2, trimf(x, c2 - 45, c2, c2 + 45)); // Output MF 2
        float mu3 = min(w3, trimf(x, c3 - 45, c3, c3 + 45)); // Output MF 3

        // Agregasi: Ambil nilai maksimum dari semua output yang dipotong
        float aggregate = max(mu1, max(mu2, mu3));

        numerator += aggregate * x;
        denominator += aggregate;
    }

    if (denominator == 0.0) {
        return 90.0; // Nilai default jika tidak ada rule yang aktif
    }

    return numerator / denominator;
}

// ==========================================================
// ============= LOGIKA KONTROL FUZZY SUHU ==================
// ==========================================================

int fuzzy_control_temperature(float tempF) {
    // 1. Fuzzifikasi (Input: Suhu F)
    float mu_dingin = trapmf(tempF, 30, 30, 40, 50);
    float mu_sedang = trimf(tempF, 45, 55, 65);
    float mu_panas = trapmf(tempF, 60, 70, 80, 80);

    // 2. Inference (Rule Base, Operator MIN/MAX)
    // Rule 1: IF Dingin THEN TUTUP (Konsekuen: 0)
    float w_tutup = mu_dingin;
    float c_tutup = 0.0; 

    // Rule 2: IF Sedang THEN BUKA_SEDIKIT (Konsekuen: 90)
    float w_buka_sedikit = mu_sedang;
    float c_buka_sedikit = 90.0;

    // Rule 3: IF Panas THEN BUKA_PENUH (Konsekuen: 180)
    float w_buka_penuh = mu_panas;
    float c_buka_penuh = 180.0;

    // 3. Defuzzifikasi (Output: Sudut Servo Suhu)
    float angle = defuzzify_coa(
        w_tutup, c_tutup, 
        w_buka_sedikit, c_buka_sedikit, 
        w_buka_penuh, c_buka_penuh
    );

    // Batasi sudut antara 0 dan 180
    return constrain((int)round(angle), 0, 180);
}

// ==========================================================
// =========== LOGIKA KONTROL FUZZY KELEMBAPAN ==============
// ==========================================================

int fuzzy_control_humidity(float absHumid) {
    // 1. Fuzzifikasi (Input: Kelembapan Absolut g/m³)
    float mu_kosong = trapmf(absHumid, 60, 60, 70, 80);
    float mu_nyaman = trimf(absHumid, 75, 85, 95);
    float mu_jenuhfull = trapmf(absHumid, 90, 100, 110, 110);

    // 2. Inference (Rule Base, Operator MIN/MAX)
    // Rule 1: IF Kosong THEN RENDAH (Konsekuen: 0)
    float w_rendah = mu_kosong;
    float c_rendah = 0.0;

    // Rule 2: IF Nyaman THEN SEDANG (Konsekuen: 90)
    float w_sedang = mu_nyaman;
    float c_sedang = 90.0;

    // Rule 3: IF Jenuhfull THEN TINGGI (Konsekuen: 180)
    float w_tinggi = mu_jenuhfull;
    float c_tinggi = 180.0;

    // 3. Defuzzifikasi (Output: Sudut Servo Kelembapan)
    float angle = defuzzify_coa(
        w_rendah, c_rendah, 
        w_sedang, c_sedang, 
        w_tinggi, c_tinggi
    );

    // Batasi sudut antara 0 dan 180
    return constrain((int)round(angle), 0, 180);
}

// ==========================================================
// ======================= FUNGSI UTAMA =====================
// ==========================================================


// Fungsi untuk menghubungkan dan menjaga koneksi MQTT
void MQTT_connect() {
  int8_t ret;
  if (mqtt.connected()) { return; }
  Serial.print("Menghubungkan ke MQTT... ");
  uint8_t retries = 3;
  while ((ret = mqtt.connect()) != 0) {
        Serial.println(mqtt.connectErrorString(ret));
        Serial.println("Gagal terhubung, mencoba lagi...");
        mqtt.disconnect();
        delay(5000);
        retries--;
        if (retries == 0) { while (1); }
  }
  Serial.println("MQTT Terhubung!");
}

void setup() {
  Serial.begin(115200);
  dht.begin();
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  servoTemperature.attach(SERVO_TEMP_PIN);
  servoHumidity.attach(SERVO_HUMIDITY_PIN);

  // Menghubungkan ke WiFi
  Serial.print("Menghubungkan ke WiFi... ");
  WiFi.begin(WLAN_SSID, WLAN_PASS);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println(" Terhubung!");
}

void loop() {
  MQTT_connect();

  // Baca sensor dan lakukan semua konversi
  float humidityRH = dht.readHumidity();
  
  // Rumus untuk MODUL NTC 3-PIN
  const float BETA = 3950;
  int rawADC = analogRead(NTC_PIN);
  float tempC = 1 / (log(1 / (4095.0 / rawADC - 1)) / BETA + 1.0 / 298.15) - 273.15;

  if (isnan(humidityRH) || isnan(tempC)) {
    Serial.println("Gagal membaca sensor.");
    delay(5000);
    return;
  }

  float tempF = (tempC * 9.0/5.0) + 32.0;
  float tempK = tempC + 273.15;
  // Menghitung Kelembapan Absolut dalam g/m³
  float absoluteHumidity = (6.112 * exp((17.67 * tempC) / (tempC + 243.5)) * (humidityRH / 100.0) * 100) / (461.5 * tempK) * 1000;

  // --- LOGIKA KONTROL AKTUATOR DENGAN FUZZY LOGIC ---
  int servoTempAngle = fuzzy_control_temperature(tempF);
  servoTemperature.write(servoTempAngle);

  int servoHumidAngle = fuzzy_control_humidity(absoluteHumidity);
  servoHumidity.write(servoHumidAngle);

  // --- KIRIM SEMUA DATA KE ADAFRUIT IO ---
  suhuFahrenheitFeed.publish(tempF);
  kelembapanAbsolutFeed.publish(absoluteHumidity);
  servoTempAngleFeed.publish((float)servoTempAngle);
  servoHumidAngleFeed.publish((float)servoHumidAngle);

  // --- TAMPILKAN DATA DI OLED ---
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(1);
  display.setCursor(0, 0);
  display.print("SUHU (F): " + String(tempF, 1));
  display.setCursor(0, 12);
  display.print("VENTILASI: " + String(servoTempAngle) + (char)247); // Sudut Ventilasi

  display.setCursor(0, 32);
  display.print("LEMBAP (g/m3): " + String(absoluteHumidity, 1));
  display.setCursor(0, 44);
  display.print("KIPAS: " + String(servoHumidAngle) + (char)247); // Sudut Kipas
  display.display();

  delay(5000); // Interval 5 detik untuk responsifitas
}