#include "vision.h"
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "esp_heap_caps.h"
#include "esp_http_server.h"
#include "esp_log.h"
#include "esp_timer.h"
#include "freertos/FreeRTOS.h"
#include "freertos/semphr.h"
#include "img_converters.h"
static const char *TAG = "vision";
static SemaphoreHandle_t s_lock;
static bool s_local_enabled;
static bool s_show_mask;
static vision_method_t s_method;
static uint8_t s_h;
static uint8_t s_s;
static uint8_t s_v;
static uint8_t s_h_tol;
static uint8_t s_s_tol;
static uint8_t s_v_tol;
static uint8_t s_disp_b;
static uint8_t s_disp_g;
static uint8_t s_disp_r;
static int s_coarse_stride;
static int s_min_area;
static bool s_mask_full;
static bool s_morph_roi;
static vision_stats_t s_stats;
#define DEFAULT_H         15
#define DEFAULT_S         180
#define DEFAULT_V         200
#define DEFAULT_H_TOL     10
#define DEFAULT_S_TOL     80
#define DEFAULT_V_TOL     80
#define DEFAULT_STRIDE    4
#define DEFAULT_MIN_AREA  80
#define REFINE_MARGIN     16
typedef struct {
    uint8_t h_c;
    uint8_t s_c;
    uint8_t v_c;
    uint8_t h_tol;
    uint8_t s_tol;
    uint8_t v_tol;
} hsv_params_t;
typedef struct {
    int min_gx;
    int min_gy;
    int max_gx;
    int max_gy;
    int area;
} coarse_blob_t;
static coarse_blob_t s_last_coarse;
static bool s_have_coarse;

static void hsv_to_bgr_opencv(uint8_t h, uint8_t s, uint8_t v,
                              uint8_t *b, uint8_t *g, uint8_t *r);

