/*
 * 斑马线实时检测 — 龙芯2K0300 小车摄像头版 (含逆透视变换 IPM)
 * =================================================================
 *
 * 【设计目的】
 *   在车辆行驶过程中实时识别前方斑马线，将检测结果写入标志文件
 *   供巡线主控模块 (control.cpp) 读取，做出减速/停车等安全决策。
 *
 * 【算法来源】
 *   移植自 Python 开源项目: https://github.com/TomMao23/ZebraCrossing_Detection
 *   利用斑马线的四个视觉特征做传统图像处理检测:
 *     (1) 梯度一致性 — 黑白交替 → 边缘方向高度集中
 *     (2) 等间隔      — 线条等距分布 (当前未使用)
 *     (3) 多根线      — 一个区域内有大量平行边缘
 *     (4) 宽度大      — 斑马线宽度明显大于车道线
 *
 * 【处理流水线 (每帧)】
 *
 *   摄像头 320×240 (MJPG, /dev/video0)
 *     ↓
 *   ┌── [IPM] 逆透视变换 ──────────────────────────────┐
 *   │ 前视图 → 鸟瞰图 (BEV, Bird's Eye View)            │
 *   │ cv::warpPerspective(frame, bev, iM, 640×640)     │
 *   │ 将倾斜的透视角度"拉平"，使斑马线恢复矩形形状      │
 *   │ 矩阵 M 需针对摄像头安装角度/高度单独标定           │
 *   │ 标定方法：拍摄地面矩形标定板 → 四点透视变换        │
 *   └──────────────────────────────────────────────────┘
 *     ↓ if USE_IPM
 *   640×640 BEV 图 (已校正透视畸变)
 *     ↓ cv::resize
 *   400×400 正方形 BGR 图
 *     ↓ extractChannel(0) → 只取 B 通道 (利用蓝色通道衰减黄色减速带)
 *   medianBlur 5×5 → 去砖缝/椒盐噪声, 保留梯度大小
 *     ↓
 *   MORPH_OPEN 3×3 iter=4 → 多次腐蚀去窄车道线, 斑马线保留
 *     ↓
 *   MORPH_CLOSE 5×5 iter=3 → 先膨胀填斑马线缺损, 再腐蚀抑制路面箭头碎片
 *     ↓
 *   Canny(30, 90) → 双阈值边缘检测, 利用连通性合并弱边缘
 *     ↓
 *   Sobel 3×3 → 计算每个边缘像素的梯度模值 Amplitude 和方向 theta(0~90°)
 *     ↓ 弱边缘过滤 (Amplitude < 30 → 置零)
 *   滑动窗口 100×302, 步长 50, 水平范围固定 [39, 341]
 *     ↓
 *   每个窗口内:
 *     - 统计有效梯度点 (Amplitude > 0, theta ∈ [0, 70°)) 的方向直方图
 *     - 14 个 bin, 每 bin 5° (0°~5°, 5°~10°, ..., 65°~70°)
 *     - 取峰值 bin 的点数
 *     - 峰值 > 1500 → 判为斑马线
 *     ↓
 *   所有阳性窗口的纵向跨度 = 最终斑马线位置 (紫色框)
 *     ↓
 *   LCD 叠加显示 + 写入 ./zebra_detected 文件
 *
 * 【为何需要逆透视变换 (IPM)】
 *   前视摄像头拍到的斑马线是透视畸变的 (近大远小、倾斜变形)
 *   → 斑马线的平行特征被破坏 → 梯度一致性减弱 → 检测效果下降
 *   IPM 将图像变换为鸟瞰视角 → 斑马线恢复矩形/平行特征
 *   → Sobel 梯度方向集中在同一角度 → 直方图峰值更高 → 检测更准
 *
 * 【注意】IPM 矩阵必须针对实车摄像头标定！
 *   当前矩阵是 Python 项目原始标定值，在 2K0300 小车上可能不适用。
 *   建议先关闭 IPM 测试 (USE_IPM=0)，确认检测逻辑正确后，
 *   再用标定板重新标定并开启 IPM。
 *
 * 【为何方向限制在 0~70°】
 *   排除水平停止线干扰:
 *     停止线 = 横线 → Sobel dx 大 / dy → 0  → 方向接近 0°
 *     斑马线 = 纵线/斜线 → 方向偏大 (30°~90°)
 *     实际上 bin 0 也在统计范围但峰值不高, 主要靠阈值过滤
 *
 * 【为何窗口固定水平范围 [39, 341]】
 *   排除道路两侧的行人道/建筑/绿化等干扰
 *   400 像素宽, 窗口 302 宽, 居中左偏 39px, 右侧留 59px
 *
 * 【为何单独用蓝色通道而非灰度图】
 *   BGR 三通道中:
 *     B 通道 = 蓝色分量
 *     黄色减速带 ≈ R+G (无蓝色) → B 通道响应 ≈ 0 → 完全消除减速带边缘
 *     白色斑马线 ≈ R+G+B 均等  → B 通道正常响应 → 斑马线边缘保留
 *   如果用灰度 (0.299R + 0.587G + 0.114B), 黄色减速带会产生强边缘
 *
 * 【为何用中值滤波而非高斯/均值滤波】
 *   中值滤波最大程度保留原始梯度大小 (不模糊边缘), 同时完美消灭人行道砖缝纹理
 *   高斯/均值会平滑边缘 → 梯度幅度衰减 → 影响判定阈值
 *
 * 【为何先开运算再闭运算】
 *   开运算 (先 Erosion 后 Dilation):
 *     iter=4 次连续腐蚀 → 窄车道线被彻底腐蚀消失
 *     后续膨胀 → 恢复剩余斑马线的尺寸
 *     等效: 保留 "宽度 > 某阈值" 的线条 (斑马线)，去除 "窄的" 线条 (车道线)
 *   闭运算 (先 Dilation 后 Erosion):
 *     先膨胀 → 填补斑马线内部的微小缺损/裂缝
 *     后腐蚀 → 恢复原始尺寸，同时消除箭头碎片的虚假边缘
 *
 * 【为何用 Canny 而非直接对 Sobel 结果判定】
 *   (1) Canny 双阈值 + 连通性分析 → 孤立噪点被抑制, 只有真正连续的边缘通过
 *   (2) Canny 使一条边缘只保留最细的 1px 脊线 → 斑马线 "多根线" 特征更突出
 *       (直接 Sobel 一根粗边缘产生 N 个梯度点 → 降低了线数的区分度)
 *   (3) Canny 两个阈值让调参范围更大, 比单梯度阈值更鲁棒
 *
 * 【2K0300 算力评估 (400×400 分辨率)】
 *   IPM warpPerspective: O(640×640×常数) ≈ 2.5M
 *   蓝色通道提取:        O(160K) ≈ 忽略
 *   medianBlur 5×5:      O(160K × 25) ≈ 4M
 *   morphOpen ×4:        O(160K × 9 × 8) ≈ 11.5M
 *   morphClose ×3:       O(160K × 25 × 6) ≈ 24M
 *   Canny:               O(160K × 常数) ≈ 几M
 *   Sobel ×2:            O(160K × 9 × 2) ≈ 2.9M
 *   梯度逐像素:          O(160K × 10) ≈ 1.6M
 *   滑动窗口 (7窗):      O(7 × 30200) ≈ 0.2M
 *   ---------------------------------------------------
 *   总计约 50~55M 操作/帧, 2K0300 0.43 GOPS → 理论 7~10 FPS
 *   优化: 关闭 IPM 可节省 ~2.5M/帧; 增大 WIN_STEP 减少窗口数
 *
 * 用法:
 *   ./zebra_demo                      带 LCD 显示 + 检测 (含 IPM)
 *   ./zebra_demo --no-display         无头模式 (只写状态文件, 省算力)
 *   ./zebra_demo --no-ipm            关闭逆透视变换 (使用前视图)
 *
 * 跨模块通信:
 *   写入 ./zebra_detected: "1" = 检测到斑马线, "0" = 未检测到
 *   控制模块 (control.cpp) 可用 readFlag("./zebra_detected") 读取
 *   仅在状态变化时写入, 减少文件 I/O
 *
 * 退出: 按 Ctrl+C (SIGINT) 或 kill (SIGTERM) 优雅清理资源
 */

