// firmware/lib/cv/cv.cpp
#include "cv.h"
#include <string.h>
#include <math.h>
#include <algorithm>
#include <vector>

// File-local defaults. Runtime values live in CVState::tuning and can be
// overridden via config_load_tuning() on boot or server push at runtime.
static constexpr uint8_t CV_DEFAULT_DIFF_THRESH = 30;
static constexpr int     CV_DEFAULT_MIN_BLOB_PX = 64;
static constexpr float   CV_DEFAULT_MAX_MOVE    = 15.0f;
static constexpr int     CV_DEFAULT_MAX_MISSED  = 10;
static constexpr uint8_t CV_DEFAULT_LINE_OFFSET = 50;

void cv_init(CVState& state) {
    // Initialize members directly — avoid CVState{} temporary which puts 9KB on stack
    memset(state.background, 0, sizeof(state.background));
    state.bg_valid          = false;
    state.last_motion_frame = 0;
    state.frame_index       = 0;
    state.next_id           = 1;
    state.tracks.clear();
    state.entries = 0;
    state.exits   = 0;
    state.tuning.diff_thresh = CV_DEFAULT_DIFF_THRESH;
    state.tuning.min_blob_px = CV_DEFAULT_MIN_BLOB_PX;
    state.tuning.max_move    = CV_DEFAULT_MAX_MOVE;
    state.tuning.max_missed  = CV_DEFAULT_MAX_MISSED;
    state.tuning.line_offset = CV_DEFAULT_LINE_OFFSET;
    state.tuning.cfg_version = 0;
}

void cv_reset_counts(CVState& state) {
    state.entries = 0;
    state.exits   = 0;
}

bool cv_tuning_validate(const CVTuning& t) {
    if (t.cfg_version == 0) return false;
    if (t.diff_thresh < 5   || t.diff_thresh > 120)  return false;
    if (t.min_blob_px < 16  || t.min_blob_px > 4096) return false;
    if (t.max_move    < 2.0f || t.max_move   > 50.0f) return false;
    if (t.max_missed  < 1   || t.max_missed  > 60)   return false;
    if (t.line_offset > 100)                         return false; // uint8, min 0
    return true;
}

struct Point { int x, y; };

// Note: queue may grow to CV_PIXELS entries (~72KB) on large blobs.
// Requires PSRAM (enabled via -DBOARD_HAS_PSRAM in platformio.ini).
// BFS flood fill. Marks visited pixels (sets fg to 0). Returns {-1,-1} if blob < min_blob_px.
static std::pair<float,float> extract_blob(uint8_t* fg, int start_x, int start_y,
                                           int min_blob_px) {
    std::vector<Point> queue;
    queue.reserve(512);
    queue.push_back({start_x, start_y});
    fg[start_y * CV_W + start_x] = 0;

    float sum_x = 0, sum_y = 0;
    int count = 0;

    while (!queue.empty()) {
        Point p = queue.back(); queue.pop_back();
        sum_x += p.x; sum_y += p.y; count++;

        const int dx[] = {-1, 1, 0, 0};
        const int dy[] = {0, 0, -1, 1};
        for (int d = 0; d < 4; d++) {
            int nx = p.x + dx[d], ny = p.y + dy[d];
            if (nx < 0 || nx >= CV_W || ny < 0 || ny >= CV_H) continue;
            int ni = ny * CV_W + nx;
            if (!fg[ni]) continue;
            fg[ni] = 0;
            queue.push_back({nx, ny});
        }
    }

    if (count < min_blob_px) return {-1.0f, -1.0f};
    return {sum_x / count, sum_y / count};
}

static std::vector<std::pair<float,float>> find_centroids(const uint8_t* fg,
                                                          int min_blob_px) {
    std::vector<std::pair<float,float>> result;
    static uint8_t fg_copy[CV_PIXELS];  // static to avoid 9KB stack allocation
    memcpy(fg_copy, fg, CV_PIXELS);

    for (int y = 0; y < CV_H; y++) {
        for (int x = 0; x < CV_W; x++) {
            if (!fg_copy[y * CV_W + x]) continue;
            auto c = extract_blob(fg_copy, x, y, min_blob_px);
            if (c.first >= 0) result.push_back(c);
        }
    }
    return result;
}

