/*
 * Hybrid Inverter/UPS Firmware - Arduino Nano
 * 50Hz Fundamental, 25kHz Carrier, 3-Stage Charging
 * 
 * Architecture: Unipolar SPWM
 * H-Bridge: TLP250 Drivers
 */

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

// --- Configuration ---
#define SPWM_FREQ 25000   // 25kHz
#define SYS_CLOCK 16000000
#define ICR_VAL   (SYS_CLOCK / (2 * SPWM_FREQ)) // Phase Correct PWM (Approx 320)

// --- Pin Definitions ---
#define PIN_HI_L    9   // Timer1 OC1A
#define PIN_HI_R    10  // Timer1 OC1B
#define PIN_LO_L    7
#define PIN_LO_R    8
#define PIN_FAN     6
#define PIN_BUZZER  5
#define PIN_RELAY   4   // Relay: HIGH=Grid(Charge), LOW=Inverter

// Zero Crossing Detection Pin (Critical for synchronization)
#define PIN_ZC      13  // Digital pin for zero-crossing detection

#define BTN_PWR     2
#define BTN_ENT     3
#define BTN_UP      11
#define BTN_DWN     12

#define SENS_AC_IN  A0
#define SENS_CT     A1
#define SENS_BAT    A2
#define SENS_NTC    A3
#define SENS_FB     A6  // AC Out Feedback

// --- Zero Crossing Variables ---
volatile unsigned long zcTime = 0;          // Time of last zero crossing
volatile unsigned long lastZcTime = 0;      // Time of previous zero crossing
volatile unsigned long 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 unsigned long phaseError = 0;      // Phase error in microseconds
bool gridStable = false;                    // Grid frequency stability flag

// --- Objects ---
LiquidCrystal_I2C lcd(0x27, 20, 4); 

// --- 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
float batVolts = 0.0;
float acInVolts = 0.0;
float acOutVolts = 0.0;
float currentAmps = 0.0;
float loadWatts = 0.0;
int tempC = 0;

// Settings (Saved to EEPROM)
struct Settings {
  float batLowCut;
  float batFull;
  float chargeCurrent;
  int acMin;
  int acMax;
  int phaseSyncWindow;    // Phase synchronization window in microseconds
  int gridStableCount;    // Number of stable cycles required
} settings;

// State Machine
enum Mode { STARTUP, INVERTER, GRID_BYPASS, CHARGING, SYNCHRONIZING, ERROR_STATE };
Mode currentMode = STARTUP;
String errorMsg = "";

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

unsigned long lastUpdate = 0;
unsigned long lastZcCheck = 0;
bool systemOn = false;

// --- Zero Crossing Interrupt Service Routine ---
void zeroCrossingISR() {
  unsigned long currentTime = micros();
  
  // Calculate half period
  if (lastZcTime > 0) {
    halfPeriod = currentTime - lastZcTime;
    
    // Check for valid 50Hz range (8ms to 12ms for half period)
    if (halfPeriod >= 8000 && halfPeriod <= 12000) {
      // Calculate phase error (for synchronization)
      unsigned long expectedTime = lastZcTime + 10000; // Expected 10ms for 50Hz
      phaseError = (currentTime > expectedTime) ? (currentTime - expectedTime) : (expectedTime - currentTime);
      
      // Update polarity (alternates each zero crossing)
      zcPolarity = !zcPolarity;
      zcDetected = true;
      lastZcTime = currentTime;
      zcTime = currentTime;
      
      // Grid stability check
      static int stableCount = 0;
      if (phaseError < 200) { // Within 200µs error
        stableCount++;
        if (stableCount > 10) { // 10 stable cycles
          gridStable = true;
        }
      } else {
        stableCount = 0;
        gridStable = false;
      }
    }
  } else {
    lastZcTime = currentTime;
  }
}

// --- Lookup Table Generation ---
void generateSineTable() {
  for (int i = 0; i < 250; i++) {
    // Generate sine values scaled to Timer1 ICR (320)
    // 250 steps per half cycle (50Hz / 25kHz = 500 steps total)
    float rads = PI * i / 250.0;
    sineTable[i] = (int)(sin(rads) * (float)ICR_VAL);
  }
}

