/**
 * LF Inverter/UPS Firmware - Arduino Nano Based
 * 50Hz Pure Sine Wave Inverter - 4 Pin Version
 * Inverter Mode: 25.0 kHz Carrier
 * Charging Mode: 20.0 kHz Carrier (Reduced switching losses)
 * Logic: Unipolar Switching (Pins 9, 10 PWM | Pins 7, 8 Steering)
 * Auto-Start with integrated charging control
 * Author: MNG proSystems South Africa
 * Email: [email protected] 
 * Firmware: V1.2.12 (Integrated Charger) - Non-Blocking
 */

#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <math.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>

// ================= PINS CONFIGURATION =================
// SPWM High Side (Timer1 PWM)
#define PIN_HI_LEFT  9   // OC1A (PWM)
#define PIN_HI_RIGHT 10  // OC1B (PWM)

// Low Side (50Hz Steering)
#define PIN_LO_LEFT  7   // Steering
#define PIN_LO_RIGHT 8   // Steering

// Feedback & Sensors
#define PIN_BAT_V    A2  // Batt Sens
#define PIN_AC_OUT   A6  // AC output Sens
#define PIN_SHUNT    A1  // Current Sensor
#define PIN_NTC      A3  // Temperature Sensor (10k NTC)
#define PIN_AC_IN    A0  // AC in Input Sens
#define PIN_ZC       2   // Zero Cross Detection
#define PIN_RELAY    4   // Transfer Relay
#define PIN_FAN      6   // PWM Fan
#define PIN_BUZZER   5   // Buzzer

// ================= BUTTON PIN =================
#define PIN_INVERTER_BUTTON 11  // Digital input for Inverter On/Off button

// ================= SPWM CONSTANTS =================
#define SYS_FREQ  16000000

// Inverter Mode: 25kHz
#define INV_FREQ   25000
#define INV_ICR    639  // TOP value for 25kHz PWM: (16MHz / 25kHz) / 2 = 320, but using 639 for better resolution

// Charging Mode: 20kHz (reduced switching losses)
#define CHG_FREQ   20000
#define CHG_ICR    799  // TOP value for 20kHz PWM: (16MHz / 20kHz) / 2 = 400, but using 799 for better resolution

// Current carrier frequency
volatile uint16_t currentICR = INV_ICR;
volatile bool isChargingMode = false;

// 100 samples for a half-sine wave (scaled appropriately)
const int sineTable[] PROGMEM = {
  0, 20, 40, 60, 80, 100, 120, 139, 159, 178, 197, 216, 235, 253, 271, 289, 
  307, 324, 341, 357, 373, 389, 404, 419, 434, 448, 461, 475, 487, 500, 511, 522, 
  533, 543, 552, 561, 569, 577, 584, 591, 597, 603, 608, 612, 616, 620, 623, 625, 
  627, 628, 629, 628, 627, 625, 623, 620, 616, 612, 608, 603, 597, 591, 584, 577, 
  569, 561, 552, 543, 533, 522, 511, 500, 487, 475, 461, 448, 434, 419, 404, 389, 
  373, 357, 341, 324, 307, 289, 271, 253, 235, 216, 197, 178, 159, 139, 120, 100, 
  80, 60, 40, 20
};

const int SINE_STEPS = 200; // 100 samples per half-cycle * 2

// ================= SYSTEM VARIABLES =================
enum ChargeStage { BULK, ABSORPTION, FLOAT, OFF };
ChargeStage chargeState = OFF;

enum SysState { ST_CHECK_START, ST_SOFT_START, ST_INVERTER, ST_GRID_WAIT, ST_UPS_CHARGE, ST_ERROR, ST_MANUAL_OFF };
SysState currentState = ST_CHECK_START;

// SPWM Variables
volatile int sineIndex = 0;
volatile bool spwmEnabled = false;
volatile float modulationIndex = 0.0;

// Global Variables
volatile bool syncPulse = false;
volatile unsigned long lastZCTime = 0;

float batVolts = 0.0;
float acOutVolts = 0.0;
float acInVolts = 0.0;
float loadAmps = 0.0;
float loadWatts = 0.0;
float powerFactor = 0.95;
float temperature = 0.0;

// Charging Parameters
const float BULK_VOLTAGE = 14.4;
const float ABSORPTION_VOLTAGE = 14.4;
const float FLOAT_VOLTAGE = 13.8;

