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

DebugOverlay g_dbg;

static uint16* g_buf    = nullptr;
static int     g_w      = 0;
static int     g_h      = 0;

static inline uint16 rgb565(uint8 r, uint8 g, uint8 b)
{
    return ((r >> 3) << 11) | ((g >> 2) << 5) | (b >> 3);
}

static inline bool in_screen(int x, int y)
{
    return x >= 0 && x < g_w && y >= 0 && y < g_h;
}

void dbg_draw_init(uint16* fb_buffer, int w, int h)
{
    g_buf = fb_buffer;
    g_w = w; g_h = h;
}

void dbg_draw_line(int x0, int y0, int x1, int y1, uint8 r, uint8 g, uint8 b)
{
    if (!g_buf) return;
    uint16 color = rgb565(r, g, b);
    int dx = std::abs(x1 - x0), sx = x0 < x1 ? 1 : -1;
    int dy = -std::abs(y1 - y0), sy = y0 < y1 ? 1 : -1;
    int err = dx + dy;

    while (true)
    {
        if (in_screen(x0, y0)) g_buf[y0 * g_w + x0] = color;
        if (x0 == x1 && y0 == y1) break;
        int e2 = 2 * err;
        if (e2 >= dy) { err += dy; x0 += sx; }
        if (e2 <= dx) { err += dx; y0 += sy; }
    }
}

void dbg_draw_cross(int cx, int cy, int size, uint8 r, uint8 g, uint8 b)
{
    dbg_draw_line(cx - size, cy, cx + size, cy, r, g, b);
    dbg_draw_line(cx, cy - size, cx, cy + size, r, g, b);
}

void dbg_draw_rect(int x, int y, int w, int h, uint8 r, uint8 g, uint8 b)
{
    dbg_draw_line(x, y, x + w, y, r, g, b);
    dbg_draw_line(x + w, y, x + w, y + h, r, g, b);
    dbg_draw_line(x + w, y + h, x, y + h, r, g, b);
    dbg_draw_line(x, y + h, x, y, r, g, b);
}

void dbg_draw_points(const int points[][2], int count, uint8 r, uint8 g, uint8 b)
{
    if (!g_buf) return;
    uint16 color = rgb565(r, g, b);
    for (int i = 0; i < count; ++i)
    {
        int x = points[i][0], y = points[i][1];
        if (in_screen(x, y)) g_buf[y * g_w + x] = color;
    }
}

void dbg_draw_edge_zoom(const EdgeLines& lines, uint8 r, uint8 g, uint8 b)
{
    const int thick = 7;  // 车道线宽度 (像素)

    for (int y = 0; y < IPM_ROW_COUNT; ++y)
    {
        if (lines.left[y] > 0)
            for (int dx = -thick; dx <= thick; ++dx)
                dbg_draw_line(lines.left[y] + dx, y, lines.left[y] + dx, y, r, g, b);

        if (lines.right[y] > 0)
            for (int dx = -thick; dx <= thick; ++dx)
                dbg_draw_line(lines.right[y] + dx, y, lines.right[y] + dx, y, 0, 255, 0);

        if (lines.mid[y] > 0)
            for (int dx = -thick; dx <= thick; ++dx)
                dbg_draw_line(lines.mid[y] + dx, y, lines.mid[y] + dx, y, 255, 255, 0);
    }
}