// --- Interrupt Service Routine (SPWM Core) ---
ISR(TIMER1_OVF_vect) {
  if (currentMode == INVERTER || currentMode == SYNCHRONIZING) {
    int duty = (int)(sineTable[spwmIndex] * modulationIndex);
    
    // Unipolar Switching Logic
    // Positive Half: Left High (PWM), Right Low (ON)
    // Negative Half: Right High (PWM), Left Low (ON)
    
    if (cyclePositive) {
      OCR1A = duty;       // Drive Left High
      OCR1B = 0;
      digitalWrite(PIN_LO_L, LOW);
      digitalWrite(PIN_LO_R, HIGH);
    } else {
      OCR1A = 0;
      OCR1B = duty;       // Drive Right High
      digitalWrite(PIN_LO_L, HIGH);
      digitalWrite(PIN_LO_R, LOW);
    }

    spwmIndex++;
    if (spwmIndex >= 250) {
      spwmIndex = 0;
      cyclePositive = !cyclePositive;
    }
  } 
  else if (currentMode == CHARGING) {
    // Charging with zero-crossing synchronization
    // Only start charging cycle at zero crossing
    static bool chargingCycle = false;
    
    if (zcDetected && zcPolarity) { // Start on positive zero crossing
      chargingCycle = true;
      zcDetected = false;
    }
    
    if (chargingCycle) {
      digitalWrite(PIN_LO_L, LOW); 
      digitalWrite(PIN_LO_R, LOW);
      
      // Determine charging PWM based on stage
      int chargeDuty = (int)(modulationIndex * ICR_VAL);
      
      // Apply PWM synchronized with grid
      OCR1A = chargeDuty;
      OCR1B = chargeDuty;
      
      // End charging cycle after appropriate delay
      static unsigned long chargeStart = 0;
      if (chargeStart == 0) chargeStart = micros();
      
      if (micros() - chargeStart > 8000) { // Charge for 8ms
        OCR1A = 0;
        OCR1B = 0;
        chargingCycle = false;
        chargeStart = 0;
      }
    } else {
      OCR1A = 0;
      OCR1B = 0;
    }
  }
  else {
    // Disable outputs
    OCR1A = 0;
    OCR1B = 0;
    digitalWrite(PIN_LO_L, LOW);
    digitalWrite(PIN_LO_R, LOW);
  }
}

void setup() {
  Serial.begin(9600);
  
  // Pin Setup
  pinMode(PIN_HI_L, OUTPUT);
  pinMode(PIN_HI_R, OUTPUT);
  pinMode(PIN_LO_L, OUTPUT);
  pinMode(PIN_LO_R, OUTPUT);
  pinMode(PIN_RELAY, OUTPUT);
  pinMode(PIN_FAN, OUTPUT);
  pinMode(PIN_BUZZER, OUTPUT);
  
  // Zero Crossing Pin Setup
  pinMode(PIN_ZC, INPUT_PULLUP); // Assuming active LOW signal
  
  pinMode(BTN_PWR, INPUT_PULLUP);
  pinMode(BTN_ENT, INPUT_PULLUP);
  pinMode(BTN_UP, INPUT_PULLUP);
  pinMode(BTN_DWN, INPUT_PULLUP);

  // Attach Zero Crossing Interrupt
  attachInterrupt(digitalPinToInterrupt(PIN_ZC), zeroCrossingISR, FALLING);

  // Load Defaults or EEPROM
  settings = {10.5, 14.2, 10.0, 200, 250, 200, 10}; 

  // LCD Init
  lcd.begin(20, 4);
  lcd.backlight();
  lcd.setCursor(0,0);
  lcd.print("   MNG proSystems   ");
  lcd.setCursor(0,1);
  lcd.print(" Inverter UPS v1.0a" );
  
  generateSineTable();
  
  // Timer1 Setup for Phase Correct PWM 25kHz
  TCCR1A = 0;
  TCCR1B = 0;
  TCCR1A = (1 << COM1A1) | (1 << COM1B1) | (1 << WGM11); // Non-inverted PWM
  TCCR1B = (1 << WGM13) | (1 << CS10); // Phase Correct, ICR1 Top, No Prescaling
  ICR1 = ICR_VAL;
  TIMSK1 |= (1 << TOIE1); // Enable Overflow Interrupt
  
  delay(2000);
  lcd.clear();
}

