#include <Fuzzy.h>
#include <FuzzyInput.h>
#include <FuzzyOutput.h>
#include <FuzzyRule.h>
#include <FuzzyRuleAntecedent.h>
#include <FuzzyRuleConsequent.h>
#include <FuzzySet.h>
#include <Adafruit_NeoPixel.h>

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

// ===== PIN ESP32 =====
#define DHTPIN 15
#define LED_PIN 19          // Pin data untuk Neopixel LED Ring
#define DHTTYPE DHT22
#define NUM_LEDS 16         // Jumlah LED pada ring (sesuaikan dengan ring Anda)

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

// DHT Object
DHT dht(DHTPIN, DHTTYPE);

// Neopixel Object
Adafruit_NeoPixel strip = Adafruit_NeoPixel(NUM_LEDS, LED_PIN, NEO_GRB + NEO_KHZ800);

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

// Variabel untuk efek motor
int motorPosition = 0;
unsigned long previousMillis = 0;
int motorSpeed = 0;  // Kecepatan putaran (0-255)
int ledBrightness = 50;  // Kecerahan LED (0-255)

void setup() {
  Serial.begin(115200);
  dht.begin();
  
  // Inisialisasi Neopixel
  strip.begin();
  strip.setBrightness(ledBrightness);
  strip.show(); // Initialize all pixels to 'off'
  
  // ===== OLED =====
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);
  }
  
  // Tampilkan splash screen
  display.clearDisplay();
  display.setTextColor(WHITE);
  display.setTextSize(2);
  display.setCursor(10, 15);
  display.println("FUZZY");
  display.setCursor(10, 35);
  display.println("LED MOTOR");
  display.setTextSize(1);
  display.setCursor(30, 55);
  display.println("Starting...");
  display.display();
  delay(1500);

  // ===== Membership Function Suhu =====
  FuzzySet *dingin = new FuzzySet(0, 0, 20, 24);
  FuzzySet *normal = new FuzzySet(22, 26, 30, 34);
  FuzzySet *panas = new FuzzySet(32, 36, 45, 45);

  FuzzyInput *suhu = new FuzzyInput(1);
  suhu->addFuzzySet(dingin);
  suhu->addFuzzySet(normal);
  suhu->addFuzzySet(panas);
  fuzzy->addFuzzyInput(suhu);

  // ===== OUTPUT Kecepatan Motor (0–255) =====
  FuzzySet *lambat  = new FuzzySet(0, 0, 30, 60);
  FuzzySet *sedang  = new FuzzySet(50, 90, 110, 150);
  FuzzySet *cepat = new FuzzySet(140, 180, 255, 255);

  FuzzyOutput *motorOut = new FuzzyOutput(1);
  motorOut->addFuzzySet(lambat);
  motorOut->addFuzzySet(sedang);
  motorOut->addFuzzySet(cepat);
  fuzzy->addFuzzyOutput(motorOut);

  // ===== Rules =====
  // Rule 1: Jika dingin → Motor LAMBAT
  FuzzyRuleAntecedent *IF_dingin = new FuzzyRuleAntecedent();
  IF_dingin->joinSingle(dingin);
  FuzzyRuleConsequent *THEN_lambat = new FuzzyRuleConsequent();
  THEN_lambat->addOutput(lambat);
  fuzzy->addFuzzyRule(new FuzzyRule(1, IF_dingin, THEN_lambat));

  // Rule 2: Jika normal → Motor SEDANG
  FuzzyRuleAntecedent *IF_normal = new FuzzyRuleAntecedent();
  IF_normal->joinSingle(normal);
  FuzzyRuleConsequent *THEN_sedang = new FuzzyRuleConsequent();
  THEN_sedang->addOutput(sedang);
  fuzzy->addFuzzyRule(new FuzzyRule(2, IF_normal, THEN_sedang));

  // Rule 3: Jika panas → Motor CEPAT
  FuzzyRuleAntecedent *IF_panas = new FuzzyRuleAntecedent();
  IF_panas->joinSingle(panas);
  FuzzyRuleConsequent *THEN_cepat = new FuzzyRuleConsequent();
  THEN_cepat->addOutput(cepat);
  fuzzy->addFuzzyRule(new FuzzyRule(3, IF_panas, THEN_cepat));
  
  Serial.println("System Ready!");
  Serial.println("==========================");
}

