import asyncio
import threading
import time
from .steppable_system import SteppableSystem
from .motors.motors_manager import MotorsManager
from ..utils import constants
from ..utils.files import read_json_file
from abc import ABC, abstractmethod
from enum import Enum
from smbus2 import SMBus, i2c_msg
import serial
import serial.tools.list_ports



def find_pico_port():
    """Find the Raspberry Pi Pico port."""
    PICO_VID = 0x2E8A
    PICO_PID = 0x0005
    ports = serial.tools.list_ports.comports()
    for port in ports:
        if port.vid == PICO_VID and port.pid == PICO_PID:
            return port.device
    return None

def i2c_write(bus, addr, data):
    """Write data to I2C device.
    
    Args:
        bus: SMBus instance
        addr: I2C device address
        data: Data bytes to write
    """
    msg = i2c_msg.write(addr, data)
    bus.i2c_rdwr(msg)

def i2c_read(bus, addr, reg, length):
    """Read data from I2C device register.
    
    Args:
        bus: SMBus instance
        addr: I2C device address
        reg: Register address to read from
        length: Number of bytes to read
    
    Returns:
        list: List of bytes read from the register
    """
    write = i2c_msg.write(addr, [reg])
    read = i2c_msg.read(addr, length)
    bus.i2c_rdwr(write, read)
    return list(read)

# Sample Estimates of Battery Voltage -> State of Charge When Not Charging
ESTIMATED_BATTERY_PERCENTAGES = [
    49.0, # 1%
    50.0,
    50.25,
    50.4,
    50.5, # 5%
    50.55,
    50.60,
    50.65,
    50.70,
    50.75, # 10%
    50.8,
    50.85,
    50.9,
    50.95,
    51.0, # 15%
    51.05,
    51.10,
    51.15,
    51.20,
    51.25, # 20%
    51.30,
    51.35,
    51.40,
    51.45,
    51.50, # 25%
    51.55,
    51.60,
    51.65,
    51.70,
    51.75, # 30%
    51.775,
    51.8,
    51.825,
    51.85,
    51.875, # 35%
    51.90,
    51.925,
    51.95,
    51.975,
    52.0, # 40%
    52.015,
    52.03,
    52.045,
    52.06,
    52.075, # 45%
    52.09,
    52.105,
    52.12,
    52.135,
    52.15, # 50%
    52.165,
    52.18,
    52.195,
    52.21,
    52.225, # 55%
    52.24,
    52.255,
    52.27,
    52.285,
    52.30, # 60%
    52.315,
    52.33,
    52.345,
    52.36,
    52.375, # 65%
    52.39,
    52.405,
    52.42,
    52.435,
    52.45, # 70%
    52.47,
    52.49,
    52.51,
    52.53,
    52.55, # 75%
    52.57,
    52.59,
    52.61,
    52.63,
    52.65, # 80%
    52.67,
    52.69,
    52.71,
    52.73,
    52.75, # 85%
    52.77,
    52.79,
    52.81,
    52.83,
    52.85, # 90%
    52.87,
    52.89,
    52.91,
    52.93,
    52.95, # 95%
    52.97,
    53,
    53.5,
    54.0,
    55.0, # 100%
]

def binary_search_max_above(value, list):
    # Find the first value in the sorted list that is greater than the value
    left = 0
    right = len(list) - 1
    while left <= right:
        mid = (left + right) // 2
        if list[mid] > value:
            right = mid - 1
        else:
            left = mid + 1
    return left

class PowerEvent(Enum):
    """Power system events."""
    POWER_OFF = 1
    POWER_RESTORED = 2

class PowerEventListener(ABC):
    """Abstract base class for power event listeners."""
    @abstractmethod
    def on_power_event(self, event: PowerEvent):
        """Handle power event notification.
        
        Args:
            event (PowerEvent): The power event that occurred
        """
        pass

class BatterySystem(SteppableSystem):
    """Base class for battery monitoring and power event management."""

    def __init__(self):
        super().__init__()
        self.operating_frequency = 4
        self.power_event_listeners = []

    def add_power_event_listener(self, listener):
        """Add a power event listener.
        
        Args:
            listener (PowerEventListener): Listener to receive power event notifications
        """
        self.power_event_listeners.append(listener)

    def remove_power_event_listener(self, listener):
        """Remove a power event listener.
        
        Args:
            listener (PowerEventListener): Listener to remove
        """
        self.power_event_listeners.remove(listener)

    def notify_power_event_listeners(self, event: PowerEvent):
        """Notify all registered listeners of a power event.
        
        Args:
            event (PowerEvent): The power event to notify listeners about
        """
        for listener in self.power_event_listeners:
            listener.on_power_event(event)

    def get_battery_voltage(self):
        """Get current battery voltage.
        
        Returns:
            float: Battery voltage in volts
        """
        pass

    def last_powered_up_time(self):
        """Get the last time the system was powered up.
        
        Returns:
            float: Timestamp of last power-up event
        """
        pass