#include <opencv2/opencv.hpp>
#include <iostream>
#include <vector>
#include <cmath>
#include <fstream>
#include <csignal>
#include <atomic>

// Linux 特定: 帧缓冲屏 + mmap + ioctl
#include <fcntl.h>
#include <unistd.h>
#include <sys/mman.h>
#include <sys/ioctl.h>
#include <linux/fb.h>

// ============================================================
// 可调参数 — 与 Python 版算法完全对齐
// ============================================================

// --- 逆透视变换 (IPM) ---
// 是否启用 IPM: 0=关闭(使用前视图), 1=开启(使用鸟瞰图)
// 启用前必须用标定板重新标定矩阵 M, 否则效果可能比关闭更差!
#define USE_IPM  1

// IPM 输出鸟瞰图尺寸 (与 Python 版 gd.py 一致)
const int   IPM_SIZE          = 640;

// --- 滑动窗口 ---
const int   WIN_H             = 100;
const int   WIN_W             = 302;
const int   WIN_STEP          = 50;
const int   WIN_COL_L         = 39;
const int   WIN_COL_R         = 341;

// --- 处理分辨率 ---
const int   PROC_SIZE         = 400;

// --- 梯度过滤 ---
const float GRAD_THRESHOLD    = 30.0f;

// --- 方向直方图 ---
const int   HIST_BIN_SIZE     = 5;
const int   HIST_ANGLE_MAX    = 70;
const int   HIST_BIN_COUNT    = 14;

