/*
 * camera.cpp — 每帧感知-决策-执行流水线
 *
 * 入口: CameraHandler() 由 main.cpp 主循环每帧调用一次。
 *
 * 10 步流水线:
 *   1. capture_frame          MJPEG 取帧 → 1/4 解码 160×120 BGR
 *   2. save_image_if_requested  debug 存图
 *   3. image_main             视觉巡线 → left/right/mid_line[]
 *   4. lidar_avoid_process    VL53L0X 激光避障 → 中线变形 (已禁用)
 *   5. zebra/tl/cone          场景识别状态机 (用上一帧 g_boxes[])
 *   6. steering_update        舵机比例控制
 *   7. motor_update           电机开环 + 编码器刹车 + 弯道减速
 *   8. run_model_inference    Mild v12 每2帧推理 → g_boxes[] (放最后, 不阻塞舵机)
 *   9. lcd_render             LCD RGB565 渲染
 *  10. fps_log                分步耗时日志
 *
 * 硬件依赖:
 *   - 摄像头: /dev/video0 (640×480 MJPEG, V4L2)
 *   - LCD:    /dev/fb0 (RGB565, mmap)
 *   - 舵机:   pwmchip1/pwm0 (3ms 周期, 1.2~1.8ms 占空比)
 *   - 语音:   /dev/i2c-2 从地址 0x34 (斑马线播报)
 *   - 激光:   VL53L0X via /dev/stmvl53l0x_ranging
 */
#include "camera.h"
#include "model_v10.hpp"
#include "vl53l0x.h"

#include <fcntl.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <linux/i2c-dev.h>
#include <linux/i2c.h>
#include <linux/fb.h>
#include <algorithm>
#include <chrono>
#include <thread>
#include <cstring>
#include <sstream>
#include <iomanip>
#include <malloc.h>

// ═══════════════════════════════════════════════════════════
//  模型检测类别索引
//  Mild v12 输出 4 类 + 背景, 对应 g_thresh[] 阈值
// ═══════════════════════════════════════════════════════════
enum { MD_CONE = 0, MD_RED = 1, MD_GREEN = 2, MD_ZEBRA = 3 };

// ═══════════════════════════════════════════════════════════
//  摄像头 & 帧缓冲全局变量
// ═══════════════════════════════════════════════════════════
cv::VideoCapture cap;                   // V4L2 摄像头句柄
cv::Mat raw_mat, decoded_frame;         // raw_mat=V4L2原始帧, decoded_frame=MJPEG解码结果
static int g_decode_mode = -1;          // -1=未检测, 1=MJPEG 1/4解码, 0=全尺寸回退

int screenWidth, screenHeight;          // LCD 屏幕物理分辨率 (从 /dev/fb0 读取)
int newWidth, newHeight;                // 自适应显示分辨率 = 摄像头分辨率 × 缩放因子
int fb;                                 // /dev/fb0 文件描述符
uint16_t *fb_buffer;                    // LCD 帧缓冲 mmap 地址
static size_t g_fb_size = 0;            // 帧缓冲字节数
static cv::Mat g_stop_image;            // 斑马线停车提示图片
PwmController servo(1, 0);             // 舵机: pwmchip1/pwm0

// 巡线空间到显示空间的缩放因子
// line_tracking_width = newWidth / calc_scale
// line_tracking_height = newHeight / calc_scale
static constexpr int calc_scale = 2;

// ═══════════════════════════════════════════════════════════
//  模型推理相关
//
//  g_thresh: 四类各自置信度阈值 (锥桶/红灯/绿灯/斑马线)
//  g_boxes:  检测结果缓冲区 (最多 16 个框)
//  g_box_count: 当前帧有效检测框数
// ═══════════════════════════════════════════════════════════
static float g_thresh[4] = {0.80f, 0.70f, 0.80f, 0.85f};
static DetectBoxV10 g_boxes[16];
static int          g_box_count = 0;

// ═══════════════════════════════════════════════════════════
//  斑马线状态机
//
//  状态: Z_NORMAL → Z_STOP(4s) → Z_COOLDOWN(5s) → Z_NORMAL
//  触发条件: 连续 5 帧 + 起源于远(cy≤50) + 当前近(cy>zebrasee)
//
//  g_zc_frames:  去抖连续帧计数 (未检测时每帧 -2 衰减)
//  g_zc_min_cy:  本轮追踪中斑马线出现时的最小 cy (越小=越远)
//  g_ztime:      进入 Z_STOP / Z_COOLDOWN 的时间戳
// ═══════════════════════════════════════════════════════════
static constexpr int ZEBRA_MIN_FRAMES = 3;      // 连续确认帧数
static constexpr int ZEBRA_FAR_CY     = 80;     // 远处 cy 阈值: 斑马线必须曾出现在 cy≤80
static int g_zc_frames = 0;
static int g_zc_min_cy = 120;                    // 初始设远 (120=从未检测到)

enum ZState { Z_NORMAL, Z_STOP, Z_COOLDOWN };
static ZState  g_zstate     = Z_NORMAL;
static time_t  g_ztime      = 0;
static bool    g_bangbang_used   = false; // 本轮 Z_COOLDOWN 打满机会是否已用
static bool    g_bangbang_active = false; // 是否处于打满->等回正序列中
static int     g_bangbang_dir    = 0;     // 锁定方向: +1=右满, -1=左满

// ═══════════════════════════════════════════════════════════
//  红绿灯状态机
//
//  状态: TL_NORMAL → TL_STOP → TL_WAIT_GREEN → TL_NORMAL
//  红灯≥3帧确认停车, 红灯消失→等绿灯, 绿灯≥3帧确认通行
//
//  g_tl_red_frames:   红灯连续帧计数 (未检测时 -1 衰减)
//  g_tl_green_frames: 绿灯连续帧计数
// ═══════════════════════════════════════════════════════════
enum TLState { TL_NORMAL, TL_STOP, TL_WAIT_GREEN };
static TLState g_tl_state        = TL_NORMAL;
static int     g_tl_red_frames   = 0;
static int     g_tl_green_frames = 0;

// ═══════════════════════════════════════════════════════════
//  锥桶检测 & 中线变形状态
//
//  生命周期: 确认(avoid) → 保持衰减(hold) → 回弹(return) → 空闲
//
//  g_cone_frames:      去抖连续帧计数 (未检测时 -1 衰减)
//  g_cone_last_row/col: 上帧锥桶位置 (用于位置容差判定)
//  g_cone_confirmed:   是否通过去抖确认
//  g_cone_hold_ctr:    保持衰减计数器 (确认时=1, 每帧++)
//  g_cone_hold_src_row/col: 保持阶段使用的锥桶位置 (确认时冻结)
//  g_cone_return_ctr:  回弹计数器 (保持结束后=1, 每帧++)
//  g_cone_return_dir:  回弹方向 (+1=推右, -1=推左, 与避让方向相反)
// ═══════════════════════════════════════════════════════════
static int  g_cone_frames       = 0;
static int  g_cone_last_row     = -1;
static int  g_cone_last_col     = -1;
static bool g_cone_confirmed    = false;
static int  g_cone_hold_ctr     = 0;
static int  g_cone_hold_src_row = -1;
static int  g_cone_hold_src_col = -1;
static int  g_cone_return_ctr   = 0;
static double g_cone_return_dir = 0;

// ═══════════════════════════════════════════════════════════
//  激光雷达挡板避障
//
//  每 2 帧非阻塞读取 VL53L0X 距离, 纯距离触发(不检查视觉边线)。
//  绕行方向固定朝右 (g_lidar_hold_dir = 1.0)。
//
//  g_lidar_sensor:       VL53L0X 驱动实例
//  g_lidar_ok:           硬件是否初始化成功
//  g_lidar_frames:       去抖连续帧计数
//  g_lidar_confirmed:    是否通过去抖确认
//  g_lidar_obstacle_row: 障碍物对应的巡线空间行号
//  g_lidar_hold_src_row: 保持阶段使用的障碍物行号 (确认时冻结)
//  g_lidar_hold_ctr:     保持衰减计数器
//  g_lidar_hold_dir:     绕行方向 (固定 1.0 = 朝右)
// ═══════════════════════════════════════════════════════════
static VL53L0X             g_lidar_sensor;
static bool                g_lidar_ok       = false;
static int                 g_lidar_frames   = 0;
static bool                g_lidar_confirmed = false;
static int                 g_lidar_obstacle_row = -1;
static int                 g_lidar_hold_src_row = -1;
static int                 g_lidar_hold_ctr     = 0;
static double              g_lidar_hold_dir     = 0;

