/*
 * @Author: ilikara 3435193369@qq.com
 * @Date: 2025-01-04 06:50:56
 * @LastEditors: ilikara 3435193369@qq.com
 * @LastEditTime: 2025-03-13 08:15:10
 * @FilePath: /2k300_smartcar/src/image_cv.cpp
 * @Description: 视觉巡线管线 —— HSV 二值化 → 洪泛填充 → 逐行边界搜索 → EMA 滤波
 *
 * 处理分辨率: 80×60（line_tracking_width × line_tracking_height）
 * 这是 2K0300 CPU 上的最优平衡点：再大 Otsu 不稳，再小细节丢失
 */
#include "image_cv.h"

// ============================================================
// 全局图像变量
// ============================================================

cv::Mat raw_frame;
cv::Mat binarizedFrame;
cv::Mat morphologyExFrame;
cv::Mat track;

// ============================================================
// 边界线数组 — 左/右边缘和中线的逐行列坐标
//
// 坐标系: row=0 = 图像顶部(远处), row=59 = 底部(近处)
// 列坐标范围: 0 ~ line_tracking_width-1 (0~79)
// 未检测到边界 = -1
// ============================================================
std::vector<int> left_line;
std::vector<int> right_line;
std::vector<int> mid_line;
std::vector<int> mid_line_raw;

int g_lost_rows = 0;

int line_tracking_width;                 // 处理宽度 = 80
int line_tracking_height;                // 处理高度 = 60

// ============================================================
// image_binerize — HSV 双通道 Otsu 二值化
//
// 输入: BGR 彩色帧 (80×60)
// 输出: 二值图，赛道区域 = 白色(255)，边界/背景 = 黑色(0)
//
// 原理:
//   蓝底赛道 → 蓝色区域饱和度高(S通道高)，色调集中(H通道100~130)
//   灰度路面 → 饱和度低，色调分散
//
//   1. BGR → HSV 色彩空间转换
//   2. H 通道 Otsu 阈值 → 分离蓝色与非蓝色
//   3. S 通道 Otsu 阈值 → 分离高饱和(蓝)与低饱和(灰)
//   4. 两通道做 bitwise_or 取并集 (THRESH_BINARY_INV 确保赛道=255)
//      即只要 H 或 S 任一判定为赛道，就标记为赛道区域
//
//   THRESH_BINARY_INV + THRESH_OTSU:
//     Otsu 自动计算最优阈值 T
//     pixel > T → 0 (黑色), pixel ≤ T → 255 (白色)
//     赛道(蓝色/高饱和)通常偏向一侧，Otsu 将其归到低值区间
//     INV 反转后赛道 = 白色(前景, 255), 背景 = 黑色(0)
// ============================================================
cv::Mat image_binerize(cv::Mat &frame)
{
    static cv::Mat output;
    static cv::Mat binarizedFrame;
    static cv::Mat hsvImage;
    static std::vector<cv::Mat> hsvChannels(3);

    cv::cvtColor(frame, hsvImage, cv::COLOR_BGR2HSV);
    cv::split(hsvImage, hsvChannels);

    cv::threshold(hsvChannels[0], binarizedFrame,
                  0, 255, cv::THRESH_BINARY_INV | cv::THRESH_OTSU);
    cv::threshold(hsvChannels[1], output,
                  0, 255, cv::THRESH_BINARY_INV | cv::THRESH_OTSU);

    cv::bitwise_or(output, binarizedFrame, output);

    return output;
}

