#include "deviation.hpp"
#include "vision/preprocess.hpp"
#include <algorithm>
#include <cmath>
#include <cstring>

// ============================================================
// IPM 坐标矫正 — 相机透视 → 鸟瞰
// 搜线在原始相机视角完成 (准确), 这里只对坐标做透视变换
// ============================================================
void ipm_correct_lines(EdgeLines& lines)
{
    if (g_ipm_matrix.empty() || g_ipm_matrix.type() != CV_64F) return;

    cv::Mat H = g_ipm_matrix.inv();  // 相机→鸟瞰 (原矩阵 map 是鸟瞰→相机)
    double* m = (double*)H.ptr<double>();

    uint8 new_left[IPM_ROW_COUNT] = {0};
    uint8 new_right[IPM_ROW_COUNT] = {0};

    for (int y = 0; y < IPM_ROW_COUNT; ++y)
    {
        if (lines.left[y] > 0)
        {
            double sx = lines.left[y] * 0.5;  // 160→80 缩放
            double sy = y * 0.5;
            double w  = m[6]*sx + m[7]*sy + m[8];
            if (std::abs(w) > 1e-9) {
                double bx = (m[0]*sx + m[1]*sy + m[2]) / w;
                double by = (m[3]*sx + m[4]*sy + m[5]) / w;
                int nx = (int)(bx * 2.0 + 0.5);  // 80→160 缩放
                int ny = (int)(by * 2.0 + 0.5);
                if (nx >= 0 && nx < 256 && ny >= 0 && ny < IPM_ROW_COUNT)
                    new_left[ny] = (uint8)nx;
            }
        }
        if (lines.right[y] > 0)
        {
            double sx = lines.right[y] * 0.5;
            double sy = y * 0.5;
            double w  = m[6]*sx + m[7]*sy + m[8];
            if (std::abs(w) > 1e-9) {
                double bx = (m[0]*sx + m[1]*sy + m[2]) / w;
                double by = (m[3]*sx + m[4]*sy + m[5]) / w;
                int nx = (int)(bx * 2.0 + 0.5);
                int ny = (int)(by * 2.0 + 0.5);
                if (nx >= 0 && nx < 256 && ny >= 0 && ny < IPM_ROW_COUNT)
                    new_right[ny] = (uint8)nx;
            }
        }
    }

    // 插值填充缺失行
    for (int y = 1; y < IPM_ROW_COUNT - 1; ++y)
    {
        if (new_left[y] == 0 && new_left[y-1] > 0 && new_left[y+1] > 0)
            new_left[y] = (new_left[y-1] + new_left[y+1]) / 2;
        if (new_right[y] == 0 && new_right[y-1] > 0 && new_right[y+1] > 0)
            new_right[y] = (new_right[y-1] + new_right[y+1]) / 2;
    }

    for (int y = 0; y < IPM_ROW_COUNT; ++y)
    {
        lines.left[y]  = new_left[y];
        lines.right[y] = new_right[y];
        if (new_left[y] > 0 && new_right[y] > 0)
            lines.mid[y] = (new_left[y] + new_right[y]) / 2;
    }
}

void fit_midline(EdgeLines& lines)
{
    static EdgeLines prev;
    static bool has_prev = false;

    if (!has_prev)
    {
        std::memcpy(&prev, &lines, sizeof(EdgeLines));
        has_prev = true;
        return;
    }

    for (int y = 0; y < IPM_ROW_COUNT; ++y)
    {
        if (lines.left[y] == 0 || lines.right[y] == 0) continue;

        if (std::abs(static_cast<int>(lines.mid[y]) - static_cast<int>(prev.mid[y])) > 20)
            lines.mid[y] = prev.mid[y];
    }

    for (int y = 0; y < IPM_ROW_COUNT; ++y)
    {
        if (y >= 2 && y < IPM_ROW_COUNT - 2 && lines.mid[y] > 0)
        {
            int sum = 0, cnt = 0;
            for (int dy = -2; dy <= 2; ++dy)
                if (lines.mid[y + dy] > 0) { sum += lines.mid[y + dy]; ++cnt; }
            if (cnt > 0) lines.mid[y] = sum / cnt;
        }
    }

    std::memcpy(&prev, &lines, sizeof(EdgeLines));
}

float calc_deviation(const EdgeLines& lines)
{
    float dev_near = 0, dev_mid = 0, dev_far = 0;
    float cnt_near = 0, cnt_mid = 0, cnt_far = 0;
    int mid_ref = IMAGE_WIDTH / 2;

    int total = IPM_ROW_COUNT;
    int zone_far  = total / 5;
    int zone_mid  = total * 3 / 5;

    for (int y = 0; y < total; ++y)
    {
        if (lines.mid[y] == 0) continue;
        float d = static_cast<float>(lines.mid[y]) - mid_ref;

        if (y < zone_far)
            { dev_far  += d; cnt_far  += 1; }
        else if (y < zone_mid)
            { dev_mid  += d; cnt_mid  += 1; }
        else
            { dev_near += d; cnt_near += 1; }
    }

    if (cnt_far  < 3) cnt_far  = 3;
    if (cnt_mid  < 3) cnt_mid  = 3;
    if (cnt_near < 3) cnt_near = 3;

    float dev = (dev_far  / cnt_far)  * 0.50f +
                (dev_mid  / cnt_mid)  * 0.30f +
                (dev_near / cnt_near) * 0.20f;

    return std::clamp(dev / mid_ref, -1.0f, 1.0f);
}