/*
 * 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

#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

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

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

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

unsigned long lastUpdate = 0;
bool systemOn = false;

// --- 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) {
    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 via H-Bridge (Buck Mode)
    // We drive High Side switches as synchronous buck
    // Fixed frequency, variable duty cycle based on charging algorithm
    digitalWrite(PIN_LO_L, LOW); 
    digitalWrite(PIN_LO_R, LOW);
    
    // Determine charging PWM based on stage
    // For simplicity here, we apply same PWM to both High Sides to act as rectifiers/buck
    // In reality, you sync with mains zero cross, but purely bucking rectified DC is easier.
    // Here we assume Transformer -> Rectified by Body Diodes -> Bucked by MOSFETs
    
    int chargeDuty = (int)(modulationIndex * ICR_VAL); // ModulationIndex reused for charge duty
    OCR1A = chargeDuty;
    OCR1B = chargeDuty; 
  }
  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);
  
  pinMode(BTN_PWR, INPUT_PULLUP);
  pinMode(BTN_ENT, INPUT_PULLUP);
  pinMode(BTN_UP, INPUT_PULLUP);
  pinMode(BTN_DWN, INPUT_PULLUP);

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

  // LCD Init
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0,0);
  lcd.print("   MNG proSystems   ");
  lcd.setCursor(0,1);
  lcd.print(" Inverter UPS v1.0a" );
  lcd.setCursor(0,2);
  lcd.print("  Init SPWM 25kHz...");
  lcd.setCursor(0,3);
  lcd.print("--------------------");
  
  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();
  
  // Update Display Slow
  if (millis() - lastUpdate > 300) {
    updateLCD();
    lastUpdate = millis();
  }
}

// --- Sensor Reading & Math ---
void readSensors() {
  // Simple averaging for DC, RMS calculation for AC
  // NOTE: In real world, use a non-blocking RMS library like EmonLib or manual sampling
  
  // Battery
  batVolts = analogRead(SENS_BAT) * (20.0 / 1023.0); // Adjust Multiplier
  
  // Temp
  int tempRaw = analogRead(SENS_NTC);
  // Simplify NTC math map for example
  tempC = map(tempRaw, 0, 1023, 0, 100); 

  // AC Input Voltage (Simplified PEAK detection for speed)
  // For true RMS, you need a tight loop sampling 100 times over 20ms
  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) - FIXED: Use SENS_CT instead of SENS_AC_IN
  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 > 6100) { // Example 1000W limit
    currentMode = ERROR_STATE;
    errorMsg = "OVERLOAD";
  }

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

    case INVERTER:
      digitalWrite(PIN_RELAY, LOW); // Relay OFF (Inverter Mode)
      
      // Mains Check
      if (acInVolts >= settings.acMin && acInVolts <= settings.acMax) {
        // Grid is good, switch to bypass/charge
        modulationIndex = 0; // Stop Inverting
        delay(100); // Wait before relay switch
        currentMode = GRID_BYPASS;
        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 GRID_BYPASS:
      digitalWrite(PIN_RELAY, HIGH); // Relay ON (Grid Pass-through)
      
      if (acInVolts < settings.acMin || acInVolts > settings.acMax) {
         // Grid lost
         currentMode = INVERTER;
         modulationIndex = 0; // Reset soft start
         return;
      }
      
      // Check if charging needed
      if (batVolts < settings.batFull) {
        currentMode = CHARGING;
      }
      break;

    case CHARGING:
      digitalWrite(PIN_RELAY, HIGH);
      
      // 3 Stage Logic
      // 1. Bulk (Constant Current)
      if (chgStage == BULK) {
        if (batVolts >= 14.0) chgStage = ABSORPTION;
        // Adjust PWM to limit current (using modulationIndex variable as Duty)
        if (currentAmps < 5.0) modulationIndex += 0.001; // Ramp up charge
        else modulationIndex -= 0.001;
      }
      // 2. Absorption (Constant Voltage)
      else if (chgStage == ABSORPTION) {
        if (currentAmps < 1.0) chgStage = FLOAT;
        if (batVolts > settings.batFull) modulationIndex -= 0.001;
        else modulationIndex += 0.001;
      }
      // 3. Float
      else {
        if (batVolts > 13.5) modulationIndex -= 0.001;
        else modulationIndex += 0.001;
      }
      
      // Cap duty cycle
      if (modulationIndex > 0.4) modulationIndex = 0.4; // Limit max charge duty
      if (modulationIndex < 0) modulationIndex = 0;

      // Exit Charging if Mains bad
      if (acInVolts < settings.acMin) currentMode = INVERTER;
      break;

    case ERROR_STATE:
      modulationIndex = 0;
      digitalWrite(PIN_BUZZER, HIGH);
      delay(100);
      digitalWrite(PIN_BUZZER, LOW);
      if (digitalRead(BTN_PWR) == LOW) { // Reset
        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, standard Up/Down/Enter logic
}

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 == 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("Temp:"); lcd.print(tempC); lcd.print("C ");
}