bool chargingActive = false;

// Button handling
bool buttonPressed = false;
bool inverterEnabled = true;  // System starts enabled
unsigned long lastButtonCheck = 0;
unsigned long buttonPressTime = 0;
const unsigned long BUTTON_DEBOUNCE_TIME = 50;
const unsigned long BUTTON_LONG_PRESS_TIME = 2000;  // 2 seconds for long press

// State machine timers (non-blocking)
unsigned long stateTimer = 0;
unsigned long softStartTimer = 0;
const unsigned long GRID_DEBOUNCE_TIME = 1000;
const unsigned long SOFT_START_INTERVAL = 20;

// Settings
const float BAT_LOW_CUTOFF = 10.5;
const int AC_MIN = 200;
const int AC_MAX = 250;
const int MAX_LOAD_WATTS = 7000;

LiquidCrystal_I2C lcd(0x27, 20, 4);
unsigned long lastScreenUpdate = 0;

// ================= SETUP =================
void setup() {
  Serial.begin(115200);
  
  // Pin Setup
  pinMode(PIN_HI_LEFT, OUTPUT);   // High Side A (PWM)
  pinMode(PIN_HI_RIGHT, OUTPUT);  // High Side B (PWM)
  pinMode(PIN_LO_LEFT, OUTPUT);   // Low Side A (Steer)
  pinMode(PIN_LO_RIGHT, OUTPUT);  // Low Side B (Steer)
  pinMode(PIN_RELAY, OUTPUT);
  pinMode(PIN_FAN, OUTPUT);
  pinMode(PIN_BUZZER, OUTPUT);
  pinMode(PIN_ZC, INPUT); 
  
  // Button Pin Setup
  pinMode(PIN_INVERTER_BUTTON, INPUT_PULLUP);

  // Ensure all outputs are LOW initially
  digitalWrite(PIN_HI_LEFT, LOW);
  digitalWrite(PIN_HI_RIGHT, LOW);
  digitalWrite(PIN_LO_LEFT, LOW);
  digitalWrite(PIN_LO_RIGHT, LOW);
  digitalWrite(PIN_RELAY, LOW);
  digitalWrite(PIN_FAN, LOW);
  digitalWrite(PIN_BUZZER, LOW);  // Ensure buzzer is OFF initially

  lcd.begin(20,4);
  lcd.backlight();
  lcd.setCursor(0,1);
  lcd.print("SYSTEM STARTING     ");
  delay(500);
  lcd.setCursor(0,1);
  lcd.print("SYSTEM STARTING.    ");
  delay(500);
  lcd.setCursor(0,1);
  lcd.print("SYSTEM STARTING..   ");
  delay(500);
  lcd.setCursor(0,1);
  lcd.print("SYSTEM STARTING...  ");
  delay(500);
  lcd.setCursor(0,1);
  lcd.print("SYSTEM STARTING.... ");
  delay(500);
  lcd.setCursor(0,1);
  lcd.print("SYSTEM STARTING.....");
  
  unsigned long setupStart = millis();
  while (millis() - setupStart < 1000) {}
  
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("   MNG proSystems   ");
  lcd.setCursor(0,1);
  lcd.print("      INV/UPS       ");
  lcd.setCursor(0,2);
  lcd.print("  Firmware V1.2.12  ");
  lcd.setCursor(0,3);
  lcd.print("--------------------");
  
  setupStart = millis();
  while (millis() - setupStart < 3000) {}
  
  lcd.clear();

  // Initialize SPWM Timer (start in inverter mode - 25kHz)
  initSPWMTimer(false);  // false = inverter mode (25kHz)
  
  attachInterrupt(digitalPinToInterrupt(PIN_ZC), zeroCrossISR, RISING);

  // Startup beep (temporary - will turn off automatically)
  startupBeep();
  
  stateTimer = millis();
  softStartTimer = millis();
}

// ================= MAIN LOOP =================
void loop() {
  readSensors();
  thermalControl();
  checkButton();
  stateMachine();
  manageCharging();
  updateLCD();
  
  // Ensure buzzer is off during normal operation
  // Buzzer only ON during ST_ERROR state
  if (currentState != ST_ERROR) {
    digitalWrite(PIN_BUZZER, LOW);
  }
  
  static unsigned long lastYield = 0;
  if (millis() - lastYield > 100) {
    lastYield = millis();
  }
}

