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

void cv_init(CVState& state) {
    memset(&state, 0, sizeof(CVState));
    state.next_id = 1;
}

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

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

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

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

    uint8_t fg[CV_PIXELS];
    frame_diff(frame, state.background, fg, CV_PIXELS);

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

    bool motion = fg_count > CV_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(),
                [](const CVTrack& t){ return t.missed > CV_MAX_MISSED; }),
            state.tracks.end());
        return result;
    }

    state.last_motion_frame = state.frame_index;
    // Blob detection and tracking added in Tasks 5-6
    return result;
}