/*
 * 48V 150Ah Lead Acid Battery Management System - V1.32
 * 
 * Optimized version of the BMS code to improve compilation speed and memory usage.
 * Core functionality remains intact, and space is left for PWM control of generator starter.
 * 
 * Components:
 * - Arduino Uno
 * - ACS758 100A Current Sensor
 * - Voltage sensors for battery and AC
 * - Temperature sensors for battery and generator
 * - EEPROM for storing calibration values
 * 
 * Created: February 2025
 * Updated: February 2025
 */

// Include necessary libraries
#include <Wire.h>            // For I2C communication
#include <EEPROM.h>          // For EEPROM storage of calibration values
#include <LiquidCrystal_I2C.h> // For LCD display

// Initialize LCD (address 0x27, 16 columns, 2 rows)
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Pin Definitions - Relay Control
const int SOLAR_RELAY_PIN = 2;      // Controls solar array connection
const int GEN_START_PIN = 3;        // Controls generator starter (PWM will be added here later)
const int GEN_CHOKE_PIN = 4;        // Controls generator choke
const int CHARGER_RELAY_PIN = 5;    // Controls AC power to charger
const int GEN_RUN_STOP_PIN = 6;     // Controls generator run/stop (renamed from GEN_STOP_PIN)

// Pin Definitions - Analog Sensors
const int BATTERY_VOLTAGE_PIN = A0; // Battery voltage sensor
const int BATTERY_CURRENT_PIN = A1; // ACS758 current sensor
const int BATTERY_TEMP_PIN = A2;    // Battery temperature sensor
const int GEN_TEMP_PIN = A3;        // Generator temperature sensor
const int AC_VOLTAGE_PIN = A4;      // AC voltage from generator sensor

// Constants - Battery Parameters
const float BATTERY_CAPACITY_AH = 150.0;      // Battery capacity in amp-hours
const float FLOAT_VOLTAGE_BASE = 54.0;        // Base float voltage at 77°F
const float LOW_VOLTAGE_BASE = 47.5;          // Base low voltage threshold at 77°F
const float CRITICAL_VOLTAGE_BASE = 46.0;     // Base critical low voltage at 77°F
const float TEMP_COMPENSATION = -0.028;       // Voltage adjustment per °F above/below 77°F
const float REFERENCE_TEMP_F = 77.0;          // Reference temperature in °F

// Variables for amp-hour tracking
float ampHoursConsumed = 0.0;                 // Net amp-hours consumed from battery (negative = charged)
unsigned long lastAhCalculation = 0;          // Last time amp-hours were calculated
float socFromVoltage = 0.0;                   // SoC calculated from voltage
float socFromAmpHours = 0.0;                  // SoC calculated from amp-hour tracking
const float VOLTAGE_WEIGHT = 0.3;             // Weight for voltage-based SoC in combined calculation
const float AH_WEIGHT = 0.7;                  // Weight for amp-hour based SoC in combined calculation

// Variables - System State
float batteryVoltage = 0.0;         // Current battery voltage
float batteryCurrent = 0.0;         // Current flowing in/out of battery (+ = charging, - = discharging)
float batteryTemp = 0.0;            // Battery temperature in °F
float genTemp = 0.0;                // Generator temperature in °F
float acVoltage = 0.0;              // AC voltage from generator
float stateOfCharge = 0.0;          // Calculated state of charge (0-100%)
bool generatorRunning = false;      // Flag for generator status
bool solarCharging = false;         // Flag for solar charging status

// Variables to track relay states
bool chokeRelayState = false;       // Tracks state of the choke relay
bool chargerRelayState = false;     // Tracks state of the charger relay

// Variables - Calibration (loaded from EEPROM)
float voltageCalibration = 0.0295;  // Default calibration factor for voltage divider
float currentCalibration = 0.195;   // Default calibration factor for current sensor
float tempCalibration = 0.48875;    // Default calibration factor for temperature sensor

// Variables - Timing
unsigned long currentMillis = 0;         // Current time
unsigned long previousMillis = 0;        // Previous time for main loop
unsigned long lcdUpdateMillis = 0;       // Time for LCD updates
const long INTERVAL = 1000;              // Main loop interval (1 second)
const long LCD_UPDATE_INTERVAL = 2000;   // LCD update interval (2 seconds)

