#include <LiquidCrystal_I2C.h>
#include <Wire.h>

// Pin definitions
#define POT_N_PIN  34  // Potensiometer N (Analog)
#define POT_P_PIN  35  // Potensiometer P (Analog)
#define POT_K_PIN  32  // Potensiometer K (Analog)
#define BUTTON_PIN 5   // Button untuk setting mode
#define RELAY_PIN  19  // Relay output (PWM capable)

// LCD I2C address (bisa 0x27 atau 0x3F)
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Fuzzy membership variables
float nValue = 0, pValue = 0, kValue = 0;
float nLevel = 0, pLevel = 0, kLevel = 0;  // 0-100 normalized
float fuzzyOutput = 0;

// Thresholds untuk fuzzy membership (adjustable)
const float LOW_THRESHOLD = 30.0;
const float MED_THRESHOLD = 60.0;

// Mode selection
enum Mode {
  MODE_1_PARAM,  // Only 1 parameter below threshold triggers relay
  MODE_2_PARAM,  // 2 parameters below threshold triggers relay
  MODE_3_PARAM   // All 3 parameters below threshold triggers relay
};

Mode currentMode = MODE_1_PARAM;
int buttonState = HIGH;
int lastButtonState = HIGH;
unsigned long lastDebounceTime = 0;
const unsigned long debounceDelay = 50;

// Display update timing
unsigned long lastDisplayUpdate = 0;
const unsigned long displayInterval = 500;

// PWM variable declaration (was missing)
int pwmValue = 0;

void setup() {
  Serial.begin(115200);
  
  // Initialize pins
  pinMode(BUTTON_PIN, INPUT_PULLUP);
  pinMode(RELAY_PIN, OUTPUT);
  digitalWrite(RELAY_PIN, LOW);  // Relay OFF initially
  
  // Initialize ADC resolution
  analogReadResolution(12);  // ESP32 has 12-bit ADC (0-4095)
  
  // Initialize LCD
  lcd.init();
  lcd.backlight();
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Fuzzy NPK System");
  lcd.setCursor(0, 1);
  lcd.print("Initializing...");
  delay(2000);
  
  // Display initial mode
  displayMode();
}

void loop() {
  // Read potentiometers (0-4095 mapped to 0-100%)
  int rawN = analogRead(POT_N_PIN);
  int rawP = analogRead(POT_P_PIN);
  int rawK = analogRead(POT_K_PIN);
  
  nLevel = map(rawN, 0, 4095, 0, 100);
  pLevel = map(rawP, 0, 4095, 0, 100);
  kLevel = map(rawK, 0, 4095, 0, 100);
  
  // Calculate fuzzy membership values
  float nMembershipLow = fuzzyLow(nLevel);
  float nMembershipMed = fuzzyMed(nLevel);
  float nMembershipHigh = fuzzyHigh(nLevel);
  
  float pMembershipLow = fuzzyLow(pLevel);
  float pMembershipMed = fuzzyMed(pLevel);
  float pMembershipHigh = fuzzyHigh(pLevel);
  
  float kMembershipLow = fuzzyLow(kLevel);
  float kMembershipMed = fuzzyMed(kLevel);
  float kMembershipHigh = fuzzyHigh(kLevel);
  
  // Fixed: Fuzzy inference logic
  // Rule 1: If ALL nutrients are LOW -> HIGH fertilizer need (100)
  float rule1 = min3(nMembershipLow, pMembershipLow, kMembershipLow);
  
  // Rule 2: If ALL nutrients are MEDIUM -> MEDIUM fertilizer need (50)
  float rule2 = min3(nMembershipMed, pMembershipMed, kMembershipMed);
  
  // Rule 3: If ALL nutrients are HIGH -> LOW fertilizer need (0)
  float rule3 = min3(nMembershipHigh, pMembershipHigh, kMembershipHigh);
  
  // Fixed: Rule 4 - At least one LOW (not all low) -> MEDIUM-HIGH need (75)
  float anyLow = max3(nMembershipLow, pMembershipLow, kMembershipLow);
  float rule4 = anyLow - rule1;  // Subtract the "all low" case to avoid double counting
  if (rule4 < 0) rule4 = 0;
  
  // Fixed: Rule 5 - At least one HIGH (not all high) -> LOW-MEDIUM need (25)
  float anyHigh = max3(nMembershipHigh, pMembershipHigh, kMembershipHigh);
  float rule5 = anyHigh - rule3;  // Subtract the "all high" case
  if (rule5 < 0) rule5 = 0;
  
  // Defuzzification using Sugeno-style weighted average
  float numerator = rule1 * 100 + rule2 * 50 + rule3 * 0 + rule4 * 75 + rule5 * 25;
  float denominator = rule1 + rule2 + rule3 + rule4 + rule5;
  
  if (denominator > 0) {
    fuzzyOutput = numerator / denominator;
  } else {
    fuzzyOutput = 50; // Default mid value when no rules fire
  }
  
  // Fixed: Determine if relay should be ON based on fuzzy output, not just threshold
  bool relayState = false;
  int lowCount = 0;
  
  if (nLevel < LOW_THRESHOLD) lowCount++;
  if (pLevel < LOW_THRESHOLD) lowCount++;
  if (kLevel < LOW_THRESHOLD) lowCount++;
  
  // Fixed: Use fuzzy output for relay control, not just simple threshold
  switch(currentMode) {
    case MODE_1_PARAM:
      // Relay ON if at least 1 parameter is low OR fuzzy output indicates need
      relayState = (lowCount >= 1) || (fuzzyOutput > 50);
      break;
    case MODE_2_PARAM:
      relayState = (lowCount >= 2) || (fuzzyOutput > 65);
      break;
    case MODE_3_PARAM:
      relayState = (lowCount >= 3) || (fuzzyOutput > 75);
      break;
  }
  
  // Apply relay state with PWM based on fuzzy output
  if (relayState) {
    pwmValue = map(fuzzyOutput, 0, 100, 0, 255);
  } else {
    pwmValue = 0;  // Turn off relay when not needed
  }
  analogWrite(RELAY_PIN, pwmValue);
  
  // Handle button press for mode change
  handleButton();
  
  // Update display periodically
  if (millis() - lastDisplayUpdate >= displayInterval) {
    updateDisplay(lowCount, relayState);
    lastDisplayUpdate = millis();
  }
  
  // Serial debug output
  Serial.print("N:");
  Serial.print(nLevel);
  Serial.print("% P:");
  Serial.print(pLevel);
  Serial.print("% K:");
  Serial.print(kLevel);
  Serial.print("% Fuzzy:");
  Serial.print(fuzzyOutput);
  Serial.print("% LowCount:");
  Serial.print(lowCount);
  Serial.print(" Mode:");
  Serial.print(currentMode + 1);
  Serial.print(" Relay:");
  Serial.println(pwmValue);
  
  delay(50);
}

