#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
电源管理模块（INA226）
提供电压、电流监测和充电状态检测
"""
import config
import os
import subprocess
from logger_manager import logger_manager
from maix import time as maix_time


_INA226_PRESENT = None


def _ina226_ready() -> bool:
    """
    是否允许访问 INA226。

    重要：
    - 这里刻意不做任何 I2C 探测/读写。
    - 经验上，在 INA226 未供电/未响应时，I2C 的 readfrom_mem 可能直接触发底层崩溃（SIGSEGV），try/except 无法拦截。
    - 因此只在开机 init_ina226() 成功后才允许后续读电压/电流。
    """
    return bool(getattr(config, "INA226_ENABLE", True)) and (_INA226_PRESENT is True)


def write_register(reg, value):
    """写入INA226寄存器"""
    from hardware import hardware_manager
    logger = logger_manager.logger
    data = [(value >> 8) & 0xFF, value & 0xFF]
    # 某些底层驱动在失败时只打印 “write failed” 并返回 -1，而不是抛异常；
    # 为避免误判“初始化成功”导致后续 readfrom_mem SIGSEGV，这里把失败显式转成异常。
    ret = hardware_manager.bus.writeto_mem(config.INA226_ADDR, reg, bytes(data))
    if isinstance(ret, int) and ret < 0:
        if logger:
            logger.error(f"[INA226] writeto_mem 失败: addr=0x{config.INA226_ADDR:02X} reg=0x{reg:02X} ret={ret}")
        raise OSError(ret)


def read_register(reg):
    """读取INA226寄存器"""
    from hardware import hardware_manager
    data = hardware_manager.bus.readfrom_mem(config.INA226_ADDR, reg, 2)
    return (data[0] << 8) | data[1]


def init_ina226():
    """初始化 INA226 芯片：配置模式 + 校准值"""
    global _INA226_PRESENT
    logger = logger_manager.logger
    if not getattr(config, "INA226_ENABLE", True):
        if logger:
            logger.info("[INA226] INA226_ENABLE=False，跳过初始化与 I2C 探测")
        # 显式标记不可用，避免后续误读
        _INA226_PRESENT = False
        return False
    try:
        # 仅通过“写寄存器成功”来判定可用，避免额外的读操作触发底层崩溃
        write_register(config.REG_CONFIGURATION, 0x4527)
        write_register(config.REG_CALIBRATION, config.CALIBRATION_VALUE)
        _INA226_PRESENT = True
        return True
    except Exception as e:
        _INA226_PRESENT = False
        if logger:
            logger.error(f"[INA226] 初始化失败：{e}")
        return False


def get_bus_voltage():
    """读取总线电压（单位：V）。未探测到 INA226 或读失败时返回 0.0（上报用，避免 null）。"""
    logger = logger_manager.logger
    if not _ina226_ready():
        return 0.0
    try:
        raw = read_register(config.REG_BUS_VOLTAGE)
        return raw * 1.25 / 1000
    except Exception as e:
        if logger:
            logger.error(f"[INA226] 读取电压失败：{e}")
        return 0.0


def get_current():
    """
    读取电流（单位：mA）
    当前电源板实测：正数表示放电，负数表示充电。
    
    INA226 电流计算公式：
    Current = (Current Register Value) × Current_LSB
    Current_LSB = 0.001 × CALIBRATION_VALUE / 4096
    """
    try:
        if not _ina226_ready():
            return 0.0
        raw = read_register(config.REG_CURRENT)
        # INA226 电流寄存器是16位有符号整数
        # 最高位是符号位；电流方向含义取决于电源板的采样电阻接线方向。
        # 计算 Current_LSB（根据 CALIBRATION_VALUE）
        current_lsb = 0.001 * config.CALIBRATION_VALUE / 4096  # 单位：A
        # 处理有符号数：如果最高位为1，转换为负数
        if raw & 0x8000:
            signed_raw = raw - 0x10000  # 转换为有符号整数
        else:
            signed_raw = raw
        # 转换为毫安
        current_ma = signed_raw * current_lsb * 1000
        return current_ma
    except Exception as e:
        logger = logger_manager.logger
        if logger:
            logger.error(f"[INA226] 读取电流失败: {e}")
        else:
            print(f"[INA226] 读取电流失败: {e}")
        return 0.0


def is_charging(threshold_ma=10.0):
    """
    检测是否在充电（通过电流方向判断）
    
    Args:
        threshold_ma: 电流阈值（毫安），超过此值认为在充电，默认10mA
    
    Returns:
        True: 正在充电
        False: 未充电或读取失败
    """
    try:
        current = get_current()
        is_charge = current < -abs(float(threshold_ma))
        return is_charge
    except Exception as e:
        logger = logger_manager.logger
        if logger:
            logger.error(f"[CHARGE] 检测充电状态失败: {e}")
        else:
            print(f"[CHARGE] 检测充电状态失败: {e}")
        return False


def charging_shutdown_monitor():
    """独立监测 INA226；连续确认充电后通知服务器并退出应用。"""
    logger = logger_manager.logger
    shutdown_enabled = bool(getattr(config, "CHARGING_SHUTDOWN_ENABLED", False))
    diagnostic_enabled = bool(getattr(config, "CHARGING_DIAGNOSTIC_LOG_ENABLED", False))
    if not shutdown_enabled and not diagnostic_enabled:
        if logger:
            logger.info("[CHARGE] 充电退出监测已禁用")
        return

    interval_ms = max(100, int(getattr(config, "CHARGING_CHECK_INTERVAL_MS", 5000)))
    threshold_ma = float(getattr(config, "CHARGING_CURRENT_THRESHOLD_MA", 10.0))
    confirm_required = max(1, int(getattr(config, "CHARGING_CONFIRM_COUNT", 2)))
    confirm_count = 0

    if logger:
        logger.info(
            f"[CHARGE] 独立监测线程启动: interval={interval_ms}ms, "
            f"threshold={threshold_ma:.1f}mA, confirm={confirm_required}, "
            f"shutdown={'on' if shutdown_enabled else 'off'}"
        )

    while True:
        current_ma = get_current()
        if diagnostic_enabled and logger:
            voltage = get_bus_voltage()
            logger.info(
                f"[CHARGE-DIAG] INA226 voltage={voltage:.3f}V, "
                f"current={current_ma:.1f}mA"
            )

        if not shutdown_enabled:
            maix_time.sleep_ms(interval_ms)
            continue

        if current_ma < -abs(threshold_ma):
            confirm_count += 1
            if logger:
                logger.info(
                    f"[CHARGE] INA226 充电电流 {current_ma:.1f}mA "
                    f"({confirm_count}/{confirm_required})"
                )
        else:
            confirm_count = 0

        if confirm_count >= confirm_required:
            script_path = getattr(
                config,
                "CHARGING_EXIT_SCRIPT",
                config.APP_DIR + "/charging_exit.sh",
            )
            if not os.path.isfile(script_path):
                if logger:
                    logger.error(f"[CHARGE] 退出脚本不存在: {script_path}")
                confirm_count = 0
            else:
                if logger:
                    logger.warning(
                        f"[CHARGE] 已连续确认充电，通知服务器后退出应用: current={current_ma:.1f}mA"
                    )
                try:
                    from network import network_manager

                    notify_timeout_ms = max(
                        0,
                        int(getattr(config, "CHARGING_NOTIFY_TIMEOUT_MS", 30000)),
                    )
                    notification_sent = network_manager.safe_enqueue_and_wait(
                        {"poweroff": "充电中"},
                        2,
                        high=True,
                        timeout_ms=notify_timeout_ms,
                    )
                    if notification_sent:
                        if logger:
                            logger.info("[CHARGE] 充电状态已发送到服务器")
                    elif logger:
                        logger.warning(
                            f"[CHARGE] 等待服务器发送超时({notify_timeout_ms}ms)，继续执行退出"
                        )
                except Exception as e:
                    if logger:
                        logger.error(f"[CHARGE] 充电状态上报失败，继续执行退出: {e}")
                try:
                    from laser_manager import laser_manager

                    laser_manager.turn_off_laser()
                    if logger:
                        logger.info("[CHARGE] 激光关闭命令已发送")
                except Exception as e:
                    if logger:
                        logger.error(f"[CHARGE] Python 关闭激光失败，交由退出脚本兜底: {e}")
                try:
                    subprocess.Popen([
                        "/bin/sh",
                        script_path,
                        str(os.getpid()),
                        str(getattr(config, "DISTANCE_SERIAL_DEVICE", "/dev/ttyS1")),
                        str(getattr(config, "DISTANCE_SERIAL_BAUDRATE", 9600)),
                    ])
                    return
                except Exception as e:
                    if logger:
                        logger.error(f"[CHARGE] 调用退出脚本失败: {e}")
                    confirm_count = 0

        maix_time.sleep_ms(interval_ms)


def voltage_to_percent(voltage):
    """
    根据电压估算电池百分比（高密度查表插值 + 滤波）。

    - 电压先做 5 点移动平均（抑制瞬时抖动）
    - SOC 再做一阶低通（抑制“跳电量”）

    注意：
    - 该方法仍是“开路电压→SOC”的近似；负载较大/瞬时大电流时电压会下沉，SOC 会偏低。
    - 滤波会带来滞后：电量变化会更平滑，但更新更慢。
    """
    if voltage is None:
        return 0
    try:
        v = float(voltage)
    except Exception:
        return 0
    if v <= 0:
        return 0
    return int(int(_BATTERY_MONITOR.get_soc(v) * 10) / 10) # 截断而不是四舍五入


class BatteryMonitor:
    """
    电压→SOC 估算器（查表 + 线性插值 + 双重滤波）。

    说明：
    - 表为单节锂电“静态电压”近似曲线；不同电池/温度/老化会有偏差。
    - 这里不区分充电/放电曲线（滞后），主要用于“显示电量/粗略判断”。
    """

    def __init__(self, avg_window: int = 5, alpha: float = 0.2):
        # 电压-SOC对照表（电压从高到低）
        self.voltages = [
            4.20, 4.15, 4.10, 4.05, 4.00,
            3.95, 3.90, 3.88, 3.85, 3.82,
            3.80, 3.78, 3.75, 3.72, 3.70,
            3.65, 3.60, 3.55, 3.50, 3.45,
            3.40, 3.35, 3.30, 3.20, 2.50,
        ]
        self.socs = [
            100, 98, 95, 90, 85,
            80, 75, 72, 68, 64,
            60, 56, 52, 48, 44,
            38, 32, 26, 20, 14,
            10, 6, 3, 1, 0,
        ]

        self.avg_window = max(1, int(avg_window))
        self.alpha = float(alpha) if alpha is not None else 0.2
        if not (0.0 < self.alpha <= 1.0):
            self.alpha = 0.2

        self.voltage_history = []
        self.last_soc = 50.0

    def _voltage_to_soc_raw(self, voltage: float) -> float:
        # 越界
        if voltage >= self.voltages[0]:
            return 100.0
        if voltage <= self.voltages[-1]:
            return 0.0

        # 表是降序，二分查找
        left, right = 0, len(self.voltages) - 1
        while left <= right:
            mid = (left + right) // 2
            vm = self.voltages[mid]
            if vm == voltage:
                return float(self.socs[mid])
            elif vm < voltage:
                right = mid - 1
            else:
                left = mid + 1

        # 线性插值：right 在高电压侧，left 在低电压侧（降序表）
        # 例：voltages = [4.2,4.15,...]，则 v_high=voltages[right] >= voltage >= voltages[left]=v_low
        v_high, v_low = float(self.voltages[right]), float(self.voltages[left])
        soc_high, soc_low = float(self.socs[right]), float(self.socs[left])
        if abs(v_high - v_low) < 1e-9:
            return soc_low
        soc = soc_low + (voltage - v_low) * (soc_high - soc_low) / (v_high - v_low)
        return soc

    def get_soc(self, raw_voltage: float) -> float:
        # 1) 电压滤波（移动平均）
        self.voltage_history.append(float(raw_voltage))
        if len(self.voltage_history) > self.avg_window:
            self.voltage_history.pop(0)
        voltage = sum(self.voltage_history) / float(len(self.voltage_history))

        # 2) 查表插值
        raw_soc = self._voltage_to_soc_raw(voltage)

        # 3) SOC 低通滤波
        a = self.alpha
        self.last_soc = a * raw_soc + (1.0 - a) * float(self.last_soc)

        # clip
        if self.last_soc < 0.0:
            self.last_soc = 0.0
        if self.last_soc > 100.0:
            self.last_soc = 100.0
        return float(self.last_soc)


# 模块级单例：保留历史，实现平滑（进程重启会重置）
_BATTERY_MONITOR = BatteryMonitor(
    avg_window=int(getattr(config, "BATTERY_SOC_AVG_WINDOW", 5)),
    alpha=float(getattr(config, "BATTERY_SOC_LPF_ALPHA", 0.2)),
)