void loop() {
  readSensors();
  handleButtons();
  handleFan();
  stateMachine();
  checkGridSync();
  
  // Update Display Slow
  if (millis() - lastUpdate > 300) {
    updateLCD();
    lastUpdate = millis();
  }
}

// --- Grid Synchronization Check ---
void checkGridSync() {
  static unsigned long lastCheck = 0;
  
  if (millis() - lastCheck > 1000) {
    lastCheck = millis();
    
    if (gridStable && halfPeriod > 0) {
      // Calculate frequency
      float freq = 500000.0 / halfPeriod; // Convert to Hz (half period in µs)
      
      if (freq >= 48.0 && freq <= 52.0) {
        // Grid frequency is stable
        if (currentMode == SYNCHRONIZING) {
          // Try to synchronize phase
          synchronizePhase();
        }
      } else {
        gridStable = false;
      }
    }
  }
}

// --- Phase Synchronization Function ---
void synchronizePhase() {
  // Adjust SPWM index to match grid phase
  if (phaseError > 100) { // If error > 100µs
    if (micros() - zcTime < halfPeriod / 2) {
      // We're in first half of cycle
      int targetIndex = map(micros() - zcTime, 0, halfPeriod, 0, 250);
      
      if (abs(spwmIndex - targetIndex) > 2) {
        // Adjust index gradually
        if (spwmIndex < targetIndex) spwmIndex++;
        else if (spwmIndex > targetIndex) spwmIndex--;
        
        // Also adjust polarity
        cyclePositive = zcPolarity;
      }
    }
    
    // If synchronized within tolerance
    if (phaseError < 50) { // 50µs tolerance
      currentMode = GRID_BYPASS;
      digitalWrite(PIN_RELAY, HIGH); // Switch to grid
      modulationIndex = 0; // Stop SPWM
    }
  }
}

// --- Sensor Reading & Math ---
void readSensors() {
  // Simple averaging for DC, RMS calculation for AC
  
  // Battery
  batVolts = analogRead(SENS_BAT) * (20.0 / 1023.0); // Adjust Multiplier
  
  // Temp
  int tempRaw = analogRead(SENS_NTC);
  tempC = map(tempRaw, 0, 1023, 0, 100);

  // AC Input Voltage 
  acInVolts = analogRead(SENS_AC_IN) * (350.0 / 1023.0); 
  
  // AC Output Voltage Feedback
  acOutVolts = analogRead(SENS_FB) * (350.0 / 1023.0);
  
  // Current (Offset 2.5V = 512)
  int iRaw = analogRead(SENS_CT) - 512;
  currentAmps = (abs(iRaw) * (50.0 / 512.0)) * 0.707; // Approx RMS
  loadWatts = acOutVolts * currentAmps;
}