// Fuzzy membership functions (unchanged - correct)
float fuzzyLow(float value) {
  if (value <= LOW_THRESHOLD) return 1.0;
  if (value >= MED_THRESHOLD) return 0.0;
  return (MED_THRESHOLD - value) / (MED_THRESHOLD - LOW_THRESHOLD);
}

float fuzzyMed(float value) {
  if (value <= LOW_THRESHOLD || value >= 100 - LOW_THRESHOLD) return 0.0;
  if (value >= MED_THRESHOLD - 15 && value <= MED_THRESHOLD + 15) return 1.0;
  if (value < MED_THRESHOLD - 15) return (value - LOW_THRESHOLD) / (MED_THRESHOLD - 15 - LOW_THRESHOLD);
  return (100 - LOW_THRESHOLD - value) / (100 - LOW_THRESHOLD - (MED_THRESHOLD + 15));
}

float fuzzyHigh(float value) {
  if (value <= MED_THRESHOLD) return 0.0;
  if (value >= 100 - LOW_THRESHOLD) return 1.0;
  return (value - MED_THRESHOLD) / (100 - LOW_THRESHOLD - MED_THRESHOLD);
}

// Helper functions
float min3(float a, float b, float c) {
  return min(min(a, b), c);
}

float max3(float a, float b, float c) {
  return max(max(a, b), c);
}

void handleButton() {
  int reading = digitalRead(BUTTON_PIN);
  
  if (reading != lastButtonState) {
    lastDebounceTime = millis();
  }
  
  if ((millis() - lastDebounceTime) > debounceDelay) {
    if (reading == LOW && buttonState == HIGH) {
      // Button pressed - cycle through modes
      currentMode = static_cast<Mode>((currentMode + 1) % 3);
      displayMode();
    }
    buttonState = reading;
  }
  
  lastButtonState = reading;
}

void displayMode() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Mode: ");
  
  switch(currentMode) {
    case MODE_1_PARAM:
      lcd.print("1 Parameter");
      break;
    case MODE_2_PARAM:
      lcd.print("2 Parameter");
      break;
    case MODE_3_PARAM:
      lcd.print("3 Parameter");
      break;
  }
  
  lcd.setCursor(0, 1);
  lcd.print("Press to change");
  delay(1500);
  lcd.clear();
}

void updateDisplay(int lowCount, bool relayState) {
  lcd.clear();
  
  // Baris 1: Status NPK
  lcd.setCursor(0, 0);
  lcd.print("N:");
  lcd.print((int)nLevel);
  lcd.print("% P:");
  lcd.print((int)pLevel);
  lcd.print("%");
  
  // Baris 2: Status K, LowCount, Relay PWM
  lcd.setCursor(0, 1);
  lcd.print("K:");
  lcd.print((int)kLevel);
  lcd.print("% L:");
  lcd.print(lowCount);
  lcd.print(" R:");
  lcd.print(pwmValue);  // Fixed: Now showing actual PWM value
  
  // Indikator mode di pojok kanan
  lcd.setCursor(14, 1);
  lcd.print("M");
  lcd.print(currentMode + 1);
}