#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <DHT.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#define DHTPIN 2
#define DHTTYPE DHT22

DHT dht(DHTPIN, DHTTYPE);

// ======================
// MEMBERSHIP FUNCTION
// ======================

float trapesium(float x, float a, float b, float c, float d) {
  if (x <= a || x >= d) return 0;
  else if (x >= b && x <= c) return 1;
  else if (x > a && x < b) return (x - a) / (b - a);
  else return (d - x) / (d - c);
}

void setup() {

  Serial.begin(9600);
  dht.begin();
  runAllTests();

  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println("OLED gagal!");
    while(true);
  }

  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
}

void loop() {

  float hum = dht.readHumidity();
  float temp = dht.readTemperature();

  if (isnan(hum) || isnan(temp)) {
    Serial.println("Sensor error!");
    return;
  }

  // ======================
  // FUZZIFIKASI SUHU
  // ======================

  float dingin = trapesium(temp, 0,0,25,27);
  float normal = trapesium(temp, 26,27,31,33);
  float panas  = trapesium(temp, 32,34,39,41);
  float sangatPanas = trapesium(temp, 40,42,60,60);

  // ======================
  // FUZZIFIKASI KELEMBAPAN
  // ======================

  float normalH = trapesium(hum, 0,0,60,70);
  float lembap  = trapesium(hum, 65,70,100,100);

  // ======================
  // RULE BASE (≥5 RULE)
  // ======================

  float r1 = dingin;                    // Aman
  float r2 = min(normal, normalH);      // Aman
  float r3 = min(normal, lembap);       // Waspada
  float r4 = min(panas, normalH);       // Waspada
  float r5 = min(panas, lembap);        // Bahaya
  float r6 = sangatPanas;               // Bahaya

  // ======================
  // DEFUZZIFIKASI
  // ======================

  float aman_val = 25;
  float waspada_val = 60;
  float bahaya_val = 90;

  float numerator =
    (r1 * aman_val) +
    (r2 * aman_val) +
    (r3 * waspada_val) +
    (r4 * waspada_val) +
    (r5 * bahaya_val) +
    (r6 * bahaya_val);

  float denominator = r1+r2+r3+r4+r5+r6;

  float output = 0;
  if(denominator != 0){
    output = numerator / denominator;
  }

  String status;

  if (output < 45) status="AMAN";
  else if (output < 75) status="WASPADA";
  else status="BAHAYA";

  // ======================
  // SERIAL OUTPUT
  // ======================

  Serial.print("Temp: ");
  Serial.print(temp);
  Serial.print(" C | Hum: ");
  Serial.print(hum);
  Serial.print(" % | Output: ");
  Serial.print(output);
  Serial.print(" | Status: ");
  Serial.println(status);

  // ======================
  // OLED OUTPUT
  // ======================

  display.clearDisplay();

  display.setCursor(0,0);
  display.print("FUZZY MONITOR");

  display.setCursor(0,15);
  display.print("Temp: ");
  display.print(temp);
  display.print(" C");

  display.setCursor(0,30);
  display.print("Hum : ");
  display.print(hum);
  display.print(" %");

  display.setCursor(0,45);
  display.print("Status: ");
  display.print(status);

  display.display();

  delay(2000);
}

void unitTest(float temp, float hum, String expected){

  Serial.println("================================");

  // ======================
  // FUZZIFIKASI
  // ======================
  float dingin = trapesium(temp, 0,0,25,27);
  float normal = trapesium(temp, 26,27,31,33);
  float panas  = trapesium(temp, 32,34,39,41);
  float sangatPanas = trapesium(temp, 40,42,60,60);

  float normalH = trapesium(hum, 0,0,60,70);
  float lembap  = trapesium(hum, 65,70,100,100);

  // ======================
  // RULE
  // ======================
  float r1 = dingin;
  float r2 = min(normal, normalH);
  float r3 = min(normal, lembap);
  float r4 = min(panas, normalH);
  float r5 = min(panas, lembap);
  float r6 = sangatPanas;

  // ======================
  // DEFUZZ
  // ======================
  float aman_val = 25;
  float waspada_val = 60;
  float bahaya_val = 90;

  float numerator =
    (r1 * aman_val) +
    (r2 * aman_val) +
    (r3 * waspada_val) +
    (r4 * waspada_val) +
    (r5 * bahaya_val) +
    (r6 * bahaya_val);

  float denominator = r1+r2+r3+r4+r5+r6;

  float output = 0;
  if(denominator != 0){
    output = numerator / denominator;
  }

  String status;

  if (output < 45) status="AMAN";
  else if (output < 75) status="WASPADA";
  else status="BAHAYA";

  Serial.print("Input Temp: ");
  Serial.print(temp);
  Serial.print(" Hum: ");
  Serial.println(hum);

  Serial.print("Expected: ");
  Serial.print(expected);
  Serial.print(" | Result: ");
  Serial.println(status);

  if(status == expected){
    Serial.println("TEST PASSED");
  }else{
    Serial.println("TEST FAILED");
  }
}

void runAllTests(){

  Serial.println("===== FUZZY UNIT TEST =====");

  unitTest(24,50,"AMAN");     
  unitTest(28,50,"AMAN");     
  unitTest(29,80,"WASPADA");  
  unitTest(34,50,"WASPADA");  
  unitTest(35,75,"BAHAYA");   
  unitTest(42,40,"BAHAYA");   

  Serial.println("===== TEST DONE =====");
}