/**
 * LF Inverter/UPS Firmware - Arduino Nano
 * 50Hz Pure Sine Wave Inverter - 4 Pin Version
 * Switching Frequency: 25.0 kHz
 * Logic: Unipolar Switching (Pins 9, 10 PWM | Pins 7, 8 Steering)
 * Updated: Integrated Synchronous Buck Charger using Transformer Inductance
 */

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

// ================= PINS CONFIGURATION =================
#define PIN_HI_LEFT  9   
#define PIN_HI_RIGHT 10  
#define PIN_LO_LEFT  7   
#define PIN_LO_RIGHT 8   
#define PIN_BAT_V    A2
#define PIN_AC_OUT   A6
#define PIN_SHUNT    A1  
#define PIN_NTC      A3  
#define PIN_AC_IN    A0  
#define PIN_ZC       13   // NOTE: Moved to D2 for Hardware Interrupt Support
#define PIN_RELAY    4   
#define PIN_FAN      6   
#define PIN_BUZZER   5

// ================= SPWM CONSTANTS =================
#define SPWM_FREQ 25000
#define SYS_FREQ  16000000
#define ICR_MAX   639  

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
};

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

volatile int sineIndex = 0;
volatile bool spwmEnabled = false;
volatile float modulationIndex = 0.0;
volatile int chargePWM = 0; // New: Target duty for charger

volatile bool syncPulse = false;
volatile unsigned long lastZCTime = 0;

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

const float BULK_VOLTAGE = 14.4, ABSORPTION_VOLTAGE = 14.4, FLOAT_VOLTAGE = 13.8;
const float CHG_MAX_AMPS = 20.0; // Safety current limit
bool chargingActive = false;

unsigned long stateTimer = 0, softStartTimer = 0, lastScreenUpdate = 0;
const unsigned long GRID_DEBOUNCE_TIME = 1000, SOFT_START_INTERVAL = 20;
const float BAT_LOW_CUTOFF = 10.5;
const int AC_MIN = 200, AC_MAX = 250, MAX_LOAD_WATTS = 7000;

LiquidCrystal_I2C lcd(0x27, 20, 4);

// ================= 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);

  initSPWMTimer();
  attachInterrupt(digitalPinToInterrupt(PIN_ZC), zeroCrossISR, RISING);
  startupBeep();
  
  lcd.begin(20,4); lcd.backlight();
  lcd.setCursor(0,1); lcd.print("MNG proSystems CHG");
  delay(2000); lcd.clear();
}

void loop() {
  readSensors();
  thermalControl();
  stateMachine();
  manageCharging();
  updateLCD();
  if (currentState != ST_ERROR) digitalWrite(PIN_BUZZER, LOW);
}

// ================= SPWM & CHARGER ENGINE =================
void initSPWMTimer() {
  TCCR1A = _BV(COM1A1) | _BV(COM1B1) | _BV(WGM11); 
  TCCR1B = _BV(WGM13) | _BV(WGM12) | _BV(CS10); 
  ICR1 = ICR_MAX;
  OCR1A = 0; OCR1B = 0;

  TCCR2A = _BV(WGM21); TCCR2B = _BV(CS21); OCR2A = 199; 
  TIMSK2 = _BV(OCIE2A);
}

ISR(TIMER2_COMPA_vect) {
  // --- Charger Logic (Synchronous Buck) ---
  if (currentState == ST_UPS_CHARGE && chargingActive) {
    if (sineIndex < 100) { // Positive Half Cycle
      digitalWrite(PIN_LO_LEFT, LOW);
      digitalWrite(PIN_LO_RIGHT, HIGH); // Common Path
      OCR1A = chargePWM;                // Buck Switch
      OCR1B = 0;
    } else { // Negative Half Cycle
      digitalWrite(PIN_LO_RIGHT, LOW);
      digitalWrite(PIN_LO_LEFT, HIGH);  // Common Path
      OCR1A = 0;
      OCR1B = chargePWM;                // Buck Switch
    }
    sineIndex++;
    if (sineIndex >= 200) sineIndex = 0;
    return;
  }

  // --- Inverter Logic (Pure Sine) ---
  if (!spwmEnabled) {
    digitalWrite(PIN_LO_LEFT, LOW); digitalWrite(PIN_LO_RIGHT, LOW);
    OCR1A = 0; OCR1B = 0;
    return;
  }

  int val;
  if (sineIndex < 100) {
    val = pgm_read_word(&sineTable[sineIndex]);
    digitalWrite(PIN_LO_LEFT, LOW); digitalWrite(PIN_LO_RIGHT, HIGH);
    OCR1A = (int)(val * modulationIndex); OCR1B = 0;
  } else {
    val = pgm_read_word(&sineTable[sineIndex - 100]);
    digitalWrite(PIN_LO_RIGHT, LOW); digitalWrite(PIN_LO_LEFT, HIGH);
    OCR1A = 0; OCR1B = (int)(val * modulationIndex);
  }
  sineIndex++;
  if (sineIndex >= 200) sineIndex = 0;
}

void enableSPWM() { spwmEnabled = true; modulationIndex = 0.0; sineIndex = 0; TCCR1A |= (_BV(COM1A1) | _BV(COM1B1)); }
void disableSPWM() { spwmEnabled = false; TCCR1A &= ~(_BV(COM1A1) | _BV(COM1B1)); 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); }