// ================= BUTTON HANDLING =================
void checkButton() {
  static bool lastButtonState = HIGH;
  static unsigned long lastDebounceTime = 0;
  
  bool reading = digitalRead(PIN_INVERTER_BUTTON);
  
  // Check for state change
  if (reading != lastButtonState) {
    lastDebounceTime = millis();
  }
  
  if ((millis() - lastDebounceTime) > BUTTON_DEBOUNCE_TIME) {
    // Button state is stable
    if (reading == LOW && !buttonPressed) {
      // Button just pressed
      buttonPressed = true;
      buttonPressTime = millis();
      Serial.println("Button pressed");
    }
    else if (reading == HIGH && buttonPressed) {
      // Button just released
      buttonPressed = false;
      unsigned long pressDuration = millis() - buttonPressTime;
      
      if (pressDuration < BUTTON_LONG_PRESS_TIME) {
        // Short press - toggle inverter
        toggleInverter();
      } else {
        // Long press - emergency stop
        emergencyStop();
      }
    }
  }
  
  lastButtonState = reading;
  
  // Check for long press while button is still pressed
  if (buttonPressed && (millis() - buttonPressTime > BUTTON_LONG_PRESS_TIME)) {
    // Long press detected (button still pressed)
    // Visual feedback for long press
    static unsigned long lastBlink = 0;
    if (millis() - lastBlink > 200) {
      lastBlink = millis();
      digitalWrite(PIN_BUZZER, !digitalRead(PIN_BUZZER));
    }
  }
}

void toggleInverter() {
  Serial.println("Button: Toggle Inverter");
  
  if (currentState == ST_MANUAL_OFF) {
    // Turn ON from manual off state
    if (acInVolts >= AC_MIN && acInVolts <= AC_MAX) {
      // Grid is present, go to grid mode
      currentState = ST_GRID_WAIT;
      stateTimer = millis();
      Serial.println("Manual ON -> Grid mode");
    } else {
      // No grid, start inverter
      currentState = ST_SOFT_START;
      enableSPWM(false);  // Inverter mode (25kHz)
      setSPWMAmplitude(0.0);
      softStartTimer = millis();
      Serial.println("Manual ON -> Inverter mode");
    }
  } else if (currentState != ST_ERROR) {
    // Turn OFF if not already off and not in error state
    currentState = ST_MANUAL_OFF;
    disableSPWM();
    stopCharging();
    digitalWrite(PIN_RELAY, LOW);
    Serial.println("Manual OFF");
  }
  
  // Short beep to acknowledge button press
  digitalWrite(PIN_BUZZER, HIGH);
  delay(50);
  digitalWrite(PIN_BUZZER, LOW);
}

void emergencyStop() {
  Serial.println("Button: Emergency Stop (Long Press)");
  
  // Always go to manual off regardless of current state
  currentState = ST_MANUAL_OFF;
  disableSPWM();
  stopCharging();
  digitalWrite(PIN_RELAY, LOW);
  
  // Triple beep for emergency stop
  for (int i = 0; i < 3; i++) {
    digitalWrite(PIN_BUZZER, HIGH);
    delay(100);
    digitalWrite(PIN_BUZZER, LOW);
    delay(50);
  }
  
  Serial.println("Emergency STOP activated");
}

// ================= SPWM ENGINE =================
void initSPWMTimer(bool chargingMode) {
  // Store mode
  isChargingMode = chargingMode;
  
  if (chargingMode) {
    // Charging Mode: 20kHz
    currentICR = CHG_ICR;
    Serial.println("SPWM: Charging Mode (20kHz)");
  } else {
    // Inverter Mode: 25kHz
    currentICR = INV_ICR;
    Serial.println("SPWM: Inverter Mode (25kHz)");
  }
  
  // --- Timer 1: PWM Generation ---
  // Disable interrupts while reconfiguring timer
  TIMSK1 = 0;
  
  // Configure Timer1 for Fast PWM, Mode 14, Prescaler 1
  TCCR1A = _BV(COM1A1) | _BV(COM1B1) | _BV(WGM11); 
  TCCR1B = _BV(WGM13) | _BV(WGM12) | _BV(CS10);
  
  // Set TOP value for desired frequency
  ICR1 = currentICR;
  
  // Start with 0 duty cycle
  OCR1A = 0;
  OCR1B = 0;
  
  // --- Timer 2: 10kHz Timing for 50Hz Sine ---
  // (50Hz * 200 updates per cycle = 10,000 updates per sec)
  TCCR2A = _BV(WGM21); // CTC Mode
  TCCR2B = _BV(CS21);  // Prescaler 8
  OCR2A = 199;         // (16MHz / (8 * 10,000)) - 1 = 199
  TIMSK2 = _BV(OCIE2A); // Enable interrupt
  
  // Initially disable SPWM output
  disableSPWM();
}