class DualBatterySystemPico():
    """Battery system implementation for dual battery configuration with Pico monitoring."""

    def __init__(self):
        self.serial_port = find_pico_port()
        self.baud_rate = 115200

        if self.serial_port is None:
            raise Exception("No Pico found")

        # Keep port open so it isn't closed/reopened on every read (reopen can reset Pico)
        self._ser = serial.Serial(self.serial_port, self.baud_rate, timeout=1)

    def get_battery_voltage(self):
        """Get current battery voltage from Pico."""
        self.voltage = float(self._ser.readline().decode("utf-8").strip())
        self.actual_voltage = (self.voltage * 3.3 * 21 / 65535) - 0.7
        return self.actual_voltage

    def close(self):
        """Close the serial port when done."""
        if hasattr(self, "_ser") and self._ser.is_open:
            self._ser.close()

class DualBatterySystem(BatterySystem):
    """Battery system implementation for dual battery configuration with I2C monitoring."""

    I2C_DEV_BUS = 1
    ADS1115_ADDR = 0x48
    CONFIG_REG = 0x01
    CONV_REG = 0x00

    ESTOP_VOLTAGE_THRESHOLD = 45
    
    def __init__(self, config: dict = {}):
        """Initialize dual battery system.
        
        Args:
            config (dict): Configuration dictionary with optional keys:
                - rated_voltage: Battery rated voltage in volts (default: 48)
                - capacity_amp_hours: Battery capacity in amp-hours (default: 50)
                - num_batteries: Number of batteries (default: 2)
        
        Raises:
            Exception: If battery monitoring is not healthy
        """
        super().__init__()
        self.battery_voltage = 0
        self.update_power_state()
        self.cfg = config
        self.rated_voltage = config.get("rated_voltage", 48)
        self.capacity_amp_hours = config.get("capacity_amp_hours", 50)
        self.num_batteries = config.get("num_batteries", 2)
        try:
            self.get_battery_voltage()
        except Exception:
            raise Exception("Warning: Battery monitoring is not healthy.")

    def _step(self):
        """Private method to monitor power state and notify listeners of power events."""
        last_estop_pressed_time = self.last_estop_pressed_time
        last_estop_released_time = self.last_estop_released_time
        self.update_power_state()
        if self.last_estop_pressed_time != last_estop_pressed_time:
            self.notify_power_event_listeners(PowerEvent.POWER_OFF)
        if self.last_estop_released_time != last_estop_released_time:
            self.notify_power_event_listeners(PowerEvent.POWER_RESTORED)

    def update_power_state(self):
        """Update power state from system state file.
        
        Reads power state from JSON file and updates internal state variables.
        
        Raises:
            Exception: If battery monitoring is not healthy
        """
        state = read_json_file(constants.POWER_STATE_PATH)
        print(f"{state=}")
        self.battery_monitoring_healthy = state.get("battery_monitoring_healthy")
        self.current_boot_time = state.get("boot_time")
        self.last_estop_pressed_time = state.get("last_estop_pressed_time")
        self.last_estop_released_time = state.get("last_estop_released_time")
        self.last_reliable_seen_voltage = state.get("last_reliable_seen_voltage")
        self.state_update_time = state.get("state_update_time")


        if not self.battery_monitoring_healthy:
            raise Exception("Battery monitoring is not healthy.")
        
    def get_estimated_battery_percentage(self):
        try:
            state = read_json_file(constants.BATTERY_HISTORY_PATH)
            percentage = state.get("estimated_battery", None)
            if percentage is not None:
                return percentage
        except Exception as e:
            # error reading battery history, return None to indicate unknown percentage
            pass
        voltage = self.get_battery_voltage()
        return binary_search_max_above(voltage, ESTIMATED_BATTERY_PERCENTAGES)


    def is_estop_pressed(self):
        """Check if emergency stop is pressed based on battery voltage.
        
        Returns:
            bool: True if battery voltage is below ESTOP threshold, False otherwise
        """
        return self.get_battery_voltage() < self.ESTOP_VOLTAGE_THRESHOLD

    def last_powered_up_time(self):
        """Get the last time the system was powered up.
        
        Returns the maximum of current boot time and last estop release time.
        
        Returns:
            float: Timestamp of last power-up event
        """
        return max(self.current_boot_time or 0, self.last_estop_released_time or 0)

    def get_battery_voltage(self):
        """Get current battery voltage from I2C ADC.
        
        Reads voltage from ADS1115 ADC via I2C and applies scaling factors.
        
        Returns:
            float: Battery voltage in volts
        """
        with SMBus(self.I2C_DEV_BUS) as bus:
            # Configure ADS1115
            config = [
                self.CONFIG_REG,
                0xC0,  # AIN0, single-shot, PGA bits
                0x83   # 128 SPS, comparator disabled
            ]
            i2c_write(bus, self.ADS1115_ADDR, config)

            # Wait for conversion (~8 ms at 128 SPS)
            time.sleep(0.009)

            # Read conversion register
            data = i2c_read(bus, self.ADS1115_ADDR, self.CONV_REG, 2)

            # Combine bytes (signed 16-bit)
            raw_adc = (data[0] << 8) | data[1]
            if raw_adc & 0x8000:
                raw_adc -= 0x10000

            # Convert to voltage (ADS1115 input)
            voltage = (raw_adc * 6.114) / 32768.0

            # Apply resistor divider scaling (same as C code)
            return voltage * 11
