"""
48V 150Ah Lead Acid Battery Management System - V1.0 for Raspberry Pi Pico
Wokwi Simulation Version - Modified for potentiometer inputs and LED outputs

Components:
- Raspberry Pi Pico
- 4x Potentiometers for simulating sensors:
  - Battery Voltage (ADC0/GP26)     - Range: 45-56V
  - Battery Current (ADC1/GP27)     - Range: -50A to +50A
  - Battery Temperature (ADC2/GP28) - Range: 50-120°F
  - Generator Temperature (ADC3/GP29) - Range: 60-200°F
- 5x LEDs to simulate outputs:
  - SOLAR_RELAY (2)  - Green
  - GEN_START (3)    - Red
  - GEN_CHOKE (4)    - Blue
  - CHARGER_RELAY (5) - Yellow
  - GEN_RUN_STOP (6) - White
- I2C LCD (1602) connected to GP8(SDA) and GP9(SCL)

Created: February 2025
"""

import machine
import utime
from machine import Pin, ADC, I2C
from time import sleep
import math

# Import LCD library for Wokwi simulation
# In Wokwi, we'll use the pre-defined I2C LCD library
from lcd_i2c import LCD

# Pin Definitions - Relay Control (All are LEDs in the simulation)
SOLAR_RELAY_PIN = 2      # GREEN LED - Controls solar array connection
GEN_START_PIN = 3        # RED LED - Controls generator starter
GEN_CHOKE_PIN = 4        # BLUE LED - Controls generator choke
CHARGER_RELAY_PIN = 5    # YELLOW LED - Controls AC power to charger
GEN_RUN_STOP_PIN = 6     # WHITE LED - Controls generator run/stop

# Pin Definitions - Analog Sensors (using ADC pins on Pico)
# ADC pins on Raspberry Pi Pico are: GP26 (ADC0), GP27 (ADC1), GP28 (ADC2), GP29 (ADC3)
BATTERY_VOLTAGE_PIN = 26  # ADC0 - Battery voltage sensor (45-56V)
BATTERY_CURRENT_PIN = 27  # ADC1 - Battery current sensor (-50A to +50A)
BATTERY_TEMP_PIN = 28     # ADC2 - Battery temperature sensor (50-120°F)
GEN_TEMP_PIN = 29         # ADC3 - Generator temperature sensor (60-200°F)

# LED pin (onboard LED on Pico)
LED_PIN = 25

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

# Variables for amp-hour tracking
amp_hours_consumed = 0.0          # Net amp-hours consumed from battery (negative = charged)
last_ah_calculation = 0           # Last time amp-hours were calculated
soc_from_voltage = 0.0            # SoC calculated from voltage
soc_from_amp_hours = 0.0          # SoC calculated from amp-hour tracking
VOLTAGE_WEIGHT = 0.3              # Weight for voltage-based SoC in combined calculation
AH_WEIGHT = 0.7                   # Weight for amp-hour based SoC in combined calculation

# Variables - System State
battery_voltage = 0.0         # Current battery voltage
battery_current = 0.0         # Current flowing in/out of battery (+ = charging, - = discharging)
battery_temp = 0.0            # Battery temperature in °F
gen_temp = 0.0                # Generator temperature in °F
ac_voltage = 0.0              # AC voltage from generator (simulated from battery voltage in this demo)
state_of_charge = 0.0         # Calculated state of charge (0-100%)
generator_running = False     # Flag for generator status
solar_charging = False        # Flag for solar charging status

# Variables to track relay states
choke_relay_state = False     # Tracks state of the choke relay
charger_relay_state = False   # Tracks state of the charger relay

# Variables - Timing
current_millis = 0            # Current time
previous_millis = 0           # Previous time for main loop
lcd_update_millis = 0         # Time for LCD updates
INTERVAL = 1000               # Main loop interval (1 second) in ms
LCD_UPDATE_INTERVAL = 2000    # LCD update interval (2 seconds) in ms

# Variables for generator control timing
gen_start_time = 0            # When generator start was initiated
gen_running_time = 0          # How long generator has been running
max_cranking_time = 3000      # Maximum cranking time (3 seconds) in ms
choke_time = 15000            # Time to keep choke on (15 seconds) in ms
generator_starting = False    # Flag to track if we're in starting process
generator_stopping = False    # Flag to track if we're in stopping process

# Initialize pins
solar_relay = Pin(SOLAR_RELAY_PIN, Pin.OUT)
gen_start = Pin(GEN_START_PIN, Pin.OUT)
gen_choke = Pin(GEN_CHOKE_PIN, Pin.OUT)
charger_relay = Pin(CHARGER_RELAY_PIN, Pin.OUT)
gen_run_stop = Pin(GEN_RUN_STOP_PIN, Pin.OUT)
led = Pin(LED_PIN, Pin.OUT)