// Variables for generator control timing
unsigned long genStartTime = 0;        // When generator start was initiated
unsigned long genRunningTime = 0;      // How long generator has been running
unsigned long maxCrankingTime = 3000;  // Maximum cranking time (3 seconds)
unsigned long chokeTime = 15000;       // Time to keep choke on (15 seconds)
bool generatorStarting = false;        // Flag to track if we're in starting process
bool generatorStopping = false;        // Flag to track if we're in stopping process

// Function prototypes (forward declarations)
void readSensors();
void calculateBatteryStatus();
void updateAmpHourTracking();
void manageSolar();
void manageGenerator();
void applySafetyChecks();
void updateLCD();
void logData();
float mapFloat(float x, float in_min, float in_max, float out_min, float out_max);
void loadCalibrationValues();
void saveCalibrationValues();
void resetAmpHourCounter();
float calculateWeightedSoC();

// Utility function to map a value from one range to another
float mapFloat(float x, float in_min, float in_max, float out_min, float out_max) {
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

// Function to reset amp-hour counter (when battery is fully charged)
void resetAmpHourCounter() {
  ampHoursConsumed = 0.0;
  Serial.println("Amp-hour counter reset - battery fully charged");
}

// Function to calculate weighted SoC from voltage and amp-hour methods
float calculateWeightedSoC() {
  return (socFromVoltage * VOLTAGE_WEIGHT) + (socFromAmpHours * AH_WEIGHT);
}

// Setup function
void setup() {
  // Initialize serial communication
  Serial.begin(9600);
  Serial.println("Battery Management System Initializing...");

  // Initialize LCD
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("BMS Initializing");
  
  // Initialize all relay pins as outputs and set to OFF state
  pinMode(SOLAR_RELAY_PIN, OUTPUT);
  pinMode(GEN_START_PIN, OUTPUT);
  pinMode(GEN_CHOKE_PIN, OUTPUT);
  pinMode(CHARGER_RELAY_PIN, OUTPUT);
  pinMode(GEN_RUN_STOP_PIN, OUTPUT);

  // Set all relays to OFF initially (relays are active LOW)
  // In Wokwi, we want the LEDs to represent the actual device state, not the pin state
  // So we use LOW to turn OFF the relay (and LED), and HIGH to turn it ON
  digitalWrite(SOLAR_RELAY_PIN, LOW);    // Solar relay OFF
  digitalWrite(GEN_START_PIN, LOW);      // Generator start relay OFF
  digitalWrite(GEN_CHOKE_PIN, LOW);      // Generator choke relay OFF
  chokeRelayState = false;               // Track choke state
  digitalWrite(CHARGER_RELAY_PIN, LOW);  // Charger relay OFF
  chargerRelayState = false;             // Track charger state
  digitalWrite(GEN_RUN_STOP_PIN, LOW);   // Generator run/stop relay OFF

  // Set sensor pins as inputs
  pinMode(BATTERY_VOLTAGE_PIN, INPUT);
  pinMode(BATTERY_CURRENT_PIN, INPUT);
  pinMode(BATTERY_TEMP_PIN, INPUT);
  pinMode(GEN_TEMP_PIN, INPUT);
  pinMode(AC_VOLTAGE_PIN, INPUT);

  // Load calibration values from EEPROM
  loadCalibrationValues();

  // Initial sensor readings
  readSensors();
  
  // Initialize amp-hour tracking
  lastAhCalculation = millis();
  
  // Initial battery status calculation
  calculateBatteryStatus();
  
  // Update LCD with initial values
  updateLCD();
  
  Serial.println("Battery Management System Ready!");
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("BMS Ready");
  delay(1000);
}

// Main loop
void loop() {
  // Get current time
  currentMillis = millis();

  // Main loop runs at INTERVAL rate (typically 1 second)
  if (currentMillis - previousMillis >= INTERVAL) {
    previousMillis = currentMillis;

    // 1. Read all sensors
    readSensors();

    // 2. Update amp-hour tracking
    updateAmpHourTracking();

    // 3. Calculate battery status and adjust thresholds for temperature
    calculateBatteryStatus();

    // 4. Apply safety checks
    applySafetyChecks();

    // 5. Manage solar charging
    manageSolar();

    // 6. Manage generator control
    manageGenerator();

    // 7. Log data (commented out for simplicity)
    // logData();
  }
  
  // Update LCD at LCD_UPDATE_INTERVAL rate
  if (currentMillis - lcdUpdateMillis >= LCD_UPDATE_INTERVAL) {
    lcdUpdateMillis = currentMillis;
    updateLCD();
  }
}

// Function to read sensors and convert raw values into measurements
void readSensors() {
  // Read battery voltage (voltage divider) - Map 0-1023 to 45-56V range
  int rawVoltage = analogRead(BATTERY_VOLTAGE_PIN);
  batteryVoltage = mapFloat(rawVoltage, 0, 1023, 45.0, 56.0);

  // Read battery current from ACS758 (100A sensor) - Map 0-1023 to -50A to +50A range
  int rawCurrent = analogRead(BATTERY_CURRENT_PIN);
  batteryCurrent = mapFloat(rawCurrent, 0, 1023, -50.0, 50.0);

  // Read battery temperature from sensor - Map 0-1023 to 50-120°F range
  int rawBatteryTemp = analogRead(BATTERY_TEMP_PIN);
  batteryTemp = mapFloat(rawBatteryTemp, 0, 1023, 50.0, 120.0);

  // Read generator temperature - Map 0-1023 to 60-200°F range
  int rawGenTemp = analogRead(GEN_TEMP_PIN);
  genTemp = mapFloat(rawGenTemp, 0, 1023, 60.0, 200.0);

  // Read AC voltage from generator - Map 0-1023 to 0-240V range
  int rawACVoltage = analogRead(AC_VOLTAGE_PIN);
  acVoltage = mapFloat(rawACVoltage, 0, 1023, 0.0, 240.0);

  // Detect if generator is running based on AC voltage
  // Update the generatorRunning flag
  bool wasRunning = generatorRunning;
  generatorRunning = (acVoltage > 100.0);
  
  // Update the Run/Stop LED whenever the generator state changes
  if (generatorRunning != wasRunning) {
    digitalWrite(GEN_RUN_STOP_PIN, generatorRunning ? HIGH : LOW);
    
    // If generator just started running, turn off starter after a delay
    if (generatorRunning && !wasRunning) {
      // Generator has started, turn off starter motor
      digitalWrite(GEN_START_PIN, LOW);
      
      // Keep choke on for warm-up (15 seconds)
      // In a real implementation, you would use a timer here
      // For simulation, we'll turn it off when generator temp rises
    }
  }

  // Detect if solar is charging (simple threshold check)
  if (!generatorRunning && batteryCurrent > 1.0) {
    solarCharging = true;
  } else {
    solarCharging = false;
  }

  // Simplified debug print at regular intervals
  if ((currentMillis / 1000) % 10 == 0) {
    // Print battery status
    Serial.println("-------- BMS STATUS --------");
    Serial.print("Battery: ");
    Serial.print(batteryVoltage, 1);
    Serial.print("V, ");
    Serial.print(batteryCurrent, 1);
    Serial.print("A, ");
    Serial.print(batteryTemp, 1);
    Serial.print("°F, SOC: ");
    Serial.print(stateOfCharge, 1);
    Serial.println("%");
    
    // Print generator status
    Serial.print("Generator: ");
    if (generatorRunning) {
      Serial.print("RUNNING, ");
    } else if (generatorStarting) {
      Serial.print("STARTING, ");
    } else {
      Serial.print("OFF, ");
    }
    Serial.print(genTemp, 1);
    Serial.print("°F, AC: ");
    Serial.print(acVoltage, 1);
    Serial.println("V");
    
    // Print system status
    Serial.print("System: ");
    if (digitalRead(SOLAR_RELAY_PIN) == HIGH) {
      Serial.print("Solar CONNECTED, ");
    } else {
      Serial.print("Solar DISCONNECTED, ");
    }
    
    if (digitalRead(CHARGER_RELAY_PIN) == HIGH) {
      Serial.print("Charger ON, ");
    } else {
      Serial.print("Charger OFF, ");
    }
    
    if (chokeRelayState) {
      Serial.print("Choke ON");
    } else {
      Serial.print("Choke OFF");
    }
    Serial.println();
    
    // Print amp-hour data (simplified)
    Serial.print("Amp-hours consumed: ");
    Serial.print(ampHoursConsumed, 2);
    Serial.println("Ah");
    
    Serial.println("---------------------------");
  }
}

// Function to update amp-hour tracking
void updateAmpHourTracking() {
  unsigned long currentTime = millis();
  float timeElapsedHours = (float)(currentTime - lastAhCalculation) / 3600000.0; // Convert ms to hours
  
  // Integrate current over time to get amp-hours
  // Negative current = charging, positive current = discharging
  ampHoursConsumed -= batteryCurrent * timeElapsedHours;
  
  // Calculate SoC based on amp-hours
  socFromAmpHours = 100.0 * (1.0 - (ampHoursConsumed / BATTERY_CAPACITY_AH));
  
  // Constrain SoC between 0-100%
  socFromAmpHours = constrain(socFromAmpHours, 0.0, 100.0);
  
  // Update timestamp for next calculation
  lastAhCalculation = currentTime;
  
  // Reset counter if we detect full charge from voltage
  if (batteryVoltage >= FLOAT_VOLTAGE_BASE && batteryCurrent > -2.0 && batteryCurrent < 2.0) {
    resetAmpHourCounter();
  }
}

// Function to calculate the battery state of charge and adjust thresholds
void calculateBatteryStatus() {
  // Apply temperature compensation to voltage thresholds
  float tempOffset = (batteryTemp - REFERENCE_TEMP_F) * TEMP_COMPENSATION;
  float floatVoltage = FLOAT_VOLTAGE_BASE + tempOffset;
  float lowVoltageThreshold = LOW_VOLTAGE_BASE + tempOffset;
  float criticalVoltage = CRITICAL_VOLTAGE_BASE + tempOffset;

  // Update state of charge based on voltage
  if (batteryVoltage >= floatVoltage) {
    socFromVoltage = 100.0;
  } else if (batteryVoltage <= criticalVoltage) {
    socFromVoltage = 0.0;
  } else {
    socFromVoltage = mapFloat(batteryVoltage, criticalVoltage, floatVoltage, 0.0, 100.0);
  }
  
  // Calculate combined SoC using weighted average of voltage and amp-hour methods
  stateOfCharge = calculateWeightedSoC();
}

// Function to manage solar charging (simplified)
void manageSolar() {
  // Connect solar if battery is not fully charged and temperature is safe
  if (batteryVoltage >= FLOAT_VOLTAGE_BASE || batteryTemp >= 113.0) {
    digitalWrite(SOLAR_RELAY_PIN, LOW);  // Disconnect solar (LED OFF)
    Serial.println("Solar disconnected.");
  } else if (batteryVoltage < (FLOAT_VOLTAGE_BASE - 1.0) && batteryTemp < 113.0) {
    digitalWrite(SOLAR_RELAY_PIN, HIGH); // Connect solar (LED ON)
    Serial.println("Solar connected.");
  }
}

// Function to manage the generator
void manageGenerator() {
  // Get current time for timing control
  unsigned long now = millis();
  
  // STARTING LOGIC - Start generator if voltage is low or state of charge is below 20%
  if ((batteryVoltage <= LOW_VOLTAGE_BASE || stateOfCharge <= 80.0) && !generatorRunning && !generatorStarting) {
    // Begin generator starting sequence
    generatorStarting = true;
    genStartTime = now;
    
    // Turn on starter and choke
    digitalWrite(GEN_START_PIN, HIGH);   // Start generator (Starter LED ON)
    digitalWrite(GEN_CHOKE_PIN, HIGH);   // Engage choke (Choke LED ON)
    chokeRelayState = true;              // Update choke state
    Serial.println("Generator starting with choke.");
  }
  
  // CRANKING TIMEOUT - If cranking too long without starting
  if (generatorStarting && !generatorRunning && (now - genStartTime > maxCrankingTime)) {
    // Turn off starter if we've been cranking too long
    digitalWrite(GEN_START_PIN, LOW);    // Stop cranking (Starter LED OFF)
    Serial.println("Cranking timeout - giving starter a rest.");
    
    // Retry cranking after a rest period (in a real system)
    // For simulation: keep choke on for when we retry
  }
  
  // RUNNING STATE - Generator is now running
  if (generatorRunning) {
    // If this is the first time we detected the generator running
    if (generatorStarting) {
      generatorStarting = false;
      genRunningTime = now;
      digitalWrite(GEN_START_PIN, LOW);  // Stop cranking (Starter LED OFF)
      // Keep choke on
      Serial.println("Generator started successfully, stabilizing...");
    }
    
    // Add stabilization delay before engaging charger (30 seconds)
    if (now - genRunningTime > 5000 && !chargerRelayState) {
      digitalWrite(CHARGER_RELAY_PIN, HIGH); // Enable charger (Charger LED ON)
      chargerRelayState = true;              // Update charger state
      Serial.println("Generator stabilized, charging enabled.");
    }
    
    // Turn off choke after warm-up period or when generator is warm enough
    if (chokeRelayState && 
        (now - genRunningTime > chokeTime || genTemp > 120.0)) {
      digitalWrite(GEN_CHOKE_PIN, LOW);  // Disengage choke (Choke LED OFF)
      chokeRelayState = false;           // Update choke state
      Serial.println("Generator warm, choke disengaged.");
    }
  }
  
  // STOPPING LOGIC - Stop generator when battery is charged or generator has run long enough
  if ((batteryVoltage >= (FLOAT_VOLTAGE_BASE - 1.0) || stateOfCharge >= 95.0) && 
      generatorRunning && !generatorStopping) {
    // Begin generator stopping sequence
    generatorStopping = true;
    
    digitalWrite(CHARGER_RELAY_PIN, LOW);  // Disable charger (Charger LED OFF)
    chargerRelayState = false;           // Update charger state
    // Use the Run/Stop pin to stop the generator
    // Generator will be detected as stopped when AC voltage drops
    digitalWrite(GEN_RUN_STOP_PIN, LOW);   // Signal stop (Run/Stop LED OFF)
    Serial.println("Generator stopping.");
  }
  
  // STOPPED STATE - Generator is now stopped
  if (!generatorRunning && generatorStopping) {
    generatorStopping = false;
    // Ensure all generator-related outputs are reset
    digitalWrite(GEN_START_PIN, LOW);      // Starter OFF
    digitalWrite(GEN_CHOKE_PIN, LOW);      // Choke OFF
    chokeRelayState = false;               // Update choke state
    digitalWrite(CHARGER_RELAY_PIN, LOW);  // Charger OFF
    chargerRelayState = false;             // Update charger state
    digitalWrite(GEN_RUN_STOP_PIN, LOW);   // Run/Stop OFF
    Serial.println("Generator stopped.");
  }
}

// Function to apply safety checks (simplified)
void applySafetyChecks() {
  if (batteryVoltage <= CRITICAL_VOLTAGE_BASE) {
    Serial.println("Critical voltage! Shutting down.");
    digitalWrite(SOLAR_RELAY_PIN, LOW);     // Disconnect solar (LED OFF)
    digitalWrite(CHARGER_RELAY_PIN, LOW);   // Disable charger (LED OFF)
    chargerRelayState = false;              // Update charger state
  }
  
  // Safety check for generator temperature
  if (genTemp >= 190.0 && generatorRunning) {
    Serial.println("Generator overheating! Shutting down.");
    digitalWrite(CHARGER_RELAY_PIN, LOW);   // Disable charger (LED OFF)
    chargerRelayState = false;              // Update charger state
    digitalWrite(GEN_RUN_STOP_PIN, LOW);    // Stop generator (Run/Stop LED OFF)
    digitalWrite(GEN_START_PIN, LOW);       // Reset starter relay (LED OFF)
    digitalWrite(GEN_CHOKE_PIN, LOW);       // Turn off choke (Choke LED OFF)
    chokeRelayState = false;                // Update choke state
  }
}

// Function to update LCD display
void updateLCD() {
  lcd.clear();
  
  // First row: Battery voltage and SOC
  lcd.setCursor(0, 0);
  lcd.print("V:");
  lcd.print(batteryVoltage, 1);
  lcd.print("V ");
  lcd.print("SOC:");
  lcd.print(stateOfCharge, 0);
  lcd.print("%");
  
  // Second row: Current and status
  lcd.setCursor(0, 1);
  lcd.print("I:");
  lcd.print(batteryCurrent, 1);
  lcd.print("A ");
  
  // Show system status
  if (generatorStarting) {
    lcd.print("STARTING");
  } else if (generatorRunning) {
    if (chokeRelayState) {
      lcd.print("GEN:WARM");
    } else {
      lcd.print("GEN:RUN");
    }
  } else if (solarCharging) {
    lcd.print("SOLAR");
  } else {
    lcd.print("IDLE");
  }
}

// Function to load calibration values from EEPROM
void loadCalibrationValues() {
  // In real implementation, you would use EEPROM.get() to read floating point values
  // For this simulation, we'll just use the default values
  /*
  EEPROM.get(0, voltageCalibration);
  EEPROM.get(4, currentCalibration);
  EEPROM.get(8, tempCalibration);
  */
}

// Function to save calibration values to EEPROM
void saveCalibrationValues() {
  // In real implementation, you would use EEPROM.put() to write floating point values
  /*
  EEPROM.put(0, voltageCalibration);
  EEPROM.put(4, currentCalibration);
  EEPROM.put(8, tempCalibration);
  */
}