/*
 * Hybrid Inverter/UPS Firmware - ESP32 (ESP32-WROOM-32)
 * 50Hz Fundamental, 25kHz Carrier, 3-Stage Charging
 * 
 * Architecture: Unipolar SPWM
 * H-Bridge: TLP250 Drivers
 * 
 * Features: Dual-core processing, WiFi, Bluetooth, Web interface
 * Cores: Core 0 for SPWM and critical timing, Core 1 for UI and logic
 */

#include <Arduino.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Preferences.h>  // ESP32 non-volatile storage (better than EEPROM)
#include <driver/ledc.h>  // ESP32 LEDC PWM controller

// Task handles for FreeRTOS tasks
TaskHandle_t SPWMTaskHandle = NULL;
TaskHandle_t LogicTaskHandle = NULL;

// --- ESP32 Configuration ---
#define SPWM_FREQ 25000           // 25kHz
#define PWM_RESOLUTION LEDC_TIMER_12_BIT  // 12-bit resolution (0-4095)
#define PWM_DUTY_MAX 4095         // Max duty for 12-bit PWM

// CPU Core assignments
#define CORE_0 0
#define CORE_1 1

// --- Pin Definitions (ESP32) ---
// PWM Outputs - Using LEDC channels
#define PIN_HI_L    18   // GPIO18, LEDC Channel 0
#define PIN_HI_R    19   // GPIO19, LEDC Channel 1
#define PIN_LO_L    21   // GPIO21
#define PIN_LO_R    22   // GPIO22
#define PIN_FAN     23   // GPIO23, LEDC Channel 2
#define PIN_BUZZER  25   // GPIO25
#define PIN_RELAY   26   // GPIO26, Relay: HIGH=Grid(Charge), LOW=Inverter

// Zero Crossing Detection Pin
#define PIN_ZC      27   // GPIO27, External interrupt capable

// Button Pins (with internal pullups)
#define BTN_PWR     32   // GPIO32
#define BTN_ENT     33   // GPIO33
#define BTN_UP      34   // GPIO34 (INPUT only, no pullup)
#define BTN_DWN     35   // GPIO35 (INPUT only, no pullup)

// Analog Input Pins (ESP32 has 12-bit ADC)
#define SENS_AC_IN  36   // GPIO36, ADC1_CH0
#define SENS_CT     39   // GPIO39, ADC1_CH3
#define SENS_BAT    34   // GPIO34, ADC1_CH6 (shared with BTN_UP)
#define SENS_NTC    35   // GPIO35, ADC1_CH7 (shared with BTN_DWN)
#define SENS_FB     32   // GPIO32, ADC1_CH4 (shared with BTN_PWR)

// I2C for LCD
#define I2C_SDA     4    // GPIO4
#define I2C_SCL     5    // GPIO5

// --- Button Press Timing ---
#define LONG_PRESS_TIME 1500      // 1.5 seconds for long press
#define DEBOUNCE_TIME   50        // 50ms debounce time
#define SETTINGS_TIMEOUT 30000    // 30 seconds timeout for settings
#define DISPLAY_CHANGE_TIME 3000  // 3 seconds auto page change

// --- Zero Crossing Variables ---
volatile uint32_t zcTime = 0;          // Time of last zero crossing
volatile uint32_t lastZcTime = 0;      // Time of previous zero crossing
volatile uint32_t halfPeriod = 10000;  // 10ms default for 50Hz (half period in µs)
volatile bool zcDetected = false;      // Zero crossing flag
volatile bool zcPolarity = true;       // true = positive half, false = negative half
volatile int phaseAdjust = 0;          // Phase adjustment for synchronization
volatile uint32_t phaseError = 0;      // Phase error in microseconds
bool gridStable = false;               // Grid frequency stability flag

// --- Objects ---
LiquidCrystal_I2C lcd(0x27, 20, 4); // I2C LCD at address 0x27
Preferences preferences;            // ESP32 non-volatile storage

// --- Variables & Flags ---
volatile int sineTable[250];        // Half wave table
volatile int spwmIndex = 0;
volatile bool cyclePositive = true;
volatile float modulationIndex = 0.0;    // 0.0 to 1.0 (Soft Start)
volatile float targetModulation = 0.85;  // Feedback loop adjusts this

// Measurements (shared between cores, use mutex for thread safety)
portMUX_TYPE measurementMutex = portMUX_INITIALIZER_UNLOCKED;
float batVolts = 0.0;
float acInVolts = 0.0;
float acOutVolts = 0.0;
float currentAmps = 0.0;
float loadWatts = 0.0;
float loadVA = 0.0;
float powerFactor = 1.0;
int tempC = 0;
float gridFreq = 0.0;
float battPower = 0.0;
float efficiency = 0.0;
uint32_t uptimeHours = 0;
uint32_t uptimeMinutes = 0;
uint32_t inverterRuntime = 0;
uint32_t chargeTime = 0;
float dailyEnergy = 0.0; // kWh

// System Stats
float minBattVoltage = 99.0;
float maxBattVoltage = 0.0;
float maxLoadWatts = 0.0;
float maxTemp = 0.0;

// Settings Structure
typedef struct {
  float batLowCut;
  float batFull;
  float chargeCurrent;
  int acMin;
  int acMax;
  int phaseSyncWindow;
  int gridStableCount;
  float bulkVoltage;
  float floatVoltage;
  int fanStartTemp;
  int fanMaxTemp;
  int overloadDelay;
  int displayTimeout;
  float efficiencyOffset;
  bool enableWiFi;
  bool enableWebInterface;
} Settings;

Settings settings;

// Default settings
Settings defaultSettings = {
  10.5f,   // batLowCut
  14.2f,   // batFull
  10.0f,   // chargeCurrent
  200,     // acMin
  250,     // acMax
  200,     // phaseSyncWindow
  10,      // gridStableCount
  14.4f,   // bulkVoltage
  13.5f,   // floatVoltage
  45,      // fanStartTemp
  70,      // fanMaxTemp
  5000,    // overloadDelay
  3000,    // displayTimeout
  0.95f,   // efficiencyOffset
  false,   // enableWiFi
  false    // enableWebInterface
};

// State Machine
typedef enum {
  STARTUP, 
  INVERTER, 
  GRID_BYPASS, 
  CHARGING, 
  SYNCHRONIZING, 
  ERROR_STATE, 
  SETTINGS_MODE
} Mode;

