/**
 * LF Inverter/UPS Firmware - Arduino Nano
 * 50Hz Fundamental | 25kHz Carrier used to generate a smooth, high-quality sine wave from
 * Auto-Start with integrated charging control
 * Author: MNG proSystems South Africa
 * Email: [email protected] 
 * Firmware: V1.2.i2 (Integrated Charger) - Non-Blocking
 */

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

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

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

// Feedback & Sensors
#define PIN_BAT_V    A2
#define PIN_AC_OUT   A6
#define PIN_SHUNT    A1  // 30A Shunt
#define PIN_NTC      A3  // 10k NTC
#define PIN_AC_IN    A0  // Analog Input
#define PIN_ZC       13  // Zero Cross (INT0)
#define PIN_RELAY    4   // Transfer Relay
#define PIN_FAN      6   // PWM Fan
#define PIN_BUZZER   5

// ================= SYSTEM CONSTANTS =================
#define SPWM_FREQ 25000
#define SYS_FREQ  16000000
#define ICR_MAX   (SYS_FREQ / (2 * SPWM_FREQ)) // 320

const int SINE_STEPS = 250; 
uint16_t sineTable[SINE_STEPS];

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 };
SysState currentState = ST_CHECK_START;

// Global Variables
volatile int sineIdx = 0;
volatile bool halfCycle = 0; 
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;
float modulationIndex = 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;

// 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);
  
  pinMode(PIN_HI_LEFT, OUTPUT);
  pinMode(PIN_HI_RIGHT, OUTPUT);
  pinMode(PIN_LO_LEFT, OUTPUT);
  pinMode(PIN_LO_RIGHT, OUTPUT);
  pinMode(PIN_RELAY, OUTPUT);
  pinMode(PIN_FAN, OUTPUT);
  pinMode(PIN_BUZZER, OUTPUT);
  pinMode(PIN_ZC, INPUT); 

  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();

  generateSineTable();
  initTimer1();

  attachInterrupt(digitalPinToInterrupt(PIN_ZC), zeroCrossISR, RISING);

  digitalWrite(PIN_RELAY, LOW); 
  nonBlockingBuzzer(1);
  
  stateTimer = millis();
  softStartTimer = millis();
}

// ================= MAIN LOOP =================
void loop() {
  readSensors();
  thermalControl();
  stateMachine();
  manageCharging();
  updateLCD();
  
  static unsigned long lastYield = 0;
  if (millis() - lastYield > 100) {
    lastYield = millis();
  }
}

// ================= 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;
  nonBlockingBuzzer(2);
}

void stopCharging() {
  chargingActive = false;
  chargeState = OFF;
}

// ================= NON-BLOCKING STATE MACHINE =================
void stateMachine() {
  static unsigned long lastStateCheck = 0;
  if (millis() - lastStateCheck < 10) {
    return;
  }
  lastStateCheck = millis();
  
  switch (currentState) {
    case ST_CHECK_START:
      disableOutput();
      if (acInVolts >= AC_MIN && acInVolts <= AC_MAX) {
         currentState = ST_GRID_WAIT;
         stateTimer = millis();
      } else {
         currentState = ST_SOFT_START;
         modulationIndex = 0;
         softStartTimer = millis();
      }
      break;

    case ST_SOFT_START:
      digitalWrite(PIN_RELAY, LOW);
      
      if (millis() - softStartTimer >= SOFT_START_INTERVAL) {
        softStartTimer = millis();
        modulationIndex += 0.01; 
        
        if (modulationIndex >= 0.85) { 
          modulationIndex = 0.85; 
          currentState = ST_INVERTER;
        }
      }
      break;

    case ST_INVERTER:
      if (acOutVolts < 220) modulationIndex += 0.001;
      if (acOutVolts > 240) modulationIndex -= 0.001;
      modulationIndex = constrain(modulationIndex, 0.1, 0.95);

      if (batVolts < BAT_LOW_CUTOFF) triggerError("LOW BATT");
      if (loadWatts > MAX_LOAD_WATTS) triggerError("OVERLOAD");
      
      if (acInVolts >= AC_MIN && acInVolts <= AC_MAX) {
        currentState = ST_GRID_WAIT;
        stateTimer = millis();
      }
      break;

    case ST_GRID_WAIT:
      if (millis() - stateTimer >= GRID_DEBOUNCE_TIME) {
        if (acInVolts >= AC_MIN && acInVolts <= AC_MAX) {
          disableOutput(); 
          digitalWrite(PIN_RELAY, HIGH);
          currentState = ST_UPS_CHARGE;
        } else {
          currentState = ST_INVERTER;
        }
      }
      break;

    case ST_UPS_CHARGE:
      digitalWrite(PIN_RELAY, HIGH);
      
      if (acInVolts < AC_MIN || acInVolts > AC_MAX) {
        stopCharging();
        digitalWrite(PIN_RELAY, LOW);
        modulationIndex = 0; 
        currentState = ST_SOFT_START;
        softStartTimer = millis();
      }
      break;

    case ST_ERROR:
      disableOutput();
      stopCharging();
      break;
  }
}

