vision/main/main.c

#include "board_svc.h"
#include "eye_i2c.h"
#include "robot_link.h"
#include "status_led.h"
#include "vision.h"

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>

#include "esp_camera.h"
#include "esp_check.h"
#include "esp_event.h"
#include "esp_heap_caps.h"
#include "esp_http_server.h"
#include "esp_log.h"
#include "esp_netif.h"
#include "esp_timer.h"
#include "esp_wifi.h"
#include "freertos/FreeRTOS.h"
#include "freertos/event_groups.h"
#include "freertos/task.h"
#include "nvs_flash.h"

#include "camera_pins.h"

static const char *TAG = "cam_stream";

/* -------- put your Wi‑Fi credentials here -------- */
#define WIFI_SSID "Police Surveillance Van"
#define WIFI_PASS "ourpassword"
/* ------------------------------------------------ */

/* Stream tuning for navigation — lower = steadier / lower latency */
#define CAM_FRAME_SIZE   FRAMESIZE_QVGA /* 320x240 */
#define CAM_JPEG_QUALITY 18             /* 0-63, higher = smaller files */
#define STREAM_FPS       10

#define WIFI_CONNECTED_BIT BIT0
#define WIFI_FAIL_BIT      BIT1
#define WIFI_MAX_RETRY     10

#define PART_BOUNDARY "123456789000000000000987654321"
#define _STR(x) #x
#define STR(x) _STR(x)

static EventGroupHandle_t s_wifi_event_group;
static int s_retry_num = 0;
static camera_config_t s_cam_config;
static bool s_camera_ok = false;
static pixformat_t s_camera_pix_fmt = PIXFORMAT_JPEG;
static SemaphoreHandle_t s_cam_mutex;
static httpd_handle_t s_httpd = NULL;
static bool s_wifi_driver_up = false;
static bool s_wifi_netif_ready = false;
static volatile bool s_want_camera = false;
static volatile bool s_want_wifi = false;
static volatile bool s_wifi_restart = false;
static char s_wifi_ssid[33];
static char s_wifi_pass[65];
static bool s_wifi_cred_from_i2c = false;

static void set_no_cache(httpd_req_t *req);

static const char *STREAM_CONTENT_TYPE =
    "multipart/x-mixed-replace;boundary=" PART_BOUNDARY;
static const char *STREAM_BOUNDARY = "\r\n--" PART_BOUNDARY "\r\n";
static const char *STREAM_PART =
    "Content-Type: image/jpeg\r\nContent-Length: %u\r\n\r\n";

/* Embedded UI (main/www/index.html) — video + local vision controls */
extern const uint8_t index_html_start[] asm("_binary_index_html_start");
extern const uint8_t index_html_end[] asm("_binary_index_html_end");

static esp_err_t init_camera_with_format(pixformat_t fmt)
{
    if (s_cam_mutex) {
        xSemaphoreTake(s_cam_mutex, portMAX_DELAY);
    }

    s_cam_config = (camera_config_t){
        .pin_pwdn = CAM_PIN_PWDN,
        .pin_reset = CAM_PIN_RESET,
        .pin_xclk = CAM_PIN_XCLK,
        .pin_sccb_sda = CAM_PIN_SIOD,
        .pin_sccb_scl = CAM_PIN_SIOC,
        .pin_d7 = CAM_PIN_D7,
        .pin_d6 = CAM_PIN_D6,
        .pin_d5 = CAM_PIN_D5,
        .pin_d4 = CAM_PIN_D4,
        .pin_d3 = CAM_PIN_D3,
        .pin_d2 = CAM_PIN_D2,
        .pin_d1 = CAM_PIN_D1,
        .pin_d0 = CAM_PIN_D0,
        .pin_vsync = CAM_PIN_VSYNC,
        .pin_href = CAM_PIN_HREF,
        .pin_pclk = CAM_PIN_PCLK,
        .xclk_freq_hz = 20000000,
        .ledc_timer = LEDC_TIMER_0,
        .ledc_channel = LEDC_CHANNEL_0,
        .pixel_format = fmt,
        .frame_size = CAM_FRAME_SIZE,
        .jpeg_quality = CAM_JPEG_QUALITY,
        .fb_count = 2,
        .fb_location = CAMERA_FB_IN_PSRAM,
        .grab_mode = CAMERA_GRAB_LATEST,
    };

    if (s_camera_ok) {
        esp_camera_deinit();
        s_camera_ok = false;
    }

    esp_err_t err = esp_camera_init(&s_cam_config);
    if (err != ESP_OK) {
        ESP_LOGE(TAG, "Camera init failed (%s): %s",
                 fmt == PIXFORMAT_RGB565 ? "RGB565" : "JPEG", esp_err_to_name(err));
        if (s_cam_mutex) {
            xSemaphoreGive(s_cam_mutex);
        }
        return err;
    }

    sensor_t *sensor = esp_camera_sensor_get();
    if (sensor) {
        ESP_LOGI(TAG, "Camera PID=0x%04x format=%s", sensor->id.PID,
                 fmt == PIXFORMAT_RGB565 ? "RGB565" : "JPEG");
        sensor->set_vflip(sensor, 1);
        sensor->set_hmirror(sensor, 0);
    }

    s_camera_ok = true;
    s_camera_pix_fmt = fmt;
    ESP_LOGI(TAG, "Camera ready");
    if (s_cam_mutex) {
        xSemaphoreGive(s_cam_mutex);
    }
    return ESP_OK;
}