// ============================================================
// find_road — 形态学去噪 + 洪泛填充提取连通赛道区域
//
// 输入: image_binerize 的输出 (赛道=255, 背景=0)
// 输出: 仅保留与底边中点连通的赛道区域蒙版
//
// 处理步骤:
//   1. 形态学开运算 (MORPH_OPEN): 先腐蚀后膨胀，消除孤立噪点
//      核大小 2×2 十字形 — 极小核，避免吞没细弯
//   2. 在底边中心上方偏移处放置种子点
//   3. 洪泛填充 (floodFill):
//      - 从种子点出发，填充容差范围内的连通区域
//      - 填充值 = 128 (灰色)，区分于原始白色(255)
//      - loDiff/upDiff = 20: 像素值在 [108, 148] 范围内的像素被填充
//      - 邻域 8 连通
//   4. 将蒙版 mask 的 ROI 区域复制到输出图像
//      - 赛道内部 = 128 (非零)，外部 = 0
//
// 为什么要洪泛填充？
//   二值化后可能有多块白色区域（赛道 + 赛道外的反光/杂物）
//   洪泛填充确保只处理"从车底到前方连通的"那块区域
//   排除远处/边缘的伪赛道碎片
// ============================================================
cv::Mat find_road(cv::Mat &frame)
{
    static cv::Mat kernel = cv::getStructuringElement(cv::MORPH_CROSS, cv::Size(2, 2));
    cv::morphologyEx(binarizedFrame, morphologyExFrame, cv::MORPH_OPEN, kernel);

    static cv::Mat mask;
    if (mask.empty()) mask.create(line_tracking_height + 2, line_tracking_width + 2, CV_8UC1);
    mask.setTo(0);

    // 3. 种子点位置
    //    X = 图像水平中心 (line_tracking_width/2)
    //    Y = 距底部 10 行 (line_tracking_height-10)
    //    假设车在赛道中央附近，底部中心一定是赛道上
    cv::Point seedPoint(line_tracking_width / 2, line_tracking_height - 10);

    // 4. 在种子点位置画一个白色实心圆，确保种子点落在赛道区域内
    //    避免因二值化偶尔在种子位置为黑色导致洪泛失败
    cv::circle(morphologyExFrame, seedPoint, 5, 255, -1);

    // 5. 洪泛填充参数
    cv::Scalar newVal(128);               // 填充值 = 128 (标记为赛道内部)
    cv::Scalar loDiff = cv::Scalar(20);   // 下界: 当前像素值 - 20 = 108
    cv::Scalar upDiff = cv::Scalar(20);   // 上界: 当前像素值 + 20 = 148

    // 6. 执行洪泛填充
    //    从种子点向 8 邻域扩散，填充像素值在 [108, 148] 之间的所有连通像素
    //    填充结果直接写入 morphologyExFrame（in-place）
    //    蒙版 mask 记录被填充的像素位置
    cv::floodFill(morphologyExFrame, mask, seedPoint, newVal,
                  nullptr, loDiff, upDiff, 8);

    static cv::Mat outputImage;
    if (outputImage.empty()) outputImage.create(line_tracking_height, line_tracking_width, CV_8UC1);
    outputImage.setTo(0);
    mask(cv::Rect(1, 1, line_tracking_width, line_tracking_height)).copyTo(outputImage);

    return outputImage;
}