Mode currentMode = STARTUP;
char errorMsg[17] = "";

// Charging State
typedef enum { BULK, ABSORPTION, FLOAT } ChargeStage;
ChargeStage chgStage = BULK;

// Display Pages
typedef enum { 
  PAGE_MAIN,      // Main operational data
  PAGE_DETAILED,  // Detailed measurements
  PAGE_STATS,     // System statistics
  PAGE_SYSTEM     // System info and uptime
} DisplayPage;

DisplayPage currentPage = PAGE_MAIN;
DisplayPage manualPage = PAGE_MAIN; // For manual navigation
bool autoPageChange = true;
uint32_t lastPageChange = 0;
int totalPages = 4;

// Settings Menu
typedef enum { 
  MAIN_MENU, 
  BATTERY_SETTINGS, 
  AC_SETTINGS, 
  CHARGING_SETTINGS, 
  TEMP_SETTINGS,
  SYSTEM_SETTINGS,
  DISPLAY_SETTINGS,
  WIFI_SETTINGS,
  SAVE_EXIT
} MenuPage;

MenuPage currentMenu = MAIN_MENU;
int menuItem = 0;
int maxMenuItems = 0;
bool editingValue = false;
uint32_t settingsEnterTime = 0;
bool settingsChanged = false;

// Button States
uint32_t btnEntPressTime = 0;
bool btnEntPressed = false;
bool btnEntLongPress = false;
uint32_t btnUpPressTime = 0;
bool btnUpPressed = false;
uint32_t btnDwnPressTime = 0;
bool btnDwnPressed = false;
uint32_t btnPwrPressTime = 0;
bool btnPwrPressed = false;

uint32_t lastUpdate = 0;
uint32_t lastZcCheck = 0;
uint32_t startupTime = 0;
uint32_t lastEnergyUpdate = 0;
bool systemOn = false;

// --- Function Prototypes ---
void generateSineTable();
void IRAM_ATTR zeroCrossingISR();
void setupPWM();
void setupADC();
void handleButtons();
void handleEnterShortPress();
void handleUpButton();
void handleDownButton();
void handlePowerButton();
void exitSettingsMode();
void editValue(int direction);
void displaySettingsMenu();
void updateDisplayPage();
void updateStatistics();
void readSensors();
void stateMachine();
void handleFan();
void checkGridSync();
void synchronizePhase();
void saveSettings();
bool loadSettings();

// Task functions for FreeRTOS
void SPWMTask(void *parameter);
void LogicTask(void *parameter);

// --- Lookup Table Generation ---
void generateSineTable() {
  for (int i = 0; i < 250; i++) {
    float rads = PI * i / 250.0f;
    sineTable[i] = (int)(sin(rads) * (float)PWM_DUTY_MAX); // Scale to 0-4095
  }
}

// --- Zero Crossing Interrupt Service Routine (IRAM_ATTR for speed) ---
void IRAM_ATTR zeroCrossingISR() {
  uint32_t currentTime = micros();
  
  if (lastZcTime > 0) {
    halfPeriod = currentTime - lastZcTime;
    
    if (halfPeriod >= 8000 && halfPeriod <= 12000) {
      uint32_t expectedTime = lastZcTime + 10000;
      phaseError = (currentTime > expectedTime) ? (currentTime - expectedTime) : (expectedTime - currentTime);
      
      zcPolarity = !zcPolarity;
      zcDetected = true;
      lastZcTime = currentTime;
      zcTime = currentTime;
      
      gridFreq = 500000.0f / halfPeriod;
      
      static int stableCount = 0;
      if (phaseError < 200) {
        stableCount++;
        if (stableCount > 10) {
          gridStable = true;
        }
      } else {
        stableCount = 0;
        gridStable = false;
      }
    }
  } else {
    lastZcTime = currentTime;
  }
}

// --- Setup PWM using ESP32 LEDC ---
void setupPWM() {
  // Configure LEDC Timer 0 for SPWM (25kHz, 12-bit resolution)
  ledc_timer_config_t timer_conf = {
    .speed_mode = LEDC_HIGH_SPEED_MODE,
    .duty_resolution = PWM_RESOLUTION,
    .timer_num = LEDC_TIMER_0,
    .freq_hz = SPWM_FREQ,
    .clk_cfg = LEDC_AUTO_CLK
  };
  ledc_timer_config(&timer_conf);
  
  // Configure LEDC Channel 0 for PIN_HI_L
  ledc_channel_config_t channel_conf_0 = {
    .gpio_num = PIN_HI_L,
    .speed_mode = LEDC_HIGH_SPEED_MODE,
    .channel = LEDC_CHANNEL_0,
    .intr_type = LEDC_INTR_DISABLE,
    .timer_sel = LEDC_TIMER_0,
    .duty = 0,
    .hpoint = 0
  };
  ledc_channel_config(&channel_conf_0);
  
  // Configure LEDC Channel 1 for PIN_HI_R
  ledc_channel_config_t channel_conf_1 = {
    .gpio_num = PIN_HI_R,
    .speed_mode = LEDC_HIGH_SPEED_MODE,
    .channel = LEDC_CHANNEL_1,
    .intr_type = LEDC_INTR_DISABLE,
    .timer_sel = LEDC_TIMER_0,
    .duty = 0,
    .hpoint = 0
  };
  ledc_channel_config(&channel_conf_1);
  
  // Configure LEDC Timer 1 for Fan PWM (25kHz)
  ledc_timer_config_t fan_timer_conf = {
    .speed_mode = LEDC_HIGH_SPEED_MODE,
    .duty_resolution = PWM_RESOLUTION,
    .timer_num = LEDC_TIMER_1,
    .freq_hz = SPWM_FREQ,
    .clk_cfg = LEDC_AUTO_CLK
  };
  ledc_timer_config(&fan_timer_conf);
  
  // Configure LEDC Channel 2 for Fan
  ledc_channel_config_t fan_channel_conf = {
    .gpio_num = PIN_FAN,
    .speed_mode = LEDC_HIGH_SPEED_MODE,
    .channel = LEDC_CHANNEL_2,
    .intr_type = LEDC_INTR_DISABLE,
    .timer_sel = LEDC_TIMER_1,
    .duty = 0,
    .hpoint = 0
  };
  ledc_channel_config(&fan_channel_conf);
}