static camera_fb_t *camera_capture(void)
{
    if (s_cam_mutex) {
        xSemaphoreTake(s_cam_mutex, portMAX_DELAY);
    }
    camera_fb_t *fb = s_camera_ok ? esp_camera_fb_get() : NULL;
    if (s_cam_mutex) {
        xSemaphoreGive(s_cam_mutex);
    }
    return fb;
}

static void camera_release(camera_fb_t *fb)
{
    if (!fb) {
        return;
    }
    if (s_cam_mutex) {
        xSemaphoreTake(s_cam_mutex, portMAX_DELAY);
    }
    esp_camera_fb_return(fb);
    if (s_cam_mutex) {
        xSemaphoreGive(s_cam_mutex);
    }
}

static esp_err_t capture_send_jpeg(httpd_req_t *req, const camera_fb_t *fb)
{
    uint8_t *jpg_buf = NULL;
    size_t jpg_len = 0;
    vision_stats_t stats = {0};
    esp_err_t enc = vision_encode_capture_jpeg(fb, CAM_JPEG_QUALITY, &jpg_buf, &jpg_len, &stats);
    if (enc != ESP_OK || !jpg_buf || jpg_len == 0) {
        ESP_LOGW(TAG, "capture encode failed: %s", esp_err_to_name(enc));
        free(jpg_buf);
        return ESP_FAIL;
    }

    set_no_cache(req);
    httpd_resp_set_type(req, "image/jpeg");
    if (vision_local_enabled()) {
        vision_add_capture_headers(req, &stats);
    }
    esp_err_t res = httpd_resp_send(req, (const char *)jpg_buf, jpg_len);
    free(jpg_buf);
    return res;
}

static camera_fb_t *camera_snapshot_copy(camera_fb_t *snap_out)
{
    camera_fb_t *fb = camera_capture();
    if (!fb) {
        return NULL;
    }

    *snap_out = *fb;
    if (fb->format != PIXFORMAT_RGB565 && fb->format != PIXFORMAT_JPEG) {
        camera_release(fb);
        return NULL;
    }

    uint8_t *copy = heap_caps_malloc(fb->len, MALLOC_CAP_SPIRAM | MALLOC_CAP_8BIT);
    if (!copy) {
        copy = malloc(fb->len);
    }
    if (!copy) {
        camera_release(fb);
        return NULL;
    }
    memcpy(copy, fb->buf, fb->len);
    snap_out->buf = copy;
    camera_release(fb);
    return snap_out;
}

static void camera_snapshot_free(camera_fb_t *snap)
{
    if (snap && snap->buf) {
        heap_caps_free(snap->buf);
        snap->buf = NULL;
    }
}

static void wifi_event_handler(void *arg, esp_event_base_t event_base,
                               int32_t event_id, void *event_data)
{
    if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_START) {
        esp_wifi_connect();
    } else if (event_base == WIFI_EVENT &&
               event_id == WIFI_EVENT_STA_DISCONNECTED) {
        if (s_retry_num < WIFI_MAX_RETRY) {
            esp_wifi_connect();
            s_retry_num++;
            ESP_LOGW(TAG, "Retry Wi‑Fi connect (%d/%d)", s_retry_num,
                     WIFI_MAX_RETRY);
        } else {
            xEventGroupSetBits(s_wifi_event_group, WIFI_FAIL_BIT);
        }
    } else if (event_base == IP_EVENT && event_id == IP_EVENT_STA_GOT_IP) {
        ip_event_got_ip_t *event = (ip_event_got_ip_t *)event_data;
        ESP_LOGI(TAG, "Got IP: " IPSTR, IP2STR(&event->ip_info.ip));
        ESP_LOGI(TAG, "Open http://" IPSTR "/", IP2STR(&event->ip_info.ip));
        s_retry_num = 0;
        xEventGroupSetBits(s_wifi_event_group, WIFI_CONNECTED_BIT);
    }
}