// ================= SPWM ENGINE =================
void initTimer1() {
  TCCR1A = 0;
  TCCR1B = 0;
  TCCR1A |= (1 << WGM11); 
  TCCR1B |= (1 << WGM13) | (1 << CS10); 
  ICR1 = ICR_MAX; 
  TIMSK1 |= (1 << TOIE1); 
}

ISR(TIMER1_OVF_vect) {
  if (currentState != ST_INVERTER && currentState != ST_SOFT_START) {
    TCCR1A &= ~((1 << COM1A1) | (1 << COM1B1)); 
    digitalWrite(PIN_LO_LEFT, LOW);
    digitalWrite(PIN_LO_RIGHT, LOW);
    return;
  }

  sineIdx++;
  if (sineIdx >= SINE_STEPS) {
    sineIdx = 0;
    halfCycle = !halfCycle; 
  }

  int duty = (int)((float)sineTable[sineIdx] * modulationIndex);

  if (!halfCycle) {
    TCCR1A |= (1 << COM1A1);  
    TCCR1A &= ~(1 << COM1B1); 
    OCR1A = duty;
    digitalWrite(PIN_HI_RIGHT, LOW);
    digitalWrite(PIN_HI_LEFT, HIGH); 
    digitalWrite(PIN_LO_LEFT, LOW);
    digitalWrite(PIN_LO_RIGHT, HIGH); 
  } else {
    TCCR1A &= ~(1 << COM1A1); 
    TCCR1A |= (1 << COM1B1);  
    OCR1B = duty;
    digitalWrite(PIN_HI_LEFT, LOW);
    digitalWrite(PIN_HI_RIGHT, HIGH);
    digitalWrite(PIN_LO_RIGHT, LOW);
    digitalWrite(PIN_LO_LEFT, HIGH);
  }
}

// ================= UTILS =================
void generateSineTable() {
  for (int i = 0; i < SINE_STEPS; i++) {
    float angle = (PI * i) / SINE_STEPS; 
    sineTable[i] = (uint16_t)(sin(angle) * ICR_MAX);
  }
}

void readSensors() {
  static unsigned long lastSensorRead = 0;
  if (millis() - lastSensorRead < 50) {
    return;
  }
  lastSensorRead = millis();
  
  batVolts = (analogRead(PIN_BAT_V) * 5.0 / 1023.0) * 12.0;
  acOutVolts = (analogRead(PIN_AC_OUT) * 5.0 / 1023.0) * 100.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) * 100.0;
  int ntcVal = analogRead(PIN_NTC);
  temperature = map(ntcVal, 0, 1023, 0, 100);
  
  static unsigned long lastSerialOutput = 0;
  if (millis() - lastSerialOutput > 1000) {
    lastSerialOutput = millis();
    Serial.print("Bat: "); Serial.print(batVolts, 1);
    Serial.print("V, 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.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 > 85) {
    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 disableOutput() {
  TCCR1A &= ~((1 << COM1A1) | (1 << COM1B1)); 
  digitalWrite(PIN_HI_LEFT, LOW);
  digitalWrite(PIN_HI_RIGHT, LOW);
  digitalWrite(PIN_LO_LEFT, LOW);
  digitalWrite(PIN_LO_RIGHT, LOW);
  modulationIndex = 0;
}

void triggerError(const char* msg) {
  currentState = ST_ERROR;
  disableOutput();
  stopCharging();
  lcd.clear();
  lcd.setCursor(4,1);
  lcd.print("SYSTEM ERROR");
  lcd.setCursor(6,2);
  lcd.print(msg);
  nonBlockingBuzzer(5);
}

void nonBlockingBuzzer(int beeps) {
  static unsigned long buzzerStartTime = 0;
  static int buzzerBeepsRemaining = 0;
  static bool buzzerState = false;
  
  if (beeps > 0 && buzzerBeepsRemaining == 0) {
    buzzerBeepsRemaining = beeps * 2;
    buzzerStartTime = millis();
    buzzerState = true;
    digitalWrite(PIN_BUZZER, HIGH);
  }
  
  if (buzzerBeepsRemaining > 0) {
    if (millis() - buzzerStartTime >= 100) {
      buzzerStartTime = millis();
      buzzerBeepsRemaining--;
      buzzerState = !buzzerState;
      digitalWrite(PIN_BUZZER, buzzerState ? HIGH : LOW);
    }
  }
}

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

  if (currentState == ST_ERROR) 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      ");
      }
    }
  }
}