// --- 判定阈值 ---
const int   ZEBRA_THRESHOLD   = 1500;

// --- 摄像头 ---
const int   CAM_WIDTH          = 320;
const int   CAM_HEIGHT         = 240;

// ============================================================
// IPM 逆透视变换矩阵
//
// Python 源码中的变换逻辑:
//   M = 原图 → BEV 的 3×3 透视变换矩阵
//   iM = M.inv() = BEV → 原图的逆矩阵
//
//   xy = 640×640 BEV 坐标网格 (px, py)
//   ixy = perspectiveTransform(xy, iM)  → 每个 BEV 像素在原图中的坐标
//   remap(frame, mapx, mapy)            → 采样原图得到 BEV 图像
//
// C++ 等价实现:
//   iM = M.inv()
//   warpPerspective(frame, bev, iM, Size(640,640), INTER_LINEAR)
//
//   warpPerspective 的矩阵含义是 dst→src 映射, 恰好就是 iM
//
// 【标定方法】
//   1. 在小车前方地面放置一个已知尺寸的矩形标定板 (如 A4 纸或棋盘格)
//   2. 拍摄一张包含整个矩形的前视图
//   3. 获取矩形的 4 个角点在原图中的像素坐标 pts_src
//   4. 在 BEV 中定义矩形应有的 4 个角点坐标 pts_dst
//   5. M = cv::getPerspectiveTransform(pts_src, pts_dst)
//   6. 将 M 填入下方矩阵中
//
//   当前矩阵是 Python 项目的原始标定值，需替换为实车标定值!
// ============================================================

// Python 原始矩阵 M (前视图 → BEV), 来源: gd.py:255-257
// 元素按 (row, col) = (0,0) (0,1) (0,2)
//                  (1,0) (1,1) (1,2)
//                  (2,0) (2,1) (2,2)
static const double g_M_data[3][3] = {
    {-1.86073726e-01, -5.02678929e-01,  4.72322899e+02},
    {-1.39150388e-02, -1.50260445e+00,  1.00507430e+03},
    {-1.77785988e-05, -1.65517173e-03,  1.00000000e+00}
};

/**
 * 计算逆矩阵 (BEV → 前视图), 用于 warpPerspective
 * 在启动时调用一次, 结果全局缓存
 */
