#include "preprocess.hpp"
#include <algorithm>
#include <cmath>

uint8 g_ipm_image[IMAGE_HEIGHT][IMAGE_WIDTH]   = {{0}};
uint8 g_valid_l_bound[IMAGE_HEIGHT]              = {0};
uint8 g_valid_r_bound[IMAGE_HEIGHT]              = {0};

static constexpr int PROC_W = IMAGE_WIDTH  / 2;   // 80 — 与demo对齐
static constexpr int PROC_H = IMAGE_HEIGHT / 2;   // 60

// ============================================================
// IPM 4 点标定 — 80×60 相机视角 ↔ 80×60 俯视输出
//
// 地面实况: 矩形赛道 40cm 宽 × 50cm 长, 近边距摄像头 55cm
// 摄像头垂直投影 ≈ 车头后 5cm, 车头距矩形 ≈ 50cm
// ============================================================
static constexpr float CAL_NEAR_Y_MM  = 550.0f;   // 矩形近边 Y (mm)
static constexpr float CAL_FAR_Y_MM   = 1050.0f;  // 矩形远边 Y (mm)
static constexpr float CAL_HALF_W_MM  = 200.0f;   // 矩形半宽 (mm)

static bool  g_ipm_enabled   = true;        // 启用 IPM
static cv::Mat g_ipm_matrix;                // 3×3 透视变换矩阵

void preprocess_init() {}

static void init_ipm_calibration()
{
    // 四点: (相机 80×60 像素) → 标注的矩形角
    std::vector<cv::Point2f> src = {
        {19.f, 17.f},  // A 近左
        {57.f, 15.f},  // B 近右
        {27.f,  8.f},  // C 远左
        {49.f,  7.f}   // D 远右
    };

    // 目标: 俯视图中也保持矩形 (同宽), 近边 row=55, 远边 row=20
    // mm/pixel ≈ 14.3, 半宽 200mm → 14px
    const float cx  = PROC_W / 2.0f;   // 40
    const float hw  = 14.0f;           // 半宽像素
    std::vector<cv::Point2f> dst = {
        {cx - hw, 55.f},  // A' 近左
        {cx + hw, 55.f},  // B' 近右
        {cx - hw, 20.f},  // C' 远左
        {cx + hw, 20.f}   // D' 远右
    };

    // getPerspectiveTransform(dst, src): 输出(俯视) → 输入(相机)
    g_ipm_matrix = cv::getPerspectiveTransform(dst, src);

    printf("[IPM] 4点标定完成\n");
    printf("  近边 Y=%.0f mm  远边 Y=%.0f mm  半宽=%.0f mm\n",
           CAL_NEAR_Y_MM, CAL_FAR_Y_MM, CAL_HALF_W_MM);
}

static void update_bounds(const cv::Mat& binary);

static cv::Mat track_binarize(const cv::Mat& bgr)
{
    cv::Mat hsv, thresh_h, thresh_s, result;
    cv::cvtColor(bgr, hsv, cv::COLOR_BGR2HSV);

    std::vector<cv::Mat> ch;
    cv::split(hsv, ch);

    cv::threshold(ch[0], thresh_h, 0, 255, cv::THRESH_BINARY_INV | cv::THRESH_OTSU);
    cv::threshold(ch[1], thresh_s, 0, 255, cv::THRESH_BINARY_INV | cv::THRESH_OTSU);
    cv::bitwise_or(thresh_h, thresh_s, result);

    return result;
}

static cv::Mat flood_fill_track(const cv::Mat& binary)
{
    cv::Mat kernel = cv::getStructuringElement(cv::MORPH_CROSS, cv::Size(2, 2));
    cv::Mat opened;
    cv::morphologyEx(binary, opened, cv::MORPH_OPEN, kernel);

    cv::Mat mask = cv::Mat::zeros(PROC_H + 2, PROC_W + 2, CV_8UC1);
    cv::Point seed(PROC_W / 2, PROC_H - 10);
    cv::circle(opened, seed, 10, cv::Scalar(255), -1);

    cv::floodFill(opened, mask, seed, cv::Scalar(128),
                  nullptr,
                  cv::Scalar(20), cv::Scalar(20), 8);

    cv::Mat result = cv::Mat::zeros(PROC_H, PROC_W, CV_8UC1);
    mask(cv::Rect(1, 1, PROC_W, PROC_H)).copyTo(result);
    return result;
}

void preprocess_run(const cv::Mat& bgr_frame)
{
    cv::Mat small;
    cv::resize(bgr_frame, small, cv::Size(PROC_W, PROC_H));

    cv::Mat bin = track_binarize(small);

    // IPM 透视校正 (首次调用时计算矩阵)
    if (g_ipm_enabled)
    {
        if (g_ipm_matrix.empty())
            init_ipm_calibration();

        cv::Mat ipm_out;
        cv::warpPerspective(bin, ipm_out, g_ipm_matrix,
                            cv::Size(PROC_W, PROC_H),
                            cv::INTER_LINEAR,
                            cv::BORDER_CONSTANT, cv::Scalar(0));
        bin = ipm_out;
    }

    cv::Mat track = flood_fill_track(bin);
    cv::Mat full;
    cv::resize(track, full, cv::Size(IMAGE_WIDTH, IMAGE_HEIGHT), 0, 0, cv::INTER_NEAREST);

    update_bounds(full);
    std::memcpy(g_ipm_image, full.data, IMAGE_WIDTH * IMAGE_HEIGHT);
}

static void update_bounds(const cv::Mat& binary)
{
    for (int y = 0; y < IMAGE_HEIGHT; ++y)
    {
        int l = 0, r = IMAGE_WIDTH - 1;
        while (l < IMAGE_WIDTH  && binary.at<uint8>(y, l) == 0) ++l;
        while (r >= 0            && binary.at<uint8>(y, r) == 0) --r;
        g_valid_l_bound[y] = static_cast<uint8>(std::max(0, l));
        g_valid_r_bound[y] = static_cast<uint8>(std::min(IMAGE_WIDTH - 1, r));
    }
}