static void load_wifi_credentials(char *ssid, size_t ssid_len, char *pass, size_t pass_len)
{
    if (s_wifi_cred_from_i2c && s_wifi_ssid[0] != '\0') {
        strncpy(ssid, s_wifi_ssid, ssid_len - 1);
        ssid[ssid_len - 1] = '\0';
        strncpy(pass, s_wifi_pass, pass_len - 1);
        pass[pass_len - 1] = '\0';
        return;
    }

    strncpy(ssid, WIFI_SSID, ssid_len - 1);
    ssid[ssid_len - 1] = '\0';
    strncpy(pass, WIFI_PASS, pass_len - 1);
    pass[pass_len - 1] = '\0';

    nvs_handle_t nvs;
    if (nvs_open("wifi", NVS_READONLY, &nvs) != ESP_OK) {
        return;
    }
    size_t len = ssid_len;
    if (nvs_get_str(nvs, "ssid", ssid, &len) == ESP_OK) {
        ESP_LOGI(TAG, "Wi‑Fi SSID from NVS");
    }
    len = pass_len;
    if (nvs_get_str(nvs, "pass", pass, &len) != ESP_OK) {
        /* keep default pass if missing */
    }
    nvs_close(nvs);
}

static esp_err_t wifi_driver_init_once(void)
{
    if (s_wifi_netif_ready) {
        return ESP_OK;
    }

    s_wifi_event_group = xEventGroupCreate();
    ESP_RETURN_ON_ERROR(esp_netif_init(), TAG, "netif");
    ESP_RETURN_ON_ERROR(esp_event_loop_create_default(), TAG, "event loop");
    esp_netif_create_default_wifi_sta();

    wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
    ESP_RETURN_ON_ERROR(esp_wifi_init(&cfg), TAG, "wifi_init");
    ESP_RETURN_ON_ERROR(esp_event_handler_instance_register(
                            WIFI_EVENT, ESP_EVENT_ANY_ID, &wifi_event_handler, NULL, NULL),
                        TAG, "wifi event");
    ESP_RETURN_ON_ERROR(esp_event_handler_instance_register(
                            IP_EVENT, IP_EVENT_STA_GOT_IP, &wifi_event_handler, NULL, NULL),
                        TAG, "ip event");
    s_wifi_netif_ready = true;
    return ESP_OK;
}

static esp_err_t wifi_start_sta(void)
{
    ESP_RETURN_ON_ERROR(wifi_driver_init_once(), TAG, "wifi_driver_init_once");

    char ssid[33];
    char pass[65];
    load_wifi_credentials(ssid, sizeof(ssid), pass, sizeof(pass));

    wifi_config_t wifi_config = {0};
    strncpy((char *)wifi_config.sta.ssid, ssid, sizeof(wifi_config.sta.ssid));
    strncpy((char *)wifi_config.sta.password, pass, sizeof(wifi_config.sta.password));
    wifi_config.sta.threshold.authmode = WIFI_AUTH_WPA2_PSK;

    s_retry_num = 0;
    xEventGroupClearBits(s_wifi_event_group, WIFI_CONNECTED_BIT | WIFI_FAIL_BIT);

    ESP_RETURN_ON_ERROR(esp_wifi_set_mode(WIFI_MODE_STA), TAG, "set_mode");
    ESP_RETURN_ON_ERROR(esp_wifi_set_config(WIFI_IF_STA, &wifi_config), TAG, "set_config");
    ESP_RETURN_ON_ERROR(esp_wifi_start(), TAG, "wifi_start");
    ESP_RETURN_ON_ERROR(esp_wifi_set_ps(WIFI_PS_NONE), TAG, "set_ps");

    eye_i2c_load_wifi_bufs(ssid, pass);
    ESP_LOGI(TAG, "Connecting to Wi‑Fi SSID:%s ...", ssid);

    EventBits_t bits = xEventGroupWaitBits(
        s_wifi_event_group, WIFI_CONNECTED_BIT | WIFI_FAIL_BIT, pdFALSE, pdFALSE,
        pdMS_TO_TICKS(20000));

    if (bits & WIFI_CONNECTED_BIT) {
        s_wifi_driver_up = true;
        return ESP_OK;
    }
    ESP_LOGW(TAG, "Wi‑Fi not connected (timeout/fail) — check WIFI_SSID / WIFI_PASS");
    s_wifi_driver_up = true; /* driver running; STATUS_WIFI stays clear until IP */
    return ESP_ERR_TIMEOUT;
}