static cv::Mat build_ipm_inverse_matrix()
{
    cv::Mat M(3, 3, CV_64F);
    for (int r = 0; r < 3; ++r)
        for (int c = 0; c < 3; ++c)
            M.at<double>(r, c) = g_M_data[r][c];

    // iM = M.inv(): BEV 坐标 → 原始图像坐标
    // warpPerspective 接受 dst→src 映射, 即 iM
    cv::Mat iM = M.inv();
    return iM;
}

// 全局变量: 在 main() 中计算一次, 之后每帧复用
static cv::Mat g_iM;

// ============================================================
// 全局状态 — 优雅退出机制
// ============================================================

static std::atomic<bool> g_running{true};

static void signal_handler(int)
{
    g_running.store(false);
}

// ============================================================
// apply_ipm — 逆透视变换: 前视图 → 鸟瞰图 (BEV)
//
// 输入:  原始摄像头帧 (320×240 或任意分辨率)
//        预计算的逆矩阵 g_iM (BEV→原图)
// 输出:  640×640 鸟瞰图 (消除透视畸变, 斑马线恢复矩形)
//
// 等价于 Python 版:
//   remap(frame, mapx, mapy, INTER_LINEAR)  // 预计算映射表
//
// 为什么选 640×640 输出?
//   1. BEV 鸟瞰需要比原图更大的空间来容纳"拉平"后的路面
//   2. 640 是 2 的幂次方的近似值, 对 resize 和后续操作友好
//   3. Python 版使用此尺寸, 算法参数基于此调优
//
// 如果不开启 IPM, 此函数会被跳过, 直接用前视图进入处理管线
// ============================================================
static cv::Mat apply_ipm(const cv::Mat &frame)
{
    cv::Mat bev;
    cv::warpPerspective(frame, bev, g_iM,
                        cv::Size(IPM_SIZE, IPM_SIZE),
                        cv::INTER_LINEAR);
    // warpPerspective 使用 INTER_LINEAR 双线性插值,
    // 比 INTER_NEAREST 最近邻更平滑, 避免产生锯齿状伪边缘
    return bev;
}

// ============================================================
// preprocess — 图像预处理 (蓝色通道 + 滤波 + 形态学)
//
// 输入: BGR 彩色图 (400×400)
// 输出: 预处理后的单通道图 (实际是蓝色通道处理结果)
//
// 五步处理:
//   [1] extractChannel(bgr, blue, 0)
//       取出 B 通道 — 关键设计!
//       黄色减速带在 B 通道中亮度极低 (黄色≈R+G, 无B)
//       白色斑马线在 B 通道中亮度正常 (白色≈R=G=B)
//       → B 通道天然过滤减速带, 保留斑马线
//
//   [2] medianBlur(blue, 5)
//       中值滤波核大小=5
//       替代像素值为邻域中值 → 椒盐噪声/砖缝等独立噪点直接抹除
//       与均值滤波不同: 中值保持边缘处的实际梯度值不衰减
//
//   [3] morphologyEx(MORPH_OPEN, 3×3, iter=4)
//       开运算 = Erode ×4 → Dilate ×4
//       3×3 矩形核较小, 但 4 次迭代累积效果强:
//         - 窄车道线 (宽度 < 8px) → 在连续腐蚀中被完全消除
//         - 宽斑马线 (宽度 >> 8px) → 腐蚀后仍有残留 → 膨胀恢复
//       本质: 保留"粗线条", 删除"细线条"
//
//   [4] morphologyEx(MORPH_CLOSE, 5×5, iter=3)
//       闭运算 = Dilate ×3 → Erode ×3
//       5×5 较大核, 3 次迭代:
//         - 先膨胀填平斑马线内部的小缺口/裂缝 (缺损斑马线恢复)
//         - 后腐蚀恢复原始尺寸, 同时消除路面箭头碎片
//       注意: 如果闭运算过强, 会把相邻的细斑马线粘成一片 → iter=3 是经验平衡点
// ============================================================
static cv::Mat preprocess(const cv::Mat &bgr)
{
    cv::Mat blue;

    // [1] 提取 B 通道 (蓝底色 + 减弱黄减速带)
    cv::extractChannel(bgr, blue, 0);

    // [2] 中值滤波 5×5 → 去砖缝纹理/椒盐噪声/虚假小梯度点
    cv::medianBlur(blue, blue, 5);

    // [3] 开运算: 3×3 矩形核, 4 次迭代 → 去除细窄车道线
    cv::Mat kernel_open  = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3));
    cv::morphologyEx(blue, blue, cv::MORPH_OPEN, kernel_open, cv::Point(-1, -1), 4);

    // [4] 闭运算: 5×5 矩形核, 3 次迭代 → 填补缺损 + 抑制箭头碎片
    cv::Mat kernel_close = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5));
    cv::morphologyEx(blue, blue, cv::MORPH_CLOSE, kernel_close, cv::Point(-1, -1), 3);

    return blue;
}

