import time
from pathlib import Path

from .steppable_system import SteppableSystem
from abc import ABC, abstractmethod
from enum import Enum
from smbus2 import SMBus, i2c_msg
from feathersdk.comms.system import run_system_command
from feathersdk.utils.feathertypes import Optional, Tuple, Union
from feathersdk.utils.hydra_helper import load_hydra_config
from feathersdk.utils.logger import info
from dataclasses import dataclass, asdict
import traceback
import tempfile
import json
import os


MIN_VOLTAGE, MAX_VOLTAGE = 49.0, 55.0
ESTOP_VOLTAGE_THRESHOLD = 45
POWER_STATE_PATH = Path("/feathersys/state/power_state.json")
PICO_MAIN_PATH = Path(__file__).parent / "pico_main.py"

# Constants for I2C ADC
I2C_DEV_BUS = 1
ADS1115_ADDR = 0x48
CONFIG_REG = 0x01
CONV_REG = 0x00


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


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)


@dataclass
class BatteryState:
    monitor_start_time: float
    last_estop_pressed_time: float
    last_estop_released_time: float
    last_reliable_seen_voltage: float
    state_update_time: float
    error: Optional[str] = None


def read_last_battery_state() -> BatteryState:
    text = "text wasn't read"
    for _ in range(5):
        try:
            text = POWER_STATE_PATH.read_text()
            if text is not None and text.strip() != "":
                break
        except (PermissionError, FileNotFoundError):
            pass
        time.sleep(0.005)
    try:
        return BatteryState(**json.loads(text))
    except:
        raise ValueError(f"Failed to parse battery state from {POWER_STATE_PATH}: \n\"{text}\"")


class PowerEvent(Enum):
    """Power system events."""
    POWER_OFF = 1
    POWER_RESTORED = 2
    POWER_UNHEALTHY = 3  # When we lose connection to the battery daemon


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


def _get_battery_voltage_pico() -> Optional[float]:
    result = run_system_command(["/usr/bin/python", "-m", "mpremote", "run", PICO_MAIN_PATH], allow_error=True)
    if result is None or result.stdout is None or result.stdout.strip() == "":
        raise ValueError(f"Failed readingvoltage from Pico - got bad result or stdout: {result}, {result.stdout}")
    # voltage is computed battery voltage, adc_read is the raw ADC read value [0-3.3v]
    return list(map(float, result.stdout.strip().split()))[0]


def _get_battery_voltage_adc():
    """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(I2C_DEV_BUS) as bus:
        # Configure ADS1115
        config = [
            CONFIG_REG,
            0xC0,  # AIN0, single-shot, PGA bits
            0x83   # 128 SPS, comparator disabled
        ]
        i2c_write(bus, ADS1115_ADDR, config)

        time.sleep(0.009)
        data = i2c_read(bus, ADS1115_ADDR, 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) and apply resistor divider scaling (same as C code)
        return ((raw_adc * 6.114) / 32768.0) * 11


def get_battery_voltage(read_method: str) -> Union[float, str, None]:
    """Shouldn't call from feathersdk. Only used by battery daemon."""
    try:
        if read_method == "pico":
            ret_val = _get_battery_voltage_pico()
        elif read_method == "adc":
            ret_val = _get_battery_voltage_adc()
        else:
            raise ValueError(f"Invalid read method: {read_method}")
    except Exception as e:
        return traceback.format_exc()
    
    return None if ret_val is None or ret_val < 0.0 or ret_val > ESTOP_VOLTAGE_THRESHOLD * 1.5 else ret_val


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

    def __init__(self, config: dict = {}):
        super().__init__()
        self.operating_frequency = 10.0
        self.power_event_listeners = []
        self.last_state = read_last_battery_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)

    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 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.last_state.last_reliable_seen_voltage < 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.last_state.monitor_start_time, self.last_state.last_estop_released_time)
    
    def _step(self):
        """Update the last battery state."""
        prev_state, self.last_state = self.last_state, read_last_battery_state()
        if self.last_state.last_estop_pressed_time != prev_state.last_estop_pressed_time:
            info(
                f"E-stop pressed (voltage {self.last_state.last_reliable_seen_voltage:.3f} V at {time.time():.3f}s, "
                f"last_estop_pressed_time={self.last_state.last_estop_pressed_time})"
            )
            self.notify_power_event_listeners(PowerEvent.POWER_OFF)
        if self.last_state.last_estop_released_time != prev_state.last_estop_released_time:
            info(
                f"E-stop released (voltage {self.last_state.last_reliable_seen_voltage:.3f} V at {time.time():.3f}s, "
                f"last_estop_released_time={self.last_state.last_estop_released_time})"
            )
            self.notify_power_event_listeners(PowerEvent.POWER_RESTORED)
        if self.last_state.state_update_time - prev_state.state_update_time > 10.0:
            self.notify_power_event_listeners(PowerEvent.POWER_UNHEALTHY)
    
    def get_estimated_battery_percentage(self):
        """Get estimated battery percentage based on the last reliable seen voltage."""
        voltage = max(MIN_VOLTAGE, min(self.last_state.last_reliable_seen_voltage, MAX_VOLTAGE))
        return (voltage - MIN_VOLTAGE) / (MAX_VOLTAGE - MIN_VOLTAGE) * 100