ISR(TIMER2_COMPA_vect) {
  if (!spwmEnabled) {
    // SPWM disabled, set all outputs LOW
    digitalWrite(PIN_LO_LEFT, LOW);
    digitalWrite(PIN_LO_RIGHT, LOW);
    OCR1A = 0;
    OCR1B = 0;
    return;
  }
  
  // The table contains 100 samples (half cycle)
  // We use the same table for both halves to save memory
  int val;
  
  // Scale factor based on current ICR value
  float scaleFactor;
  if (isChargingMode) {
    // Charging mode: scale for 799 max (20kHz)
    scaleFactor = 799.0 / 639.0;  // Scale from 639 base to 799
  } else {
    // Inverter mode: scale for 639 max (25kHz)
    scaleFactor = 1.0;
  }
  
  if (sineIndex < 100) {
    // POSITIVE HALF CYCLE
    val = pgm_read_word(&sineTable[sineIndex]);
    val = (int)(val * scaleFactor);  // Scale for current frequency
    val = constrain(val, 0, currentICR);  // Ensure within bounds
    
    digitalWrite(PIN_LO_LEFT, LOW);   
    digitalWrite(PIN_LO_RIGHT, HIGH);  // Ground through Low-B
    OCR1A = (int)(val * modulationIndex);  // PWM on High-A with modulation
    OCR1B = 0;
  } else {
    // NEGATIVE HALF CYCLE
    val = pgm_read_word(&sineTable[sineIndex - 100]);
    val = (int)(val * scaleFactor);  // Scale for current frequency
    val = constrain(val, 0, currentICR);  // Ensure within bounds
    
    digitalWrite(PIN_LO_RIGHT, LOW);   
    digitalWrite(PIN_LO_LEFT, HIGH);   // Ground through Low-A
    OCR1A = 0;
    OCR1B = (int)(val * modulationIndex);  // PWM on High-B with modulation
  }

  sineIndex++;
  if (sineIndex >= 200) sineIndex = 0;
}

void enableSPWM(bool chargingMode) {
  // Switch frequency if mode changed
  if (isChargingMode != chargingMode) {
    initSPWMTimer(chargingMode);
  }
  
  spwmEnabled = true;
  modulationIndex = 0.0;  // Start from 0 for soft start
  sineIndex = 0;
  
  // Ensure Timer 1 outputs are enabled
  TCCR1A |= (_BV(COM1A1) | _BV(COM1B1));
}

void disableSPWM() {
  spwmEnabled = false;
  
  // Disable Timer 1 outputs
  TCCR1A &= ~(_BV(COM1A1) | _BV(COM1B1));
  
  // Set all outputs LOW
  digitalWrite(PIN_HI_LEFT, LOW);
  digitalWrite(PIN_HI_RIGHT, LOW);
  digitalWrite(PIN_LO_LEFT, LOW);
  digitalWrite(PIN_LO_RIGHT, LOW);
  OCR1A = 0;
  OCR1B = 0;
}

void setSPWMAmplitude(float amplitude) {
  modulationIndex = constrain(amplitude, 0.0, 1.0);
}