// --- Setup ADC ---
void setupADC() {
  analogReadResolution(12); // ESP32 has 12-bit ADC
  analogSetAttenuation(ADC_11db); // Full range 0-3.3V
  
  // Configure ADC pins
  pinMode(SENS_AC_IN, INPUT);
  pinMode(SENS_CT, INPUT);
  pinMode(SENS_BAT, INPUT);
  pinMode(SENS_NTC, INPUT);
  pinMode(SENS_FB, INPUT);
}

// --- EEPROM Functions using Preferences ---
void saveSettings() {
  preferences.begin("inverter", false); // RW mode (false = no readonly)
  
  preferences.putFloat("batLowCut", settings.batLowCut);
  preferences.putFloat("batFull", settings.batFull);
  preferences.putFloat("chargeCurrent", settings.chargeCurrent);
  preferences.putInt("acMin", settings.acMin);
  preferences.putInt("acMax", settings.acMax);
  preferences.putInt("phaseSyncWindow", settings.phaseSyncWindow);
  preferences.putInt("gridStableCount", settings.gridStableCount);
  preferences.putFloat("bulkVoltage", settings.bulkVoltage);
  preferences.putFloat("floatVoltage", settings.floatVoltage);
  preferences.putInt("fanStartTemp", settings.fanStartTemp);
  preferences.putInt("fanMaxTemp", settings.fanMaxTemp);
  preferences.putInt("overloadDelay", settings.overloadDelay);
  preferences.putInt("displayTimeout", settings.displayTimeout);
  preferences.putFloat("efficiencyOffset", settings.efficiencyOffset);
  preferences.putBool("enableWiFi", settings.enableWiFi);
  preferences.putBool("enableWebInterface", settings.enableWebInterface);
  
  preferences.end();
  
  // Show saved message
  lcd.clear();
  lcd.setCursor(0, 1);
  lcd.print("  Settings Saved!  ");
  delay(1000);
}

bool loadSettings() {
  preferences.begin("inverter", true); // Readonly mode
  
  // Check if preferences exist
  if (!preferences.isKey("batLowCut")) {
    preferences.end();
    settings = defaultSettings;
    saveSettings();
    return false;
  }
  
  settings.batLowCut = preferences.getFloat("batLowCut", defaultSettings.batLowCut);
  settings.batFull = preferences.getFloat("batFull", defaultSettings.batFull);
  settings.chargeCurrent = preferences.getFloat("chargeCurrent", defaultSettings.chargeCurrent);
  settings.acMin = preferences.getInt("acMin", defaultSettings.acMin);
  settings.acMax = preferences.getInt("acMax", defaultSettings.acMax);
  settings.phaseSyncWindow = preferences.getInt("phaseSyncWindow", defaultSettings.phaseSyncWindow);
  settings.gridStableCount = preferences.getInt("gridStableCount", defaultSettings.gridStableCount);
  settings.bulkVoltage = preferences.getFloat("bulkVoltage", defaultSettings.bulkVoltage);
  settings.floatVoltage = preferences.getFloat("floatVoltage", defaultSettings.floatVoltage);
  settings.fanStartTemp = preferences.getInt("fanStartTemp", defaultSettings.fanStartTemp);
  settings.fanMaxTemp = preferences.getInt("fanMaxTemp", defaultSettings.fanMaxTemp);
  settings.overloadDelay = preferences.getInt("overloadDelay", defaultSettings.overloadDelay);
  settings.displayTimeout = preferences.getInt("displayTimeout", defaultSettings.displayTimeout);
  settings.efficiencyOffset = preferences.getFloat("efficiencyOffset", defaultSettings.efficiencyOffset);
  settings.enableWiFi = preferences.getBool("enableWiFi", defaultSettings.enableWiFi);
  settings.enableWebInterface = preferences.getBool("enableWebInterface", defaultSettings.enableWebInterface);
  
  preferences.end();
  return true;
}

// --- SPWM Task (Runs on Core 0 for precise timing) ---
void SPWMTask(void *parameter) {
  // High priority task for SPWM generation
  vTaskPrioritySet(NULL, 3); // Higher priority
  
  // Timer for SPWM updates (12500Hz = 50Hz * 250 steps)
  const TickType_t xFrequency = 1; // 1ms base
  TickType_t xLastWakeTime = xTaskGetTickCount();
  
  while (1) {
    if (currentMode == INVERTER || currentMode == SYNCHRONIZING) {
      int duty = (int)(sineTable[spwmIndex] * modulationIndex);
      
      // Unipolar Switching Logic
      if (cyclePositive) {
        ledc_set_duty(LEDC_HIGH_SPEED_MODE, LEDC_CHANNEL_0, duty);
        ledc_set_duty(LEDC_HIGH_SPEED_MODE, LEDC_CHANNEL_1, 0);
        ledc_update_duty(LEDC_HIGH_SPEED_MODE, LEDC_CHANNEL_0);
        ledc_update_duty(LEDC_HIGH_SPEED_MODE, LEDC_CHANNEL_1);
        
        digitalWrite(PIN_LO_L, LOW);
        digitalWrite(PIN_LO_R, HIGH);
      } else {
        ledc_set_duty(LEDC_HIGH_SPEED_MODE, LEDC_CHANNEL_0, 0);
        ledc_set_duty(LEDC_HIGH_SPEED_MODE, LEDC_CHANNEL_1, duty);
        ledc_update_duty(LEDC_HIGH_SPEED_MODE, LEDC_CHANNEL_0);
        ledc_update_duty(LEDC_HIGH_SPEED_MODE, LEDC_CHANNEL_1);
        
        digitalWrite(PIN_LO_L, HIGH);
        digitalWrite(PIN_LO_R, LOW);
      }
      
      spwmIndex++;
      if (spwmIndex >= 250) {
        spwmIndex = 0;
        cyclePositive = !cyclePositive;
      }
    } 
    else if (currentMode == CHARGING) {
      static bool chargingCycle = false;
      
      if (zcDetected && zcPolarity) {
        chargingCycle = true;
        zcDetected = false;
      }
      
      if (chargingCycle) {
        digitalWrite(PIN_LO_L, LOW); 
        digitalWrite(PIN_LO_R, LOW);
        
        int chargeDuty = (int)(modulationIndex * PWM_DUTY_MAX);
        ledc_set_duty(LEDC_HIGH_SPEED_MODE, LEDC_CHANNEL_0, chargeDuty);
        ledc_set_duty(LEDC_HIGH_SPEED_MODE, LEDC_CHANNEL_1, chargeDuty);
        ledc_update_duty(LEDC_HIGH_SPEED_MODE, LEDC_CHANNEL_0);
        ledc_update_duty(LEDC_HIGH_SPEED_MODE, LEDC_CHANNEL_1);
        
        static uint32_t chargeStart = 0;
        if (chargeStart == 0) chargeStart = micros();
        
        if (micros() - chargeStart > 8000) {
          ledc_set_duty(LEDC_HIGH_SPEED_MODE, LEDC_CHANNEL_0, 0);
          ledc_set_duty(LEDC_HIGH_SPEED_MODE, LEDC_CHANNEL_1, 0);
          ledc_update_duty(LEDC_HIGH_SPEED_MODE, LEDC_CHANNEL_0);
          ledc_update_duty(LEDC_HIGH_SPEED_MODE, LEDC_CHANNEL_1);
          chargingCycle = false;
          chargeStart = 0;
        }
      } else {
        ledc_set_duty(LEDC_HIGH_SPEED_MODE, LEDC_CHANNEL_0, 0);
        ledc_set_duty(LEDC_HIGH_SPEED_MODE, LEDC_CHANNEL_1, 0);
        ledc_update_duty(LEDC_HIGH_SPEED_MODE, LEDC_CHANNEL_0);
        ledc_update_duty(LEDC_HIGH_SPEED_MODE, LEDC_CHANNEL_1);
      }
    }
    else {
      ledc_set_duty(LEDC_HIGH_SPEED_MODE, LEDC_CHANNEL_0, 0);
      ledc_set_duty(LEDC_HIGH_SPEED_MODE, LEDC_CHANNEL_1, 0);
      ledc_update_duty(LEDC_HIGH_SPEED_MODE, LEDC_CHANNEL_0);
      ledc_update_duty(LEDC_HIGH_SPEED_MODE, LEDC_CHANNEL_1);
      digitalWrite(PIN_LO_L, LOW);
      digitalWrite(PIN_LO_R, LOW);
    }
    
    // Wait for next SPWM update (80µs for 12500Hz)
    vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(1));
  }
}