// ============================================================
// compute_gradient — Sobel 边缘梯度模值和方向
//
// 输入:  Canny 边缘检测结果 (单通道, 像素值 0 或 255)
// 输出:  Amplitude — 梯度模值 (浮点, 弱边缘 < 30 置零)
//        theta     — 梯度方向 0~90° (浮点)
//
// 方向定义:
//   对每个像素 (r, c) 计算 Sobel 梯度:
//        gx = ∂I/∂c   → 水平方向的变化率
//        gy = ∂I/∂r   → 垂直方向的变化率
//
//   Amplitude = sqrt(gx² + gy²)       → 边缘强度
//   theta     = atan2(|gy|, |gx|)     → 边缘方向 (0~90°)
//
//   其中 atan2 取 abs 意味着不区分 "从左到右" 和 "从右到左" 的边缘,
//   也不区分 "白→黑" 和 "黑→白" 的方向, 统一归到 0~90°。
//   这对斑马线检测是合适的 — 我们只关心边缘的方向一致性, 不关心极性。
//
//   方向参考:
//     0°  = 纯水平边缘 (dy→0, dx 大)
//     45° = 135° 角斜线
//     90° = 纯垂直边缘 (dx→0, dy 大)
//     斑马线纵线 ≈ 70~90°, 斜线 ≈ 30~60°
//
// 弱边缘过滤:
//   Canny 之后仍有少量噪声梯度 (<30)
//   直接置零 → 后续直方图统计不参与
// ============================================================
static void compute_gradient(const cv::Mat &canny,
                             cv::Mat &amplitude, cv::Mat &theta)
{
    cv::Mat dx, dy;

    // Sobel 算子 3×3:
    //   dx = [-1 0 1; -2 0 2; -1 0 1]   (x方向导数)
    //   dy = [-1 -2 -1; 0 0 0; 1 2 1]   (y方向导数)
    // CV_32F = 32 位浮点, 保留梯度方向亚度数精度
    cv::Sobel(canny, dx, CV_32F, 1, 0, 3);
    cv::Sobel(canny, dy, CV_32F, 0, 1, 3);

    // 初始化输出矩阵为全零
    amplitude = cv::Mat::zeros(canny.size(), CV_32F);
    theta     = cv::Mat::zeros(canny.size(), CV_32F);

    // 逐像素计算模值和方向
    // 逐像素循环对于 400×400 = 160K 像素来说不是瓶颈,
    // 瓶颈在 medianBlur 和 morphologyEx 的核运算上
    for (int r = 0; r < canny.rows; ++r)
    {
        for (int c = 0; c < canny.cols; ++c)
        {
            float gx = dx.at<float>(r, c);
            float gy = dy.at<float>(r, c);

            float amp = std::sqrt(gx * gx + gy * gy);
            float ang = std::atan2(std::abs(gy), std::abs(gx) + 1e-10f)
                        * 180.0f / static_cast<float>(CV_PI);

            // 弱边缘置零 (不参与后续方向统计)
            amplitude.at<float>(r, c) = (amp >= GRAD_THRESHOLD) ? amp : 0.0f;
            theta.at<float>(r, c)     = ang;
        }
    }
}