void vision_init(void)
{
    s_lock = xSemaphoreCreateMutex();
    s_local_enabled = false;
    s_show_mask = false;
    s_method = VISION_METHOD_CONTOUR;
    s_h = DEFAULT_H;
    s_s = DEFAULT_S;
    s_v = DEFAULT_V;
    s_h_tol = DEFAULT_H_TOL;
    s_s_tol = DEFAULT_S_TOL;
    s_v_tol = DEFAULT_V_TOL;
    hsv_to_bgr_opencv(s_h, s_s, s_v, &s_disp_b, &s_disp_g, &s_disp_r);
    s_coarse_stride = DEFAULT_STRIDE;
    s_min_area = DEFAULT_MIN_AREA;
    s_mask_full = false;
    s_morph_roi = true;
    memset(&s_stats, 0, sizeof(s_stats));
    ESP_LOGI(TAG, "ready (local vision off)");
}
bool vision_local_enabled(void)
{
    bool v = false;
    if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
        v = s_local_enabled;
        xSemaphoreGive(s_lock);
    }
    return v;
}
bool vision_show_mask(void)
{
    bool v = false;
    if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
        v = s_show_mask;
        xSemaphoreGive(s_lock);
    }
    return v;
}
vision_method_t vision_get_method(void)
{
    vision_method_t m = VISION_METHOD_CONTOUR;
    if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
        m = s_method;
        xSemaphoreGive(s_lock);
    }
    return m;
}
void vision_set_local_enabled(bool enabled)
{
    if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
        s_local_enabled = enabled;
        xSemaphoreGive(s_lock);
    }
    ESP_LOGI(TAG, "local vision %s", enabled ? "ON" : "OFF");
}
void vision_set_show_mask(bool show)
{
    if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
        s_show_mask = show;
        xSemaphoreGive(s_lock);
    }
}
void vision_set_method(vision_method_t method)
{
    if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
        s_method = method;
        xSemaphoreGive(s_lock);
    }
}
void vision_set_coarse_stride(int stride)
{
    if (stride < 2) {
        stride = 2;
    }
    if (stride > 8) {
        stride = 8;
    }
    if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
        s_coarse_stride = stride;
        xSemaphoreGive(s_lock);
    }
}
void vision_set_min_area(int area)
{
    if (area < 1) {
        area = 1;
    }
    if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
        s_min_area = area;
        xSemaphoreGive(s_lock);
    }
}
void vision_set_mask_full(bool full)
{
    if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
        s_mask_full = full;
        xSemaphoreGive(s_lock);
    }
}
void vision_set_morph_roi(bool enable)
{
    if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
        s_morph_roi = enable;
        xSemaphoreGive(s_lock);
    }
}
int vision_get_coarse_stride(void)
{
    int v = DEFAULT_STRIDE;
    if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
        v = s_coarse_stride;
        xSemaphoreGive(s_lock);
    }
    return v;
}
int vision_get_min_area(void)
{
    int v = DEFAULT_MIN_AREA;
    if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
        v = s_min_area;
        xSemaphoreGive(s_lock);
    }
    return v;
}
bool vision_get_mask_full(void)
{
    bool v = false;
    if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
        v = s_mask_full;
        xSemaphoreGive(s_lock);
    }
    return v;
}
bool vision_get_morph_roi(void)
{
    bool v = true;
    if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
        v = s_morph_roi;
        xSemaphoreGive(s_lock);
    }
    return v;
}
void vision_get_stats(vision_stats_t *out)
{
    if (!out) {
        return;
    }
    if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
        *out = s_stats;
        xSemaphoreGive(s_lock);
    } else {
        *out = s_stats;
    }
}
void vision_set_hsv(uint8_t h, uint8_t s, uint8_t v)
{
    uint8_t db, dg, dr;
    hsv_to_bgr_opencv(h, s, v, &db, &dg, &dr);
    if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
        s_h = h;
        s_s = s;
        s_v = v;
        s_disp_b = db;
        s_disp_g = dg;
        s_disp_r = dr;
        xSemaphoreGive(s_lock);
    }
    ESP_LOGI(TAG, "HSV centre -> (%u,%u,%u)", h, s, v);
}
void vision_get_hsv(uint8_t *h, uint8_t *s, uint8_t *v)
{
    if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
        if (h) {
            *h = s_h;
        }
        if (s) {
            *s = s_s;
        }
        if (v) {
            *v = s_v;
        }
        xSemaphoreGive(s_lock);
    }
}
void vision_set_tolerance(uint8_t h_tol, uint8_t s_tol, uint8_t v_tol)
{
    if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
        if (h_tol < 1) {
            h_tol = 1;
        }
        if (s_tol < 1) {
            s_tol = 1;
        }
        if (v_tol < 1) {
            v_tol = 1;
        }
        s_h_tol = h_tol;
        s_s_tol = s_tol;
        s_v_tol = v_tol;
        xSemaphoreGive(s_lock);
    }
}
void vision_get_tolerance(uint8_t *h_tol, uint8_t *s_tol, uint8_t *v_tol)
{
    if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
        if (h_tol) {
            *h_tol = s_h_tol;
        }
        if (s_tol) {
            *s_tol = s_s_tol;
        }
        if (v_tol) {
            *v_tol = s_v_tol;
        }
        xSemaphoreGive(s_lock);
    }
}
static void bgr_to_hsv_opencv(uint8_t b, uint8_t g, uint8_t r,
                              uint8_t *h, uint8_t *s, uint8_t *v)
{
    uint8_t v_max = b;
    if (g > v_max) {
        v_max = g;
    }
    if (r > v_max) {
        v_max = r;
    }
    uint8_t v_min = b;
    if (g < v_min) {
        v_min = g;
    }
    if (r < v_min) {
        v_min = r;
    }
    *v = v_max;
    int diff = (int)v_max - (int)v_min;
    if (diff == 0) {
        *h = 0;
        *s = 0;
        return;
    }
    *s = (uint8_t)((diff * 255) / v_max);
    float hf;
    if (v_max == r) {
        hf = 60.f * (float)((int)g - (int)b) / (float)diff;
    } else if (v_max == g) {
        hf = 60.f * (float)((int)b - (int)r) / (float)diff + 120.f;
    } else {
        hf = 60.f * (float)((int)r - (int)g) / (float)diff + 240.f;
    }
    if (hf < 0.f) {
        hf += 360.f;
    }
    int hi = (int)(hf * 0.5f + 0.5f);
    if (hi > 179) {
        hi = 179;
    }
    *h = (uint8_t)hi;
}