// --- Button Handling Functions ---
void handleButtons() {
  // Enter Button
  if (digitalRead(BTN_ENT) == LOW) {
    if (!btnEntPressed) {
      btnEntPressed = true;
      btnEntPressTime = millis();
    } else {
      if (!btnEntLongPress && (millis() - btnEntPressTime > LONG_PRESS_TIME)) {
        btnEntLongPress = true;
        
        if (currentMode != SETTINGS_MODE && currentMode != ERROR_STATE) {
          currentMode = SETTINGS_MODE;
          currentMenu = MAIN_MENU;
          menuItem = 0;
          editingValue = false;
          settingsEnterTime = millis();
          settingsChanged = false;
          
          modulationIndex = 0;
          
          lcd.clear();
          lcd.setCursor(0, 0);
          lcd.print("   SETTINGS MODE   ");
          lcd.setCursor(0, 1);
          lcd.print("Long press Exit    ");
          delay(500);
        } else if (currentMode == SETTINGS_MODE) {
          exitSettingsMode();
        }
      }
    }
  } else {
    if (btnEntPressed) {
      if (!btnEntLongPress) {
        handleEnterShortPress();
      }
      btnEntPressed = false;
      btnEntLongPress = false;
    }
  }

  // Up Button (Note: GPIO34/35 don't have internal pullups)
  if (digitalRead(BTN_UP) == LOW) {
    if (!btnUpPressed && (millis() - btnUpPressTime > DEBOUNCE_TIME)) {
      btnUpPressed = true;
      btnUpPressTime = millis();
      handleUpButton();
    }
  } else {
    btnUpPressed = false;
  }

  // Down Button
  if (digitalRead(BTN_DWN) == LOW) {
    if (!btnDwnPressed && (millis() - btnDwnPressTime > DEBOUNCE_TIME)) {
      btnDwnPressed = true;
      btnDwnPressTime = millis();
      handleDownButton();
    }
  } else {
    btnDwnPressed = false;
  }

  // Power Button
  if (digitalRead(BTN_PWR) == LOW) {
    if (!btnPwrPressed && (millis() - btnPwrPressTime > DEBOUNCE_TIME)) {
      btnPwrPressed = true;
      btnPwrPressTime = millis();
      handlePowerButton();
    }
  } else {
    btnPwrPressed = false;
  }

  // Settings timeout
  if (currentMode == SETTINGS_MODE && (millis() - settingsEnterTime > SETTINGS_TIMEOUT)) {
    exitSettingsMode();
  }
}

void handleEnterShortPress() {
  if (currentMode == SETTINGS_MODE) {
    if (!editingValue) {
      switch (currentMenu) {
        case MAIN_MENU:
          switch (menuItem) {
            case 0: currentMenu = BATTERY_SETTINGS; menuItem = 0; break;
            case 1: currentMenu = AC_SETTINGS; menuItem = 0; break;
            case 2: currentMenu = CHARGING_SETTINGS; menuItem = 0; break;
            case 3: currentMenu = TEMP_SETTINGS; menuItem = 0; break;
            case 4: currentMenu = SYSTEM_SETTINGS; menuItem = 0; break;
            case 5: currentMenu = DISPLAY_SETTINGS; menuItem = 0; break;
            case 6: currentMenu = WIFI_SETTINGS; menuItem = 0; break;
            case 7: currentMenu = SAVE_EXIT; menuItem = 0; break;
          }
          break;
        case SAVE_EXIT:
          if (menuItem == 0) {
            if (settingsChanged) {
              saveSettings();
            }
            exitSettingsMode();
          } else {
            loadSettings();
            exitSettingsMode();
          }
          break;
        default:
          editingValue = true;
          break;
      }
    } else {
      editingValue = false;
      settingsChanged = true;
    }
  } else {
    autoPageChange = !autoPageChange;
    if (!autoPageChange) {
      currentPage = manualPage;
    }
    lastPageChange = millis();
  }
}