// ============================================================
// is_zebra_window — 对单个滑动窗口做斑马线分类
//
// 输入:  窗口内的梯度模值图 (amp_win) 和方向图 (ang_win)
// 输出:  true = 斑马线, false = 背景
//
// 判定逻辑:
//   [1] 扫描窗口内每个像素
//       - 跳过 amp ≤ 0 的像素 (弱边缘/噪声/平滑区域)
//       - 跳过 ang ≥ 70° 的像素 (方向在统计范围外)
//
//   [2] 统计方向直方图 hist[0..13]
//       hist[k] = 方向在 [k*5, k*5+5) 的有效梯度点数量
//       e.g. hist[0] = 方向 0~4.999° 的点数
//
//   [3] 取峰值 bin 的点数 = peak
//       peak 反映了窗口内 "某个特定方向上" 有多少条平行的边缘线
//
//   [4] peak > ZEBRA_THRESHOLD (1500) → 斑马线
//       斑马线 = 同一方向上大量平行边缘 → peak 极高
//       普通路面 = 边缘方向杂乱 → 峰值被分散到不同 bin → 各 bin 点数低
//
// 在 IPM 开启时, 鸟瞰图将斑马线恢复为垂直矩形
// → 所有边缘方向集中在 90° 附近 → hist[13] 极高 → 检测更灵敏
// ============================================================
static bool is_zebra_window(const cv::Mat &amp_win, const cv::Mat &ang_win)
{
    int hist[HIST_BIN_COUNT] = {0};

    for (int r = 0; r < amp_win.rows; ++r)
    {
        for (int c = 0; c < amp_win.cols; ++c)
        {
            float amp = amp_win.at<float>(r, c);
            if (amp <= 0.0f)
                continue;

            float ang = ang_win.at<float>(r, c);
            if (ang < 0.0f || ang >= HIST_ANGLE_MAX)
                continue;

            int bin = static_cast<int>(ang) / HIST_BIN_SIZE;
            if (bin >= 0 && bin < HIST_BIN_COUNT)
                hist[bin]++;
        }
    }

    int peak = 0;
    for (int i = 0; i < HIST_BIN_COUNT; ++i)
        if (hist[i] > peak)
            peak = hist[i];

    return peak > ZEBRA_THRESHOLD;
}

// ============================================================
// 帧缓冲显示 — 将 OpenCV BGR Mat 转为 RGB565 格式直写 /dev/fb0
//
// 硬件: SPI LCD 通过 /dev/fb0 (RGB565 格式)
//
// RGB565 编码:
//   16 位: [R4 R3 R2 R1 R0] [G5 G4 G3] [G2 G1 G0] [B4 B3 B2 B1 B0]
//   R 取 高 5 位 (>>3, &0xF8)
//   G 取 高 6 位 (>>2, &0xFC)
//   B 取 高 5 位 (>>3)
// ============================================================

static uint16_t convertRGBToRGB565(uint8_t r, uint8_t g, uint8_t b)
{
    return ((r & 0xF8) << 8)  |
           ((g & 0xFC) << 3)  |
           (b >> 3);
}

static void display_on_fb(const cv::Mat &bgr, int /* fb_fd */,
                          uint16_t *fb_buf, int scr_w, int scr_h)
{
    cv::Mat display;
    cv::resize(bgr, display, cv::Size(scr_w, scr_h));

    for (int y = 0; y < scr_h; ++y)
    {
        for (int x = 0; x < scr_w; ++x)
        {
            cv::Vec3b pixel = display.at<cv::Vec3b>(y, x);
            fb_buf[y * scr_w + x] = convertRGBToRGB565(
                pixel[2], pixel[1], pixel[0]);
        }
    }
}