/* Inverse of OpenCV-style HSV (H 0-179) used above — for UI swatch. */
static void hsv_to_bgr_opencv(uint8_t h, uint8_t s, uint8_t v,
                              uint8_t *b, uint8_t *g, uint8_t *r)
{
    if (s == 0) {
        *b = *g = *r = v;
        return;
    }
    int region = h / 30;
    int remainder = (h - (region * 30)) * 6;
    int p = (v * (255 - s)) / 255;
    int q = (v * (255 - ((s * remainder) / 255))) / 255;
    int t = (v * (255 - ((s * (255 - remainder)) / 255))) / 255;
    switch (region) {
    case 0:
        *r = v;
        *g = (uint8_t)t;
        *b = (uint8_t)p;
        break;
    case 1:
        *r = (uint8_t)q;
        *g = v;
        *b = (uint8_t)p;
        break;
    case 2:
        *r = (uint8_t)p;
        *g = v;
        *b = (uint8_t)t;
        break;
    case 3:
        *r = (uint8_t)p;
        *g = (uint8_t)q;
        *b = v;
        break;
    case 4:
        *r = (uint8_t)t;
        *g = (uint8_t)p;
        *b = v;
        break;
    default:
        *r = v;
        *g = (uint8_t)p;
        *b = (uint8_t)q;
        break;
    }
}
static void cam_bytes_to_bgr(const uint8_t *p, uint8_t *b, uint8_t *g, uint8_t *r)
{
    uint8_t hb = p[0];
    uint8_t lb = p[1];
    *r = (uint8_t)((hb & 0xF8) | ((hb & 0xF8) >> 5));
    *g = (uint8_t)(((hb & 0x07) << 5) | ((lb & 0xE0) >> 3));
    *b = (uint8_t)(((lb & 0x1F) << 3) | ((lb & 0x1F) >> 2));
}
static bool hsv_in_range(uint8_t h, uint8_t s, uint8_t v, const hsv_params_t *p)
{
    int s_lo = (int)p->s_c - (int)p->s_tol;
    int s_hi = (int)p->s_c + (int)p->s_tol;
    int v_lo = (int)p->v_c - (int)p->v_tol;
    int v_hi = (int)p->v_c + (int)p->v_tol;
    if (s < s_lo || s > s_hi) {
        return false;
    }
    if (v < v_lo || v > v_hi) {
        return false;
    }
    if (p->h_c - p->h_tol < 0) {
        int wrap_lo = 179 + (p->h_c - p->h_tol);
        return (h <= p->h_c + p->h_tol) || (h >= wrap_lo);
    }
    if (p->h_c + p->h_tol > 179) {
        int wrap_hi = p->h_c + p->h_tol - 179;
        return (h >= p->h_c - p->h_tol) || (h <= wrap_hi);
    }
    return h >= p->h_c - p->h_tol && h <= p->h_c + p->h_tol;
}
static bool pixel_matches(const uint8_t *raw, int w, int x, int y, const hsv_params_t *p)
{
    uint8_t b, g, r, hh, ss, vv;
    cam_bytes_to_bgr(raw + ((size_t)y * (size_t)w + (size_t)x) * 2, &b, &g, &r);
    bgr_to_hsv_opencv(b, g, r, &hh, &ss, &vv);
    return hsv_in_range(hh, ss, vv, p);
}
static inline uint8_t clamp_u8(int v, int lo, int hi)
{
    if (v < lo) {
        return (uint8_t)lo;
    }
    if (v > hi) {
        return (uint8_t)hi;
    }
    return (uint8_t)v;
}
static inline int clamp_i(int v, int lo, int hi)
{
    if (v < lo) {
        return lo;
    }
    if (v > hi) {
        return hi;
    }
    return v;
}
static void snapshot_params(hsv_params_t *p, int *stride, int *min_area,
                            bool *mask_full, bool *morph_roi)
{
    if (xSemaphoreTake(s_lock, portMAX_DELAY) != pdTRUE) {
        return;
    }
    p->h_c = s_h;
    p->s_c = s_s;
    p->v_c = s_v;
    p->h_tol = s_h_tol;
    p->s_tol = s_s_tol;
    p->v_tol = s_v_tol;
    *stride = s_coarse_stride;
    *min_area = s_min_area;
    *mask_full = s_mask_full;
    *morph_roi = s_morph_roi;
    xSemaphoreGive(s_lock);
}
void vision_set_color_bgr(uint8_t b, uint8_t g, uint8_t r)
{
    uint8_t h, s, v;
    bgr_to_hsv_opencv(b, g, r, &h, &s, &v);
    if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
        s_h = h;
        s_s = s;
        s_v = v;
        s_disp_b = b;
        s_disp_g = g;
        s_disp_r = r;
        xSemaphoreGive(s_lock);
    }
    ESP_LOGI(TAG, "colour BGR(%u,%u,%u) -> HSV(%u,%u,%u)", b, g, r, h, s, v);
}
esp_err_t vision_sample_hsv_from_frame(const camera_fb_t *fb, int x, int y)
{
    if (!fb || fb->format != PIXFORMAT_RGB565) {
        return ESP_ERR_INVALID_STATE;
    }
    int w = (int)fb->width;
    int h = (int)fb->height;
    if (w <= 0 || h <= 0) {
        return ESP_ERR_INVALID_SIZE;
    }
    x = clamp_i(x, 0, w - 1);
    y = clamp_i(y, 0, h - 1);
    const uint8_t *raw = (const uint8_t *)fb->buf;
    const uint8_t *px = raw + (size_t)y * (size_t)w * 2 + (size_t)x * 2;
    uint8_t b, g, r, hh, ss, vv;
    cam_bytes_to_bgr(px, &b, &g, &r);
    bgr_to_hsv_opencv(b, g, r, &hh, &ss, &vv);
    if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
        s_h = hh;
        s_s = ss;
        s_v = vv;
        s_disp_b = b;
        s_disp_g = g;
        s_disp_r = r;
        xSemaphoreGive(s_lock);
    }
    ESP_LOGI(TAG, "live pick @ (%d,%d) BGR(%u,%u,%u) -> HSV(%u,%u,%u)", x, y, b, g, r, hh, ss, vv);
    return ESP_OK;
}
static bool find_largest_coarse_blob(const uint8_t *grid, int gw, int gh, int min_coarse_area,
                                     coarse_blob_t *best)
{
    uint8_t *visited = calloc((size_t)gw * (size_t)gh, 1);
    if (!visited) {
        return false;
    }
    int *queue = heap_caps_malloc((size_t)gw * (size_t)gh * 2 * sizeof(int),
                                  MALLOC_CAP_SPIRAM | MALLOC_CAP_8BIT);
    if (!queue) {
        queue = malloc((size_t)gw * (size_t)gh * 2 * sizeof(int));
    }
    if (!queue) {
        free(visited);
        return false;
    }
    bool found = false;
    best->area = 0;
    for (int gy = 0; gy < gh; gy++) {
        for (int gx = 0; gx < gw; gx++) {
            int idx = gy * gw + gx;
            if (!grid[idx] || visited[idx]) {
                continue;
            }
            int head = 0;
            int tail = 0;
            int area = 0;
            int min_gx = gx;
            int min_gy = gy;
            int max_gx = gx;
            int max_gy = gy;
            queue[tail++] = gx;
            queue[tail++] = gy;
            visited[idx] = 1;
            while (head < tail) {
                int cx = queue[head++];
                int cy = queue[head++];
                area++;
                if (cx < min_gx) {
                    min_gx = cx;
                }
                if (cy < min_gy) {
                    min_gy = cy;
                }
                if (cx > max_gx) {
                    max_gx = cx;
                }
                if (cy > max_gy) {
                    max_gy = cy;
                }
                static const int dx[4] = {1, -1, 0, 0};
                static const int dy[4] = {0, 0, 1, -1};
                for (int k = 0; k < 4; k++) {
                    int nx = cx + dx[k];
                    int ny = cy + dy[k];
                    if (nx < 0 || nx >= gw || ny < 0 || ny >= gh) {
                        continue;
                    }
                    int nidx = ny * gw + nx;
                    if (!grid[nidx] || visited[nidx]) {
                        continue;
                    }
                    visited[nidx] = 1;
                    queue[tail++] = nx;
                    queue[tail++] = ny;
                }
            }
            if (area >= min_coarse_area && area > best->area) {
                best->area = area;
                best->min_gx = min_gx;
                best->min_gy = min_gy;
                best->max_gx = max_gx;
                best->max_gy = max_gy;
                found = true;
            }
        }
    }
    free(queue);
    free(visited);
    return found;
}
static bool refine_blob(const camera_fb_t *fb, const hsv_params_t *p,
                        const coarse_blob_t *coarse, int stride, int min_area,
                        int margin, vision_stats_t *stats)
{
    int w = (int)fb->width;
    int h = (int)fb->height;
    const uint8_t *raw = (const uint8_t *)fb->buf;
    int x0 = coarse->min_gx * stride - margin;
    int y0 = coarse->min_gy * stride - margin;
    int x1 = (coarse->max_gx + 1) * stride + margin;
    int y1 = (coarse->max_gy + 1) * stride + margin;
    x0 = clamp_i(x0, 0, w - 1);
    y0 = clamp_i(y0, 0, h - 1);
    x1 = clamp_i(x1, 0, w - 1);
    y1 = clamp_i(y1, 0, h - 1);
    int64_t sum_x = 0;
    int64_t sum_y = 0;
    int area = 0;
    for (int y = y0; y <= y1; y++) {
        for (int x = x0; x <= x1; x++) {
            if (!pixel_matches(raw, w, x, y, p)) {
                continue;
            }
            sum_x += x;
            sum_y += y;
            area++;
        }
    }
    if (area < min_area) {
        stats->found = false;
        return false;
    }
    stats->found = true;
    stats->area = area;
    stats->cx = (float)sum_x / (float)area;
    stats->cy = (float)sum_y / (float)area;
    stats->radius = sqrtf((float)area / (float)M_PI);
    return true;
}
static void morph_3x3_roi(uint8_t *mask, int w, int h, int x0, int y0, int x1, int y1, bool dilate)
{
    uint8_t *tmp = heap_caps_malloc((size_t)w * (size_t)h, MALLOC_CAP_SPIRAM | MALLOC_CAP_8BIT);
    if (!tmp) {
        return;
    }
    memcpy(tmp, mask, (size_t)w * (size_t)h);
    x0 = clamp_i(x0, 1, w - 2);
    y0 = clamp_i(y0, 1, h - 2);
    x1 = clamp_i(x1, 1, w - 2);
    y1 = clamp_i(y1, 1, h - 2);
    for (int y = y0; y <= y1; y++) {
        for (int x = x0; x <= x1; x++) {
            int on = dilate ? 0 : 1;
            for (int dy = -1; dy <= 1; dy++) {
                for (int dx = -1; dx <= 1; dx++) {
                    if (dilate) {
                        if (tmp[(y + dy) * w + (x + dx)]) {
                            on = 1;
                        }
                    } else if (!tmp[(y + dy) * w + (x + dx)]) {
                        on = 0;
                    }
                }
            }
            mask[y * w + x] = (uint8_t)(on ? 255 : 0);
        }
    }
    heap_caps_free(tmp);
}
static void morph_open_close_roi(uint8_t *mask, int w, int h, int x0, int y0, int x1, int y1)
{
    morph_3x3_roi(mask, w, h, x0, y0, x1, y1, true);
    morph_3x3_roi(mask, w, h, x0, y0, x1, y1, false);
    morph_3x3_roi(mask, w, h, x0, y0, x1, y1, false);
    morph_3x3_roi(mask, w, h, x0, y0, x1, y1, true);
}
static void fill_mask_region(const camera_fb_t *fb, const hsv_params_t *p,
                             uint8_t *mask, int x0, int y0, int x1, int y1)
{
    int w = (int)fb->width;
    const uint8_t *raw = (const uint8_t *)fb->buf;
    for (int y = y0; y <= y1; y++) {
        for (int x = x0; x <= x1; x++) {
            mask[y * w + x] = pixel_matches(raw, w, x, y, p) ? 255 : 0;
        }
    }
}
static esp_err_t build_full_mask(const camera_fb_t *fb, const hsv_params_t *p, uint8_t **out_mask)
{
    int w = (int)fb->width;
    int h = (int)fb->height;
    uint8_t *mask = heap_caps_calloc((size_t)w * (size_t)h, 1, MALLOC_CAP_SPIRAM | MALLOC_CAP_8BIT);
    if (!mask) {
        return ESP_ERR_NO_MEM;
    }
    fill_mask_region(fb, p, mask, 0, 0, w - 1, h - 1);
    *out_mask = mask;
    return ESP_OK;
}
static esp_err_t build_roi_mask(const camera_fb_t *fb, const hsv_params_t *p,
                                bool morph, const coarse_blob_t *coarse, int stride,
                                uint8_t **out_mask, uint32_t *morph_us)
{
    int w = (int)fb->width;
    int h = (int)fb->height;
    uint8_t *mask = heap_caps_calloc((size_t)w * (size_t)h, 1, MALLOC_CAP_SPIRAM | MALLOC_CAP_8BIT);
    if (!mask) {
        return ESP_ERR_NO_MEM;
    }
    int x0 = coarse->min_gx * stride - REFINE_MARGIN;
    int y0 = coarse->min_gy * stride - REFINE_MARGIN;
    int x1 = (coarse->max_gx + 1) * stride + REFINE_MARGIN;
    int y1 = (coarse->max_gy + 1) * stride + REFINE_MARGIN;
    x0 = clamp_i(x0, 0, w - 1);
    y0 = clamp_i(y0, 0, h - 1);
    x1 = clamp_i(x1, 0, w - 1);
    y1 = clamp_i(y1, 0, h - 1);
    fill_mask_region(fb, p, mask, x0, y0, x1, y1);
    if (morph) {
        int64_t t0 = esp_timer_get_time();
        morph_open_close_roi(mask, w, h, x0, y0, x1, y1);
        if (morph_us) {
            *morph_us = (uint32_t)(esp_timer_get_time() - t0);
        }
    } else if (morph_us) {
        *morph_us = 0;
    }
    *out_mask = mask;
    return ESP_OK;
}
static esp_err_t mask_to_jpeg(const uint8_t *mask, int w, int h, int quality,
                              uint8_t **jpg_buf, size_t *jpg_len)
{
    camera_fb_t fake = {
        .buf = (uint8_t *)mask,
        .len = (size_t)w * (size_t)h,
        .width = (size_t)w,
        .height = (size_t)h,
        .format = PIXFORMAT_GRAYSCALE,
    };
    return frame2jpg(&fake, quality, jpg_buf, jpg_len) ? ESP_OK : ESP_FAIL;
}
static void run_detection(const camera_fb_t *fb, vision_stats_t *stats)
{
    memset(stats, 0, sizeof(*stats));
    if (fb->format != PIXFORMAT_RGB565) {
        return;
    }
    int64_t t_detect = esp_timer_get_time();
    hsv_params_t p;
    int stride;
    int min_area;
    bool mask_full;
    bool morph_roi;
    snapshot_params(&p, &stride, &min_area, &mask_full, &morph_roi);
    (void)mask_full;
    int w = (int)fb->width;
    int h = (int)fb->height;
    const uint8_t *raw = (const uint8_t *)fb->buf;
    int gw = (w + stride - 1) / stride;
    int gh = (h + stride - 1) / stride;
    int min_coarse = min_area / (stride * stride);
    if (min_coarse < 1) {
        min_coarse = 1;
    }
    int64_t t0 = esp_timer_get_time();
    uint8_t *grid = heap_caps_calloc((size_t)gw * (size_t)gh, 1, MALLOC_CAP_SPIRAM | MALLOC_CAP_8BIT);
    if (!grid) {
        return;
    }
    for (int gy = 0; gy < gh; gy++) {
        int py = gy * stride + stride / 2;
        if (py >= h) {
            py = h - 1;
        }
        for (int gx = 0; gx < gw; gx++) {
            int px = gx * stride + stride / 2;
            if (px >= w) {
                px = w - 1;
            }
            grid[gy * gw + gx] = pixel_matches(raw, w, px, py, &p) ? 1 : 0;
        }
    }
    stats->coarse_us = (uint32_t)(esp_timer_get_time() - t0);
    coarse_blob_t coarse = {0};
    t0 = esp_timer_get_time();
    if (!find_largest_coarse_blob(grid, gw, gh, min_coarse, &coarse)) {
        heap_caps_free(grid);
        s_have_coarse = false;
        stats->detect_us = (uint32_t)(esp_timer_get_time() - t_detect);
        return;
    }
    stats->coarse_us += (uint32_t)(esp_timer_get_time() - t0);
    s_last_coarse = coarse;
    s_have_coarse = true;
    t0 = esp_timer_get_time();
    refine_blob(fb, &p, &coarse, stride, min_area, REFINE_MARGIN, stats);
    stats->refine_us = (uint32_t)(esp_timer_get_time() - t0);
    heap_caps_free(grid);
    stats->detect_us = (uint32_t)(esp_timer_get_time() - t_detect);
    if (stats->found) {
        ESP_LOGD(TAG, "blob @ (%.0f,%.0f) r=%.0f area=%d",
                 stats->cx, stats->cy, stats->radius, stats->area);
    }
}
esp_err_t vision_encode_capture_jpeg(
    const camera_fb_t *fb,
    int quality,
    uint8_t **jpg_buf,
    size_t *jpg_len,
    vision_stats_t *stats_out)
{
    if (!fb || !jpg_buf || !jpg_len) {
        return ESP_ERR_INVALID_ARG;
    }
    *jpg_buf = NULL;
    *jpg_len = 0;
    int64_t t_total = esp_timer_get_time();
    vision_stats_t stats = {0};
    if (vision_local_enabled() && fb->format == PIXFORMAT_RGB565) {
        run_detection(fb, &stats);
        if (vision_show_mask()) {
            hsv_params_t p;
            int stride;
            int min_area;
            bool mask_full;
            bool morph_roi;
            snapshot_params(&p, &stride, &min_area, &mask_full, &morph_roi);
            int64_t t0 = esp_timer_get_time();
            uint8_t *mask = NULL;
            esp_err_t err;
            if (mask_full) {
                err = build_full_mask(fb, &p, &mask);
            } else if (stats.found && s_have_coarse) {
                err = build_roi_mask(fb, &p, morph_roi, &s_last_coarse, stride, &mask,
                                     &stats.morph_us);
            } else {
                mask = heap_caps_calloc((size_t)fb->width * (size_t)fb->height, 1,
                                          MALLOC_CAP_SPIRAM | MALLOC_CAP_8BIT);
                err = mask ? ESP_OK : ESP_ERR_NO_MEM;
            }
            stats.mask_us = (uint32_t)(esp_timer_get_time() - t0);
            if (err != ESP_OK || !mask) {
                heap_caps_free(mask);
                return err;
            }
            t0 = esp_timer_get_time();
            err = mask_to_jpeg(mask, (int)fb->width, (int)fb->height, quality, jpg_buf, jpg_len);
            stats.jpeg_us = (uint32_t)(esp_timer_get_time() - t0);
            heap_caps_free(mask);
            if (err != ESP_OK) {
                return err;
            }
        } else {
            int64_t t0 = esp_timer_get_time();
            camera_fb_t fb_copy = *fb;
            esp_err_t err = frame2jpg(&fb_copy, quality, jpg_buf, jpg_len) ? ESP_OK : ESP_FAIL;
            stats.jpeg_us = (uint32_t)(esp_timer_get_time() - t0);
            if (err != ESP_OK) {
                return err;
            }
        }
    } else {
        if (vision_local_enabled() && vision_show_mask() && fb->format != PIXFORMAT_RGB565) {
            ESP_LOGW(TAG, "mask requested but frame is not RGB565 - passthrough");
        }
        if (fb->format == PIXFORMAT_JPEG) {
            *jpg_buf = (uint8_t *)malloc(fb->len);
            if (!*jpg_buf) {
                return ESP_ERR_NO_MEM;
            }
            memcpy(*jpg_buf, fb->buf, fb->len);
            *jpg_len = fb->len;
        } else if (fb->format == PIXFORMAT_RGB565) {
            int64_t t0 = esp_timer_get_time();
            camera_fb_t fb_copy = *fb;
            esp_err_t err = frame2jpg(&fb_copy, quality, jpg_buf, jpg_len) ? ESP_OK : ESP_FAIL;
            stats.jpeg_us = (uint32_t)(esp_timer_get_time() - t0);
            if (err != ESP_OK) {
                return err;
            }
        } else {
            return ESP_ERR_NOT_SUPPORTED;
        }
    }
    stats.total_us = (uint32_t)(esp_timer_get_time() - t_total);
    if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
        s_stats = stats;
        xSemaphoreGive(s_lock);
    }
    if (stats_out) {
        *stats_out = stats;
    }
    return ESP_OK;
}
esp_err_t vision_detect_on_frame(const camera_fb_t *fb, vision_stats_t *stats_out)
{
    if (!fb || !stats_out) {
        return ESP_ERR_INVALID_ARG;
    }
    int64_t t0 = esp_timer_get_time();
    run_detection(fb, stats_out);
    stats_out->total_us = (uint32_t)(esp_timer_get_time() - t0);
    if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
        s_stats = *stats_out;
        xSemaphoreGive(s_lock);
    }
    return ESP_OK;
}
size_t vision_format_stats_json(const vision_stats_t *stats, char *buf, size_t len)
{
    if (!stats || !buf || len == 0) {
        return 0;
    }
    return (size_t)snprintf(
        buf, len,
        "{\"found\":%s,\"cx\":%.1f,\"cy\":%.1f,\"radius\":%.0f,\"area\":%d,"
        "\"total_ms\":%.1f,\"detect_ms\":%.1f,\"coarse_ms\":%.1f,\"refine_ms\":%.1f,"
        "\"morph_ms\":%.1f,\"mask_ms\":%.1f,\"jpeg_ms\":%.1f}",
        stats->found ? "true" : "false",
        stats->cx, stats->cy, stats->radius, stats->area,
        stats->total_us / 1000.0f, stats->detect_us / 1000.0f, stats->coarse_us / 1000.0f,
        stats->refine_us / 1000.0f, stats->morph_us / 1000.0f, stats->mask_us / 1000.0f,
        stats->jpeg_us / 1000.0f);
}
void vision_add_capture_headers(httpd_req_t *req, const vision_stats_t *s)
{
    if (!req || !s) {
        return;
    }
    static char data[256];
    if (xSemaphoreTake(s_lock, portMAX_DELAY) != pdTRUE) {
        return;
    }
    if (s->found) {
        snprintf(data, sizeof(data),
                 "found=1;cx=%.1f;cy=%.1f;r=%.0f;area=%d;total=%.1f;detect=%.1f;coarse=%.1f;"
                 "refine=%.1f;morph=%.1f;mask=%.1f;jpeg=%.1f",
                 s->cx, s->cy, s->radius, s->area,
                 s->total_us / 1000.0f, s->detect_us / 1000.0f, s->coarse_us / 1000.0f,
                 s->refine_us / 1000.0f, s->morph_us / 1000.0f, s->mask_us / 1000.0f,
                 s->jpeg_us / 1000.0f);
    } else {
        snprintf(data, sizeof(data),
                 "found=0;total=%.1f;detect=%.1f;coarse=%.1f;refine=%.1f;morph=%.1f;mask=%.1f;"
                 "jpeg=%.1f",
                 s->total_us / 1000.0f, s->detect_us / 1000.0f, s->coarse_us / 1000.0f,
                 s->refine_us / 1000.0f, s->morph_us / 1000.0f, s->mask_us / 1000.0f,
                 s->jpeg_us / 1000.0f);
    }
    esp_err_t err = httpd_resp_set_hdr(req, "X-Vision-Data", data);
    xSemaphoreGive(s_lock);
    if (err != ESP_OK) {
        ESP_LOGW(TAG, "X-Vision-Data header rejected: %s", esp_err_to_name(err));
    }
}
static const char *method_name(vision_method_t m)
{
    return m == VISION_METHOD_RECT ? "rect" : "contour";
}
void vision_format_ack(char *ack_buf, size_t ack_len)
{
    uint8_t hh, ss, vv, ht, st, vt, db, dg, dr;
    int stride;
    int min_area;
    bool mask_full;
    bool morph_roi;
    if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
        hh = s_h;
        ss = s_s;
        vv = s_v;
        ht = s_h_tol;
        st = s_s_tol;
        vt = s_v_tol;
        db = s_disp_b;
        dg = s_disp_g;
        dr = s_disp_r;
        stride = s_coarse_stride;
        min_area = s_min_area;
        mask_full = s_mask_full;
        morph_roi = s_morph_roi;
        xSemaphoreGive(s_lock);
    } else {
        hh = ss = vv = ht = st = vt = 0;
        db = dg = dr = 0;
        stride = DEFAULT_STRIDE;
        min_area = DEFAULT_MIN_AREA;
        mask_full = false;
        morph_roi = true;
    }
    snprintf(
        ack_buf, ack_len,
        "{\"ok\":true,\"type\":\"vision\",\"local\":%s,\"show_mask\":%s,"
        "\"method\":\"%s\",\"h\":%u,\"s\":%u,\"v\":%u,"
        "\"b\":%u,\"g\":%u,\"r\":%u,"
        "\"h_tol\":%u,\"s_tol\":%u,\"v_tol\":%u,"
        "\"coarse_stride\":%d,\"min_area\":%d,"
        "\"mask_full\":%s,\"morph_roi\":%s}",
        vision_local_enabled() ? "true" : "false",
        vision_show_mask() ? "true" : "false",
        method_name(vision_get_method()),
        hh, ss, vv,
        db, dg, dr,
        ht, st, vt,
        stride, min_area,
        mask_full ? "true" : "false",
        morph_roi ? "true" : "false");
}
esp_err_t vision_handle_update(
    bool has_local, bool local,
    bool has_show_mask, bool show_mask,
    bool has_method, const char *method,
    bool has_h, int h,
    bool has_s, int s,
    bool has_v, int v,
    bool has_h_tol, int h_tol,
    bool has_s_tol, int s_tol,
    bool has_v_tol, int v_tol,
    bool has_b, int b,
    bool has_g, int g,
    bool has_r, int r,
    bool has_coarse_stride, int coarse_stride,
    bool has_min_area, int min_area,
    bool has_mask_full, bool mask_full,
    bool has_morph_roi, bool morph_roi,
    char *ack_buf, size_t ack_len)
{
    if (has_local) {
        vision_set_local_enabled(local);
    }
    if (has_show_mask) {
        vision_set_show_mask(show_mask);
    }
    if (has_method && method) {
        if (strcmp(method, "rect") == 0) {
            vision_set_method(VISION_METHOD_RECT);
        } else {
            vision_set_method(VISION_METHOD_CONTOUR);
        }
    }
    if (has_coarse_stride) {
        vision_set_coarse_stride(coarse_stride);
    }
    if (has_min_area) {
        vision_set_min_area(min_area);
    }
    if (has_mask_full) {
        vision_set_mask_full(mask_full);
    }
    if (has_morph_roi) {
        vision_set_morph_roi(morph_roi);
    }
    if (has_h_tol || has_s_tol || has_v_tol) {
        uint8_t ht = s_h_tol, st = s_s_tol, vt = s_v_tol;
        if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
            ht = s_h_tol;
            st = s_s_tol;
            vt = s_v_tol;
            xSemaphoreGive(s_lock);
        }
        if (has_h_tol) {
            ht = clamp_u8(h_tol, 1, 90);
        }
        if (has_s_tol) {
            st = clamp_u8(s_tol, 1, 255);
        }
        if (has_v_tol) {
            vt = clamp_u8(v_tol, 1, 255);
        }
        vision_set_tolerance(ht, st, vt);
    }
    if (has_b && has_g && has_r && !(has_h && has_s && has_v)) {
        vision_set_color_bgr(clamp_u8(b, 0, 255), clamp_u8(g, 0, 255), clamp_u8(r, 0, 255));
    }
    if (has_h && has_s && has_v) {
        vision_set_hsv(clamp_u8(h, 0, 179), clamp_u8(s, 0, 255), clamp_u8(v, 0, 255));
    }
    vision_format_ack(ack_buf, ack_len);
    return ESP_OK;
}