void updateMotorEffect() {
  unsigned long currentMillis = millis();
  
  // Hitung interval berdasarkan kecepatan motor (semakin cepat, interval semakin kecil)
  int interval = map(motorSpeed, 0, 255, 200, 20);
  interval = constrain(interval, 20, 200);
  
  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;
    
    // Bersihkan semua LED
    strip.clear();
    
    // Tentukan jumlah LED yang menyala berdasarkan kecepatan
    int ledsOn = map(motorSpeed, 0, 255, 2, NUM_LEDS/2);
    ledsOn = constrain(ledsOn, 2, NUM_LEDS/2);
    
    // Tentukan warna berdasarkan kecepatan
    uint32_t color;
    if (motorSpeed < 85) {
      color = strip.Color(0, 255, 0); // Hijau untuk lambat
    } else if (motorSpeed < 170) {
      color = strip.Color(255, 255, 0); // Kuning untuk sedang
    } else {
      color = strip.Color(255, 0, 0); // Merah untuk cepat
    }
    
    // Nyalakan LED dalam pola seperti motor berputar
    for (int i = 0; i < ledsOn; i++) {
      int ledIndex = (motorPosition + i) % NUM_LEDS;
      strip.setPixelColor(ledIndex, color);
      
      // LED berikutnya lebih redup untuk efek blur
      if (i < ledsOn - 1) {
        int nextLed = (ledIndex + 1) % NUM_LEDS;
        strip.setPixelColor(nextLed, strip.Color(
          red(color) / 2,
          green(color) / 2,
          blue(color) / 2
        ));
      }
    }
    
    // Update posisi motor
    motorPosition = (motorPosition + 1) % NUM_LEDS;
    
    // Tampilkan perubahan
    strip.show();
  }
}

// Fungsi helper untuk mendapatkan komponen warna
uint8_t red(uint32_t color) {
  return (color >> 16) & 0xFF;
}

uint8_t green(uint32_t color) {
  return (color >> 8) & 0xFF;
}

uint8_t blue(uint32_t color) {
  return color & 0xFF;
}

void loop() {
  float t = dht.readTemperature();

  if (isnan(t)) {
    Serial.println("Sensor Error!");
    
    // Tampilkan error di OLED
    display.clearDisplay();
    display.setTextSize(2);
    display.setCursor(15, 10);
    display.println("ERROR!");
    display.setTextSize(1);
    display.setCursor(10, 35);
    display.println("DHT22 Sensor Not");
    display.setCursor(25, 45);
    display.println("Found");
    display.setTextSize(1);
    display.setCursor(20, 55);
    display.println("Check Connection");
    display.display();
    
    // LED error pattern
    for(int i=0; i<NUM_LEDS; i++) {
      strip.setPixelColor(i, strip.Color(255, 0, 0));
    }
    strip.show();
    
    delay(2000);
    return;
  }

  // ===== FUZZY PROCESS =====
  fuzzy->setInput(1, t);
  fuzzy->fuzzify();

  motorSpeed = fuzzy->defuzzify(1);
  motorSpeed = constrain(motorSpeed, 0, 255);

  // Update efek motor
  updateMotorEffect();

  // Status motor berdasarkan output fuzzy
  String status;
  if (motorSpeed < 60) {
    status = "SLOW";
  } else if (motorSpeed < 150) {
    status = "MEDIUM";
  } else {
    status = "FAST";
  }

  // ===== OLED DISPLAY =====
  display.clearDisplay();
  
  // Header
  display.drawRect(0, 0, 128, 16, WHITE);
  display.fillRect(0, 0, 128, 16, WHITE);
  display.setTextColor(BLACK);
  display.setTextSize(1);
  display.setCursor(10, 5);
  display.println("MOTOR EFFECT LED");
  display.setTextColor(WHITE);
  
  // Informasi Suhu
  display.setTextSize(1);
  display.setCursor(0, 20);
  display.print("SUHU:");
  display.setTextSize(2);
  display.setCursor(40, 18);
  display.print(t, 1);
  display.setTextSize(1);
  display.setCursor(85, 25);
  display.print("C");
  
  // Separator line
  display.drawLine(0, 38, 128, 38, WHITE);
  
  // Status dan Kecepatan
  display.setTextSize(1);
  display.setCursor(0, 42);
  display.print("STATUS: ");
  display.println(status);
  
  display.setCursor(0, 52);
  display.print("SPEED: ");
  display.print(motorSpeed);
  display.print("/255");
  
  // Bar meter kecepatan
  display.drawRect(70, 42, 56, 20, WHITE);
  
  // Isi bar meter
  int barLength = map(motorSpeed, 0, 255, 0, 54);
  display.fillRect(72, 44, barLength, 16, WHITE);
  
  // Label 0 dan 255
  display.setTextSize(1);
  display.setCursor(70, 62);
  display.print("0");
  display.setCursor(120, 62);
  display.print("255");
  
  // Indicator segitiga
  int trianglePos = map(motorSpeed, 0, 255, 70, 124);
  display.fillTriangle(trianglePos, 41, trianglePos-3, 36, trianglePos+3, 36, WHITE);
  
  // Tampilkan pola LED di OLED
  display.drawCircle(64, 32, 2, WHITE); // Titik pusat
  for(int i = 0; i < 8; i++) {
    float angle = (motorPosition * 22.5 + i * 45) * 3.14159 / 180; // 16 LED = 22.5 dereach
    int x = 64 + cos(angle) * 20;
    int y = 32 + sin(angle) * 20;
    display.fillCircle(x, y, 1, WHITE);
  }
  
  display.display();

  // Serial Monitor
  Serial.print("Suhu: ");
  Serial.print(t, 1);
  Serial.print("C | Speed: ");
  Serial.print(motorSpeed);
  Serial.print(" | Status: ");
  Serial.println(status);

  delay(10);  // Delay kecil untuk stabilitas
}