// ============================================================
// main — 初始化 + 主检测循环 + 清理
// ============================================================
int main(int argc, char *argv[])
{
    // ---- 命令行参数解析 ----
    bool no_display = false;
    bool use_ipm    = (USE_IPM != 0);   // 默认跟随编译宏

    for (int i = 1; i < argc; ++i)
    {
        if (std::strcmp(argv[i], "--no-display") == 0)
            no_display = true;
        else if (std::strcmp(argv[i], "--no-ipm") == 0)
            use_ipm = false;
    }

    // ---- 注册信号处理 ----
    std::signal(SIGINT,  signal_handler);
    std::signal(SIGTERM, signal_handler);

    // ---- 预计算 IPM 逆矩阵 (启用时) ----
    if (use_ipm)
    {
        g_iM = build_ipm_inverse_matrix();
        printf("IPM 逆透视变换: 已启用 (%dx%d BEV)\n", IPM_SIZE, IPM_SIZE);
        printf("  注意: 矩阵未针对实车标定, 效果可能不佳\n");
    }
    else
    {
        printf("IPM 逆透视变换: 已关闭 (使用前视图)\n");
    }

    // ========================================================
    // 第 1 步: 打开 USB 摄像头
    // ========================================================
    cv::VideoCapture cap;

    cap.open(0, cv::CAP_V4L2);
    if (!cap.isOpened())
    {
        cap.open(0);
        if (!cap.isOpened())
        {
            std::cerr << "无法打开摄像头 /dev/video0" << std::endl;
            return 1;
        }
    }

    cap.set(cv::CAP_PROP_FRAME_WIDTH,  CAM_WIDTH);
    cap.set(cv::CAP_PROP_FRAME_HEIGHT, CAM_HEIGHT);
    cap.set(cv::CAP_PROP_FOURCC,
            cv::VideoWriter::fourcc('M', 'J', 'P', 'G'));

    printf("摄像头已打开 %dx%d\n",
           (int)cap.get(cv::CAP_PROP_FRAME_WIDTH),
           (int)cap.get(cv::CAP_PROP_FRAME_HEIGHT));

    // ========================================================
    // 第 2 步: 打开帧缓冲 /dev/fb0 → mmap 映射到用户空间
    // ========================================================
    int fb_fd = -1;
    uint16_t *fb_buf = nullptr;
    int scr_w = 0, scr_h = 0;

    if (!no_display)
    {
        fb_fd = open("/dev/fb0", O_RDWR);
        if (fb_fd < 0)
        {
            std::cerr << "警告: 无法打开 /dev/fb0，切换为无头模式" << std::endl;
            no_display = true;
        }
        else
        {
            struct fb_var_screeninfo vinfo;
            if (ioctl(fb_fd, FBIOGET_VSCREENINFO, &vinfo) < 0)
            {
                std::cerr << "无法获取屏幕信息" << std::endl;
                close(fb_fd);
                no_display = true;
            }
            else
            {
                scr_w = vinfo.xres;
                scr_h = vinfo.yres;
                size_t fb_size = vinfo.yres_virtual * vinfo.xres_virtual
                                 * vinfo.bits_per_pixel / 8;

                fb_buf = (uint16_t *)mmap(nullptr, fb_size,
                                          PROT_READ | PROT_WRITE,
                                          MAP_SHARED, fb_fd, 0);
                if (fb_buf == MAP_FAILED)
                {
                    std::cerr << "无法映射帧缓冲区" << std::endl;
                    close(fb_fd);
                    no_display = true;
                }
                else
                {
                    printf("屏幕分辨率: %dx%d\n", scr_w, scr_h);
                }
            }
        }
    }

    // ========================================================
    // 第 3 步: 主循环
    // ========================================================
    cv::Mat frame, bev, proc, gray, canny, amplitude, theta, result;
    int  frame_count   = 0;
    int  zebra_frames  = 0;
    bool was_zebra     = false;

    printf("\n开始斑马线检测 (按 Ctrl+C 退出)...\n");

    while (g_running.load())
    {
        // ---- 3a. 摄像头采集一帧 ----
        if (!cap.read(frame) || frame.empty())
        {
            usleep(5000);
            continue;
        }

        // ---- 3b. [IPM] 逆透视变换: 前视图 → 鸟瞰图 ----
        // 将倾斜透视拉平为俯瞰视角, 让斑马线恢复矩形平行特征
        // 未启用 IPM 时直接使用原始帧
        if (use_ipm)
        {
            bev = apply_ipm(frame);
        }
        else
        {
            bev = frame;  // 使用前视图
        }

        // ---- 3c. 尺寸归一化: BEV/原图 → 400×400 ----
        cv::resize(bev, proc, cv::Size(PROC_SIZE, PROC_SIZE));

        // ---- 3d. 预处理: 蓝色通道 + 中值滤波 + 形态学 ----
        gray = preprocess(proc);

        // ---- 3e. Canny 边缘检测 (双阈值 30/90) ----
        cv::Canny(gray, canny, 30, 90, 3);

        // ---- 3f. 梯度模值与方向 ----
        compute_gradient(canny, amplitude, theta);

        // ---- 3g. 滑动窗口逐窗检测 ----
        int img_h     = proc.rows;
        bool detected = false;
        int  z_ymin   = img_h;
        int  z_ymax   = 0;

        for (int top = 0; top <= img_h - WIN_H; top += WIN_STEP)
        {
            cv::Rect roi(WIN_COL_L, top, WIN_W, WIN_H);

            if (is_zebra_window(amplitude(roi), theta(roi)))
            {
                detected = true;
                if (top < z_ymin)          z_ymin = top;
                if (top + WIN_H > z_ymax)  z_ymax = top + WIN_H;
            }
        }

        // ---- 3h. 写入状态文件 (仅在状态变化时写) ----
        if (detected != was_zebra)
        {
            std::ofstream ofs("./zebra_detected", std::ios::trunc);
            ofs << (detected ? "1" : "0");
            was_zebra = detected;
        }

        // ---- 3i. 绘制调试叠加图 ----
        result = proc.clone();

        for (int top = 0; top <= img_h - WIN_H; top += WIN_STEP)
        {
            cv::line(result, cv::Point(WIN_COL_L, top),
                     cv::Point(WIN_COL_R, top),
                     cv::Scalar(128, 128, 128), 1);
        }

        if (detected)
        {
            cv::rectangle(result,
                          cv::Rect(WIN_COL_L, z_ymin, WIN_W, z_ymax - z_ymin),
                          cv::Scalar(255, 0, 255), 3);
        }

        if (detected)
        {
            cv::putText(result, "ZEBRA CROSSING!",
                        cv::Point(10, 30),
                        cv::FONT_HERSHEY_SIMPLEX, 0.8,
                        cv::Scalar(0, 0, 255), 2);
        }

        // ---- 3j. LCD 显示 ----
        if (!no_display && fb_buf)
            display_on_fb(result, fb_fd, fb_buf, scr_w, scr_h);

        // ---- 3k. 统计 ----
        frame_count++;
        if (detected) zebra_frames++;

        if (frame_count % 30 == 0)
        {
            printf("帧 %d | 斑马线: %s | 累计命中: %d/%d | IPM: %s\n",
                   frame_count,
                   detected ? "YES" : "NO ",
                   zebra_frames, frame_count,
                   use_ipm ? "ON" : "OFF");
        }
    }

    // ========================================================
    // 第 4 步: 清理
    // ========================================================
    cap.release();

    if (!no_display && fb_buf && fb_buf != MAP_FAILED)
    {
        struct fb_var_screeninfo vinfo;
        if (ioctl(fb_fd, FBIOGET_VSCREENINFO, &vinfo) == 0)
        {
            size_t fb_size = vinfo.yres_virtual * vinfo.xres_virtual
                           * vinfo.bits_per_pixel / 8;
            munmap(fb_buf, fb_size);
        }
        close(fb_fd);
    }

    printf("\n退出。共处理 %d 帧，命中 %d 帧 (%.1f%%), IPM: %s\n",
           frame_count, zebra_frames,
           frame_count > 0 ? (100.0 * zebra_frames / frame_count) : 0.0,
           use_ipm ? "ON" : "OFF");

    return 0;
}