class PowerMonitor():
    """Class used by feather deamon to monitor battery voltage"""

    def __init__(self):
        self.cfg = load_hydra_config("/feathersys/user_config/conf", "system/default.yaml")['system']['power']
        self.adc_type = self.cfg["adc_type"]
        self.hz = self.cfg["hz"]
        self.num_null_reads = 0
        self.last_state = BatteryState(
            monitor_start_time=time.time(),
            last_estop_pressed_time=0,
            last_estop_released_time=0,
            last_reliable_seen_voltage=-1,
            state_update_time=time.time(),
            error=None
        )

        if not POWER_STATE_PATH.parent.exists():
            POWER_STATE_PATH.parent.mkdir(parents=True, exist_ok=True)
            POWER_STATE_PATH.parent.chmod(0o777)
    
    def set_new_state(self, new_voltage: float, error: Optional[str] = None):
        if new_voltage < ESTOP_VOLTAGE_THRESHOLD and self.last_state.last_estop_pressed_time == 0:
            self.last_state.last_estop_pressed_time = time.time()  # If we start below estop threshold, trigger estop
        elif new_voltage < ESTOP_VOLTAGE_THRESHOLD and self.last_state.last_reliable_seen_voltage >= ESTOP_VOLTAGE_THRESHOLD:
            self.last_state.last_estop_pressed_time = time.time()  # Normal triggering of estop
        elif new_voltage > ESTOP_VOLTAGE_THRESHOLD and self.last_state.last_reliable_seen_voltage < ESTOP_VOLTAGE_THRESHOLD:
            self.last_state.last_estop_released_time = time.time()  # Normal releasing of estop
        
        self.last_state.error = error
        self.last_state.last_reliable_seen_voltage = new_voltage
        self.last_state.state_update_time = time.time()

        with tempfile.NamedTemporaryFile(mode="w", delete=False) as f:
            f.write(json.dumps(asdict(self.last_state)))
            path = f.name
        os.rename(path, POWER_STATE_PATH)
        POWER_STATE_PATH.chmod(0o666)
    
    def step(self):
        new_voltage = get_battery_voltage(self.adc_type)

        if isinstance(new_voltage, str):
            self.set_new_state(0.0, error=new_voltage)
            return

        # If we read too many nulls in a row, set monitoring to unhealthy
        if new_voltage is None:
            self.num_null_reads += 1
            if self.num_null_reads > self.hz * 5:  # 5 seconds of null reads
                self.set_new_state(0.0)
                self.num_null_reads = 0
            return
        
        self.set_new_state(new_voltage)
        self.num_null_reads = 0