// ================= SIMPLIFIED CHARGING LOGIC =================
void manageCharging() {
  // Only charge when in UPS/Grid mode
  if (currentState != ST_UPS_CHARGE) {
    if (chargingActive) {
      stopCharging();
    }
    return;
  }
  
  // Check if AC is present and valid
  bool validAC = (acInVolts >= AC_MIN && acInVolts <= AC_MAX);
  
  // Start charging when AC is detected
  if (!chargingActive && validAC) {
    startCharging();
  }
  
  // Stop charging if AC is lost
  if (chargingActive && !validAC) {
    stopCharging();
    return;
  }
  
  // Manage charging stages based on voltage only
  if (chargingActive) {
    switch (chargeState) {
      case BULK:
        // Stay in Bulk until battery reaches 14.4V
        if (batVolts >= BULK_VOLTAGE - 0.1) {
          chargeState = ABSORPTION;
        }
        break;
        
      case ABSORPTION:
        // Stay in Absorption while battery is at 14.4V
        // Move to Float when battery starts to drop
        if (batVolts < ABSORPTION_VOLTAGE - 0.2) {
          chargeState = FLOAT;
        }
        break;
        
      case FLOAT:
        // Stay in Float mode while battery is between 13.6V and 13.8V
        // Stop charging if battery voltage is stable at float level
        if (batVolts >= FLOAT_VOLTAGE + 0.1) {
          // Battery voltage rising above float, go back to Absorption
          chargeState = ABSORPTION;
        } else if (batVolts <= FLOAT_VOLTAGE - 0.2) {
          // Battery voltage dropping, keep charging in Float
          // Float continues indefinitely
        }
        break;
        
      case OFF:
        break;
    }
    
    // Safety: Stop charging if battery voltage is too high
    if (batVolts > 15.0) {
      stopCharging();
    }
  }
}

void startCharging() {
  chargingActive = true;
  chargeState = BULK;
  // No buzzer for charging start - buzzer only for faults
  Serial.println("Charging: Started");
}

void stopCharging() {
  chargingActive = false;
  chargeState = OFF;
  Serial.println("Charging: Stopped");
}

// ================= NON-BLOCKING STATE MACHINE =================
void stateMachine() {
  static unsigned long lastStateCheck = 0;
  static SysState previousState = ST_CHECK_START;
  
  if (millis() - lastStateCheck < 10) {
    return;
  }
  lastStateCheck = millis();
  
  // Ensure buzzer is off when leaving error state
  if (previousState == ST_ERROR && currentState != ST_ERROR) {
    digitalWrite(PIN_BUZZER, LOW);
  }
  previousState = currentState;
  
  switch (currentState) {
    case ST_CHECK_START:
      disableSPWM();
      // Buzzer already OFF in main loop
      if (acInVolts >= AC_MIN && acInVolts <= AC_MAX) {
         currentState = ST_GRID_WAIT;
         stateTimer = millis();
      } else {
         currentState = ST_SOFT_START;
         enableSPWM(false);  // Inverter mode (25kHz)
         setSPWMAmplitude(0.0);
         softStartTimer = millis();
      }
      break;

    case ST_SOFT_START:
      digitalWrite(PIN_RELAY, LOW);
      // Buzzer OFF during soft start
      digitalWrite(PIN_BUZZER, LOW);
      
      if (millis() - softStartTimer >= SOFT_START_INTERVAL) {
        softStartTimer = millis();
        
        // Gradually increase SPWM amplitude
        if (modulationIndex < 0.85) {
          setSPWMAmplitude(modulationIndex + 0.01);
        }
        
        if (modulationIndex >= 0.85) { 
          currentState = ST_INVERTER;
          Serial.println("State: Soft Start -> Inverter");
        }
      }
      break;

    case ST_INVERTER:
      // Buzzer OFF during inverter operation
      digitalWrite(PIN_BUZZER, LOW);
      
      // Voltage regulation
      if (acOutVolts < 220) setSPWMAmplitude(modulationIndex + 0.001);
      if (acOutVolts > 240) setSPWMAmplitude(modulationIndex - 0.001);
      
      // Fault detection
      if (batVolts < BAT_LOW_CUTOFF) triggerError("LOW BATT");
      if (loadWatts > MAX_LOAD_WATTS) triggerError("OVERLOAD");
      
      // Grid detection for transfer
      if (acInVolts >= AC_MIN && acInVolts <= AC_MAX) {
        currentState = ST_GRID_WAIT;
        stateTimer = millis();
        Serial.println("State: Inverter -> Grid Wait");
      }
      break;

    case ST_GRID_WAIT:
      // Buzzer OFF during grid wait
      digitalWrite(PIN_BUZZER, LOW);
      
      if (millis() - stateTimer >= GRID_DEBOUNCE_TIME) {
        if (acInVolts >= AC_MIN && acInVolts <= AC_MAX) {
          disableSPWM(); 
          digitalWrite(PIN_RELAY, HIGH);
          currentState = ST_UPS_CHARGE;
          Serial.println("State: Grid Wait -> UPS Charge");
        } else {
          currentState = ST_INVERTER;
          Serial.println("State: Grid Wait -> Inverter");
        }
      }
      break;

    case ST_UPS_CHARGE:
      // Buzzer OFF during UPS/charge mode
      digitalWrite(PIN_BUZZER, LOW);
      
      digitalWrite(PIN_RELAY, HIGH);
      
      if (acInVolts < AC_MIN || acInVolts > AC_MAX) {
        stopCharging();
        digitalWrite(PIN_RELAY, LOW);
        enableSPWM(false);  // Switch back to inverter mode (25kHz)
        setSPWMAmplitude(0.85);  // Start at full amplitude immediately (was 0.0)
        currentState = ST_INVERTER;  // Go directly to INVERTER state (was ST_SOFT_START)
        Serial.println("State: UPS Charge -> Inverter (Grid lost)");
      } else if (!chargingActive && acInVolts >= AC_MIN && acInVolts <= AC_MAX) {
        // Grid is present, start charging if not already
        startCharging();
        // Switch to charging mode frequency (20kHz)
        enableSPWM(true);  // Charging mode (20kHz)
        setSPWMAmplitude(0.0);
        Serial.println("Switched to Charging Mode");
      }
      break;

    case ST_MANUAL_OFF:
      // Manual off state - everything is off
      disableSPWM();
      stopCharging();
      digitalWrite(PIN_RELAY, LOW);
      // Buzzer OFF in manual off state
      digitalWrite(PIN_BUZZER, LOW);
      break;

    case ST_ERROR:
      disableSPWM();
      stopCharging();
      digitalWrite(PIN_RELAY, LOW);
      // Buzzer stays ON in error state (handled in triggerError)
      break;
  }
}

