#include <stdio.h>
#include "pico/stdlib.h"
#include "test_frames.h"

#define MAX_BLOB 64        // max pixels we'll track per blob
#define MAX_DETECTIONS 16  // max blobs per frame

typedef struct {
    float row, col;        // blob centre
    float max_temp;
    int   size;
    int   high_confidence; // 1 = HIGH, 0 = LOW
} detection_t;

// The same algorithm as Python's detect(), in C
int detect(const float frame[FRAME_H][FRAME_W],
           float t_min, float t_max, int min_pixels,
           detection_t out[MAX_DETECTIONS])
{
    int  hot[FRAME_H][FRAME_W];
    int  visited[FRAME_H][FRAME_W];
    int  n_detections = 0;

    // stage 1: threshold band
    for (int r = 0; r < FRAME_H; r++)
        for (int c = 0; c < FRAME_W; c++) {
            hot[r][c] = (frame[r][c] >= t_min && frame[r][c] <= t_max);
            visited[r][c] = 0;
        }

    // stage 2: flood fill each unvisited hot pixel
    for (int r = 0; r < FRAME_H; r++) {
        for (int c = 0; c < FRAME_W; c++) {
            if (!hot[r][c] || visited[r][c]) continue;

            // our "stack" is a fixed array + counter (no malloc on embedded!)
            int stack_r[MAX_BLOB * 4], stack_c[MAX_BLOB * 4];
            int stack_top = 0;
            int blob_r[MAX_BLOB], blob_c[MAX_BLOB];
            int blob_size = 0;

            stack_r[stack_top] = r; stack_c[stack_top] = c; stack_top++;

            while (stack_top > 0) {
                stack_top--;
                int rr = stack_r[stack_top], cc = stack_c[stack_top];

                if (rr < 0 || rr >= FRAME_H || cc < 0 || cc >= FRAME_W) continue;
                if (!hot[rr][cc] || visited[rr][cc]) continue;

                visited[rr][cc] = 1;
                if (blob_size < MAX_BLOB) {
                    blob_r[blob_size] = rr;
                    blob_c[blob_size] = cc;
                    blob_size++;
                }
                // push 4 neighbours
                stack_r[stack_top] = rr + 1; stack_c[stack_top] = cc; stack_top++;
                stack_r[stack_top] = rr - 1; stack_c[stack_top] = cc; stack_top++;
                stack_r[stack_top] = rr; stack_c[stack_top] = cc + 1; stack_top++;
                stack_r[stack_top] = rr; stack_c[stack_top] = cc - 1; stack_top++;
            }

            // filter + record
            if (blob_size >= min_pixels && n_detections < MAX_DETECTIONS) {
                float sum_r = 0, sum_c = 0, max_t = -999.0f;
                for (int i = 0; i < blob_size; i++) {
                    sum_r += blob_r[i];
                    sum_c += blob_c[i];
                    float t = frame[blob_r[i]][blob_c[i]];
                    if (t > max_t) max_t = t;
                }
                detection_t *d = &out[n_detections++];
                d->row      = sum_r / blob_size;
                d->col      = sum_c / blob_size;
                d->max_temp = max_t;
                d->size     = blob_size;
                d->high_confidence =
                    (max_t >= 28.0f && blob_size >= 2 && blob_size <= 8);
            }
        }
    }
    return n_detections;
}

int main() {
    stdio_init_all();
    sleep_ms(3000);   // give the serial console time to connect

    printf("=== C detector cross-validation ===\n");
    for (int f = 0; f < NUM_FRAMES; f++) {
        detection_t dets[MAX_DETECTIONS];
        int n = detect(test_frames[f], 25.0f, 40.0f, 2, dets);
        printf("Frame %d: %d detection(s)\n", f, n);
        for (int i = 0; i < n; i++) {
            printf("  C detects [%s]: row=%.2f, col=%.2f, max=%.2f, size=%d\n",
                   dets[i].high_confidence ? "HIGH" : "LOW",
                   dets[i].row, dets[i].col, dets[i].max_temp, dets[i].size);
        }
    }
    while (true) sleep_ms(1000);
}