# Initialize all relays to OFF state
solar_relay.value(0)
gen_start.value(0)
gen_choke.value(0)
charger_relay.value(0)
gen_run_stop.value(0)

# Initialize ADC pins (potentiometers in the simulation)
battery_voltage_adc = ADC(Pin(BATTERY_VOLTAGE_PIN))
battery_current_adc = ADC(Pin(BATTERY_CURRENT_PIN))
battery_temp_adc = ADC(Pin(BATTERY_TEMP_PIN))
gen_temp_adc = ADC(Pin(GEN_TEMP_PIN))

# Initialize I2C and LCD
i2c = I2C(0, sda=Pin(8), scl=Pin(9), freq=400000)
try:
    lcd = LCD(i2c, addr=0x27, cols=16, rows=2)
    lcd_available = True
    print("LCD initialized")
except Exception as e:
    print(f"LCD initialization error: {e}")
    lcd_available = False
    print("Using console output")

# Utility function to map a value from one range to another
def map_value(x, in_min, in_max, out_min, 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)
def reset_amp_hour_counter():
    global amp_hours_consumed
    amp_hours_consumed = 0.0
    print("Amp-hour counter reset - battery fully charged")

# Function to calculate weighted SoC from voltage and amp-hour methods
def calculate_weighted_soc():
    return (soc_from_voltage * VOLTAGE_WEIGHT) + (soc_from_amp_hours * AH_WEIGHT)