// ================= UTILS =================
void readSensors() {
  static unsigned long lastSensorRead = 0;
  if (millis() - lastSensorRead < 50) {
    return;
  }
  lastSensorRead = millis();
  
  batVolts = (analogRead(PIN_BAT_V) * 5.0 / 1023.0) * 3.5;
  acOutVolts = (analogRead(PIN_AC_OUT) * 5.0 / 1023.0) * 80.0;
  float shuntVoltage = (analogRead(PIN_SHUNT) * 5.0 / 1023.0) * 15.0;
  loadAmps = shuntVoltage / 2.5;
  loadWatts = acOutVolts * loadAmps * powerFactor;
  acInVolts = (analogRead(PIN_AC_IN) * 5.0 / 1023.0) * 80.0;
  int ntcVal = analogRead(PIN_NTC);
  temperature = map(ntcVal, 0, 1023, 0, 90);
  
  static unsigned long lastSerialOutput = 0;
  if (millis() - lastSerialOutput > 1000) {
    lastSerialOutput = millis();
    Serial.print("Bat: "); Serial.print(batVolts, 1);
    Serial.print("V, Out: "); Serial.print(acOutVolts, 0);
    Serial.print("V, Mod: "); Serial.print(modulationIndex, 2);
    Serial.print(", Freq: ");
    if (isChargingMode) {
      Serial.print("CHG");
    } else {
      Serial.print("INV");
    }
    Serial.print(", Charge: ");
    switch(chargeState) {
      case BULK: Serial.print("BULK"); break;
      case ABSORPTION: Serial.print("ABSORPTION"); break;
      case FLOAT: Serial.print("FLOAT"); break;
      case OFF: Serial.print("OFF"); break;
    }
    Serial.print(", State: ");
    switch(currentState) {
      case ST_CHECK_START: Serial.print("CHECK_START"); break;
      case ST_SOFT_START: Serial.print("SOFT_START"); break;
      case ST_INVERTER: Serial.print("INVERTER"); break;
      case ST_GRID_WAIT: Serial.print("GRID_WAIT"); break;
      case ST_UPS_CHARGE: Serial.print("UPS_CHARGE"); break;
      case ST_ERROR: Serial.print("ERROR"); break;
      case ST_MANUAL_OFF: Serial.print("MANUAL_OFF"); break;
    }
    Serial.println();
  }
}