// ═══════════════════════════════════════════════════════════
//  LCD 开关 & 舵机偏差
// ═══════════════════════════════════════════════════════════
static bool g_lcd_on = true;            // LCD 渲染开关 (每 10 帧从 g_cfg.showImg 刷新)
double g_steer_deviation = 0;           // 归一化舵机偏差 [-1, 1], 供 motor_update 弯道减速用


// ═══════════════════════════════════════════════════════════
//  I2C 音频 — 斑马线语音播报
//
//  硬件: I2C-2 总线, 从地址 0x34, SMBus 块写
//  协议: command=0x6E, data={0xFF, 0x10} → 触发语音
//  调用时机: 斑马线状态机进入 Z_STOP 时
// ═══════════════════════════════════════════════════════════
static int i2c_audio_fd = -1;

// 打开 I2C 音频设备 (懒初始化, 只打开一次)
static bool i2c_audio_open()
{
    if (i2c_audio_fd >= 0) return true;  // 已打开
    i2c_audio_fd = open("/dev/i2c-2", O_RDWR);
    if (i2c_audio_fd < 0) {
        fprintf(stderr, "[ZEBRA] 无法打开 I2C-2: %s\n", strerror(errno));
        return false;
    }
    if (ioctl(i2c_audio_fd, I2C_SLAVE, 0x34) < 0) {
        fprintf(stderr, "[ZEBRA] 无法设置 I2C 从地址 0x34: %s\n", strerror(errno));
        close(i2c_audio_fd); i2c_audio_fd = -1;
        return false;
    }
    return true;
}

// 触发斑马线语音播报: SMBus I2C 块写 → 音频模块
static void play_zebra_audio()
{
    if (!i2c_audio_open()) { printf("[ZEBRA] 语音失败: I2C 未打开\n"); return; }
    ioctl(i2c_audio_fd, I2C_SLAVE, 0x34);       // 确保从地址

    // SMBus I2C 块写: command=0x6E, data=[0xFF, 0x10]
    union i2c_smbus_data d;
    struct i2c_smbus_ioctl_data a;
    __u8 v[] = {0xFF, 0x10};
    d.block[0] = 2; d.block[1] = v[0]; d.block[2] = v[1];
    a.read_write = I2C_SMBUS_WRITE;
    a.command   = 0x6E;
    a.size      = I2C_SMBUS_I2C_BLOCK_DATA;
    a.data      = &d;

    if (ioctl(i2c_audio_fd, I2C_SMBUS, &a) < 0)
        printf("[ZEBRA] 语音失败: %s\n", strerror(errno));
    else
        printf("[ZEBRA] 语音播报已触发\n");
}


// ═══════════════════════════════════════════════════════════
//  CameraInit — 系统初始化
//
//  初始化顺序:
//    1. 舵机 → 中位
//    2. LCD 帧缓冲 → mmap
//    3. 摄像头 → 640×480 MJPEG, CONVERT_RGB=0
//    4. 自适应分辨率计算 → newWidth/newHeight, line_tracking_*
//    5. Mild v12 模型加载
//    6. I2C 音频设备
//    7. VL53L0X 激光 (可选, lidar_enable 控制)
// ═══════════════════════════════════════════════════════════
int CameraInit(int camera_id)
{
    // ── 1. 舵机初始化: 3ms 周期, 1.5ms 中位 ──
    servo.setPeriod(3000000);                       // 3,000,000 ns = 3ms
    servo.setDutyCycle(1500000);                    // 1,500,000 ns = 1.5ms (中位)
    servo.enable();

    // ── 2. LCD 帧缓冲初始化 ──
    fb = open("/dev/fb0", O_RDWR);
    if (fb == -1) { std::cerr << "无法打开帧缓冲区设备" << std::endl; return -1; }

    struct fb_var_screeninfo vinfo;
    if (ioctl(fb, FBIOGET_VSCREENINFO, &vinfo) == -1) {
        std::cerr << "无法获取帧缓冲区信息" << std::endl; close(fb); return -1;
    }
    screenWidth = vinfo.xres; screenHeight = vinfo.yres;
    g_fb_size = vinfo.yres_virtual * vinfo.xres_virtual * vinfo.bits_per_pixel / 8;
    fb_buffer = (uint16_t *)mmap(NULL, g_fb_size, PROT_READ | PROT_WRITE, MAP_SHARED, fb, 0);
    if (fb_buffer == MAP_FAILED) {
        std::cerr << "无法映射帧缓冲区到内存" << std::endl; close(fb); return -1;
    }

    // 加载斑马线停车提示图片（与 LCD 同分辨率 160x128）
    g_stop_image = cv::imread("./pedestrian_stop.png");
    if (!g_stop_image.empty())
        printf("[LCD] 已加载停车提示图片: %dx%d\n", g_stop_image.cols, g_stop_image.rows);
    else
        printf("[LCD] 警告: 未找到 ./pedestrian_stop.png\n");

    // ── 3. 摄像头初始化 ──
    // CONVERT_RGB=0: 让 V4L2 返回原始 MJPEG 字节流 (单通道)
    // 后续用 imdecode(IMREAD_REDUCED_COLOR_4) 做 1/4 解码 → 160×120
    cap.open(0, cv::CAP_V4L2);
    if (!cap.isOpened()) cap.open(0);               // V4L2 失败则尝试默认后端
    cap.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G'));
    cap.set(cv::CAP_PROP_FRAME_WIDTH,  640);
    cap.set(cv::CAP_PROP_FRAME_HEIGHT, 480);
    cap.set(cv::CAP_PROP_CONVERT_RGB, 0);           // 关键: 不让 V4L2 转 RGB
    if (!cap.isOpened()) {
        printf("无法打开摄像头\n");
        munmap(fb_buffer, g_fb_size); close(fb); return -1;
    }

    int camW = cap.get(cv::CAP_PROP_FRAME_WIDTH);
    int camH = cap.get(cv::CAP_PROP_FRAME_HEIGHT);
    printf("摄像头分辨率: %d x %d\n", camW, camH);

    // ── 4. 自适应分辨率: 在 LCD 屏幕上等比缩放 ──
    // newWidth × newHeight = 摄像头画面等比放入 LCD 后的像素尺寸
    double ws = (double)screenWidth  / camW;
    double hs = (double)screenHeight / camH;
    double s  = std::min(ws, hs);                   // 取小边缩放因子, 保证不超出屏幕
    newWidth  = (int)(camW * s);
    newHeight = (int)(camH * s);
    printf("自适应分辨率: %d x %d\n", newWidth, newHeight);

    double fps = cap.get(cv::CAP_PROP_FPS);
    printf("Camera fps: %f\n", fps);

    // 巡线空间 = 显示空间 / calc_scale (典型 80×60)
    line_tracking_width  = newWidth  / calc_scale;
    line_tracking_height = newHeight / calc_scale;

    // ── 5. 加载 Mild v12 模型 ──
    if (!model_v10_init("./mild_v12.bin"))
        printf("[MODEL] 警告: 模型加载失败\n");
    else
        printf("[MODEL] Mild v12 模型已加载\n");

    // ── 6. I2C 音频设备 ──
    i2c_audio_open();

    // ── 7. VL53L0X 激光测距 (可选) ──
    // 仅在 lidar_enable=1 时初始化, 避免驱动内核定时器拖慢 CPU
    // if (g_cfg.lidar_enable) {
    //     g_lidar_ok = g_lidar_sensor.init();
    //     if (g_lidar_ok) {
    //         printf("[LIDAR] VL53L0X 已初始化\n");
    //         g_lidar_sensor.startMeasure();           // 启动首次后台测量 (~20ms)
    //     } else
    //         printf("[LIDAR] VL53L0X 未连接, 避障禁用\n");
    // } else {
    //     printf("[LIDAR] 已禁用\n");
    // }

    return 0;
}

// 前置声明: LCD 辅助函数定义在后方
static void lcd_clear_screen();

