#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
激光射击系统主程序（视觉测距版）
功能：目标检测、激光校准、4G TCP 通信、OTA 升级、单目测距、INA226 电量监测
平台：MaixPy (Sipeed MAIX)
作者：ZZH
最后更新：2025-11-21

重构版本：使用模块化设计
"""
from maix import camera, display, image, app, time, uart, pinmap, i2c
from maix.peripheral import adc
import _thread
import os
import json
import time as wall_time

# 导入新模块
import config
from version import VERSION
# from logger import init_logging, get_logger, stop_logging
from logger_manager import logger_manager
from time_sync import sync_system_time_from_4g
from power import init_ina226
from laser_manager import laser_manager
from vision import start_save_shot_worker
from network import network_manager
from ota_manager import ota_manager
from hardware import hardware_manager
from camera_manager import camera_manager
from shoot_manager import process_shot, preload_triangle_calib
from target_roi_yolo import preload_yolo_detector


def laser_calibration_worker():
    """后台线程：持续检测是否需要执行激光校准"""
    from laser_manager import laser_manager
    from ota_manager import ota_manager

    logger = logger_manager.logger
    if logger:
        logger.info("[LASER] 激光校准线程启动")

    while True:
        try:
            try:
                if ota_manager.ota_in_progress:
                    time.sleep_ms(200)
                    continue
            except Exception as e:
                logger = logger_manager.logger
                if logger:
                    logger.error(f"[LASER] OTA检查异常: {e}")
                time.sleep_ms(200)
                continue

            if laser_manager.calibration_active:
                # 调用校准方法，所有逻辑都在 LaserManager 中
                result = laser_manager.calibrate_laser_position(timeout_ms=8000, check_sharpness=True)

                # 如果超时仍未成功，稍微休息一下
                if laser_manager.calibration_active:
                    time.sleep_ms(300)
            else:
                time.sleep_ms(50)
        except Exception as e:
            # 线程顶层异常捕获，防止线程静默退出
            logger = logger_manager.logger
            if logger:
                logger.error(f"[LASER] 校准线程异常: {e}")
                import traceback
                logger.error(traceback.format_exc())
            else:
                print(f"[LASER] 校准线程异常: {e}")
                import traceback
                traceback.print_exc()
            time.sleep_ms(1000)  # 等待1秒后继续

def cmd_str():
    """主程序入口"""
    # ==================== 第一阶段：硬件初始化 ====================
    # 按照 main104.py 的顺序，先完成所有硬件初始化

    # 1. 引脚功能映射
    for pin, func in config.PIN_MAPPINGS.items():
        try:
            pinmap.set_pin_function(pin, func)
        except:
            pass

    # 2. 初始化硬件对象（UART、I2C、ADC）
    hardware_manager.init_uart4g()
    hardware_manager.init_bus()
    hardware_manager.init_adc()
    hardware_manager.init_at_client()

    # 3. 初始化激光模块（串口 + 开机关闭激光防误触发）
    laser_manager.init()

    # 3. 初始化 INA226 电量监测芯片（与后续相机启动之间的耗时，便于定位启动卡顿）
    _w_boot = wall_time.time()
    print(f"[BOOT] init_ina226 开始 wall_s={_w_boot:.3f}")
    init_ina226()
    print(f"[BOOT] init_ina226 结束 wall +{int(round((wall_time.time() - _w_boot) * 1000))} ms")

    # 4. 初始化显示和相机
    _w_boot = wall_time.time()
    print(
        f"[BOOT] init_camera({getattr(config, 'CAMERA_WIDTH', 640)}x{getattr(config, 'CAMERA_HEIGHT', 480)}) "
        f"开始 wall_s={_w_boot:.3f}"
    )
    camera_manager.init_camera(getattr(config, "CAMERA_WIDTH", 640), getattr(config, "CAMERA_HEIGHT", 480))
    print(f"[BOOT] init_camera 结束 wall +{int(round((wall_time.time() - _w_boot) * 1000))} ms")

    _w_boot = wall_time.time()
    print(f"[BOOT] init_display 开始 wall_s={_w_boot:.3f}")
    camera_manager.init_display()
    print(f"[BOOT] init_display 结束 wall +{int(round((wall_time.time() - _w_boot) * 1000))} ms")

    # ==================== 第二阶段：软件初始化 ====================

    # 1. 初始化日志系统（WARNING级别，不打印/写入INFO和DEBUG日志，提高执行流畅度）
    import logging
    logger_manager.init_logging(log_level=logging.WARNING)
    logger = logger_manager.logger

    # 补充：因为初始化的时候，激光会亮，先关了它
    # laser_manager.turn_off_laser()

    # 2. 从4G模块同步系统时间（需要 at_client 已初始化）
    sync_system_time_from_4g()

    # 2.1 WiFi 热点配网兜底：仅当 STA 与 4G 均不可用时起 AP + HTTP；提交后删 /boot/wifi.ap、建 wifi.sta 并 reboot
    try:
        from wifi_config_httpd import maybe_start_wifi_ap_fallback

        maybe_start_wifi_ap_fallback(logger)
    except Exception as e:
        if logger:
            logger.error(f"[WIFI-AP] 兜底配网检测/启动失败: {e}")

    # 2.5. 启动存图 worker 线程（队列 + worker，避免主循环阻塞）
    start_save_shot_worker()

    # 2.6 预加载三角形标定/坐标文件（避免首次射箭卡顿）
    try:
        preload_triangle_calib()
    except Exception:
        pass

    # 2.7 预加载 YOLO（靶环 ROI + 黑三角）；dual_buff=False 时无需 warmup 消除一帧延迟
    try:
        _preload_yolo = bool(getattr(config, "TRIANGLE_YOLO_PRELOAD_ON_BOOT", True))
        _loc_black = str(
            getattr(config, "TRIANGLE_BLACK_TRIANGLE_LOCATE_MODE", "yolo")
        ).lower().strip()
        if _loc_black not in ("yolo", "traditional"):
            _loc_black = "yolo"
        _need_black_preload = (
            bool(getattr(config, "TRIANGLE_BLACK_YOLO_ENABLE", False))
            and _loc_black == "yolo"
            and bool(getattr(config, "TRIANGLE_BLACK_YOLO_PRELOAD_ON_BOOT", True))
        )
        _preload_yolo = _preload_yolo or _need_black_preload
        if _preload_yolo:
            preload_yolo_detector(logger)
    except Exception as e:
        if logger:
            logger.warning(f"[YOLO-ROI] 启动预加载异常（不影响后续射箭）: {e}")

    # 3. 启动时检查：是否需要恢复备份
    pending_path = f"{config.APP_DIR}/ota_pending.json"
    if os.path.exists(pending_path):
        try:
            with open(pending_path, 'r', encoding='utf-8') as f:
                pending_obj = json.load(f)

            restart_count = pending_obj.get('restart_count', 0)
            max_restarts = pending_obj.get('max_restarts', 3)
            backup_dir = pending_obj.get('backup_dir')

            if logger:
                logger.info(f"检测到 ota_pending.json，重启计数: {restart_count}/{max_restarts}")

            if restart_count >= max_restarts:
                if logger:
                    logger.error(f"[STARTUP] 重启次数 ({restart_count}) 超过阈值 ({max_restarts})，执行恢复...")

                if backup_dir and os.path.exists(backup_dir):
                    if ota_manager.restore_from_backup(backup_dir):
                        if logger:
                            logger.info(f"[STARTUP] 已从指定备份恢复: {backup_dir}")
                    else:
                        if logger:
                            logger.info(f"[STARTUP] 指定备份恢复失败，尝试恢复最新备份...")
                        ota_manager.restore_from_backup(None)
                else:
                    if ota_manager.restore_from_backup(None):
                        if logger:
                            logger.info(f"[STARTUP] 已从最新备份恢复")
                    else:
                        if logger:
                            logger.error(f"[STARTUP] 恢复备份失败")

                try:
                    os.remove(pending_path)
                    if logger:
                        logger.info(f"[STARTUP] 已删除 ota_pending.json")
                except Exception as e:
                    if logger:
                        logger.error(f"[STARTUP] 删除 pending 文件失败: {e}")

                if logger:
                    logger.info(f"[STARTUP] 恢复完成，准备重启系统...")
                time.sleep_ms(2000)
                os.system("reboot")
                return
            else:
                pending_obj['restart_count'] = restart_count + 1
                try:
                    with open(pending_path, 'w', encoding='utf-8') as f:
                        json.dump(pending_obj, f)
                    if logger:
                        logger.info(f"[STARTUP] 已更新重启计数: {pending_obj['restart_count']}")
                except Exception as e:
                    if logger:
                        logger.error(f"[STARTUP] 更新重启计数失败: {e}")
        except Exception as e:
            if logger:
                logger.error(f"[STARTUP] 检查 pending 文件时出错: {e}")
            try:
                if logger:
                    logger.info(f"[STARTUP] pending 文件可能损坏，尝试恢复备份...")
                if ota_manager.restore_from_backup(None):
                    os.remove(pending_path)
                    if logger:
                        logger.info(f"[STARTUP] 已恢复备份并删除损坏的 pending 文件")
                    time.sleep_ms(2000)
                    os.system("reboot")
                    return
            except:
                pass

    # 4. 初始化设备ID（network_manager 内部会自动设置 device_id 和 password）
    network_manager.read_device_id()

    # 5. 创建照片存储目录（如果启用图像保存或检测失败时强制保存）
    if config.SAVE_IMAGE_ENABLED or getattr(config, "SAVE_IMAGE_ON_FAILURE", False):
        photo_dir = config.PHOTO_DIR
        if photo_dir not in os.listdir("/root"):
            try:
                os.mkdir(photo_dir)
            except:
                pass

    # 6. 启动通信与校准线程
    # 若已进入 AP 配网兜底（/boot/wifi.ap），则不启动 TCP 主循环；等待用户配网后 reboot。
    try:
        if os.path.exists("/boot/wifi.ap"):
            if logger:
                logger.warning("[NET] 当前处于 AP 配网模式（/boot/wifi.ap 存在），跳过 TCP 主线程启动")
        else:
            _thread.start_new_thread(network_manager.tcp_main, ())
    except Exception as e:
        if logger:
            logger.error(f"[NET] 启动 TCP 主线程失败: {e}")
    if not config.HARDCODE_LASER_POINT:
        _thread.start_new_thread(laser_calibration_worker, ())

    # 7. 加载激光点配置
    laser_manager.load_laser_point()

    # 8. 启动空闲计时
    hardware_manager.start_idle_timer()

    if logger:
        logger.info("系统准备完成...")

    last_adc_trigger = 0
    # 读取一次ADC初始值，防止开机时传感器已有压力导致误触发
    try:
        last_adc_val = hardware_manager.adc_obj.read()
    except Exception:
        last_adc_val = 0
    peak_adc_val = 0  # 当前周期内的压力峰值
    # 气压采样：减少日志频率（每 N 个点输出一条），避免 logger.debug 拖慢采样
    PRESSURE_BATCH_SIZE = 100

    pressure_buf = []
    pressure_sum = 0
    pressure_min = 4095
    pressure_max = 0
    pressure_t0_ms = None

    def _flush_pressure_buf(reason: str):
        nonlocal pressure_buf, pressure_sum, pressure_min, pressure_max, pressure_t0_ms, logger
        if not pressure_buf:
            return
        if config.AIR_PRESSURE_lOG:
            t1_ms = time.ticks_ms()
            n = len(pressure_buf)
            avg = (pressure_sum / n) if n else 0
            line = (
                f"[气压批量] reason={reason} "
                f"t0={pressure_t0_ms} t1={t1_ms} n={n} "
                f"min={pressure_min} max={pressure_max} avg={avg:.1f} "
                f"values={','.join(map(str, pressure_buf))}"
            )
            if logger:
                logger.debug(line)
            else:
                print(line)
        # 无论是否记录日志，都必须清空 buffer，否则内存泄漏
        pressure_buf = []
        pressure_sum = 0
        pressure_min = 4095
        pressure_max = 0
        pressure_t0_ms = None

    # 主循环：检测扳机触发 → 拍照 → 分析 → 上报
    while not app.need_exit():
        try:
            current_time = time.ticks_ms()

            # OTA 期间暂停相机预览
            try:
                if ota_manager.ota_in_progress:
                    time.sleep_ms(250)
                    continue
            except Exception as e:
                logger = logger_manager.logger
                if logger:
                    logger.error(f"[MAIN] OTA检查异常: {e}")
                time.sleep_ms(250)
                continue

            # 不在 OTA 状态下，检测是否空闲足够长，自动关机
            # print(f"[MAIN] 空闲时间: {hardware_manager.get_idle_time_in_sec() }秒")
            # print(f"配置关机时间：{config.AUTO_POWER_OFF_IN_SECONDS} 秒")
            if hardware_manager.get_idle_time_in_sec() > config.AUTO_POWER_OFF_IN_SECONDS:
                logger.info("[MAIN] 超过设定时间未检测活动，自动关机")
                network_manager.safe_enqueue({"poweroff": "超过设定时间未检测活动，自动关机"}, 2)
                time.sleep_ms(100)
                hardware_manager.power_off()
                time.sleep_ms(2000) # 让关机指令生效
                break

            # 读取ADC值（扳机检测）
            try:
                if network_manager.manual_trigger_flag:
                    network_manager.clear_manual_trigger()
                    adc_val = config.ADC_TRIGGER_THRESHOLD + 1
                    if logger:
                        logger.info("[TEST] TCP命令触发射箭")
                else:
                    adc_val = hardware_manager.adc_obj.read()
            except Exception as e:
                logger = logger_manager.logger
                if logger:
                    logger.error(f"[MAIN] ADC读取异常: {e}")
                time.sleep_ms(100)
                continue

            # ====== 气压采样缓存（每次循环都记录，批量输出日志）======
            if pressure_t0_ms is None:
                pressure_t0_ms = current_time
            pressure_buf.append(adc_val)
            pressure_sum += adc_val
            if adc_val < pressure_min:
                pressure_min = adc_val
            if adc_val > pressure_max:
                pressure_max = adc_val
            if len(pressure_buf) >= PRESSURE_BATCH_SIZE:
                _flush_pressure_buf("batch")
            # 峰值检测：压力从峰值下降时触发，确保捕获到最大冲击时刻
            if adc_val > peak_adc_val:
                peak_adc_val = adc_val  # 更新峰值
            if (peak_adc_val >= config.ADC_TRIGGER_THRESHOLD
                    and adc_val < peak_adc_val
                    and last_adc_val >= peak_adc_val):
                # 封顶后下降沿触发：peak是最大值，当前值开始下降，且上次值还在peak位置
                hardware_manager.start_idle_timer()  # 重新计时
                diff_ms = current_time - last_adc_trigger
                if diff_ms < 3000:
                    peak_adc_val = 0  # 去抖期间重置峰值
                    time.sleep_ms(5)
                    continue
                last_adc_trigger = current_time
                peak_adc_val = 0  # 触发后重置峰值
                # 触发前先把缓存刷出来，避免波形被长耗时处理截断
                _flush_pressure_buf("before_trigger")

                try:
                    process_shot(adc_val)
                except Exception as e:
                    logger = logger_manager.logger
                    if logger:
                        logger.error(f"[MAIN] 图像处理异常: {e}")
                        import traceback
                        logger.error(traceback.format_exc())
                    time.sleep_ms(100)
                    continue
            else:
                if config.SHOW_CAMERA_PHOTO_WHILE_SHOOTING:
                    try:
                        camera_manager.show(camera_manager.read_frame())
                    except Exception as e:
                        pass
                time.sleep_ms(5)
            last_adc_val = adc_val

        except Exception as e:
            # 主循环的顶层异常捕获，防止程序静默退出
            logger = logger_manager.logger
            if logger:
                logger.error(f"[MAIN] 主循环异常: {e}")
                import traceback
                logger.error(traceback.format_exc())
            else:
                print(f"[MAIN] 主循环异常: {e}")
                import traceback
                traceback.print_exc()
            # 异常发生时尽量把缓存刷盘，方便定位问题
            try:
                _flush_pressure_buf("exception")
            except:
                pass
            time.sleep_ms(1000)  # 等待1秒后继续





# 主程序入口
if __name__ == "__main__":
    try:
        cmd_str()

        # 用于调用测试函数
        # test()

        # 用于测试图片清晰度
        # 方式1: 测试单张图片
        # test_sharpness("/root/phot/image.bmp")

        # 方式2: 测试目录下所有图片
        # test_sharpness("/root/phot")

        # 方式3: 使用默认路径（config.PHOTO_DIR）
        # test_sharpness("/root/phot/")

        # 用于测试激光点清晰度
        # 方式1: 测试单张图片
        # test_laser_point_sharpness("/root/phot/image.bmp")
        # 方式2: 测试目录下所有图片
        # test_laser_point_sharpness("/root/phot")
        # 方式3: 使用默认路径（config.PHOTO_DIR）
        # test_laser_point_sharpness("/root/phot/")

    except KeyboardInterrupt:
        logger = logger_manager.logger
        if logger:
            logger.info("[MAIN] 收到中断信号，程序退出")
        logger_manager.stop_logging()
    except Exception as e:
        logger = logger_manager.logger
        if logger:
            logger.error(f"[MAIN] 程序异常退出: {e}")
            import traceback
            logger.error(traceback.format_exc())
        else:
            print(f"[MAIN] 程序异常退出: {e}")
            import traceback
            traceback.print_exc()
        logger_manager.stop_logging()
        raise  # 重新抛出异常，让系统知道程序异常退出