static void wifi_stop_sta(void)
{
    if (s_httpd) {
        httpd_stop(s_httpd);
        s_httpd = NULL;
        ESP_LOGI(TAG, "HTTP server stopped");
    }
    if (s_wifi_netif_ready) {
        esp_wifi_stop();
        s_retry_num = 0;
        if (s_wifi_event_group) {
            xEventGroupClearBits(s_wifi_event_group, WIFI_CONNECTED_BIT | WIFI_FAIL_BIT);
        }
    }
    s_wifi_driver_up = false;
    ESP_LOGI(TAG, "Wi‑Fi stopped");
}

static void set_no_cache(httpd_req_t *req)
{
    httpd_resp_set_hdr(req, "Cache-Control", "no-store, no-cache, must-revalidate");
    httpd_resp_set_hdr(req, "Pragma", "no-cache");
    httpd_resp_set_hdr(req, "Access-Control-Allow-Origin", "*");
}

static esp_err_t index_handler(httpd_req_t *req)
{
    set_no_cache(req);
    httpd_resp_set_type(req, "text/html");
    const size_t len = (size_t)(index_html_end - index_html_start);
    return httpd_resp_send(req, (const char *)index_html_start, len);
}

static esp_err_t capture_handler(httpd_req_t *req)
{
    camera_fb_t snap = {0};
    if (!camera_snapshot_copy(&snap)) {
        httpd_resp_send_500(req);
        return ESP_FAIL;
    }

    esp_err_t res = capture_send_jpeg(req, &snap);
    camera_snapshot_free(&snap);
    return res;
}

static esp_err_t stream_handler(httpd_req_t *req)
{
    esp_err_t res = ESP_OK;
    char part_buf[64];
    const int64_t frame_period_us = 1000000 / STREAM_FPS;

    res = httpd_resp_set_type(req, STREAM_CONTENT_TYPE);
    if (res != ESP_OK) {
        return res;
    }
    set_no_cache(req);

    while (true) {
        int64_t frame_start = esp_timer_get_time();

        camera_fb_t snap = {0};
        if (!camera_snapshot_copy(&snap)) {
            ESP_LOGE(TAG, "Camera capture failed");
            res = ESP_FAIL;
            break;
        }

        uint8_t *jpg_buf = NULL;
        size_t jpg_len = 0;
        if (vision_encode_capture_jpeg(&snap, CAM_JPEG_QUALITY, &jpg_buf, &jpg_len, NULL) != ESP_OK ||
            !jpg_buf) {
            camera_snapshot_free(&snap);
            res = ESP_FAIL;
            break;
        }
        camera_snapshot_free(&snap);

        res = httpd_resp_send_chunk(req, STREAM_BOUNDARY, strlen(STREAM_BOUNDARY));
        if (res == ESP_OK) {
            int hlen = snprintf(part_buf, sizeof(part_buf), STREAM_PART, (unsigned)jpg_len);
            if (hlen < 0 || hlen >= (int)sizeof(part_buf)) {
                res = ESP_FAIL;
            } else {
                res = httpd_resp_send_chunk(req, part_buf, (size_t)hlen);
            }
        }
        if (res == ESP_OK) {
            res = httpd_resp_send_chunk(req, (const char *)jpg_buf, jpg_len);
        }
        free(jpg_buf);

        if (res != ESP_OK) {
            break;
        }

        int64_t elapsed = esp_timer_get_time() - frame_start;
        int64_t sleep_us = frame_period_us - elapsed;
        if (sleep_us > 1000) {
            vTaskDelay(pdMS_TO_TICKS((uint32_t)(sleep_us / 1000)));
        }
    }

    return res;
}

static esp_err_t send_json_ack(httpd_req_t *req, const robot_ack_t *ack, bool as_error)
{
    char json[384];
    int n = robot_link_format_ack(ack, json, sizeof(json));
    if (n < 0 || n >= (int)sizeof(json)) {
        httpd_resp_send_500(req);
        return ESP_FAIL;
    }

    set_no_cache(req);
    httpd_resp_set_type(req, "application/json");
    if (as_error) {
        httpd_resp_set_status(req, HTTPD_400);
    }
    return httpd_resp_send(req, json, n);
}

/* Match "key": so short keys like "h" do not hit "h_tol". */
static const char *json_find_key(const char *json, const char *key)
{
    char pattern[40];
    snprintf(pattern, sizeof(pattern), "\"%s\":", key);
    return strstr(json, pattern);
}