void handleUpButton() {
  if (currentMode == SETTINGS_MODE) {
    if (!editingValue) {
      menuItem--;
      if (menuItem < 0) menuItem = maxMenuItems - 1;
    } else {
      editValue(1);
    }
  } else if (!autoPageChange) {
    manualPage = (DisplayPage)((manualPage + 1) % totalPages);
    currentPage = manualPage;
    lastPageChange = millis();
  }
}

void handleDownButton() {
  if (currentMode == SETTINGS_MODE) {
    if (!editingValue) {
      menuItem++;
      if (menuItem >= maxMenuItems) menuItem = 0;
    } else {
      editValue(-1);
    }
  } else if (!autoPageChange) {
    manualPage = (DisplayPage)((manualPage - 1 + totalPages) % totalPages);
    currentPage = manualPage;
    lastPageChange = millis();
  }
}

void handlePowerButton() {
  if (currentMode == SETTINGS_MODE) {
    exitSettingsMode();
  } else if (currentMode == ERROR_STATE) {
    currentMode = STARTUP;
    systemOn = false;
  } else {
    systemOn = !systemOn;
    if (systemOn) {
      currentMode = INVERTER;
    } else {
      currentMode = STARTUP;
      modulationIndex = 0;
    }
    delay(300);
  }
}

void exitSettingsMode() {
  currentMode = STARTUP;
  systemOn = false;
  lcd.clear();
  lcd.setCursor(0, 1);
  lcd.print("  Exiting Settings ");
  delay(1000);
}

void editValue(int direction) {
  switch (currentMenu) {
    case BATTERY_SETTINGS:
      if (menuItem == 0) settings.batLowCut += (direction * 0.1f);
      else settings.batFull += (direction * 0.1f);
      break;
    case AC_SETTINGS:
      if (menuItem == 0) settings.acMin += direction;
      else settings.acMax += direction;
      break;
    case CHARGING_SETTINGS:
      if (menuItem == 0) settings.chargeCurrent += (direction * 0.1f);
      else if (menuItem == 1) settings.bulkVoltage += (direction * 0.1f);
      else settings.floatVoltage += (direction * 0.1f);
      break;
    case TEMP_SETTINGS:
      if (menuItem == 0) settings.fanStartTemp += direction;
      else settings.fanMaxTemp += direction;
      break;
    case SYSTEM_SETTINGS:
      if (menuItem == 0) settings.phaseSyncWindow += (direction * 10);
      else if (menuItem == 1) settings.gridStableCount += direction;
      else if (menuItem == 2) settings.overloadDelay += (direction * 100);
      else settings.efficiencyOffset += (direction * 0.01f);
      break;
    case DISPLAY_SETTINGS:
      if (menuItem == 0) {
        settings.displayTimeout += (direction * 500);
        if (settings.displayTimeout < 1000) settings.displayTimeout = 1000;
        if (settings.displayTimeout > 10000) settings.displayTimeout = 10000;
      }
      break;
    case WIFI_SETTINGS:
      if (menuItem == 0) settings.enableWiFi = !settings.enableWiFi;
      else settings.enableWebInterface = !settings.enableWebInterface;
      break;
  }
}

// --- Display Settings Menu ---
void displaySettingsMenu() {
  lcd.clear();
  char buffer[21];
  
  switch (currentMenu) {
    case MAIN_MENU:
      maxMenuItems = 8;
      lcd.setCursor(0, 0);
      lcd.print(">>MAIN MENU<<      ");
      lcd.setCursor(0, 1);
      lcd.print(menuItem == 0 ? "> " : "  ");
      lcd.print("Battery Settings");
      lcd.setCursor(0, 2);
      lcd.print(menuItem == 1 ? "> " : "  ");
      lcd.print("AC Settings     ");
      lcd.setCursor(0, 3);
      lcd.print(menuItem == 2 ? "> " : "  ");
      lcd.print("Charging Settings");
      break;
      
    case BATTERY_SETTINGS:
      maxMenuItems = 2;
      lcd.setCursor(0, 0);
      lcd.print(">>BATTERY SETTINGS<<");
      lcd.setCursor(0, 1);
      snprintf(buffer, sizeof(buffer), "%sLow Cut: %.1fV   ", menuItem == 0 ? "> " : "  ", settings.batLowCut);
      lcd.print(buffer);
      lcd.setCursor(0, 2);
      snprintf(buffer, sizeof(buffer), "%sFull:    %.1fV   ", menuItem == 1 ? "> " : "  ", settings.batFull);
      lcd.print(buffer);
      lcd.setCursor(0, 3);
      lcd.print(editingValue ? "Edit Mode - Use U/D" : "Press ENT to edit ");
      break;
      
    case WIFI_SETTINGS:
      maxMenuItems = 2;
      lcd.setCursor(0, 0);
      lcd.print(">>WIFI SETTINGS<<  ");
      lcd.setCursor(0, 1);
      snprintf(buffer, sizeof(buffer), "%sWiFi: %s", menuItem == 0 ? "> " : "  ", settings.enableWiFi ? "ENABLED " : "DISABLED");
      lcd.print(buffer);
      lcd.setCursor(0, 2);
      snprintf(buffer, sizeof(buffer), "%sWeb:  %s", menuItem == 1 ? "> " : "  ", settings.enableWebInterface ? "ENABLED " : "DISABLED");
      lcd.print(buffer);
      lcd.setCursor(0, 3);
      lcd.print(editingValue ? "Toggle with U/D   " : "Press ENT to edit ");
      break;
      
    case SAVE_EXIT:
      maxMenuItems = 2;
      lcd.setCursor(0, 0);
      lcd.print(">>SAVE & EXIT<<    ");
      lcd.setCursor(0, 1);
      lcd.print(menuItem == 0 ? "> " : "  ");
      lcd.print("Save & Exit       ");
      lcd.setCursor(0, 2);
      lcd.print(menuItem == 1 ? "> " : "  ");
      lcd.print("Exit Without Save ");
      lcd.setCursor(0, 3);
      lcd.print("Long press to exit");
      break;
      
    // Other menu cases similar...
  }
}