static void frame_diff(const uint8_t* frame, const uint8_t* bg,
                       uint8_t* fg, int pixels, uint8_t diff_thresh) {
    for (int i = 0; i < pixels; i++) {
        int diff = (int)frame[i] - (int)bg[i];
        if (diff < 0) diff = -diff;
        fg[i] = (diff > diff_thresh) ? 1 : 0;
    }
}

CVResult cv_process(CVState& state, const uint8_t* frame) {
    CVResult result = {0, 0};
    state.frame_index++;

    if (!state.bg_valid) {
        memcpy(state.background, frame, CV_PIXELS);
        state.bg_valid = true;
        return result;
    }

    const uint8_t diff_thresh = state.tuning.diff_thresh;
    const int     min_blob_px = state.tuning.min_blob_px;
    const float   max_move    = state.tuning.max_move;
    const int     max_missed  = state.tuning.max_missed;

    static uint8_t fg[CV_PIXELS];  // static: avoids 9KB on task stack
    frame_diff(frame, state.background, fg, CV_PIXELS, diff_thresh);

    int fg_count = 0;
    for (int i = 0; i < CV_PIXELS; i++) fg_count += fg[i];

    bool motion = fg_count > min_blob_px;
    if (!motion) {
        if (state.frame_index - state.last_motion_frame > 10) {
            memcpy(state.background, frame, CV_PIXELS);
        }
        for (auto& t : state.tracks) t.missed++;
        state.tracks.erase(
            std::remove_if(state.tracks.begin(), state.tracks.end(),
                [max_missed](const CVTrack& t){ return t.missed > max_missed; }),
            state.tracks.end());
        return result;
    }

    state.last_motion_frame = state.frame_index;

    auto centroids = find_centroids(fg, min_blob_px);

    std::vector<bool> centroid_matched(centroids.size(), false);

    for (auto& track : state.tracks) {
        float best_dist = max_move * max_move;
        int   best_idx  = -1;

        for (int i = 0; i < (int)centroids.size(); i++) {
            if (centroid_matched[i]) continue;
            float dx = centroids[i].first  - track.x;
            float dy = centroids[i].second - track.y;
            float d2 = dx*dx + dy*dy;
            if (d2 < best_dist) { best_dist = d2; best_idx = i; }
        }

        if (best_idx >= 0) {
            centroid_matched[best_idx] = true;
            track.x = centroids[best_idx].first;
            track.y = centroids[best_idx].second;
            track.missed = 0;
        } else {
            track.missed++;
        }
    }

    state.tracks.erase(
        std::remove_if(state.tracks.begin(), state.tracks.end(),
            [max_missed](const CVTrack& t){ return t.missed > max_missed; }),
        state.tracks.end());

    float line_y = (state.tuning.line_offset / 100.0f) * CV_H;
    for (int i = 0; i < (int)centroids.size(); i++) {
        if (centroid_matched[i]) continue;
        CVTrack t;
        t.id         = state.next_id++;
        t.x          = centroids[i].first;
        t.y          = centroids[i].second;
        t.above_line = (t.y < line_y);
        t.missed     = 0;
        state.tracks.push_back(t);
    }
    // Line crossing check
    for (auto& track : state.tracks) {
        if (track.missed > 0) continue; // only check tracks matched this frame
        bool now_above = (track.y < line_y);
        if (now_above != track.above_line) {
            if (!now_above) {
                // was above, now below → entry
                state.entries++;
                result.entries_delta++;
            } else {
                // was below, now above → exit
                state.exits++;
                result.exits_delta++;
            }
        }
        track.above_line = now_above;
    }

    return result;
}