static bool json_get_string(const char *json, const char *key, char *out, size_t outlen)
{
    const char *p = json_find_key(json, key);
    if (!p) {
        return false;
    }
    p = strchr(p, ':');
    if (!p) {
        return false;
    }
    p++;
    while (*p == ' ' || *p == '\t') {
        p++;
    }
    if (*p != '"') {
        return false;
    }
    p++;
    size_t i = 0;
    while (*p && *p != '"' && i + 1 < outlen) {
        if (*p == '\\' && p[1]) {
            p++;
        }
        out[i++] = *p++;
    }
    out[i] = '\0';
    return i > 0 || (p && *p == '"');
}

static bool json_get_bool(const char *json, const char *key, bool *out)
{
    const char *p = json_find_key(json, key);
    if (!p) {
        return false;
    }
    p = strchr(p, ':');
    if (!p) {
        return false;
    }
    p++;
    while (*p == ' ' || *p == '\t') {
        p++;
    }
    if (strncmp(p, "true", 4) == 0) {
        *out = true;
        return true;
    }
    if (strncmp(p, "false", 5) == 0) {
        *out = false;
        return true;
    }
    return false;
}

static bool json_get_int(const char *json, const char *key, int *out)
{
    const char *p = json_find_key(json, key);
    if (!p) {
        return false;
    }
    p = strchr(p, ':');
    if (!p) {
        return false;
    }
    p++;
    while (*p == ' ' || *p == '\t') {
        p++;
    }
    char *end = NULL;
    long v = strtol(p, &end, 10);
    if (end == p) {
        return false;
    }
    *out = (int)v;
    return true;
}