// --- Core Logic ---
void stateMachine() {
  // 1. Safety Checks always active
  if (tempC > 75) {
    currentMode = ERROR_STATE;
    errorMsg = "OVER HEAT";
  }
  if (loadWatts > 7500) {
    currentMode = ERROR_STATE;
    errorMsg = "OVERLOAD";
  }

  switch (currentMode) {
    case STARTUP:
      digitalWrite(PIN_RELAY, LOW); // Default Inverter
      if (digitalRead(BTN_PWR) == LOW) {
         systemOn = !systemOn;
         delay(500);
      }
      if (systemOn) currentMode = INVERTER;
      break;

    case INVERTER:
      digitalWrite(PIN_RELAY, LOW); // Relay OFF (Inverter Mode)
      
      // Mains Check
      if (acInVolts >= settings.acMin && acInVolts <= settings.acMax && gridStable) {
        // Grid is good and stable, prepare for synchronization
        modulationIndex = targetModulation; // Maintain voltage
        currentMode = SYNCHRONIZING;
        return;
      }
      
      // Battery Check
      if (batVolts < settings.batLowCut) {
        currentMode = ERROR_STATE;
        errorMsg = "LOW BATT";
        return;
      }

      // Soft Start & Feedback Logic
      if (modulationIndex < targetModulation) modulationIndex += 0.01;
      
      // Voltage Feedback Regulator (Simple P-Control)
      if (acOutVolts < 230) targetModulation += 0.001;
      if (acOutVolts > 232) targetModulation -= 0.001;
      if (targetModulation > 0.95) targetModulation = 0.95;
      break;

    case SYNCHRONIZING:
      // Inverter continues running while synchronizing phase
      // checkGridSync() will handle the synchronization
      
      // Timeout after 5 seconds
      static unsigned long syncStart = 0;
      if (syncStart == 0) syncStart = millis();
      
      if (millis() - syncStart > 5000) {
        // Failed to sync, stay in inverter mode
        currentMode = INVERTER;
        syncStart = 0;
      }
      break;

    case GRID_BYPASS:
      digitalWrite(PIN_RELAY, HIGH); // Relay ON (Grid Pass-through)
      
      if (!gridStable || acInVolts < settings.acMin || acInVolts > settings.acMax) {
         // Grid lost or unstable
         digitalWrite(PIN_RELAY, LOW);
         currentMode = INVERTER;
         modulationIndex = 0; // Reset for soft start
         return;
      }
      
      // Check if charging needed
      if (batVolts < settings.batFull) {
        currentMode = CHARGING;
      }
      break;

    case CHARGING:
      digitalWrite(PIN_RELAY, HIGH);
      
      // Ensure grid is still stable
      if (!gridStable) {
        currentMode = INVERTER;
        chgStage = BULK;
        return;
      }
      
      // 3 Stage Logic
      if (chgStage == BULK) {
        if (batVolts >= 14.0) chgStage = ABSORPTION;
        if (currentAmps < 5.0) modulationIndex += 0.001;
        else modulationIndex -= 0.001;
      }
      else if (chgStage == ABSORPTION) {
        if (currentAmps < 1.0) chgStage = FLOAT;
        if (batVolts > settings.batFull) modulationIndex -= 0.001;
        else modulationIndex += 0.001;
      }
      else {
        if (batVolts > 13.5) modulationIndex -= 0.001;
        else modulationIndex += 0.001;
      }
      
      // Cap duty cycle
      if (modulationIndex > 0.4) modulationIndex = 0.4;
      if (modulationIndex < 0) modulationIndex = 0;

      // Exit Charging if Mains bad
      if (!gridStable) currentMode = INVERTER;
      break;

    case ERROR_STATE:
      modulationIndex = 0;
      digitalWrite(PIN_BUZZER, HIGH);
      delay(100);
      digitalWrite(PIN_BUZZER, LOW);
      if (digitalRead(BTN_PWR) == LOW) {
        currentMode = STARTUP;
        systemOn = false;
      }
      break;
  }
}

void handleFan() {
  if (tempC > 45) {
    int fanSpeed = map(tempC, 45, 70, 100, 255);
    analogWrite(PIN_FAN, fanSpeed);
  } else {
    analogWrite(PIN_FAN, 0);
  }
}

void handleButtons() {
  // Implement menu system here to change 'settings' struct
  // Omitted for brevity
}

void updateLCD() {
  lcd.setCursor(0,0);
  if (currentMode == INVERTER) lcd.print("Mode: Inverter  ");
  else if (currentMode == CHARGING) lcd.print("Mode: Charging  ");
  else if (currentMode == GRID_BYPASS) lcd.print("Mode: Grid Pass ");
  else if (currentMode == SYNCHRONIZING) lcd.print("Mode: Sync Grid ");
  else if (currentMode == ERROR_STATE) lcd.print("ERR: " + errorMsg);
  
  lcd.setCursor(0,1);
  lcd.print("Vout:"); lcd.print((int)acOutVolts); lcd.print("V  ");
  lcd.print("L:"); lcd.print((int)loadWatts); lcd.print("W ");
  
  lcd.setCursor(0,2);
  lcd.print("Bat:"); lcd.print(batVolts); lcd.print("V ");
  lcd.print("I:"); lcd.print(currentAmps); lcd.print("A ");
  
  lcd.setCursor(0,3);
  lcd.print("ZC:"); 
  if (gridStable) {
    float freq = 500000.0 / halfPeriod;
    lcd.print(freq, 1); lcd.print("Hz ");
  } else {
    lcd.print("No Sync ");
  }
  lcd.print(tempC); lcd.print("C");
}