// ═══════════════════════════════════════════════════════════
//  cameraDeInit — 释放所有资源
// ═══════════════════════════════════════════════════════════
void cameraDeInit(void)
{
    cap.release();                                   // 释放摄像头
    lcd_clear_screen();                              // 程序退出前清屏
    if (fb_buffer && g_fb_size > 0) {
        munmap(fb_buffer, g_fb_size);                // 解除 LCD mmap
    }
    close(fb);                                       // 关闭 LCD fd
    if (i2c_audio_fd >= 0) close(i2c_audio_fd);      // 关闭 I2C 音频
    if (g_lidar_ok) g_lidar_sensor.stop();           // 停止 VL53L0X
    model_v10_deinit();                              // 释放模型内存
}


// ═══════════════════════════════════════════════════════════
//  Step 2: 图像保存 (debug 模式)
//
//  条件: debug=1 且 ./saveImg 文件内容为 1
//  保存路径: ./image/image_XXXXX.jpg (自增编号)
// ═══════════════════════════════════════════════════════════
static int saved_frame_count = 0;

static void save_image_if_requested()
{
    if (!g_cfg.debug) return;
    if (!readFlag(saveImg_file)) return;             // 每帧读 ./saveImg 文件
    std::ostringstream filename;
    filename << "./image/image_" << std::setw(5) << std::setfill('0') << saved_frame_count << ".jpg";
    if (cv::imwrite(filename.str(), raw_frame)) {
        printf("图像%d已保存\n", saved_frame_count);
        saved_frame_count++;
    }
}


// ═══════════════════════════════════════════════════════════
//  Step 1: 帧捕获 + MJPEG 解码
//
//  正常模式 (CONVERT_RGB=0 生效):
//    V4L2 返回单通道 MJPEG → imdecode(IMREAD_REDUCED_COLOR_4)
//    → 1/4 解码 → raw_frame = 160×120 BGR
//
//  回退模式 (CONVERT_RGB=0 未生效):
//    V4L2 返回三通道 BGR → 直用 → raw_frame = 640×480 BGR
//
//  首帧自动检测: raw_mat.channels()==1 → 正常模式, 否则回退
// ═══════════════════════════════════════════════════════════
static int capture_frame(struct timespec *t0, struct timespec *t1)
{
    clock_gettime(CLOCK_MONOTONIC, t0);              // 计时: 取帧开始
    cap.read(raw_mat);
    clock_gettime(CLOCK_MONOTONIC, t1);              // 计时: 取帧结束
    if (raw_mat.empty()) return -1;

    // 首帧自动检测解码模式
    if (g_decode_mode == -1) {
        g_decode_mode = (raw_mat.channels() == 1) ? 1 : 0;
        printf("[CAM] CONVERT_RGB=0 %s, mode=%d\n",
               g_decode_mode == 1 ? "生效->1/4解码" : "未生效->全解码回退",
               g_decode_mode);
    }

    if (g_decode_mode == 1) {
        // 正常: MJPEG 字节流 → 1/4 解码 → 160×120 BGR
        cv::imdecode(raw_mat, cv::IMREAD_REDUCED_COLOR_4, &decoded_frame);
        if (decoded_frame.empty()) return -1;
        raw_frame = decoded_frame;
    } else {
        // 回退: 直接使用 640×480 BGR (性能较差)
        raw_frame = raw_mat;
    }
    return 0;
}

// ═══════════════════════════════════════════════════════════
//  Step 5: 模型推理 (每 2 帧一次)
//
//  输入:  raw_frame (160×120 BGR)
//  输出:  g_boxes[0..g_box_count-1], 每个框含 cx,cy,w,h,cls,conf
//  阈值:  g_thresh = {0.80, 0.75, 0.80, 0.87} (锥桶/红灯/绿灯/斑马线)
//  跳帧:  每 2 帧推理一次, 奇数帧保留上次结果
// ═══════════════════════════════════════════════════════════
static void run_model_inference()
{
    static int infer_skip = 0;
    if (++infer_skip < 2) return;                    // 跳帧
    infer_skip  = 0;
    g_box_count = 0;
    if (model_v10_ready()) {
        g_box_count = model_v10_detect(raw_frame.data, raw_frame.cols, raw_frame.rows,
                                       g_boxes, 16, g_thresh);
    }
}


// ═══════════════════════════════════════════════════════════
//  Step 6a: 斑马线检测 + 状态机
//
//  检测逻辑:
//    遍历 g_boxes 找 cls=3 (斑马线), 只取第一个 (break)
//    记录 cy, 更新 g_zc_min_cy (本轮最远出现位置)
//    未检测到时 g_zc_frames 每帧 -2 衰减, 归零则重置 min_cy
//
//  触发条件 (仅 Z_NORMAL 时):
//    1. zebra_near: 当前 cy > g_cfg.zebrasee (足够近)
//    2. g_zc_frames >= 5 (连续确认)
//    3. g_zc_min_cy <= 50 (曾从远处检测到, 排除误触发)
//    → 播报语音 → Z_STOP
//
//  状态转移:
//    Z_NORMAL → Z_STOP (停车 4 秒) → Z_COOLDOWN (冷却 5 秒) → Z_NORMAL
//    冷却期间不检测斑马线, 防止重复触发
//
//  返回值: true = Z_STOP (通知 motor_update 刹车)
// ═══════════════════════════════════════════════════════════
static bool zebra_process()
{
    // 双边丢线超过50% → 不采纳任何斑马线检测
    if (g_lost_rows > line_tracking_height / 2) {
        if (g_cfg.debug) printf("[ZEBRA] 忽略: 丢线%d/%d行\n", g_lost_rows, line_tracking_height);
        return false;
    }

    bool zebra_seen = false;
    bool zebra_near = false;
    int  zebra_cy   = 0;
    float zebra_cf  = 0;

    // 遍历检测框, 找第一个斑马线
    for (int i = 0; i < g_box_count; ++i) {
        if (g_boxes[i].cls != MD_ZEBRA) continue;
        // ── 赛道内占比检查: 框80%以上在赛道外 → 忽略 ──
        {
            float bx = (float)line_tracking_width  / raw_frame.cols;
            float by = (float)line_tracking_height / raw_frame.rows;
            int bx1 = (int)((g_boxes[i].cx - g_boxes[i].w/2) * bx);
            int by1 = (int)((g_boxes[i].cy - g_boxes[i].h/2) * by);
            int bx2 = (int)((g_boxes[i].cx + g_boxes[i].w/2) * bx);
            int by2 = (int)((g_boxes[i].cy + g_boxes[i].h/2) * by);
            int overlap = 0, area = (bx2 - bx1) * (by2 - by1);
            if (area > 0) {
                int y0 = std::max(0,    std::min(by1, line_tracking_height - 1));
                int y1 = std::min(by2,  line_tracking_height - 1);
                for (int y = y0; y <= y1 && y >= 0; ++y) {
                    if (left_line[y] == -1 || right_line[y] == -1) continue;
                    int ol = std::max(0, std::min(bx2, right_line[y]) - std::max(bx1, left_line[y]));
                    overlap += ol;
                }
            }
            if (area > 0 && (float)overlap / area < 0.2f) {
                if (g_cfg.debug) printf("[ZEBRA] 忽略: 赛道内仅%.0f%% cf=%.2f\n",
                                        (float)overlap / area * 100, g_boxes[i].conf);
                continue;  // 赛道内<20% → 忽略
            }
        }
        zebra_cy  = (int)g_boxes[i].cy;
        zebra_cf  = g_boxes[i].conf;
        zebra_seen = true;
        zebra_near = (zebra_cy > g_cfg.zebrasee);    // cy > 阈值 = 足够近
        if (g_zstate == Z_NORMAL) {
            if (zebra_cy < g_zc_min_cy) g_zc_min_cy = zebra_cy;  // 追踪最远 cy
            g_zc_frames++;
        }
        break;                                       // 只取第一个斑马线框
    }

    // 未检测到 → 衰减去抖计数
    if (!zebra_seen) {
        if (g_zc_frames > 0) g_zc_frames = std::max(0, g_zc_frames - 1);  // 每帧 -1 衰减
        if (g_zc_frames == 0) g_zc_min_cy = 120;    // 完全衰减 → 重置远处追踪
    }

    time_t now = time(nullptr);
    switch (g_zstate) {
    case Z_NORMAL:
        // 触发: 近 + 连续帧够 + 曾从远处出现
        if (zebra_near && g_zc_frames >= ZEBRA_MIN_FRAMES && g_zc_min_cy <= ZEBRA_FAR_CY) {
            play_zebra_audio();                      // I2C 语音播报
            g_zstate = Z_STOP; g_ztime = now;
            if (g_cfg.debug) printf("[ZEBRA] cy=%d cf=%.2f 停车3s 冷却5s\n", zebra_cy, zebra_cf);
            g_zc_frames = 0; g_zc_min_cy = 120;     // 重置, 为下次检测做准备
        } else if (zebra_seen && g_cfg.debug) {
            if (!zebra_near)
                printf("[ZEBRA] 忽略: 太远 cy=%d <= %d\n", zebra_cy, (int)g_cfg.zebrasee);
            else if (g_zc_frames < ZEBRA_MIN_FRAMES)
                printf("[ZEBRA] 忽略: 帧数不足 %d/%d cy=%d\n", g_zc_frames, ZEBRA_MIN_FRAMES, zebra_cy);
            else if (g_zc_min_cy > ZEBRA_FAR_CY)
                printf("[ZEBRA] 忽略: 未曾远距出现 min_cy=%d > %d\n", g_zc_min_cy, ZEBRA_FAR_CY);
        }
        break;
    case Z_STOP:
        if (now - g_ztime >= 3) {                   // 停车 3 秒
            g_zstate = Z_COOLDOWN; g_ztime = now;
            if (g_cfg.debug) printf("[ZEBRA] 起步\n");
        }
        break;
    case Z_COOLDOWN:
        if (now - g_ztime >= 5) {                    // 冷却 5 秒
            g_zstate = Z_NORMAL;
            if (g_cfg.debug) printf("[ZEBRA] 恢复\n");
        }
        break;
    }
    return (g_zstate == Z_STOP);
}