static esp_err_t ws_handler(httpd_req_t *req)
{
    if (req->method == HTTP_GET) {
        ESP_LOGI(TAG, "WebSocket /ws connected");
        return ESP_OK;
    }

    httpd_ws_frame_t ws_pkt;
    memset(&ws_pkt, 0, sizeof(ws_pkt));
    ws_pkt.type = HTTPD_WS_TYPE_TEXT;

    esp_err_t ret = httpd_ws_recv_frame(req, &ws_pkt, 0);
    if (ret != ESP_OK) {
        return ret;
    }
    if (ws_pkt.len == 0 || ws_pkt.len > 512) {
        return ESP_OK;
    }

    uint8_t *buf = calloc(1, ws_pkt.len + 1);
    if (!buf) {
        return ESP_ERR_NO_MEM;
    }
    ws_pkt.payload = buf;
    ret = httpd_ws_recv_frame(req, &ws_pkt, ws_pkt.len);
    if (ret != ESP_OK) {
        free(buf);
        return ret;
    }

    char type[16] = {0};
    char dir[16] = {0};
    char text[256] = {0};
    robot_ack_t ack;
    memset(&ack, 0, sizeof(ack));

    json_get_string((const char *)buf, "type", type, sizeof(type));
    if (strcmp(type, "cmd") == 0) {
        json_get_string((const char *)buf, "dir", dir, sizeof(dir));
        robot_link_handle_cmd(dir, &ack);
    } else if (strcmp(type, "msg") == 0) {
        json_get_string((const char *)buf, "text", text, sizeof(text));
        robot_link_handle_msg(text, strlen(text), &ack);
    } else if (strcmp(type, "vision") == 0) {
        char json[512];
        bool do_get = false;
        (void)json_get_bool((const char *)buf, "get", &do_get);
        if (do_get) {
            /* Read-only: never touch camera format or params. */
            vision_format_ack(json, sizeof(json));
            free(buf);
            httpd_ws_frame_t reply = {
                .final = true,
                .fragmented = false,
                .type = HTTPD_WS_TYPE_TEXT,
                .payload = (uint8_t *)json,
                .len = strlen(json),
            };
            return httpd_ws_send_frame(req, &reply);
        }

        bool local = false;
        bool show_mask = false;
        bool has_local = json_get_bool((const char *)buf, "local", &local);
        bool has_show_mask = json_get_bool((const char *)buf, "show_mask", &show_mask);
        char method[16] = {0};
        bool has_method = json_get_string((const char *)buf, "method", method, sizeof(method));
        int h = 0, s = 0, v = 0, h_tol = 0, s_tol = 0, v_tol = 0;
        int b = 0, g = 0, r = 0;
        int pick_x = 0, pick_y = 0;
        int coarse_stride = 0, min_area = 0;
        bool mask_full = false, morph_roi = false;
        bool has_h = json_get_int((const char *)buf, "h", &h);
        bool has_s = json_get_int((const char *)buf, "s", &s);
        bool has_v = json_get_int((const char *)buf, "v", &v);
        bool has_h_tol = json_get_int((const char *)buf, "h_tol", &h_tol);
        bool has_s_tol = json_get_int((const char *)buf, "s_tol", &s_tol);
        bool has_v_tol = json_get_int((const char *)buf, "v_tol", &v_tol);
        bool has_b = json_get_int((const char *)buf, "b", &b);
        bool has_g = json_get_int((const char *)buf, "g", &g);
        bool has_r = json_get_int((const char *)buf, "r", &r);
        bool has_pick_x = json_get_int((const char *)buf, "pick_x", &pick_x);
        bool has_pick_y = json_get_int((const char *)buf, "pick_y", &pick_y);
        bool has_coarse_stride = json_get_int((const char *)buf, "coarse_stride", &coarse_stride);
        bool has_min_area = json_get_int((const char *)buf, "min_area", &min_area);
        bool has_mask_full = json_get_bool((const char *)buf, "mask_full", &mask_full);
        bool has_morph_roi = json_get_bool((const char *)buf, "morph_roi", &morph_roi);

        bool target_local = has_local ? local : vision_local_enabled();
        pixformat_t want_fmt = target_local ? PIXFORMAT_RGB565 : PIXFORMAT_JPEG;
        if (want_fmt != s_camera_pix_fmt) {
            ESP_LOGI(TAG, "Switching camera for local vision -> %s",
                     want_fmt == PIXFORMAT_RGB565 ? "RGB565" : "JPEG");
            esp_err_t cam_err = init_camera_with_format(want_fmt);
            if (cam_err != ESP_OK) {
                snprintf(
                    json, sizeof(json),
                    "{\"ok\":false,\"type\":\"vision\",\"error\":\"camera_reinit\"}");
                free(buf);
                httpd_ws_frame_t reply = {
                    .final = true,
                    .fragmented = false,
                    .type = HTTPD_WS_TYPE_TEXT,
                    .payload = (uint8_t *)json,
                    .len = strlen(json),
                };
                return httpd_ws_send_frame(req, &reply);
            }
        }

        if (target_local && has_pick_x && has_pick_y) {
            has_h = false;
            has_s = false;
            has_v = false;
        }

        vision_handle_update(
            has_local, local,
            has_show_mask, show_mask,
            has_method, method,
            has_h, h, has_s, s, has_v, v,
            has_h_tol, h_tol, has_s_tol, s_tol, has_v_tol, v_tol,
            has_b, b, has_g, g, has_r, r,
            has_coarse_stride, coarse_stride,
            has_min_area, min_area,
            has_mask_full, mask_full,
            has_morph_roi, morph_roi,
            json, sizeof(json));

        if (target_local && has_pick_x && has_pick_y) {
            camera_fb_t snap = {0};
            if (camera_snapshot_copy(&snap)) {
                if (vision_sample_hsv_from_frame(&snap, pick_x, pick_y) == ESP_OK) {
                    vision_format_ack(json, sizeof(json));
                }
                camera_snapshot_free(&snap);
            }
        }

        free(buf);
        httpd_ws_frame_t reply = {
            .final = true,
            .fragmented = false,
            .type = HTTPD_WS_TYPE_TEXT,
            .payload = (uint8_t *)json,
            .len = strlen(json),
        };
        return httpd_ws_send_frame(req, &reply);
    } else {
        ack.ok = false;
        ack.seq = 0;
        ack.type = "cmd";
        ack.fwd = "none";
        ack.error = "bad_type";
    }

    char json[384];
    int n = robot_link_format_ack(&ack, json, sizeof(json));
    free(buf);
    if (n < 0) {
        return ESP_FAIL;
    }

    httpd_ws_frame_t reply = {
        .final = true,
        .fragmented = false,
        .type = HTTPD_WS_TYPE_TEXT,
        .payload = (uint8_t *)json,
        .len = (size_t)n,
    };
    return httpd_ws_send_frame(req, &reply);
}

static esp_err_t cmd_handler(httpd_req_t *req)
{
    char query[64] = {0};
    char dir[16] = {0};

    if (httpd_req_get_url_query_str(req, query, sizeof(query)) == ESP_OK) {
        httpd_query_key_value(query, "dir", dir, sizeof(dir));
    }

    robot_ack_t ack;
    esp_err_t err = robot_link_handle_cmd(dir, &ack);
    return send_json_ack(req, &ack, err != ESP_OK);
}

