#include "search.hpp"
#include "preprocess.hpp"
#include <algorithm>
#include <cstring>

void search_init(SearchResult& r)
{
    std::memset(&r, 0, sizeof(r));
}

void search_run(SearchResult& r)
{
    search_init(r);

    int left_raw[IPM_ROW_COUNT]  = {0};
    int right_raw[IPM_ROW_COUNT] = {0};
    int mid_raw[IPM_ROW_COUNT]   = {0};

    // ============================================================
    // 逐行最长白段搜索 (与 demo image_main() 完全一致)
    // ============================================================
    for (int y = 0; y < IMAGE_HEIGHT; ++y)
    {
        int best_start = -1, best_end = -1, best_len = 0;
        int cur_start  = -1, cur_len   = 0;

        for (int x = 0; x < IMAGE_WIDTH; ++x)
        {
            if (g_ipm_image[y][x])
            {
                if (cur_len == 0) { cur_start = x; cur_len = 1; }
                else cur_len++;
                if (cur_len >= best_len)
                {
                    best_len   = cur_len;
                    best_start = cur_start;
                    best_end   = x;
                }
            }
            else
            {
                cur_len   = 0;
                cur_start = -1;
            }
        }

        if (best_len > 0)
        {
            left_raw[y]  = best_start;
            right_raw[y] = best_end;
        }
    }

    // ============================================================
    // 丢线补点 + 中线计算 (从底向上, 复用 demo 逻辑)
    // ============================================================
    for (int y = IMAGE_HEIGHT - 2; y >= 0; --y)
    {
        if (left_raw[y] == 0 && right_raw[y] == 0)
        {
            mid_raw[y] = mid_raw[y + 1];
            if (mid_raw[y] > IMAGE_WIDTH / 2)
            {
                right_raw[y] = IMAGE_WIDTH - 1;
                left_raw[y]  = mid_raw[y + 1];
            }
            else
            {
                left_raw[y]  = 0;
                right_raw[y] = mid_raw[y + 1];
            }
        }
        else
        {
            mid_raw[y] = (left_raw[y] + right_raw[y]) / 2;
        }
    }
    if (left_raw[IMAGE_HEIGHT - 1] > 0 || right_raw[IMAGE_HEIGHT - 1] > 0)
        mid_raw[IMAGE_HEIGHT - 1] = (left_raw[IMAGE_HEIGHT - 1] + right_raw[IMAGE_HEIGHT - 1]) / 2;

    // ============================================================
    // EMA 滤波 (从底向上, a=0.4, 与 demo 一致)
    // ============================================================
    float left_f[IPM_ROW_COUNT]  = {0};
    float right_f[IPM_ROW_COUNT] = {0};
    float mid_f[IPM_ROW_COUNT]   = {0};
    constexpr float a = 0.4f;
    int bot = IMAGE_HEIGHT - 1;

    for (int y = bot; y >= 0; --y)
    {
        if (y == bot)
        {
            left_f[y]  = static_cast<float>(left_raw[y]);
            right_f[y] = static_cast<float>(right_raw[y]);
            mid_f[y]   = (left_f[y] + right_f[y]) * 0.5f;
        }
        else
        {
            left_f[y]  = a * left_raw[y]  + (1.0f - a) * left_f[y + 1];
            right_f[y] = a * right_raw[y] + (1.0f - a) * right_f[y + 1];
            mid_f[y]   = (left_f[y] + right_f[y]) * 0.5f;
        }

        int lv = static_cast<int>(left_f[y]  + 0.5f);
        int rv = static_cast<int>(right_f[y] + 0.5f);
        int mv = static_cast<int>(mid_f[y]   + 0.5f);

        r.lines.left[y]  = static_cast<uint8>(std::clamp(lv, 0, 255));
        r.lines.right[y] = static_cast<uint8>(std::clamp(rv, 0, 255));
        r.lines.mid[y]   = static_cast<uint8>(std::clamp(mv, 0, 255));
    }

    int left_cnt = 0, right_cnt = 0;
    for (int y = 0; y < IPM_ROW_COUNT; ++y)
    {
        if (r.lines.left[y]  > 0) ++left_cnt;
        if (r.lines.right[y] > 0) ++right_cnt;
    }
    r.lines.left_valid  = (left_cnt  > 10) ? 1 : 0;
    r.lines.right_valid = (right_cnt > 10) ? 1 : 0;
}