// ═══════════════════════════════════════════════════════════
//  Step 6b: 红绿灯状态机
//
//  去抖: 红灯/绿灯各自独立计数, 连续 3 帧确认, 未检测时 -1 衰减
//
//  状态转移:
//    TL_NORMAL     → TL_STOP        (红灯连续 ≥3 帧)
//    TL_STOP       → TL_WAIT_GREEN  (红灯消失)
//    TL_WAIT_GREEN → TL_NORMAL      (绿灯连续 ≥3 帧)
//
//  返回值: true = 非 TL_NORMAL (通知 motor_update 刹车)
// ═══════════════════════════════════════════════════════════
static bool traffic_light_process()
{
    // 遍历检测框, 统计红/绿灯是否出现
    bool red_seen = false, green_seen = false;
    for (int i = 0; i < g_box_count; ++i) {
        if (g_boxes[i].cls == MD_RED)   red_seen   = true;
        if (g_boxes[i].cls == MD_GREEN) green_seen = true;
    }

    // 去抖: 出现 +1, 消失 -1
    if (red_seen)   g_tl_red_frames++;
    else            g_tl_red_frames   = std::max(0, g_tl_red_frames   - 1);
    if (green_seen) g_tl_green_frames++;
    else            g_tl_green_frames = std::max(0, g_tl_green_frames - 1);

    switch (g_tl_state) {
    case TL_NORMAL:
        if (g_tl_red_frames >= 3) {                  // 红灯确认 → 停车
            g_tl_state = TL_STOP;
            if (g_cfg.debug) printf("[TL] 红灯停车\n");
        }
        break;
    case TL_STOP:
        if (!red_seen) {                             // 红灯消失 → 等绿灯
            g_tl_state = TL_WAIT_GREEN;
            if (g_cfg.debug) printf("[TL] 等待绿灯\n");
        }
        break;
    case TL_WAIT_GREEN:
        if (g_tl_green_frames >= 3) {                // 绿灯确认 → 通行
            g_tl_state = TL_NORMAL;
            g_tl_red_frames = g_tl_green_frames = 0; // 清零, 重新开始
            if (g_cfg.debug) printf("[TL] 绿灯通行\n");
        }
        break;
    }
    return (g_tl_state != TL_NORMAL);
}

// ═══════════════════════════════════════════════════════════
//  Step 4: 激光雷达挡板避障
//
//  lidar_is_active(): 判断是否处于确认或保持衰减状态
//
//  lidar_avoid_process() 流程:
//    1. 每 2 帧非阻塞读取 VL53L0X (readResult + startMeasure)
//    2. 预触发去抖: d_mm >= lidar_pre → 帧数衰减; d_mm == 0 → 跳过
//    3. 距离→行映射: 线性映射 [lidar_near, lidar_far] → [lt_h-1, 10]
//    4. 触发判定: d_mm < lidar_thresh && frames >= min_frames → 确认
//    5. 保持衰减: 确认后 hold_ctr 递增, 超过 hold_frames 则失效
//    6. 中线变形: 固定朝右推, 斜坡从 row=10 上升到障碍行, 衰减渐弱
// ═══════════════════════════════════════════════════════════
static bool lidar_is_active() {
    return g_lidar_confirmed ||
           (g_lidar_hold_ctr > 0 && g_lidar_hold_ctr <= g_cfg.lidar_hold_frames);
}

static void lidar_avoid_process()
{
    // ── 跳帧 + 前置检查 ──
    static int lidar_skip = 0;
    if (!g_lidar_ok || !g_cfg.lidar_enable) return;  // 硬件未就绪或已禁用
    if (++lidar_skip < 2) return;                    // 每 2 帧一次 (~15Hz)
    lidar_skip = 0;

    // ── 非阻塞读取上次测量结果, 立即启动下次测量 ──
    VL53L0X_RangingMeasurementData_t data;
    if (!g_lidar_sensor.readResult(data)) return;    // 读取失败 → 跳过
    int d_mm = data.RangeMilliMeter;
    g_lidar_sensor.startMeasure();                   // 后台测量 ~20ms

    // ── 预触发去抖窗口 ──
    int  lt_h = line_tracking_height;
    bool skip = false;
    if (d_mm == 0) {
        skip = true;                                 // 传感器读数为 0 = 读失败
    } else if (d_mm >= g_cfg.lidar_pre) {
        if (g_lidar_frames > 0) g_lidar_frames--;    // 超出窗口 → 衰减 (不归零)
        skip = true;
    }

    if (!skip) {
    // ── 距离 → 图像行映射 (线性) ──
    // lidar_near(50mm) → row=lt_h-1 (最近), lidar_far(1200mm) → row=10 (最远)
    int row = lt_h - 1 - (d_mm - g_cfg.lidar_near) * (lt_h - 11)
              / (g_cfg.lidar_far - g_cfg.lidar_near);
    if (row < 10)    row = 10;
    if (row >= lt_h) row = lt_h - 1;

    // ── 纯距离触发, 不检查视觉边界线 ──
    g_lidar_frames++;
    g_lidar_obstacle_row = row;

    g_lidar_confirmed = (d_mm < g_cfg.lidar_thresh) && (g_lidar_frames >= g_cfg.lidar_min_frames);

    static int dbg = 0;
    if (g_cfg.debug && ++dbg >= 30) { dbg = 0;
        printf("[LIDAR] d=%d row=%d frames=%d confirm=%d\n",
               d_mm, row, g_lidar_frames, g_lidar_confirmed);
    }

    // ── 确认 → 记录保持状态 ──
    if (g_lidar_confirmed) {
        g_lidar_hold_ctr     = 0;
        g_lidar_hold_src_row = g_lidar_obstacle_row;
        g_lidar_hold_dir     = 1.0;                 // 固定朝右绕行
        if (g_cfg.debug) printf("[LIDAR] 触发 d=%d row=%d dir=右\n", d_mm, g_lidar_obstacle_row);
        g_lidar_frames = 0;                          // 重置计数
    }
    }

    // ── 保持计数器递增 (确认中和消失后都递增) ──
hold_decay:
    if (g_lidar_hold_src_row > 0 && g_lidar_hold_ctr <= g_cfg.lidar_hold_frames)
        g_lidar_hold_ctr++;

    if (!lidar_is_active()) return;                  // 非活跃 → 不变形

    // ── 中线变形: 固定朝右推 ──
    // 从 row=10 到障碍行, 斜坡线性上升; 障碍行以下维持最大推力
    // 消失后 decay 线性衰减到 0
    int    src_row = g_lidar_hold_src_row;
    double rng     = (double)g_cfg.lidar_avoid_range;      // 斜坡陡峭度
    double half_w  = line_tracking_width / 2.0;
    double dir     = g_lidar_hold_dir;                     // 1.0 = 朝右
    double decay   = g_lidar_confirmed ? 1.0 :
                     (1.0 - (double)g_lidar_hold_ctr / g_cfg.lidar_hold_frames);

    for (int i = 10; i < lt_h; ++i) {
        // 障碍行及以上: 斜坡上升; 障碍行以下: 满推力
        double t = (i <= src_row) ? std::clamp((i - 10) / rng, 0.0, 1.0) : 1.0;
        double push = t * g_cfg.lidar_avoid_gain * half_w * dir * decay;
        double nm   = std::clamp(mid_line[i] + push,
                                  (double)left_line[i] + 2.0,
                                  (double)right_line[i] - 2.0);
        mid_line[i] = (int)(nm + 0.5);
    }
}