static esp_err_t msg_handler(httpd_req_t *req)
{
    if (req->content_len <= 0 || req->content_len > 512) {
        robot_ack_t ack = {
            .ok = false,
            .seq = 0,
            .type = "msg",
            .fwd = "none",
            .error = "bad_length",
        };
        return send_json_ack(req, &ack, true);
    }

    char buf[513];
    int received = httpd_req_recv(req, buf, req->content_len);
    if (received <= 0) {
        httpd_resp_send_500(req);
        return ESP_FAIL;
    }
    buf[received] = '\0';

    robot_ack_t ack;
    esp_err_t err = robot_link_handle_msg(buf, (size_t)received, &ack);
    return send_json_ack(req, &ack, err != ESP_OK);
}

static esp_err_t vision_handler(httpd_req_t *req)
{
    camera_fb_t snap = {0};
    if (!camera_snapshot_copy(&snap)) {
        httpd_resp_send_500(req);
        return ESP_FAIL;
    }

    vision_stats_t stats = {0};
    esp_err_t err = vision_detect_on_frame(&snap, &stats);
    camera_snapshot_free(&snap);
    if (err != ESP_OK) {
        httpd_resp_send_500(req);
        return ESP_FAIL;
    }

    char json[320];
    vision_format_stats_json(&stats, json, sizeof(json));
    set_no_cache(req);
    httpd_resp_set_type(req, "application/json");
    return httpd_resp_send(req, json, HTTPD_RESP_USE_STRLEN);
}

static esp_err_t vision_settings_handler(httpd_req_t *req)
{
    char json[512];
    vision_format_ack(json, sizeof(json));
    set_no_cache(req);
    httpd_resp_set_type(req, "application/json");
    return httpd_resp_send(req, json, HTTPD_RESP_USE_STRLEN);
}

static httpd_handle_t start_webserver(void)
{
    if (s_httpd) {
        return s_httpd;
    }

    httpd_config_t config = HTTPD_DEFAULT_CONFIG();
    config.server_port = 80;
    config.lru_purge_enable = true;
    config.stack_size = 20480;
    config.max_uri_handlers = 12;
    config.max_resp_headers = 16;

    httpd_handle_t server = NULL;
    if (httpd_start(&server, &config) != ESP_OK) {
        ESP_LOGE(TAG, "Failed to start HTTP server");
        return NULL;
    }

    const httpd_uri_t index_uri = {
        .uri = "/", .method = HTTP_GET, .handler = index_handler};
    const httpd_uri_t capture_uri = {
        .uri = "/capture", .method = HTTP_GET, .handler = capture_handler};
    const httpd_uri_t vision_uri = {
        .uri = "/vision", .method = HTTP_GET, .handler = vision_handler};
    const httpd_uri_t vision_settings_uri = {
        .uri = "/vision/settings", .method = HTTP_GET, .handler = vision_settings_handler};
    const httpd_uri_t stream_uri = {
        .uri = "/stream", .method = HTTP_GET, .handler = stream_handler};
    const httpd_uri_t cmd_uri = {
        .uri = "/cmd", .method = HTTP_GET, .handler = cmd_handler};
    const httpd_uri_t msg_uri = {
        .uri = "/msg", .method = HTTP_POST, .handler = msg_handler};
    const httpd_uri_t ws_uri = {
        .uri = "/ws",
        .method = HTTP_GET,
        .handler = ws_handler,
        .is_websocket = true,
    };

    httpd_register_uri_handler(server, &index_uri);
    httpd_register_uri_handler(server, &capture_uri);
    httpd_register_uri_handler(server, &vision_uri);
    httpd_register_uri_handler(server, &vision_settings_uri);
    httpd_register_uri_handler(server, &stream_uri);
    httpd_register_uri_handler(server, &cmd_uri);
    httpd_register_uri_handler(server, &msg_uri);
    httpd_register_uri_handler(server, &ws_uri);
    ESP_LOGI(TAG, "HTTP+WS server started (/ws for commands)");
    s_httpd = server;
    return server;
}

esp_err_t board_camera_set(bool enable)
{
    s_want_camera = enable;
    return ESP_OK;
}

esp_err_t board_wifi_set(bool enable)
{
    s_want_wifi = enable;
    return ESP_OK;
}

esp_err_t board_wifi_set_credentials(const char *ssid, const char *password)
{
    if (!ssid || ssid[0] == '\0') {
        return ESP_ERR_INVALID_ARG;
    }
    memset(s_wifi_ssid, 0, sizeof(s_wifi_ssid));
    memset(s_wifi_pass, 0, sizeof(s_wifi_pass));
    strncpy(s_wifi_ssid, ssid, sizeof(s_wifi_ssid) - 1);
    if (password) {
        strncpy(s_wifi_pass, password, sizeof(s_wifi_pass) - 1);
    }
    s_wifi_cred_from_i2c = true;
    if (s_wifi_driver_up) {
        s_wifi_restart = true;
    }

    /* Persist so a later CMD 7 / reboot path can reuse them */
    nvs_handle_t nvs;
    if (nvs_open("wifi", NVS_READWRITE, &nvs) == ESP_OK) {
        (void)nvs_set_str(nvs, "ssid", s_wifi_ssid);
        (void)nvs_set_str(nvs, "pass", s_wifi_pass);
        (void)nvs_commit(nvs);
        nvs_close(nvs);
    }
    return ESP_OK;
}