// ================= UPDATED CHARGING LOGIC =================

void manageCharging() {
  if (currentState != ST_UPS_CHARGE) {
    if (chargingActive) stopCharging();
    return;
  }

  bool validAC = (acInVolts >= AC_MIN && acInVolts <= AC_MAX);
  if (!chargingActive && validAC) startCharging();
  if (chargingActive && !validAC) { stopCharging(); return; }

  if (chargingActive) {
    float targetV = (chargeState == FLOAT) ? FLOAT_VOLTAGE : BULK_VOLTAGE;
    
    // Constant Current / Constant Voltage (CC/CV) Simulation
    if (batVolts < targetV && loadAmps < CHG_MAX_AMPS) {
      if (chargePWM < (ICR_MAX - 10)) chargePWM++; // Slowly increase duty
    } else if (batVolts > (targetV + 0.1) || loadAmps > CHG_MAX_AMPS) {
      if (chargePWM > 0) chargePWM--; // Decrease duty
    }

    // Stage Management
    if (chargeState == BULK && batVolts >= BULK_VOLTAGE - 0.1) chargeState = ABSORPTION;
    if (chargeState == ABSORPTION && batVolts < ABSORPTION_VOLTAGE - 0.2) chargeState = FLOAT;
    if (batVolts > 15.5) stopCharging(); // High voltage protection
  }
}

void startCharging() { chargingActive = true; chargeState = BULK; chargePWM = 0; TCCR1A |= (_BV(COM1A1) | _BV(COM1B1)); }
void stopCharging() { chargingActive = false; chargeState = OFF; chargePWM = 0; }

// ================= STATE MACHINE =================
void stateMachine() {
  static unsigned long lastStateCheck = 0;
  if (millis() - lastStateCheck < 10) return;
  lastStateCheck = millis();

  switch (currentState) {
    case ST_CHECK_START:
      disableSPWM();
      if (acInVolts >= AC_MIN && acInVolts <= AC_MAX) { currentState = ST_GRID_WAIT; stateTimer = millis(); } 
      else { currentState = ST_SOFT_START; enableSPWM(); softStartTimer = millis(); }
      break;

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

    case ST_INVERTER:
      if (acOutVolts < 220) setSPWMAmplitude(modulationIndex + 0.001);
      if (acOutVolts > 240) setSPWMAmplitude(modulationIndex - 0.001);
      if (batVolts < BAT_LOW_CUTOFF) triggerError("LOW BATT");
      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) {
          disableSPWM(); 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); enableSPWM(); currentState = ST_SOFT_START;
      }
      break;

    case ST_ERROR:
      disableSPWM(); stopCharging(); digitalWrite(PIN_RELAY, LOW);
      break;
  }
}

// ================= SENSORS & UTILS =================
void readSensors() {
  static unsigned long lastSR = 0;
  if (millis() - lastSR < 50) return;
  lastSR = millis();
  
  batVolts = (analogRead(PIN_BAT_V) * 5.0 / 1023.0) * 4.0; // Adjusted for standard divider
  acOutVolts = (analogRead(PIN_AC_OUT) * 5.0 / 1023.0) * 100.0;
  acInVolts = (analogRead(PIN_AC_IN) * 5.0 / 1023.0) * 100.0;
  loadAmps = ((analogRead(PIN_SHUNT) * 5.0 / 1023.0) * 15.0) / 2.5;
  loadWatts = acOutVolts * loadAmps * powerFactor;
  temperature = map(analogRead(PIN_NTC), 0, 1023, 0, 100);
}

void thermalControl() {
  if (temperature > 45) analogWrite(PIN_FAN, temperature > 60 ? 255 : 150);
  else analogWrite(PIN_FAN, 0);
  if (temperature > 85) triggerError("OVERTEMP");
}

void zeroCrossISR() { 
  sineIndex = 0; // Crucial for Charger Phase Alignment
  lastZCTime = millis(); 
}

void triggerError(const char* msg) {
  currentState = ST_ERROR; disableSPWM(); stopCharging();
  digitalWrite(PIN_RELAY, LOW); digitalWrite(PIN_BUZZER, HIGH);
  lcd.clear(); lcd.setCursor(4,1); lcd.print("SYSTEM ERROR");
  lcd.setCursor(6,2); lcd.print(msg);
}

void startupBeep() { digitalWrite(PIN_BUZZER, HIGH); delay(100); digitalWrite(PIN_BUZZER, LOW); }

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

  lcd.setCursor(0,0); 
  lcd.print(currentState == ST_UPS_CHARGE ? "Mode: UPS/CHG      " : "Mode: Inverter     ");
  
  lcd.setCursor(0,1); lcd.print("Vout:"); lcd.print(acOutVolts, 0); lcd.print("V ");
  lcd.setCursor(11,1); lcd.print("W:"); lcd.print(loadWatts, 0);

  lcd.setCursor(0,2); lcd.print("Batt:"); lcd.print(batVolts, 1); lcd.print("V ");
  lcd.setCursor(11,2); lcd.print("A:"); lcd.print(loadAmps, 1);

  lcd.setCursor(0,3);
  //if (chargingActive) lcd.print("CHG Duty:"); lcd.print((chargePWM * 100) / ICR_MAX); lcd.print("%   ");
  lcd.print("Temp:"); lcd.print((int)temperature); lcd.print("C         ");
}