// ═══════════════════════════════════════════════════════════
//  Step 6c: 锥桶检测 + 中线变形
//
//  cone_is_slow(): 判断是否处于避让/保持/回弹任一阶段
//    (供 motor_update 减速用, 但减速代码当前已注释)
//
//  cone_detect_and_deform() 流程:
//    1. 仅在 Z_NORMAL && TL_NORMAL 时运行
//    2. 遍历 g_boxes 找 cls=0 (锥桶), 置信度 ≥ cone_thresh
//    3. 模型坐标→巡线空间, 边界过滤 (row 有效、赛道内)
//    4. cone_margin: 0=用中心点判断, 1=用框左右边缘判断
//    5. 取 cy 最大(最近)的锥桶
//    6. 去抖: 位置容差内连续帧计数, 突变则重计
//    7. 确认 → 保持衰减 → 回弹, 三阶段生命周期
//    8. 避让阶段: 推离锥桶方向的中线变形 (row 10 ~ 锥桶行)
//    9. 回弹阶段: 朝锥桶方向反推 (帮助车回到中央)
// ═══════════════════════════════════════════════════════════
static bool cone_is_slow() {
    return g_cone_confirmed ||
           (g_cone_hold_ctr > 0 && g_cone_hold_ctr <= g_cfg.cone_hold_frames);
    //       (g_cone_return_ctr > 0 && g_cone_return_ctr <= g_cfg.cone_return_frames);
}

static void cone_detect_and_deform()
{
    // 斑马线停车或红绿灯非正常时, 不做锥桶检测
    if (g_zstate == Z_STOP || g_tl_state != TL_NORMAL) return;

    // ── 1. 找最近锥桶 (cy 最大 = 最近) ──
    bool cone_seen = false;
    int  cone_row  = -1, cone_col = -1;

    for (int i = 0; i < g_box_count; ++i) {
        if (g_boxes[i].cls != MD_CONE) continue;
        if (g_boxes[i].conf < g_cfg.cone_thresh) continue;

        // 模型空间 → 巡线空间坐标映射
        int cl = g_boxes[i].cx * line_tracking_width  / raw_frame.cols;
        int rl = g_boxes[i].cy * line_tracking_height / raw_frame.rows;

        // 边界过滤: row 0~9 无可靠巡线数据
        if (rl < 10 || rl >= line_tracking_height) continue;
        if (left_line[rl] == -1 || right_line[rl] == -1) continue;  // 丢线行

        // 赛道内判断
        if (g_cfg.cone_margin) {
            // margin=1: 用框的左右边缘判断是否在赛道内
            int bl = (g_boxes[i].cx - g_boxes[i].w/2) * line_tracking_width  / raw_frame.cols;
            int br = (g_boxes[i].cx + g_boxes[i].w/2) * line_tracking_width  / raw_frame.cols;
            if (br < left_line[rl] || bl > right_line[rl]) continue;  // 框完全在赛道外
        } else {
            // margin=0: 用中心点判断
            if (cl < left_line[rl] || cl > right_line[rl]) continue;  // 中心在赛道外
        }

        // 取 cy 最大的 (最近的锥桶)
        if (rl > cone_row) { cone_row = rl; cone_col = cl; }
        cone_seen = true;
    }

    // ── 2. 去抖确认 ──
    {
        // 位置容差: 动态计算, 典型 row_tol=5, col_tol=10
        int row_tol = std::max(2, line_tracking_height / 12);
        int col_tol = std::max(3, line_tracking_width  / 8);
        if (cone_seen) {
            if (g_cone_frames == 0) {
                // 首次出现 → 记录位置, 开始计数
                g_cone_last_row = cone_row; g_cone_last_col = cone_col;
                g_cone_frames = 1;
            } else if (std::abs(cone_row - g_cone_last_row) <= row_tol &&
                       std::abs(cone_col - g_cone_last_col) <= col_tol) {
                // 位置接近 → 继续计数
                g_cone_frames++;
                g_cone_last_row = cone_row; g_cone_last_col = cone_col;
            } else {
                // 位置突变 → 视为新锥桶, 重新开始计数
                g_cone_frames = 1;
                g_cone_last_row = cone_row; g_cone_last_col = cone_col;
            }
        } else {
            // 未检测到 → 衰减
            if (g_cone_frames > 0) g_cone_frames = std::max(0, g_cone_frames - 1);
        }
    }

    g_cone_confirmed = (g_cone_frames >= g_cfg.cone_min_frames);

    static int cdbg = 0;
    if (g_cfg.debug && ++cdbg >= 15) { cdbg = 0;
        printf("[CONE] frames=%d confirm=%d hold=%d return=%d\n",
               g_cone_frames, g_cone_confirmed, g_cone_hold_ctr, g_cone_return_ctr);
    }

    // ── 3. 两阶段生命周期: 确认 → 保持衰减 ──
    if (g_cone_confirmed) {
        g_cone_hold_ctr   = 1;
        g_cone_return_ctr = 0;
        // 只在锥桶实际可见时更新冻结位置和回弹方向,
        // 防止 cone_row=-1 覆盖有效值 + mid_line[-1] 越界
        if (cone_seen) {
            g_cone_hold_src_row = cone_row;
            g_cone_hold_src_col = cone_col;
            // double ms = mid_line_raw[cone_row];
            // g_cone_return_dir = (cone_col < ms) ? -1.0 : 1.0;
        }
    } else if (g_cone_hold_ctr > 0 && g_cone_hold_ctr <= g_cfg.cone_hold_frames) {
        // 保持衰减阶段: 锥桶消失, 变形量逐帧衰减
        g_cone_hold_ctr++;
        // if (g_cone_hold_ctr > g_cfg.cone_hold_frames)
        //     g_cone_return_ctr = 1;                 // 保持结束 → 启动回弹
    } /* else if (g_cone_return_ctr > 0 && g_cone_return_ctr <= g_cfg.cone_return_frames) {
        // 回弹阶段: 朝锥桶方向反推, 帮助车回到赛道中央
        g_cone_return_ctr++;
    } */

    // 回弹结束 → 清零所有状态, 避免计数器卡死
    // if (g_cone_return_ctr > g_cfg.cone_return_frames) {
    //     printf("[CONE] ★ RETURN END\n");
    //     g_cone_hold_src_row = -1;
    //     g_cone_hold_ctr = 0;
    //     g_cone_return_ctr = 0;
    // }

    // 生命周期诊断 (debug 模式下每 5 帧打印)
    // static int cone_ldbg = 0;
    // if ((g_cone_confirmed || g_cone_hold_ctr > 0 || g_cone_return_ctr > 0) && ++cone_ldbg >= 5) {
    //     cone_ldbg = 0;
    //     printf("[CONE-DBG] seen=%d frm=%d conf=%d hold=%d/%d ret=%d/%d src_row=%d ret_dir=%.0f\n",
    //            cone_seen, g_cone_frames, g_cone_confirmed,
    //            g_cone_hold_ctr, g_cfg.cone_hold_frames,
    //            g_cone_return_ctr, g_cfg.cone_return_frames,
    //            g_cone_hold_src_row, g_cone_return_dir);
    // }

    // 三个阶段都不活跃 → 不做变形
    if (!cone_is_slow()) return;

    // ── 4. 中线变形 ──
    // confirmed+可见 → 实时位置; 否则(confirmed+不可见 / 保持 / 回弹) → 冻结位置
    int src_row = (g_cone_confirmed && cone_seen) ? cone_row : g_cone_hold_src_row;
    int src_col = (g_cone_confirmed && cone_seen) ? cone_col : g_cone_hold_src_col;
    if (src_row <= 10) return;

    double rng    = (double)g_cfg.cone_avoid_range;         // 斜坡陡峭度
    double half_w = line_tracking_width / 2.0;
    double ms     = mid_line_raw[src_row];
    // 推离方向: 锥桶在原始中线左侧 → 推右(+1), 右侧 → 推左(-1)
    double dir    = (src_col < ms) ? 1.0 : -1.0;
    // 保持阶段衰减: 从 1.0 线性降到 0.0
    double decay  = g_cone_confirmed ? 1.0 : (1.0 - (double)g_cone_hold_ctr / g_cfg.cone_hold_frames);

    // ── 4a. 避开阶段: 推离锥桶 (row 10 ~ 底部, 确保前瞻行一定被覆盖) ──
    // if (g_cone_return_ctr == 0) {
        for (int row = 10; row < line_tracking_height; ++row) {
            // 斜坡: row=10 处 t=0(不推), 随行号增大 t→1(满推)
            double t   = std::clamp(((row - 10) / rng), 0.0, 1.0);
            double push = t * g_cfg.cone_avoid_gain * half_w * dir * decay;
            double nm  = std::clamp(mid_line[row] + push,
                                    (double)left_line[row] + 2.0,
                                    (double)right_line[row] - 2.0);
            mid_line[row] = (int)(nm + 0.5);
        }
    // }

    // ── 4b. 回弹: 锥桶消失后朝锥桶方向反推 ──
    // if (g_cone_return_ctr > 0 && g_cone_return_ctr <= g_cfg.cone_return_frames) {
    //     static int rdbg = 0;
    //     if (++rdbg >= 10) { rdbg = 0;
    //         printf("[CONE] return frame=%d dir=%.0f gain=%.1f\n",
    //                g_cone_return_ctr, g_cone_return_dir, g_cfg.cone_return_gain);
    //     }
    //     int    lt_h   = line_tracking_height;
    //     double ret_rng = rng * 1.2;                         // 回弹斜坡比避让略缓
    //     double ret_hw  = half_w;
    //     double ret_dir = g_cone_return_dir;                 // 与避让方向相反
    //     // 回弹衰减: 从 1.0 线性降到 0.0
    //     double ret_dec = 1.0 - (double)g_cone_return_ctr / g_cfg.cone_return_frames;
    //     // 回弹覆盖全部有效行 (row 10 ~ lt_h-1), 不限于锥桶位置
    //     for (int row = 10; row < lt_h; ++row) {
    //         double t = std::clamp((row - 10) / ret_rng, 0.0, 1.0);
    //         double push = t * g_cfg.cone_return_gain * ret_hw * ret_dir * ret_dec;
    //         double nm = std::clamp(mid_line[row] + push,
    //                                 (double)left_line[row] + 2.0,
    //                                 (double)right_line[row] - 2.0);
    //         mid_line[row] = (int)(nm + 0.5);
    //     }
    // }
}