esp_err_t board_camera_sync_format(void)
{
    if (!s_camera_ok) {
        return ESP_ERR_INVALID_STATE;
    }
    pixformat_t want = vision_local_enabled() ? PIXFORMAT_RGB565 : PIXFORMAT_JPEG;
    if (want == s_camera_pix_fmt) {
        return ESP_OK;
    }
    ESP_LOGI(TAG, "Switching camera format -> %s",
             want == PIXFORMAT_RGB565 ? "RGB565" : "JPEG");
    return init_camera_with_format(want);
}

bool board_camera_is_on(void)
{
    return s_camera_ok;
}

bool board_wifi_is_on(void)
{
    return s_wifi_driver_up;
}

static void apply_camera_want(void)
{
    if (s_want_camera && !s_camera_ok) {
        /* RGB565 so I2C vision works without Wi‑Fi */
        if (init_camera_with_format(PIXFORMAT_RGB565) != ESP_OK) {
            ESP_LOGE(TAG, "Camera enable failed");
            s_want_camera = false;
        }
    } else if (!s_want_camera && s_camera_ok) {
        if (s_cam_mutex) {
            xSemaphoreTake(s_cam_mutex, portMAX_DELAY);
        }
        esp_camera_deinit();
        s_camera_ok = false;
        if (s_cam_mutex) {
            xSemaphoreGive(s_cam_mutex);
        }
        ESP_LOGI(TAG, "Camera off");
    }
    /* Format switches only via board_camera_sync_format() on FLAGS/WS — not every poll. */
}

static void apply_wifi_want(void)
{
    if (s_wifi_restart && s_wifi_driver_up) {
        ESP_LOGI(TAG, "Restarting Wi‑Fi with new credentials");
        wifi_stop_sta();
        s_wifi_restart = false;
    }
    if (s_want_wifi && !s_wifi_driver_up) {
        esp_err_t err = wifi_start_sta();
        if (err == ESP_OK || err == ESP_ERR_TIMEOUT) {
            if (!s_httpd) {
                start_webserver();
            }
        } else {
            ESP_LOGE(TAG, "Wi‑Fi enable failed: %s", esp_err_to_name(err));
            s_want_wifi = false;
        }
    } else if (!s_want_wifi && s_wifi_driver_up) {
        wifi_stop_sta();
    }
}

static void power_task(void *arg)
{
    (void)arg;
    while (true) {
        apply_camera_want();
        apply_wifi_want();
        vTaskDelay(pdMS_TO_TICKS(50));
    }
}

static void vision_loop_task(void *arg)
{
    (void)arg;
    while (true) {
        if (!s_camera_ok || !vision_local_enabled()) {
            vTaskDelay(pdMS_TO_TICKS(40));
            continue;
        }

        camera_fb_t snap = {0};
        if (camera_snapshot_copy(&snap)) {
            vision_stats_t stats = {0};
            (void)vision_detect_on_frame(&snap, &stats);
            camera_snapshot_free(&snap);
        }
        /* Detection dominates period; short yield for I2C/HTTP */
        vTaskDelay(pdMS_TO_TICKS(5));
    }
}

void app_main(void)
{
    esp_err_t ret = nvs_flash_init();
    if (ret == ESP_ERR_NVS_NO_FREE_PAGES ||
        ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
        ESP_ERROR_CHECK(nvs_flash_erase());
        ESP_ERROR_CHECK(nvs_flash_init());
    }

    s_cam_mutex = xSemaphoreCreateMutex();
    if (status_led_init() != ESP_OK) {
        ESP_LOGW(TAG, "Status LED init failed");
    }
    vision_init();
    robot_link_init();
    eye_i2c_set_cam_mutex(s_cam_mutex);
    if (eye_i2c_init() != ESP_OK) {
        ESP_LOGW(TAG, "I2C slave init failed");
    }

    xTaskCreate(power_task, "eye_power", 8192, NULL, 5, NULL);
    xTaskCreate(vision_loop_task, "eye_vision", 8192, NULL, 4, NULL);

    ESP_LOGI(TAG, "Boot: camera+Wi‑Fi off — I2C CMD 5=cam on, 6=off, 7=wifi on, 8=off");
}