# Function to read sensors and convert raw values into measurements
def read_sensors():
    global battery_voltage, battery_current, battery_temp, gen_temp, ac_voltage
    global generator_running, solar_charging
    
    # Read battery voltage (voltage divider) - Map potentiometer to 45-56V range
    raw_voltage = battery_voltage_adc.read_u16()
    battery_voltage = map_value(raw_voltage, 0, 65535, 45.0, 56.0)
    
    # Read battery current from ACS758 - Map potentiometer to -50A to +50A range
    raw_current = battery_current_adc.read_u16()
    battery_current = map_value(raw_current, 0, 65535, -50.0, 50.0)
    
    # Read battery temperature - Map potentiometer to 50-120°F range
    raw_battery_temp = battery_temp_adc.read_u16()
    battery_temp = map_value(raw_battery_temp, 0, 65535, 50.0, 120.0)
    
    # Read generator temperature - Map potentiometer to 60-200°F range
    raw_gen_temp = gen_temp_adc.read_u16()
    gen_temp = map_value(raw_gen_temp, 0, 65535, 60.0, 200.0)
    
    # Simulate AC voltage from generator based on battery voltage
    # For simulation, we'll consider the generator running if the Run/Stop LED is ON
    ac_voltage = 0.0  # Default to zero
    
    # If the generator run LED is on, simulate some AC voltage
    if gen_run_stop.value() == 1:
        # Generate AC voltage value between 110-130V if run signal is ON
        # Use some randomness based on battery voltage to simulate real-world variability
        ac_voltage = 110.0 + (battery_voltage - 45.0) * 2.0
    
    # Detect if generator is running based on AC voltage
    was_running = generator_running
    generator_running = (ac_voltage > 100.0)
    
    # Detect if solar is charging (simple threshold check)
    if not generator_running and battery_current > 1.0:
        solar_charging = True
    else:
        solar_charging = False
    
    # Debug print at regular intervals (every 10 seconds)
    if (utime.ticks_ms() // 1000) % 10 == 0:
        # Print battery status
        print("-------- BMS STATUS --------")
        print(f"Battery: {battery_voltage:.1f}V, {battery_current:.1f}A, {battery_temp:.1f}°F, SOC: {state_of_charge:.1f}%")
        
        # Print generator status
        print("Generator: ", end="")
        if generator_running:
            print(f"RUNNING, ", end="")
        elif generator_starting:
            print(f"STARTING, ", end="")
        else:
            print(f"OFF, ", end="")
        print(f"{gen_temp:.1f}°F, AC: {ac_voltage:.1f}V")
        
        # Print system status
        print("System: ", end="")
        if solar_relay.value() == 1:
            print("Solar CONNECTED, ", end="")
        else:
            print("Solar DISCONNECTED, ", end="")
        
        if charger_relay.value() == 1:
            print("Charger ON, ", end="")
        else:
            print("Charger OFF, ", end="")
        
        if choke_relay_state:
            print("Choke ON")
        else:
            print("Choke OFF")
        
        # Print amp-hour data (simplified)
        print(f"Amp-hours consumed: {amp_hours_consumed:.2f}Ah")
        print("---------------------------")

# Function to update amp-hour tracking
def update_amp_hour_tracking():
    global amp_hours_consumed, soc_from_amp_hours, last_ah_calculation
    
    current_time = utime.ticks_ms()
    time_elapsed_hours = utime.ticks_diff(current_time, last_ah_calculation) / 3600000.0  # Convert ms to hours
    
    # Integrate current over time to get amp-hours
    # Negative current = charging, positive current = discharging
    amp_hours_consumed -= battery_current * time_elapsed_hours
    
    # Calculate SoC based on amp-hours
    soc_from_amp_hours = 100.0 * (1.0 - (amp_hours_consumed / BATTERY_CAPACITY_AH))
    
    # Constrain SoC between 0-100%
    soc_from_amp_hours = max(0.0, min(soc_from_amp_hours, 100.0))
    
    # Update timestamp for next calculation
    last_ah_calculation = current_time
    
    # Reset counter if we detect full charge from voltage
    if battery_voltage >= FLOAT_VOLTAGE_BASE and battery_current > -2.0 and battery_current < 2.0:
        reset_amp_hour_counter()

# Function to calculate the battery state of charge and adjust thresholds
def calculate_battery_status():
    global soc_from_voltage, state_of_charge
    
    # Apply temperature compensation to voltage thresholds
    temp_offset = (battery_temp - REFERENCE_TEMP_F) * TEMP_COMPENSATION
    float_voltage = FLOAT_VOLTAGE_BASE + temp_offset
    low_voltage_threshold = LOW_VOLTAGE_BASE + temp_offset
    critical_voltage = CRITICAL_VOLTAGE_BASE + temp_offset
    
    # Update state of charge based on voltage
    if battery_voltage >= float_voltage:
        soc_from_voltage = 100.0
    elif battery_voltage <= critical_voltage:
        soc_from_voltage = 0.0
    else:
        soc_from_voltage = map_value(battery_voltage, critical_voltage, float_voltage, 0.0, 100.0)
    
    # Calculate combined SoC using weighted average of voltage and amp-hour methods
    state_of_charge = calculate_weighted_soc()

# Function to manage solar charging (simplified)
def manage_solar():
    # Connect solar if battery is not fully charged and temperature is safe
    if battery_voltage >= FLOAT_VOLTAGE_BASE or battery_temp >= 113.0:
        solar_relay.value(0)  # Disconnect solar
        print("Solar disconnected.")
    elif battery_voltage < (FLOAT_VOLTAGE_BASE - 1.0) and battery_temp < 113.0:
        solar_relay.value(1)  # Connect solar
        print("Solar connected.")

# Function to manage the generator
def manage_generator():
    global generator_starting, gen_start_time, choke_relay_state, generator_running
    global gen_running_time, charger_relay_state, generator_stopping
    
    # Get current time for timing control
    now = utime.ticks_ms()
    
    # STARTING LOGIC - Start generator if voltage is low or state of charge is below 80%
    if (battery_voltage <= LOW_VOLTAGE_BASE or state_of_charge <= 80.0) and not generator_running and not generator_starting:
        # Begin generator starting sequence
        generator_starting = True
        gen_start_time = now
        
        # Turn on starter and choke
        gen_start.value(1)    # Start generator
        gen_choke.value(1)    # Engage choke
        gen_run_stop.value(1) # Turn on Run/Stop LED to indicate we want generator running
        choke_relay_state = True
        print("Generator starting with choke.")
    
    # CRANKING TIMEOUT - If cranking too long without starting
    if generator_starting and not generator_running and utime.ticks_diff(now, gen_start_time) > max_cranking_time:
        # Turn off starter if we've been cranking too long
        gen_start.value(0)    # Stop cranking
        print("Cranking timeout - giving starter a rest.")
        # Keep the Run/Stop signal ON - we still want the generator to run
        gen_run_stop.value(1)
    
    # RUNNING STATE - Generator is now running
    if generator_running:
        # If this is the first time we detected the generator running
        if generator_starting:
            generator_starting = False
            gen_running_time = now
            gen_start.value(0)  # Stop cranking
            # Keep choke on
            # Ensure Run/Stop is ON while generator is running
            gen_run_stop.value(1)
            print("Generator started successfully, stabilizing...")
        
        # Add stabilization delay before engaging charger (5 seconds)
        if utime.ticks_diff(now, gen_running_time) > 5000 and not charger_relay_state:
            charger_relay.value(1)  # Enable charger
            charger_relay_state = True
            print("Generator stabilized, charging enabled.")
        
        # Turn off choke after warm-up period or when generator is warm enough
        if choke_relay_state and (
                utime.ticks_diff(now, gen_running_time) > choke_time or gen_temp > 120.0):
            gen_choke.value(0)  # Disengage choke
            choke_relay_state = False
            print("Generator warm, choke disengaged.")
    
    # STOPPING LOGIC - Stop generator when battery is charged or generator has run long enough
    if (battery_voltage >= (FLOAT_VOLTAGE_BASE - 1.0) or state_of_charge >= 95.0) and generator_running and not generator_stopping:
        # Begin generator stopping sequence
        generator_stopping = True
        
        charger_relay.value(0)  # Disable charger
        charger_relay_state = False
        # Use the Run/Stop pin to stop the generator
        gen_run_stop.value(0)   # Signal generator to stop by turning Run/Stop OFF
        print("Generator stopping.")
    
    # STOPPED STATE - Generator is now stopped
    if not generator_running and generator_stopping:
        generator_stopping = False
        # Ensure all generator-related outputs are reset
        gen_start.value(0)      # Starter OFF
        gen_choke.value(0)      # Choke OFF
        choke_relay_state = False
        charger_relay.value(0)  # Charger OFF
        charger_relay_state = False
        gen_run_stop.value(0)   # Run/Stop OFF
        print("Generator stopped.")

# Function to apply safety checks (simplified)
def apply_safety_checks():
    global charger_relay_state, choke_relay_state
    
    if battery_voltage <= CRITICAL_VOLTAGE_BASE:
        print("Critical voltage! Shutting down.")
        solar_relay.value(0)    # Disconnect solar
        charger_relay.value(0)  # Disable charger
        charger_relay_state = False
    
    # Safety check for generator temperature
    if gen_temp >= 190.0 and generator_running:
        print("Generator overheating! Shutting down.")
        charger_relay.value(0)   # Disable charger
        charger_relay_state = False
        gen_run_stop.value(0)    # Stop generator
        gen_start.value(0)       # Reset starter relay
        gen_choke.value(0)       # Turn off choke
        choke_relay_state = False

# Function to update LCD display
def update_lcd():
    if not lcd_available:
        return
        
    # Clear LCD and write new content
    lcd.clear()
    
    # First row: Battery voltage and SOC
    lcd.move_to(0, 0)
    lcd.putstr(f"V:{battery_voltage:.1f}V SOC:{state_of_charge:.0f}%")
    
    # Second row: Current and status
    lcd.move_to(0, 1)
    status_text = f"I:{battery_current:.1f}A "
    
    # Show system status
    if generator_starting:
        status_text += "STARTING"
    elif generator_running:
        if choke_relay_state:
            status_text += "GEN:WARM"
        else:
            status_text += "GEN:RUN"
    elif solar_charging:
        status_text += "SOLAR"
    else:
        status_text += "IDLE"
    
    lcd.putstr(status_text)

# Setup function - runs once at startup
def setup():
    global last_ah_calculation
    
    print("Battery Management System Initializing...")
    
    if lcd_available:
        lcd.clear()
        lcd.move_to(0, 0)
        lcd.putstr("BMS Initializing")
    
    # Initial sensor readings
    read_sensors()
    
    # Initialize amp-hour tracking
    last_ah_calculation = utime.ticks_ms()
    
    # Initial battery status calculation
    calculate_battery_status()
    
    # Update LCD with initial values
    update_lcd()
    
    print("Battery Management System Ready!")
    if lcd_available:
        lcd.clear()
        lcd.move_to(0, 0)
        lcd.putstr("BMS Ready")
    sleep(1)

# Main function to run the BMS
def main_loop():
    global current_millis, previous_millis, lcd_update_millis
    
    while True:
        # Get current time
        current_millis = utime.ticks_ms()
        
        # Toggle LED to show system is running
        led.toggle()
        
        # Main processing runs at INTERVAL rate (typically 1 second)
        if utime.ticks_diff(current_millis, previous_millis) >= INTERVAL:
            previous_millis = current_millis
            
            # 1. Read all sensors
            read_sensors()
            
            # 2. Update amp-hour tracking
            update_amp_hour_tracking()
            
            # 3. Calculate battery status and adjust thresholds for temperature
            calculate_battery_status()
            
            # 4. Apply safety checks
            apply_safety_checks()
            
            # 5. Manage solar charging
            manage_solar()
            
            # 6. Manage generator control
            manage_generator()
        
        # Update LCD at LCD_UPDATE_INTERVAL rate
        if utime.ticks_diff(current_millis, lcd_update_millis) >= LCD_UPDATE_INTERVAL:
            lcd_update_millis = current_millis
            update_lcd()
        
        # Small delay to prevent hogging CPU
        sleep(0.01)

# Run the program
if __name__ == "__main__":
    setup()
    try:
        main_loop()
    except KeyboardInterrupt:
        print("Program terminated by user")
        # Turn off all relays for safety
        solar_relay.value(0)
        gen_start.value(0)
        gen_choke.value(0)
        charger_relay.value(0)
        gen_run_stop.value(0)