// --- Update Display Pages ---
void updateDisplayPage() {
  lcd.clear();
  char buffer[21];
  
  switch (currentPage) {
    case PAGE_MAIN:
      lcd.setCursor(0, 0);
      if (currentMode == INVERTER) {
        snprintf(buffer, sizeof(buffer), "INV:ON MOD:%2.0f%% ", modulationIndex * 100);
        lcd.print(buffer);
      } else if (currentMode == CHARGING) {
        snprintf(buffer, sizeof(buffer), "CHG:%s D:%2.0f%% ", 
                chgStage == BULK ? "BULK" : chgStage == ABSORPTION ? "ABS " : "FLT ",
                modulationIndex * 100);
        lcd.print(buffer);
      } else if (currentMode == GRID_BYPASS) {
        lcd.print("GRID:BYPASS       ");
      } else if (currentMode == SYNCHRONIZING) {
        snprintf(buffer, sizeof(buffer), "SYNC:GRID %4luuS", phaseError);
        lcd.print(buffer);
      } else if (currentMode == ERROR_STATE) {
        snprintf(buffer, sizeof(buffer), "ERR:%-13s", errorMsg);
        lcd.print(buffer);
      } else {
        lcd.print("STANDBY           ");
      }
      
      lcd.setCursor(0, 1);
      snprintf(buffer, sizeof(buffer), "OUT:%3.0fV %4.0fW %3.1fA", acOutVolts, loadWatts, currentAmps);
      lcd.print(buffer);
      
      lcd.setCursor(0, 2);
      if (currentMode == INVERTER) {
        battPower = batVolts * currentAmps;
        snprintf(buffer, sizeof(buffer), "BAT:%4.1fV %4.0fW ", batVolts, battPower);
      } else if (currentMode == CHARGING) {
        snprintf(buffer, sizeof(buffer), "BAT:%4.1fV CHG:%3.1fA", batVolts, currentAmps);
      } else {
        snprintf(buffer, sizeof(buffer), "BAT:%4.1fV ---", batVolts);
      }
      lcd.print(buffer);
      
      lcd.setCursor(0, 3);
      snprintf(buffer, sizeof(buffer), "TEMP:%2dC IN:%3.0fV%s", tempC, acInVolts, !autoPageChange ? " M1" : "");
      lcd.print(buffer);
      break;
      
    case PAGE_DETAILED:
      lcd.setCursor(0, 0);
      lcd.print("DETAILED MEASURE  ");
      
      lcd.setCursor(0, 1);
      snprintf(buffer, sizeof(buffer), "Vrms:%4.1fV PF:%4.2f", acOutVolts, powerFactor);
      lcd.print(buffer);
      
      lcd.setCursor(0, 2);
      snprintf(buffer, sizeof(buffer), "VA:%4.0f Freq:%4.1fHz", loadVA, gridFreq);
      lcd.print(buffer);
      
      lcd.setCursor(0, 3);
      snprintf(buffer, sizeof(buffer), "EFF:%3.0f%% GRID:%s%s", 
              efficiency * 100, gridStable ? "OK" : "--", !autoPageChange ? " M2" : "");
      lcd.print(buffer);
      break;
      
    case PAGE_STATS:
      lcd.setCursor(0, 0);
      lcd.print("SYSTEM STATISTICS ");
      
      lcd.setCursor(0, 1);
      snprintf(buffer, sizeof(buffer), "BAT:%4.1f-%4.1fV", minBattVoltage, maxBattVoltage);
      lcd.print(buffer);
      
      lcd.setCursor(0, 2);
      snprintf(buffer, sizeof(buffer), "LOAD:%4.0fW TEMP:%2.0fC", maxLoadWatts, maxTemp);
      lcd.print(buffer);
      
      lcd.setCursor(0, 3);
      snprintf(buffer, sizeof(buffer), "ENERGY:%5.1fkWh%s", dailyEnergy, !autoPageChange ? " M3" : "");
      lcd.print(buffer);
      break;
      
    case PAGE_SYSTEM:
      lcd.setCursor(0, 0);
      lcd.print("SYSTEM INFORMATION");
      
      lcd.setCursor(0, 1);
      if (uptimeHours > 0) {
        snprintf(buffer, sizeof(buffer), "UPTIME:%luh %lum", uptimeHours, uptimeMinutes);
      } else {
        snprintf(buffer, sizeof(buffer), "UPTIME:%lum", uptimeMinutes);
      }
      lcd.print(buffer);
      
      lcd.setCursor(0, 2);
      snprintf(buffer, sizeof(buffer), "INV RT:%lum CHG:%lum", 
              inverterRuntime / 1000 / 60, chargeTime / 1000 / 60);
      lcd.print(buffer);
      
      lcd.setCursor(0, 3);
      snprintf(buffer, sizeof(buffer), "PAGE:%d/%d MOD:%2.0f%%%s", 
              currentPage + 1, totalPages, modulationIndex * 100, 
              !autoPageChange ? " M4" : "");
      lcd.print(buffer);
      break;
  }
}

// --- Update Statistics ---
void updateStatistics() {
  static uint32_t lastStatUpdate = 0;
  
  if (millis() - lastStatUpdate > 1000) {
    lastStatUpdate = millis();
    
    // Thread-safe access to measurements
    portENTER_CRITICAL(&measurementMutex);
    float localBatVolts = batVolts;
    float localLoadWatts = loadWatts;
    float localTempC = tempC;
    portEXIT_CRITICAL(&measurementMutex);
    
    if (localBatVolts < minBattVoltage) minBattVoltage = localBatVolts;
    if (localBatVolts > maxBattVoltage) maxBattVoltage = localBatVolts;
    if (localLoadWatts > maxLoadWatts) maxLoadWatts = localLoadWatts;
    if (localTempC > maxTemp) maxTemp = localTempC;
    
    if (startupTime > 0) {
      uint32_t uptimeSeconds = (millis() - startupTime) / 1000;
      uptimeHours = uptimeSeconds / 3600;
      uptimeMinutes = (uptimeSeconds % 3600) / 60;
      
      if (currentMode == INVERTER) inverterRuntime += 1000;
      if (currentMode == CHARGING) chargeTime += 1000;
    }
    
    if (lastEnergyUpdate > 0) {
      float hoursPassed = (millis() - lastEnergyUpdate) / 3600000.0f;
      portENTER_CRITICAL(&measurementMutex);
      dailyEnergy += (loadWatts * hoursPassed) / 1000.0f;
      portEXIT_CRITICAL(&measurementMutex);
    }
    lastEnergyUpdate = millis();
    
    // Calculate power factor and efficiency
    portENTER_CRITICAL(&measurementMutex);
    loadVA = acOutVolts * currentAmps;
    if (loadVA > 0) {
      powerFactor = loadWatts / loadVA;
      if (powerFactor > 1.0f) powerFactor = 1.0f;
    }
    
    if (currentMode == INVERTER && loadWatts > 0) {
      efficiency = (loadWatts / (batVolts * currentAmps)) * settings.efficiencyOffset;
      if (efficiency > 1.0f) efficiency = 1.0f;
    }
    portEXIT_CRITICAL(&measurementMutex);
  }
}

