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

// Define I2C pins for LCD
#define I2C_SDA 21
#define I2C_SCL 22

// Initialize LCD with I2C address 0x27
LiquidCrystal_I2C lcd(0x27, 20, 4);

// Define H-Bridge pins
#define H_BRIDGE_PIN_1 12
#define H_BRIDGE_PIN_2 13
#define H_BRIDGE_PIN_3 14
#define H_BRIDGE_PIN_4 15

// Define charging current control pin
#define CHARGING_CURRENT_PIN 32

// Define SPWM parameters
const float modulatingFrequency = 50.0;  // 50Hz modulation frequency
const float carrierFrequency = 23400.0;  // 23.4kHz carrier frequency

// Timing variables for SPWM
unsigned long lastModulationTime = 0;
float modulationStep = (2 * PI * modulatingFrequency) / 1000.0;
int modulationIndex = 0;

// Smart Charger parameters
int chargeStage = 0;  // 0: Bulk, 1: Absorption, 2: Float

void setup() {
    // Initialize serial communication
    Serial.begin(115200);

    // Initialize LCD
    Wire.begin(I2C_SDA, I2C_SCL);
    lcd.begin(20, 4);
    lcd.backlight();

    // Initialize H-Bridge pins
    pinMode(H_BRIDGE_PIN_1, OUTPUT);
    pinMode(H_BRIDGE_PIN_2, OUTPUT);
    pinMode(H_BRIDGE_PIN_3, OUTPUT);
    pinMode(H_BRIDGE_PIN_4, OUTPUT);

    // Initialize charging current pin
    pinMode(CHARGING_CURRENT_PIN, OUTPUT);

    // Display setup information on LCD
    lcd.setCursor(0, 0);
    lcd.print("Hybrid Inverter Init");
}

void loop() {
    // Generate SPWM signal
    generateSPWM();

    // Update charger status
    manageCharger();

    // Update LCD display
    updateLCD();
}

void generateSPWM() {
    unsigned long currentTime = millis();
    if (currentTime - lastModulationTime >= 1000 / carrierFrequency) {
        lastModulationTime = currentTime;
        
        // Generate sine wave modulation index
        float sineValue = sin(modulationIndex * modulationStep);
        modulationIndex = (modulationIndex + 1) % 1000;

        // Calculate duty cycle for SPWM
        int dutyCycle = map(sineValue, -1.0, 1.0, 0, 255);

        // Apply duty cycle to H-Bridge
        analogWrite(H_BRIDGE_PIN_1, dutyCycle);
        analogWrite(H_BRIDGE_PIN_2, 255 - dutyCycle);
        analogWrite(H_BRIDGE_PIN_3, dutyCycle);
        analogWrite(H_BRIDGE_PIN_4, 255 - dutyCycle);
    }
}

void manageCharger() {
    switch (chargeStage) {
        case 0:  // Bulk charging
            // Set high charging current
            analogWrite(CHARGING_CURRENT_PIN, 200);  // Programmable current
            lcd.setCursor(0, 1);
            lcd.print("Charging: Bulk ");
            break;
        case 1:  // Absorption charging
            // Set medium charging current
            analogWrite(CHARGING_CURRENT_PIN, 150);
            lcd.setCursor(0, 1);
            lcd.print("Charging: Absorp");
            break;
        case 2:  // Float charging
            // Set low charging current
            analogWrite(CHARGING_CURRENT_PIN, 100);
            lcd.setCursor(0, 1);
            lcd.print("Charging: Float ");
            break;
    }

    // Dummy logic to switch charge stages for testing
    if (millis() > 5000 && chargeStage == 0) chargeStage = 1;
    if (millis() > 10000 && chargeStage == 1) chargeStage = 2;
}

void updateLCD() {
    lcd.setCursor(0, 2);
    lcd.print("Battery Voltage: ");
    // Add battery voltage read and display code here

    lcd.setCursor(0, 3);
    lcd.print("AC Input Status: ");
    // Add AC input status read and display code here
}