// ============================================================
// image_main — 视觉巡线主函数，每帧调用一次
//
// 完整处理流水线：
//   raw_frame (320×240) → resize (80×60) → HSV二值化 → 形态学+洪泛
//   → 逐行最长连续段搜索 → 中线计算 → EMA 自底向上滤波 →
//   输出 left_line[60], right_line[60], mid_line[60]
//
// 注意：坐标原点在左上角
//   - row=0  = 图像顶部（远处）
//   - row=59 = 图像底部（近处，车前方）
//   - 滤波从底部(row=59)往顶部(row=0)递推
// ============================================================
void image_main()
{
    static cv::Mat resizedFrame;

    cv::resize(raw_frame, resizedFrame,
               cv::Size(line_tracking_width, line_tracking_height));

    // ── 2. HSV 双通道 Otsu 二值化 ─────────────────────
    // 赛道区域 = 白色(255)，背景/边界 = 黑色(0)
    binarizedFrame = image_binerize(resizedFrame);

    // ── 3. 洪泛填充提取赛道主体 ────────────────────────
    // 只保留与车底中点连通的赛道区域，排除伪赛道碎片
    // 输出: track = 赛道内部(128) / 赛道外部(0)
    track = find_road(binarizedFrame);

    // ── 4. 初始化边界线数组 ────────────────────────────
    left_line.clear();
    right_line.clear();
    mid_line.clear();

    left_line.resize(line_tracking_height, -1);
    right_line.resize(line_tracking_height, -1);
    mid_line.resize(line_tracking_height, -1);

    // ── 5. 逐行最长连续段搜索 ──────────────────────────
    //
    // 将 track 的 uchar* 数据解释为二维数组 IMG[row][col]
    // track 中 赛道内部 = 128(非零), 外部 = 0
    // 对每一行: 找到最长的连续非零段
    //   → 段的起点 = left_line[row]
    //   → 段的终点 = right_line[row]
    //   → 无赛道段的行 = -1
    //
    // 这在低分辨率下比八邻域边界跟踪更高效：
    //   80×60 只有 4800 像素，逐行扫描 O(W×H) 已足够快
    uchar(*IMG)[line_tracking_width] =
        reinterpret_cast<uchar(*)[line_tracking_width]>(track.data);

    for (int i = 0; i < line_tracking_height; ++i)  // i = row
    {
        int max_start = -1;       // 最长连续段的起始列
        int max_end = -1;         // 最长连续段的结束列
        int current_start = -1;   // 当前扫描中的段起始列
        int current_length = 0;   // 当前扫描中的段长度
        int max_length = 0;       // 已记录的最长段长度

        for (int j = 0; j < line_tracking_width; ++j)  // j = col
        {
            if (IMG[i][j])  // 非零 = 赛道内部像素
            {
                // 赛道段的第一个像素: 记录起点
                if (current_length == 0)
                {
                    current_start = j;
                    current_length = 1;
                }
                else
                {
                    // 赛道段延续: 长度+1
                    current_length++;
                }

                // 更新最长记录 (≥ 而非 >, 取最后一个最长段，靠右)
                if (current_length >= max_length)
                {
                    max_length = current_length;
                    max_start = current_start;
                    max_end = j;
                }
            }
            else  // 零值 = 非赛道
            {
                // 赛道段结束: 重置
                current_length = 0;
                current_start = -1;
            }
        }

        // 记录该行的左右边界
        if (max_length > 0)
        {
            left_line[i]  = max_start;   // 最长白段的左端点
            right_line[i] = max_end;     // 最长白段的右端点
        }
        else
        {
            // 该行无赛道 → 后续由中线补全逻辑处理
            left_line[i]  = -1;
            right_line[i] = -1;
        }
    }

    // ── 6. 中线计算 + 丢线补全 ─────
    // 从倒数第二行开始向上，底行(row=height-1)单独兜底避免 mid_line[row+1] 越界
    g_lost_rows = 0;
    for (int row = line_tracking_height - 2; row >= 10; --row)
    {
        // ── 6a. 丢线补全 ──────────────────────────────
        if (left_line[row] == -1 && right_line[row] == -1)
        {
            g_lost_rows++;
            // 当前行完全丢线: 用下行(row+1)的中线来虚拟补线
            mid_line[row] = mid_line[row + 1];

            if (mid_line[row] > line_tracking_width / 2)
            {
                // 中线偏右 → 实际赛道在图像右半区
                // 虚拟右边界 = 图像右边缘, 左边界 = 下行中线位置
                right_line[row] = line_tracking_width - 1;
                left_line[row]  = mid_line[row + 1];
            }
            else
            {
                // 中线偏左 → 实际赛道在图像左半区
                // 虚拟左边界 = 图像左边缘, 右边界 = 下行中线位置
                left_line[row]  = 0;
                right_line[row] = mid_line[row + 1];
            }
        }
        else
        {
            // 正常行: 中线 = 左右边界中点
            mid_line[row] = (left_line[row] + right_line[row]) / 2;
        }
    }
    // 底行兜底：正常计算，丢线时用图像中心
    {
        const int row = line_tracking_height - 1;
        if (left_line[row] == -1 && right_line[row] == -1)
        {
            g_lost_rows++;
            mid_line[row] = line_tracking_width / 2;
            left_line[row]  = 0;
            right_line[row] = line_tracking_width - 1;
        }
        else
        {
            mid_line[row] = (left_line[row] + right_line[row]) / 2;
        }
    }
}