// --- Sensor Reading ---
void readSensors() {
  static uint32_t lastSensorRead = 0;
  
  if (millis() - lastSensorRead > 100) {
    lastSensorRead = millis();
    
    // Read analog values (12-bit ADC, 0-4095)
    static float batVoltsSum = 0;
    static int batReadCount = 0;
    
    // Note: ESP32 ADC has non-linear characteristics, may need calibration
    batVoltsSum += analogReadMilliVolts(SENS_BAT) * (20.0f / 3300.0f); // Using mV for better accuracy
    batReadCount++;
    
    if (batReadCount >= 5) {
      portENTER_CRITICAL(&measurementMutex);
      batVolts = batVoltsSum / 5;
      portEXIT_CRITICAL(&measurementMutex);
      batVoltsSum = 0;
      batReadCount = 0;
    }
  
    // Temperature reading (simplified)
    int tempRaw = analogRead(SENS_NTC);
    portENTER_CRITICAL(&measurementMutex);
    tempC = map(tempRaw, 0, 4095, 0, 100);
    portEXIT_CRITICAL(&measurementMutex);

    // AC voltages
    portENTER_CRITICAL(&measurementMutex);
    acInVolts = analogReadMilliVolts(SENS_AC_IN) * (350.0f / 3300.0f);
    acOutVolts = analogReadMilliVolts(SENS_FB) * (350.0f / 3300.0f);
    
    // Current measurement (2.5V offset = 2048 in 12-bit)
    int iRaw = analogRead(SENS_CT) - 2048;
    currentAmps = (abs(iRaw) * (50.0f / 2048.0f)) * 0.707f;
    loadWatts = acOutVolts * currentAmps;
    portEXIT_CRITICAL(&measurementMutex);
  }
}

// --- Logic Task (Runs on Core 1) ---
void LogicTask(void *parameter) {
  // Lower priority task for UI and logic
  vTaskPrioritySet(NULL, 1);
  
  const TickType_t xFrequency = 10; // 10ms base
  TickType_t xLastWakeTime = xTaskGetTickCount();
  
  while (1) {
    readSensors();
    handleButtons();
    updateStatistics();
    
    if (currentMode == SETTINGS_MODE) {
      static uint32_t lastMenuUpdate = 0;
      if (millis() - lastMenuUpdate > 200) {
        displaySettingsMenu();
        lastMenuUpdate = millis();
      }
    } else {
      handleFan();
      stateMachine();
      
      if (millis() - lastUpdate > 500) {
        if (autoPageChange) {
          if (millis() - lastPageChange > settings.displayTimeout) {
            currentPage = (DisplayPage)((currentPage + 1) % totalPages);
            lastPageChange = millis();
          }
        }
        updateDisplayPage();
        lastUpdate = millis();
      }
    }
    
    checkGridSync();
    vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(10));
  }
}

// --- Setup Function ---
void setup() {
  Serial.begin(115200);
  Serial.println("ESP32 Inverter/UPS Starting...");
  
  // Initialize I2C for LCD
  Wire.begin(I2C_SDA, I2C_SCL);
  Wire.setClock(400000); // Fast I2C
  
  // Initialize LCD
  lcd.begin(20,4);
  lcd.backlight();
  lcd.setCursor(0,0);
  lcd.print("   ESP32 Inverter   ");
  lcd.setCursor(0,1);
  lcd.print("  Dual-Core System  ");
  lcd.setCursor(0,2);
  lcd.print("  SPWM: Core 0      ");
  lcd.setCursor(0,3);
  lcd.print("  Logic: Core 1     ");
  delay(2000);
  
  // Pin Setup
  pinMode(PIN_LO_L, OUTPUT);
  pinMode(PIN_LO_R, OUTPUT);
  pinMode(PIN_RELAY, OUTPUT);
  pinMode(PIN_BUZZER, OUTPUT);
  pinMode(PIN_ZC, INPUT_PULLUP);
  
  // Button Pins (note: GPIO34/35 don't have internal pullups)
  pinMode(BTN_PWR, INPUT_PULLUP);
  pinMode(BTN_ENT, INPUT_PULLUP);
  pinMode(BTN_UP, INPUT);
  pinMode(BTN_DWN, INPUT);
  
  // Attach interrupt for zero crossing
  attachInterrupt(digitalPinToInterrupt(PIN_ZC), zeroCrossingISR, FALLING);
  
  // Load settings
  loadSettings();
  
  // Initialize statistics
  portENTER_CRITICAL(&measurementMutex);
  minBattVoltage = batVolts;
  maxBattVoltage = batVolts;
  portEXIT_CRITICAL(&measurementMutex);
  
  startupTime = millis();
  lastEnergyUpdate = millis();
  
  // Generate sine table
  generateSineTable();
  
  // Setup PWM and ADC
  setupPWM();
  setupADC();
  
  // Create FreeRTOS tasks
  xTaskCreatePinnedToCore(
    SPWMTask,        // Task function
    "SPWM Task",     // Task name
    4096,            // Stack size
    NULL,            // Parameters
    3,               // Priority (higher)
    &SPWMTaskHandle, // Task handle
    CORE_0           // Core 0 for timing-critical tasks
  );
  
  xTaskCreatePinnedToCore(
    LogicTask,       // Task function
    "Logic Task",    // Task name
    8192,            // Stack size (larger for UI)
    NULL,            // Parameters
    1,               // Priority (lower)
    &LogicTaskHandle,// Task handle
    CORE_1           // Core 1 for UI and logic
  );
  
  lcd.clear();
  lcd.print("System Ready");
  Serial.println("System initialized on both cores");
}

// --- Main Loop (not used with FreeRTOS) ---
void loop() {
  // Empty - all work is done in tasks
  vTaskDelay(pdMS_TO_TICKS(1000)); // Minimal delay to prevent watchdog
}