// ═══════════════════════════════════════════════════════════
//  急弯检测 — 扫描左右边界纵向梯度
//
//  5 行窗口内边界水平位移 > 15 像素 → 直角弯
// ═══════════════════════════════════════════════════════════
static bool g_sharp_turn = false;

static void detect_sharp_turn()
{
    g_sharp_turn = false;
    const int W = 3;                     // 窗口大小(行)
    const int THR = 20;                  // 位移阈值(像素)
    int lt_h = line_tracking_height;
    for (int r = lt_h - 1; r >= 10 + W; --r) {
        if (left_line[r] == -1 || left_line[r-W] == -1) continue;
        if (std::abs(left_line[r] - left_line[r-W]) > THR) { g_sharp_turn = true; return; }
    }
    for (int r = lt_h - 1; r >= 10 + W; --r) {
        if (right_line[r] == -1 || right_line[r-W] == -1) continue;
        if (std::abs(right_line[r] - right_line[r-W]) > THR) { g_sharp_turn = true; return; }
    }
}

// ═══════════════════════════════════════════════════════════
//  Step 7: 舵机控制
//
//  读取 mid_line[foresee/calc_scale] 处的中线位置,
//  转换为显示空间偏差 → 归一化 → 乘增益 → PWM 占空比。
//
//  数学:
//    deviation = mid_line[check_row] × calc_scale - newWidth/2  (像素)
//    deviation -= center_bias                                    (偏置修正)
//    g_steer_deviation = deviation / (newWidth/2)                (归一化 [-1,1])
//
//    |deviation| < deadband → 直行 (1,500,000 ns)
//    否则: offset = norm × steer_gain × 300,000
//          duty = clamp(1,500,000 + offset, 1,200,000, 1,800,000)
//
//  舵机量程: 1,200,000 ~ 1,800,000 ns, 中位 1,500,000 ns
// ═══════════════════════════════════════════════════════════
static void steering_update()
{
    if (!g_cfg.start) return;

    // 斑马线起步后延迟2s, 再3s内缩短前瞻, 先走稳再改
    double eff_foresee = g_cfg.foresee;
    if (g_zstate == Z_COOLDOWN && time(nullptr) - g_ztime >= 2
        && time(nullptr) - g_ztime < 5)
        eff_foresee = 24;
    int check_row = (int)eff_foresee / calc_scale;
    if (check_row < 0 || check_row >= line_tracking_height) return;
    if (mid_line[check_row] == -1) return;           // 丢线行 → 跳过

    // 巡线空间中线 → 显示空间偏差
    double deviation = mid_line[check_row] * calc_scale - newWidth / 2;
    deviation -= g_cfg.center_bias;                  // 机械偏置修正
    // 归一化到 [-1, 1], 供弯道减速使用
    g_steer_deviation = deviation / (newWidth / 2.0);

    // 斑马线冷却后 3.3~5 秒：一次机会，偏差出现->方向锁定打满->方向翻转/回正即退出
    if (g_zstate == Z_COOLDOWN) {
        time_t elapsed = time(nullptr) - g_ztime;
        if (elapsed >= 3.3 && elapsed < 5 && !g_bangbang_used) {
            if (!g_bangbang_active) {
                // 序列未开始，等偏差出现才启动
                if (std::abs(deviation) < g_cfg.deadband) {
                    g_steer_deviation = 0.0;
                    servo.setDutyCycle(1500000);
                    return;
                }
                g_bangbang_active = true;
                g_bangbang_dir    = (deviation > 0) ? 1 : -1;
            } else {
                // 序列已启动：检查方向是否翻转或回到死区
                bool same_dir = (deviation > 0 && g_bangbang_dir > 0) ||
                                (deviation < 0 && g_bangbang_dir < 0);
                if (!same_dir || std::abs(deviation) < g_cfg.deadband) {
                    // 方向翻转或回正 -> 机会用完，退出，后续走正常线性转向
                    g_bangbang_active = false;
                    g_bangbang_used   = true;
                } else {
                    // 同方向，继续打满
                    unsigned int duty = (g_bangbang_dir > 0) ? 1800000 : 1200000;
                    servo.setDutyCycle(duty);
                    g_steer_deviation = (g_bangbang_dir > 0) ? 1.0 : -1.0;
                    return;
                }
            }
        }
    }

    if (std::abs(deviation) < g_cfg.deadband) {
        // 死区内 → 直行
        servo.setDutyCycle(1500000);
    } else {
        // 比例控制: 偏差 → 归一化 → × 增益 × 满偏占空比差
        double offset  = deviation / (newWidth / 2.0) * g_cfg.steer_gain * 300000;
        double duty_ns = std::clamp(1500000.0 + offset, 1200000.0, 1800000.0);
        servo.setDutyCycle((unsigned int)duty_ns);
    }
}

// ═══════════════════════════════════════════════════════════
//  Step 8: 电机控制
//
//  弯道减速:
//    factor = 1.0 - |g_steer_deviation| × curve_slope
//    factor = max(factor, curve_min)
//    final_spd = target_speed × factor
//
//  锥桶/挡板减速 (代码存在但已注释):
//    cone_is_slow()   → factor = min(factor, cone_speed)
//    lidar_is_active() → factor = min(factor, lidar_speed)
//
//  调用 ControlUpdate(final_spd, block) 执行:
//    block = zebra_block || tl_block → 编码器比例刹车
//    否则 → 开环占空比驱动
//
//  起步弹射: 启动/斑马线恢复后 1.5x 速度持续 0.5s 快速起步
// ═══════════════════════════════════════════════════════════
static struct timespec g_boost_end   = {0, 0};
static double         g_boost_mul    = 1.0;
static constexpr double BOOST_SECS        = 0.7;
static constexpr double BOOST_MUL         = 1.6;
static constexpr double BOOST_GREEN_SECS  = 0.2;
static constexpr double BOOST_GREEN_MUL   = 2.0;