void thermalControl() {
  static unsigned long lastThermalCheck = 0;
  if (millis() - lastThermalCheck < 1000) {
    return;
  }
  lastThermalCheck = millis();
  
  if (temperature > 45 && temperature <= 60) {
    analogWrite(PIN_FAN, 150);
  } else if (temperature > 60) {
    analogWrite(PIN_FAN, 255);
  } else {
    analogWrite(PIN_FAN, 0);
  }

  if (temperature > 80) {
    triggerError("OVERTEMP");
  }
  
  if (loadWatts > 1000) {
    powerFactor = 0.98;
  } else if (loadWatts > 500) {
    powerFactor = 0.95;
  } else {
    powerFactor = 0.90;
  }
}

void zeroCrossISR() {
  syncPulse = true;
  lastZCTime = millis();
}

void triggerError(const char* msg) {
  currentState = ST_ERROR;
  disableSPWM();
  stopCharging();
  digitalWrite(PIN_RELAY, LOW);
  
  // Turn on buzzer continuously for error state (fault)
  digitalWrite(PIN_BUZZER, HIGH);
  
  lcd.clear();
  lcd.setCursor(4,1);
  lcd.print("SYSTEM ERROR");
  lcd.setCursor(6,2);
  lcd.print(msg);
  
  Serial.print("ERROR: ");
  Serial.println(msg);
}

// Startup beep (only during initialization)
void startupBeep() {
  digitalWrite(PIN_BUZZER, HIGH);
  delay(100);
  digitalWrite(PIN_BUZZER, LOW);
  delay(50);
  digitalWrite(PIN_BUZZER, HIGH);
  delay(100);
  digitalWrite(PIN_BUZZER, LOW);
}

void updateLCD() {
  if (millis() - lastScreenUpdate < 500) return;
  lastScreenUpdate = millis();

  if (currentState == ST_ERROR) return;

  if (currentState == ST_MANUAL_OFF) {
    lcd.setCursor(0,0); lcd.print("                    ");
    lcd.setCursor(0,1); lcd.print("    System is OFF   ");
    lcd.setCursor(0,2); lcd.print("                    ");
    lcd.setCursor(0,3); lcd.print("                    ");
    return;
  }

  if (currentState == ST_INVERTER || currentState == ST_SOFT_START) {
    lcd.setCursor(0,0); lcd.print("Mode: Inverter      ");
    
    lcd.setCursor(0,1); 
    lcd.print("Out:");
    if (acOutVolts < 100) lcd.print(" ");
    lcd.print(acOutVolts, 0);
    lcd.print("V     ");
    
    lcd.setCursor(12,1);
    lcd.print(" ");
    if (loadWatts < 1000) lcd.print(" ");
    lcd.print(loadWatts, 0);
    lcd.print("W");
    
    lcd.setCursor(0,2); 
    lcd.print("Batt:");
    lcd.print(batVolts, 1);
    lcd.print("V    ");
    
    lcd.setCursor(12,2);
    lcd.print(" ");
    if (loadAmps < 10) lcd.print(" ");
    lcd.print(loadAmps, 1);
    lcd.print("A");
    
    lcd.setCursor(0,3); 
    lcd.print("Temp:");
    lcd.print((int)temperature);
    lcd.print("C           ");
    
  } 
  else if (currentState == ST_UPS_CHARGE || currentState == ST_GRID_WAIT) {
    lcd.setCursor(0,0); lcd.print("Mode: GRID/UPS      ");
    
    lcd.setCursor(0,1); 
    lcd.print("In: ");
    lcd.print((int)acInVolts);
    lcd.print("V           ");
    
    lcd.setCursor(0,2); 
    lcd.print("Batt:");
    lcd.print(batVolts, 1);
    lcd.print("V        ");
    
    lcd.setCursor(0,3); 
    if (chargingActive) {
      switch(chargeState) {
        case BULK: 
          lcd.print("Charging: BULK      ");
          break;
          
        case ABSORPTION: 
          lcd.print("Charging: ABSORB    ");
          break;
          
        case FLOAT: 
          lcd.print("Charging: Float     ");
          break;
          
        default: 
          lcd.print("                    ");
          break;
      }
    } else {
      if (acInVolts >= AC_MIN && acInVolts <= AC_MAX) {
        lcd.print("Grid: Ready         ");
      } else {
        lcd.print("Charger: Off        ");
      }
    }
  }
}