// --- Other Functions ---
void checkGridSync() {
  static uint32_t lastCheck = 0;
  
  if (millis() - lastCheck > 1000 && currentMode != SETTINGS_MODE) {
    lastCheck = millis();
    
    if (gridStable && halfPeriod > 0) {
      if (currentMode == SYNCHRONIZING) {
        synchronizePhase();
      }
    } else {
      gridStable = false;
    }
  }
}

void synchronizePhase() {
  if (phaseError > 100) {
    if (micros() - zcTime < halfPeriod / 2) {
      int targetIndex = map(micros() - zcTime, 0, halfPeriod, 0, 250);
      
      if (abs(spwmIndex - targetIndex) > 2) {
        if (spwmIndex < targetIndex) spwmIndex++;
        else if (spwmIndex > targetIndex) spwmIndex--;
        
        cyclePositive = zcPolarity;
      }
    }
    
    if (phaseError < 50) {
      currentMode = GRID_BYPASS;
      digitalWrite(PIN_RELAY, HIGH);
      modulationIndex = 0;
    }
  }
}

void stateMachine() {
  // Safety checks
  portENTER_CRITICAL(&measurementMutex);
  float localTempC = tempC;
  float localLoadWatts = loadWatts;
  float localBatVolts = batVolts;
  float localAcInVolts = acInVolts;
  portEXIT_CRITICAL(&measurementMutex);
  
  if (localTempC > 75) {
    currentMode = ERROR_STATE;
    strncpy(errorMsg, "OVER HEAT", sizeof(errorMsg)-1);
  }
  
  static uint32_t overloadStart = 0;
  if (localLoadWatts > 6100) {
    if (overloadStart == 0) {
      overloadStart = millis();
    } else if (millis() - overloadStart > settings.overloadDelay) {
      currentMode = ERROR_STATE;
      strncpy(errorMsg, "OVERLOAD", sizeof(errorMsg)-1);
      overloadStart = 0;
    }
  } else {
    overloadStart = 0;
  }

  switch (currentMode) {
    case STARTUP:
      digitalWrite(PIN_RELAY, LOW);
      if (systemOn) currentMode = INVERTER;
      break;

    case INVERTER:
      digitalWrite(PIN_RELAY, LOW);
      
      if (localAcInVolts >= settings.acMin && localAcInVolts <= settings.acMax && gridStable) {
        modulationIndex = targetModulation;
        currentMode = SYNCHRONIZING;
        return;
      }
      
      if (localBatVolts < settings.batLowCut) {
        currentMode = ERROR_STATE;
        strncpy(errorMsg, "LOW BATT", sizeof(errorMsg)-1);
        return;
      }

      if (modulationIndex < targetModulation) modulationIndex += 0.01f;
      
      portENTER_CRITICAL(&measurementMutex);
      if (acOutVolts < 230) targetModulation += 0.001f;
      if (acOutVolts > 232) targetModulation -= 0.001f;
      portEXIT_CRITICAL(&measurementMutex);
      
      if (targetModulation > 0.95f) targetModulation = 0.95f;
      break;

    case SYNCHRONIZING:
      static uint32_t syncStart = 0;
      if (syncStart == 0) syncStart = millis();
      
      if (millis() - syncStart > 5000) {
        currentMode = INVERTER;
        syncStart = 0;
      }
      break;

    case GRID_BYPASS:
      digitalWrite(PIN_RELAY, HIGH);
      
      if (!gridStable || localAcInVolts < settings.acMin || localAcInVolts > settings.acMax) {
         digitalWrite(PIN_RELAY, LOW);
         currentMode = INVERTER;
         modulationIndex = 0;
         return;
      }
      
      if (localBatVolts < settings.batFull) {
        currentMode = CHARGING;
      }
      break;

    case CHARGING:
      digitalWrite(PIN_RELAY, HIGH);
      
      if (!gridStable) {
        currentMode = INVERTER;
        chgStage = BULK;
        return;
      }
      
      if (chgStage == BULK) {
        if (localBatVolts >= settings.bulkVoltage) chgStage = ABSORPTION;
        portENTER_CRITICAL(&measurementMutex);
        if (currentAmps < settings.chargeCurrent) modulationIndex += 0.001f;
        else modulationIndex -= 0.001f;
        portEXIT_CRITICAL(&measurementMutex);
      }
      else if (chgStage == ABSORPTION) {
        portENTER_CRITICAL(&measurementMutex);
        if (currentAmps < 1.0f) chgStage = FLOAT;
        if (localBatVolts > settings.batFull) modulationIndex -= 0.001f;
        else modulationIndex += 0.001f;
        portEXIT_CRITICAL(&measurementMutex);
      }
      else {
        if (localBatVolts > settings.floatVoltage) modulationIndex -= 0.001f;
        else modulationIndex += 0.001f;
      }
      
      if (modulationIndex > 0.4f) modulationIndex = 0.4f;
      if (modulationIndex < 0) modulationIndex = 0;

      if (!gridStable) currentMode = INVERTER;
      break;

    case ERROR_STATE:
      modulationIndex = 0;
      static uint32_t lastBeep = 0;
      if (millis() - lastBeep > 1000) {
        digitalWrite(PIN_BUZZER, HIGH);
        delay(100);
        digitalWrite(PIN_BUZZER, LOW);
        lastBeep = millis();
      }
      break;
  }
}

void handleFan() {
  portENTER_CRITICAL(&measurementMutex);
  int localTempC = tempC;
  portEXIT_CRITICAL(&measurementMutex);
  
  if (localTempC > settings.fanStartTemp) {
    int fanSpeed = map(localTempC, settings.fanStartTemp, settings.fanMaxTemp, 100, 255);
    int fanDuty = map(fanSpeed, 0, 255, 0, PWM_DUTY_MAX);
    ledc_set_duty(LEDC_HIGH_SPEED_MODE, LEDC_CHANNEL_2, fanDuty);
    ledc_update_duty(LEDC_HIGH_SPEED_MODE, LEDC_CHANNEL_2);
  } else {
    ledc_set_duty(LEDC_HIGH_SPEED_MODE, LEDC_CHANNEL_2, 0);
    ledc_update_duty(LEDC_HIGH_SPEED_MODE, LEDC_CHANNEL_2);
  }
}