static void trigger_boost(double secs, double mul)
{
    g_boost_mul = mul;
    clock_gettime(CLOCK_MONOTONIC, &g_boost_end);
    g_boost_end.tv_nsec += (long)(secs * 1e9);
    if (g_boost_end.tv_nsec >= 1000000000L) {
        g_boost_end.tv_sec += 1;
        g_boost_end.tv_nsec -= 1000000000L;
    }
}

static double boost_factor()
{
    struct timespec now;
    clock_gettime(CLOCK_MONOTONIC, &now);
    if (now.tv_sec > g_boost_end.tv_sec) return 1.0;
    if (now.tv_sec == g_boost_end.tv_sec && now.tv_nsec >= g_boost_end.tv_nsec) return 1.0;
    return g_boost_mul;
}

static void motor_update(bool zebra_block, bool tl_block)
{
    // 起步弹射检测
    static int prev_start = 0;
    static ZState prev_zstate = Z_NORMAL;
    static TLState prev_tlstate = TL_NORMAL;
    if (g_cfg.start && !prev_start) trigger_boost(BOOST_SECS, BOOST_MUL);           // 启动
    if (prev_zstate == Z_STOP && g_zstate == Z_COOLDOWN) trigger_boost(BOOST_SECS, BOOST_MUL); // 斑马线起步
    if (prev_tlstate == TL_WAIT_GREEN && g_tl_state == TL_NORMAL) trigger_boost(BOOST_GREEN_SECS, BOOST_GREEN_MUL); // 绿灯起步
    prev_start  = g_cfg.start;
    prev_zstate = g_zstate;
    prev_tlstate = g_tl_state;

    double spd;
    if (boost_factor() > 1.0) {
        spd = 26.0;                           // 起步弹射
    } else if (g_zstate == Z_COOLDOWN) {
        spd = 16.0;                           // 冷却期基础速度
        time_t elapsed = time(nullptr) - g_ztime;
        if (elapsed >= 3.3 && elapsed < 5)
            spd = std::min(spd, 13.0);        // 3.3~5s 降速
    } else {
        spd = target_speed;                   // 正常行驶
    }

    // 弯道减速: 舵机偏差越大 → 速度越低
    double curve = 1.0 - std::abs(g_steer_deviation) * g_cfg.curve_slope;
    if (curve < g_cfg.curve_min) curve = g_cfg.curve_min;

    // 取最低倍率: 弯道 / 锥桶 / 挡板 三者不叠加, 取最小值
    double factor = curve;
    // if (g_lost_rows > 15)
    //     factor = std::min(factor, 0.6);
    // if (cone_is_slow() && g_zstate != Z_STOP && g_tl_state == TL_NORMAL)
    //     factor = std::min(factor, g_cfg.cone_speed);
    // if (lidar_is_active() && g_zstate != Z_STOP && g_tl_state == TL_NORMAL)
    //     factor = std::min(factor, g_cfg.lidar_speed);

    double final_spd = spd * factor;

    // 每 30 帧打印一次速度调试信息
    static int dbg = 0;
    if (++dbg >= 30) { dbg = 0;
        printf("[SPD] base=%.0f dev=%.2f curve=%.2f final=%.1f\n",
               spd, g_steer_deviation, curve, final_spd);
    }

    // 传递给 control.cpp: 刹车信号 = 斑马线停车 OR 红绿灯停车
    ControlUpdate(final_spd, zebra_block || tl_block);
}


// ═══════════════════════════════════════════════════════════
//  Step 9: LCD 渲染
//
//  流程:
//    1. 每 10 帧从 g_cfg.showImg 刷新 g_lcd_on 开关
//    2. track (128/0 灰度) → resize → GRAY2BGR → 居中拷贝到 lcd_fbImage
//    3. 绘制边界线: 红=左边界, 绿=右边界, 蓝=中线
//    4. 绘制检测框: 橙=锥桶, 紫=斑马线 (跳过红绿灯框)
//    5. 绘制状态指示: L=激光避障, R=红灯, G=等绿灯, S=斑马线停, C=冷却, N=正常
//    6. BGR → RGB565 → 写入 /dev/fb0 mmap
// ═══════════════════════════════════════════════════════════
static cv::Mat lcd_fbImage, lcd_resized, lcd_colored;

static void lcd_clear_screen()
{
    if (fb_buffer && g_fb_size > 0)
        std::memset(fb_buffer, 0, g_fb_size);
}

static void lcd_show_stop_image()
{
    if (g_stop_image.empty() || !fb_buffer) return;
    // 若图片尺寸与屏幕不一致则缩放
    cv::Mat display_img = g_stop_image;
    if (display_img.cols != screenWidth || display_img.rows != screenHeight)
        cv::resize(display_img, display_img, cv::Size(screenWidth, screenHeight));
    convertMatToRGB565(display_img, fb_buffer, screenWidth, screenHeight);
}

static void lcd_render()
{
    // 斑马线停车期间由 CameraHandler 单独控制清屏+图片, 此处不渲染
    if (g_zstate == Z_STOP) return;

    // 每 10 帧刷新 LCD 开关, 避免每帧读文件
    static int lcd_check = 0;
    if (--lcd_check < 0) { g_lcd_on = g_cfg.showImg; lcd_check = 10; }
    if (!g_lcd_on) return;

    // 创建黑底画布
    lcd_fbImage.create(screenHeight, screenWidth, CV_8UC3);
    lcd_fbImage.setTo(cv::Scalar(0, 0, 0));

    // 赛道灰度图居中显示
    cv::resize(track, lcd_resized, cv::Size(newWidth, newHeight));
    cv::cvtColor(lcd_resized, lcd_colored, cv::COLOR_GRAY2BGR);

    cv::Rect roi((screenWidth - newWidth) / 2, (screenHeight - newHeight) / 2, newWidth, newHeight);
    lcd_colored.copyTo(lcd_fbImage(roi));

    // ── 边界线: 逐行绘制 ──
    for (int y = 0; y < line_tracking_height; ++y) {
        int lx = (int)(left_line[y]  * calc_scale), ly = y * calc_scale;
        int rx = (int)(right_line[y] * calc_scale);
        int mx = (int)(mid_line[y]   * calc_scale);
        cv::line(lcd_fbImage(roi), cv::Point(lx, ly), cv::Point(lx, ly), cv::Scalar(0, 0, 255),   calc_scale);  // 红=左
        cv::line(lcd_fbImage(roi), cv::Point(rx, ly), cv::Point(rx, ly), cv::Scalar(0, 255, 0),   calc_scale);  // 绿=右
        cv::line(lcd_fbImage(roi), cv::Point(mx, ly), cv::Point(mx, ly), cv::Scalar(255, 0, 0),   calc_scale);  // 蓝=中
    }

    // ── 检测框 (跳过红绿灯, 不画框) ──
    float bx = (float)newWidth / raw_frame.cols, by = (float)newHeight / raw_frame.rows;
    for (int i = 0; i < g_box_count; ++i) {
        if (g_boxes[i].cls == MD_RED || g_boxes[i].cls == MD_GREEN) continue;
        // 模型空间 → 显示空间
        int x1 = (int)((g_boxes[i].cx - g_boxes[i].w/2) * bx);
        int y1 = (int)((g_boxes[i].cy - g_boxes[i].h/2) * by);
        int x2 = (int)((g_boxes[i].cx + g_boxes[i].w/2) * bx);
        int y2 = (int)((g_boxes[i].cy + g_boxes[i].h/2) * by);
        x1 = std::max(0, std::min(newWidth  - 1, x1));
        y1 = std::max(0, std::min(newHeight - 1, y1));
        x2 = std::max(0, std::min(newWidth  - 1, x2));
        y2 = std::max(0, std::min(newHeight - 1, y2));

        // 颜色: 锥桶=橙, 斑马线=紫, 其他=绿
        cv::Scalar color(0, 255, 0);
        if (g_boxes[i].cls == MD_ZEBRA) color = cv::Scalar(255, 0, 255);       // 紫
        else if (g_boxes[i].cls == MD_CONE) color = cv::Scalar(0, 165, 255);   // 橙
        cv::rectangle(lcd_fbImage(roi), cv::Point(x1, y1), cv::Point(x2, y2), color, 2);

        // 标注: 类别编号 + 置信度百分比
        char lab[16]; snprintf(lab, 16, "%d %.0f", g_boxes[i].cls, g_boxes[i].conf * 100);
        cv::putText(lcd_fbImage(roi), lab, cv::Point(x1 + 2, y1 + 10),
                    cv::FONT_HERSHEY_SIMPLEX, 0.3, color, 1);
    }

    // ── 状态指示 (左下角) ──
    // L=激光避障, R=红灯停, G=等绿灯, S=斑马线停, C=冷却, N=正常
    char ztxt[8]; int zi = 0;
    // if (lidar_is_active())              ztxt[zi++] = 'L';
    if (g_tl_state == TL_STOP)         ztxt[zi++] = 'R';
    else if (g_tl_state == TL_WAIT_GREEN) ztxt[zi++] = 'G';
    if (g_zstate == Z_STOP)            ztxt[zi++] = 'S';
    else if (g_zstate == Z_COOLDOWN)    ztxt[zi++] = 'C';
    else                                ztxt[zi++] = 'N';
    ztxt[zi] = 0;
    cv::putText(lcd_fbImage(roi), ztxt, cv::Point(2, newHeight - 4),
                cv::FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(0, 255, 255), 1);

    // BGR → RGB565 → /dev/fb0
    convertMatToRGB565(lcd_fbImage, fb_buffer, screenWidth, screenHeight);
}

// ═══════════════════════════════════════════════════════════
//  Step 10: FPS 统计
//
//  每 15 帧输出一行:
//    fps: 总帧率
//    cap: 取帧耗时(ms)
//    vis: 视觉巡线耗时(ms)
//    mdl: 模型推理耗时(ms) (管道末尾, 不阻塞舵机)
//    ctl: 控制耗时(ms)  (状态机+舵机+电机)
//    lat: 端到端延时(ms) (取帧开始→舵机输出)
//
//  ts[] 时间戳由 CameraHandler 在各步骤间采集
// ═══════════════════════════════════════════════════════════
static void fps_log(struct timespec *ts)
{
    static int fc = 0;
    static struct timespec tf0;
    if (fc == 0) clock_gettime(CLOCK_MONOTONIC, &tf0);
    fc++;
    if (fc % 15 != 0) return;                        // 每 15 帧输出一次

    struct timespec tf1;
    clock_gettime(CLOCK_MONOTONIC, &tf1);

    double dt     = (tf1.tv_sec - tf0.tv_sec) + (tf1.tv_nsec - tf0.tv_nsec) * 1e-9;
    double ms_cap = (ts[1].tv_sec - ts[0].tv_sec) * 1000.0 + (ts[1].tv_nsec - ts[0].tv_nsec) * 1e-6;
    double ms_vis = (ts[2].tv_sec - ts[1].tv_sec) * 1000.0 + (ts[2].tv_nsec - ts[1].tv_nsec) * 1e-6;
    double ms_mdl = (ts[5].tv_sec - ts[4].tv_sec) * 1000.0 + (ts[5].tv_nsec - ts[4].tv_nsec) * 1e-6;
    double ms_ctl = (ts[4].tv_sec - ts[2].tv_sec) * 1000.0 + (ts[4].tv_nsec - ts[2].tv_nsec) * 1e-6;
    double ms_lat = (ts[3].tv_sec - ts[0].tv_sec) * 1000.0 + (ts[3].tv_nsec - ts[0].tv_nsec) * 1e-6;

    printf("fps=%.1f | cap=%.0f vis=%.0f mdl=%.0f ctl=%.0f lat=%.0f ms\r",
           fc / dt, ms_cap, ms_vis, ms_mdl, ms_ctl, ms_lat);
    fflush(stdout);
}


// ═══════════════════════════════════════════════════════════
//  CameraHandler — 每帧主流程 (10 步流水线)
//
//  由 main.cpp 主循环调用, 单线程顺序执行。
//
//  附加逻辑:
//    - debug 模式每 30 帧重载全部配置 (cfg_load_all)
//
//  ts[6] 时间戳分布 (按执行顺序):
//    ts[0]: capture_frame 取帧开始
//    ts[1]: capture_frame 取帧结束
//    ts[2]: image_main 视觉巡线结束
//    ts[3]: steering_update 舵机输出结束 (端到端延时终点)
//    ts[4]: motor_update 电机控制结束
//    ts[5]: run_model_inference 模型推理结束 (管道末尾)
// ═══════════════════════════════════════════════════════════
int CameraHandler(void)
{
    // debug 模式: 每 30 帧热重载配置文件
    static int reloadCnt = 0;
    if (g_cfg.debug && ++reloadCnt >= 30) { reloadCnt = 0; cfg_load_all(); }
    // 嵌入式优化: 每 150 帧回收 glibc malloc 缓存的空闲页
    static int trimCnt = 0;
    // if (++trimCnt >= 150) { trimCnt = 0; malloc_trim(0); }

    struct timespec ts[6];

    // 1. 取帧 + MJPEG 解码
    if (capture_frame(&ts[0], &ts[1]) < 0) return -1;

    // 2. 保存图像 (debug)
    save_image_if_requested();

    // 3. 视觉巡线 → left/right/mid_line[]
    image_main();
    mid_line_raw = mid_line;
    clock_gettime(CLOCK_MONOTONIC, &ts[2]);

    // 4. 激光避障 → 修改 mid_line[] (在模型推理之前, 优先级高于锥桶)
    // lidar_avoid_process();

    // 5. 场景识别状态机 (使用上一帧的模型检测结果 g_boxes[])
    bool zebra_block = zebra_process();              // 5a. 斑马线
    bool tl_block    = traffic_light_process();      // 5b. 红绿灯
    cone_detect_and_deform();                        // 5c. 锥桶 → 修改 mid_line[]

    // detect_sharp_turn();                             // 急弯检测

    // 6. 舵机控制 (读取变形后的 mid_line)
    steering_update();
    clock_gettime(CLOCK_MONOTONIC, &ts[3]);          // 舵机输出结束 = 端到端延时终点
    // ── 临时测试: 锥桶确认后 1s 强制右转 0.5s ──
    // {
    //     static int  cone_test_timer    = -1;
    //     static bool cone_test_need_clr = false;
    // 
    //     if (cone_test_need_clr && !g_cone_confirmed)
    //         cone_test_need_clr = false;
    // 
    //     if (g_cone_confirmed && cone_test_timer < 0 && !cone_test_need_clr) {
    //         cone_test_timer = 0;
    //         printf("[CONE-TEST] 锥桶确认, 开始计时\n");
    //     }
    //     if (cone_test_timer >= 0) {
    //         cone_test_timer++;
    //         if (cone_test_timer == 30)
    //             printf("[CONE-TEST] 强制右转开始\n");
    //         if (cone_test_timer >= 30 && cone_test_timer < 38)
    //             servo.setDutyCycle(1700000);
    //         if (cone_test_timer >= 38) {
    //             printf("[CONE-TEST] 强制右转结束\n");
    //             cone_test_timer    = -1;
    //             cone_test_need_clr = true;
    //         }
    //     }
    // }

    // 7. 电机控制 (弯道减速 + 刹车)
    motor_update(zebra_block, tl_block);
    clock_gettime(CLOCK_MONOTONIC, &ts[4]);

    // 8. 模型推理 → g_boxes[] (放到最后, 供下一帧状态机使用, 不阻塞舵机)
    run_model_inference();
    clock_gettime(CLOCK_MONOTONIC, &ts[5]);

    // 9. LCD 渲染
    // 斑马线停车状态切换时清屏/显示提示图片，仅切换时操作，不每帧准备缓存
    static ZState prev_zstate = Z_NORMAL;
    if (g_zstate == Z_STOP && prev_zstate != Z_STOP) {
        lcd_clear_screen();
        lcd_show_stop_image();
    } else if (g_zstate != Z_STOP && prev_zstate == Z_STOP) {
        lcd_clear_screen();
    }
    // 进入 Z_COOLDOWN 时重置打满状态机
    if (g_zstate == Z_COOLDOWN && prev_zstate != Z_COOLDOWN) {
        g_bangbang_used  = false;
        g_bangbang_active = false;
    }
    prev_zstate = g_zstate;

    lcd_render();

    // 10. FPS 日志
    fps_log(ts);

    return 0;
}