#include "eye_i2c.h"

#include <string.h>

#include "board_svc.h"
#include "driver/i2c_slave.h"
#include "esp_camera.h"
#include "esp_check.h"
#include "esp_log.h"
#include "esp_netif.h"
#include "esp_timer.h"
#include "esp_wifi.h"
#include "freertos/FreeRTOS.h"
#include "freertos/queue.h"
#include "freertos/semphr.h"
#include "freertos/task.h"
#include "hal/i2c_ll.h"
#include "hal/i2c_types.h"
#include "nvs.h"
#include "nvs_flash.h"
#include "robot_link.h"
#include "soc/soc_caps.h"
#include "vision.h"

static const char *TAG = "eye_i2c";

#define REG_WHOAMI       0x00
#define REG_VERSION      0x01
#define REG_STATUS       0x02
#define REG_FLAGS        0x03
#define REG_METHOD       0x04
#define REG_CMD          0x05
#define REG_STRIDE       0x06
#define REG_MIN_AREA_L   0x07
#define REG_MIN_AREA_H   0x08
#define REG_H            0x09
#define REG_S            0x0A
#define REG_V            0x0B
#define REG_H_TOL        0x0C
#define REG_S_TOL        0x0D
#define REG_V_TOL        0x0E
#define REG_APPLY        0x0F
#define REG_FOUND        0x10
#define REG_CX_L         0x11
#define REG_CY_L         0x13
#define REG_RADIUS_L     0x15
#define REG_AREA_L       0x17
#define REG_TOTAL_MS_L   0x19
#define REG_DETECT_MS_L  0x1B
#define REG_SCRATCH0     0x1D
#define REG_SCRATCH1     0x1E
#define REG_SCRATCH2     0x1F
#define REG_WIFI_CRED    0x20
#define REG_IP0          0x21
#define REG_IP1          0x22
#define REG_IP2          0x23
#define REG_IP3          0x24

#define WHOAMI_VALUE     0xA5
#define VERSION_VALUE    0x07

#define FLAG_LOCAL       (1u << 0)
#define FLAG_SHOW_MASK   (1u << 1)
#define FLAG_MASK_FULL   (1u << 2)
#define FLAG_MORPH_ROI   (1u << 3)

#define STATUS_WIFI      (1u << 0)
#define STATUS_CAM       (1u << 1)
#define STATUS_LOCAL     (1u << 2)
#define STATUS_FOUND     (1u << 3)

#define APPLY_BGR        (1u << 0)
#define APPLY_PICK       (1u << 1)

#define CMD_STOP         0
#define CMD_UP           1
#define CMD_DOWN         2
#define CMD_LEFT         3
#define CMD_RIGHT        4
#define CMD_CAM_ON       5
#define CMD_CAM_OFF      6
#define CMD_WIFI_ON      7
#define CMD_WIFI_OFF     8

#define MAX_BLOB_AREA    20000
#define MAX_BLOB_RADIUS  100
#define EYE_I2C_RX_MAX   128

#if !CONFIG_I2C_ENABLE_SLAVE_DRIVER_VERSION_2
#error "eye_i2c requires CONFIG_I2C_ENABLE_SLAVE_DRIVER_VERSION_2 (FIFO callbacks)."
#endif

typedef enum {
    EVT_RX = 1,
    EVT_PTR = 2,
} eye_evt_t;

typedef struct {
    uint8_t data[EYE_I2C_RX_MAX];
    uint32_t len;
} eye_rx_pkt_t;

typedef struct {
    i2c_slave_dev_handle_t handle;
    uint8_t wr[EYE_I2C_RAM_SIZE];
    QueueHandle_t evt_q;
    QueueHandle_t rx_q;
    bool device_up;
} eye_i2c_ctx_t;

static eye_i2c_ctx_t s_ctx;
static SemaphoreHandle_t s_cam_mutex_ext;

/* Double-buffered publish image; cursor set in on_receive ISR. */
static uint8_t s_snap[2][EYE_I2C_RAM_SIZE];
static volatile uint8_t s_snap_idx;
static volatile uint8_t s_cursor;
static i2c_dev_t *s_i2c_hw;
static volatile uint32_t s_ptr_count;
static volatile uint32_t s_req_count;
static volatile uint8_t s_last_ptr;
static volatile uint8_t s_last_preload[4];

void eye_i2c_set_cam_mutex(void *mutex)
{
    s_cam_mutex_ext = (SemaphoreHandle_t)mutex;
}

void eye_i2c_load_wifi_bufs(const char *ssid, const char *pass)
{
    (void)ssid;
    (void)pass;
}

static void put_u16(uint8_t *p, uint16_t v)
{
    p[0] = (uint8_t)(v & 0xFF);
    p[1] = (uint8_t)((v >> 8) & 0xFF);
}

static uint16_t get_u16(const uint8_t *p)
{
    return (uint16_t)p[0] | ((uint16_t)p[1] << 8);
}

static bool wifi_has_ip(void)
{
    esp_netif_t *netif = esp_netif_get_handle_from_ifkey("WIFI_STA_DEF");
    if (!netif) {
        return false;
    }
    esp_netif_ip_info_t info;
    if (esp_netif_get_ip_info(netif, &info) != ESP_OK) {
        return false;
    }
    return info.ip.addr != 0;
}

static void wifi_fill_ip(uint8_t out[4])
{
    out[0] = out[1] = out[2] = out[3] = 0;
    esp_netif_t *netif = esp_netif_get_handle_from_ifkey("WIFI_STA_DEF");
    if (!netif) {
        return;
    }
    esp_netif_ip_info_t info;
    if (esp_netif_get_ip_info(netif, &info) != ESP_OK || info.ip.addr == 0) {
        return;
    }
    uint32_t addr = info.ip.addr; /* network byte order on ESP */
    out[0] = (uint8_t)(addr & 0xFF);
    out[1] = (uint8_t)((addr >> 8) & 0xFF);
    out[2] = (uint8_t)((addr >> 16) & 0xFF);
    out[3] = (uint8_t)((addr >> 24) & 0xFF);
}

static void publish_regs(const uint8_t *tmp)
{
    uint8_t next = (uint8_t)(1u - s_snap_idx);
    memcpy(s_snap[next], tmp, EYE_I2C_RAM_SIZE);
    s_snap_idx = next;
}

static void refresh_regs(void)
{
    uint8_t tmp[EYE_I2C_RAM_SIZE];
    memset(tmp, 0, sizeof(tmp));

    tmp[REG_WHOAMI] = WHOAMI_VALUE;
    tmp[REG_VERSION] = VERSION_VALUE;

    uint8_t flags = 0;
    if (vision_local_enabled()) {
        flags |= FLAG_LOCAL;
    }
    if (vision_show_mask()) {
        flags |= FLAG_SHOW_MASK;
    }
    if (vision_get_mask_full()) {
        flags |= FLAG_MASK_FULL;
    }
    if (vision_get_morph_roi()) {
        flags |= FLAG_MORPH_ROI;
    }
    tmp[REG_FLAGS] = flags;
    tmp[REG_METHOD] = (vision_get_method() == VISION_METHOD_RECT) ? 1 : 0;
    tmp[REG_STRIDE] = (uint8_t)vision_get_coarse_stride();
    put_u16(&tmp[REG_MIN_AREA_L], (uint16_t)vision_get_min_area());

    uint8_t h, s, v, ht, st, vt;
    vision_get_hsv(&h, &s, &v);
    vision_get_tolerance(&ht, &st, &vt);
    tmp[REG_H] = h;
    tmp[REG_S] = s;
    tmp[REG_V] = v;
    tmp[REG_H_TOL] = ht;
    tmp[REG_S_TOL] = st;
    tmp[REG_V_TOL] = vt;
    tmp[REG_CMD] = 0xFF;
    tmp[REG_APPLY] = 0;

    vision_stats_t stt;
    vision_get_stats(&stt);
    bool found = stt.found;
    if (found && (stt.area > MAX_BLOB_AREA || stt.radius > MAX_BLOB_RADIUS)) {
        found = false;
    }
    tmp[REG_FOUND] = found ? 1 : 0;
    if (found) {
        put_u16(&tmp[REG_CX_L], (uint16_t)(int16_t)(stt.cx + (stt.cx >= 0 ? 0.5f : -0.5f)));
        put_u16(&tmp[REG_CY_L], (uint16_t)(int16_t)(stt.cy + (stt.cy >= 0 ? 0.5f : -0.5f)));
        put_u16(&tmp[REG_RADIUS_L], (uint16_t)(stt.radius + 0.5f));
        uint16_t area = stt.area > 65535 ? 65535 : (uint16_t)stt.area;
        put_u16(&tmp[REG_AREA_L], area);
    }
    put_u16(&tmp[REG_TOTAL_MS_L], (uint16_t)(stt.total_us / 1000u));
    put_u16(&tmp[REG_DETECT_MS_L], (uint16_t)(stt.detect_us / 1000u));

    uint8_t status = 0;
    if (wifi_has_ip()) {
        status |= STATUS_WIFI;
    }
    if (board_camera_is_on()) {
        status |= STATUS_CAM;
    }
    if (vision_local_enabled()) {
        status |= STATUS_LOCAL;
    }
    if (found) {
        status |= STATUS_FOUND;
    }
    tmp[REG_STATUS] = status;

    uint8_t ip[4];
    wifi_fill_ip(ip);
    tmp[REG_IP0] = ip[0];
    tmp[REG_IP1] = ip[1];
    tmp[REG_IP2] = ip[2];
    tmp[REG_IP3] = ip[3];

    /* Preserve writable scratch from host */
    tmp[REG_SCRATCH0] = s_ctx.wr[REG_SCRATCH0];
    tmp[REG_SCRATCH1] = s_ctx.wr[REG_SCRATCH1];
    tmp[REG_SCRATCH2] = s_ctx.wr[REG_SCRATCH2];

    publish_regs(tmp);
}

static void do_cmd(uint8_t code)
{
    const char *dir = NULL;
    switch (code) {
    case CMD_STOP:
        dir = "stop";
        break;
    case CMD_UP:
        dir = "up";
        break;
    case CMD_DOWN:
        dir = "down";
        break;
    case CMD_LEFT:
        dir = "left";
        break;
    case CMD_RIGHT:
        dir = "right";
        break;
    case CMD_CAM_ON:
        ESP_LOGI(TAG, "CMD cam on");
        (void)board_camera_set(true);
        return;
    case CMD_CAM_OFF:
        ESP_LOGI(TAG, "CMD cam off");
        (void)board_camera_set(false);
        return;
    case CMD_WIFI_ON:
        ESP_LOGI(TAG, "CMD wifi on");
        (void)board_wifi_set(true);
        return;
    case CMD_WIFI_OFF:
        ESP_LOGI(TAG, "CMD wifi off");
        (void)board_wifi_set(false);
        return;
    default:
        return;
    }
    robot_ack_t ack;
    robot_link_handle_cmd(dir, &ack);
}

static esp_err_t do_pick(int x, int y)
{
    if (s_cam_mutex_ext &&
        xSemaphoreTake(s_cam_mutex_ext, pdMS_TO_TICKS(200)) != pdTRUE) {
        return ESP_ERR_TIMEOUT;
    }
    camera_fb_t *fb = esp_camera_fb_get();
    esp_err_t err = ESP_FAIL;
    if (fb) {
        err = vision_sample_hsv_from_frame(fb, x, y);
        esp_camera_fb_return(fb);
    }
    if (s_cam_mutex_ext) {
        xSemaphoreGive(s_cam_mutex_ext);
    }
    return err;
}

static void apply_write(uint8_t reg, const uint8_t *data, uint32_t len)
{
    if (reg >= EYE_I2C_RAM_SIZE || len == 0) {
        return;
    }
    if (reg + len > EYE_I2C_RAM_SIZE) {
        len = EYE_I2C_RAM_SIZE - reg;
    }

    uint8_t before[EYE_I2C_RAM_SIZE];
    memcpy(before, s_ctx.wr, EYE_I2C_RAM_SIZE);

    for (uint32_t i = 0; i < len; i++) {
        s_ctx.wr[reg + i] = data[i];
    }

    const uint8_t *rx = s_ctx.wr;
    const uint8_t *prev = before;

    if (reg <= REG_FLAGS && reg + len > REG_FLAGS && rx[REG_FLAGS] != prev[REG_FLAGS]) {
        uint8_t f = rx[REG_FLAGS];
        vision_set_local_enabled((f & FLAG_LOCAL) != 0);
        vision_set_show_mask((f & FLAG_SHOW_MASK) != 0);
        vision_set_mask_full((f & FLAG_MASK_FULL) != 0);
        vision_set_morph_roi((f & FLAG_MORPH_ROI) != 0);
        (void)board_camera_sync_format();
    }
    if (reg <= REG_METHOD && reg + len > REG_METHOD && rx[REG_METHOD] != prev[REG_METHOD]) {
        vision_set_method(rx[REG_METHOD] == 1 ? VISION_METHOD_RECT : VISION_METHOD_CONTOUR);
    }
    if (reg <= REG_CMD && reg + len > REG_CMD && rx[REG_CMD] != prev[REG_CMD] &&
        rx[REG_CMD] != 0xFF) {
        do_cmd(rx[REG_CMD]);
    }
    if (reg <= REG_STRIDE && reg + len > REG_STRIDE && rx[REG_STRIDE] != prev[REG_STRIDE]) {
        vision_set_coarse_stride(rx[REG_STRIDE]);
    }
    if ((reg <= REG_MIN_AREA_H && reg + len > REG_MIN_AREA_L) &&
        (rx[REG_MIN_AREA_L] != prev[REG_MIN_AREA_L] || rx[REG_MIN_AREA_H] != prev[REG_MIN_AREA_H])) {
        vision_set_min_area((int)get_u16(&rx[REG_MIN_AREA_L]));
    }
    if ((reg <= REG_V && reg + len > REG_H) &&
        (rx[REG_H] != prev[REG_H] || rx[REG_S] != prev[REG_S] || rx[REG_V] != prev[REG_V])) {
        vision_set_hsv(rx[REG_H], rx[REG_S], rx[REG_V]);
    }
    if ((reg <= REG_V_TOL && reg + len > REG_H_TOL) &&
        (rx[REG_H_TOL] != prev[REG_H_TOL] || rx[REG_S_TOL] != prev[REG_S_TOL] ||
         rx[REG_V_TOL] != prev[REG_V_TOL])) {
        vision_set_tolerance(rx[REG_H_TOL], rx[REG_S_TOL], rx[REG_V_TOL]);
    }
    if (reg <= REG_APPLY && reg + len > REG_APPLY && rx[REG_APPLY] != 0 &&
        rx[REG_APPLY] != prev[REG_APPLY]) {
        if (rx[REG_APPLY] & APPLY_BGR) {
            vision_set_color_bgr(rx[REG_SCRATCH0], rx[REG_SCRATCH1], rx[REG_SCRATCH2]);
        }
        if (rx[REG_APPLY] & APPLY_PICK) {
            int x = (int)get_u16(&rx[REG_SCRATCH0]);
            int y = rx[REG_SCRATCH2];
            (void)do_pick(x, y);
        }
        s_ctx.wr[REG_APPLY] = 0;
    }

    ESP_LOGI(TAG, "WR reg=0x%02x len=%lu", reg, (unsigned long)len);
    refresh_regs();
}

static void apply_wifi_cred(const uint8_t *data, uint32_t len)
{
    /* Payload: [ssid_len][ssid…][pass_len][pass…] */
    if (!data || len < 2) {
        ESP_LOGW(TAG, "WIFI_CRED short (%lu)", (unsigned long)len);
        return;
    }
    uint8_t ssid_len = data[0];
    if (ssid_len == 0 || ssid_len > 32 || (uint32_t)(1 + ssid_len) >= len) {
        ESP_LOGW(TAG, "WIFI_CRED bad ssid_len=%u len=%lu", ssid_len, (unsigned long)len);
        return;
    }
    uint8_t pass_len = data[1 + ssid_len];
    if ((uint32_t)(1 + ssid_len + 1 + pass_len) > len || pass_len > 64) {
        ESP_LOGW(TAG, "WIFI_CRED bad pass_len=%u len=%lu", pass_len, (unsigned long)len);
        return;
    }
    char ssid[33];
    char pass[65];
    memset(ssid, 0, sizeof(ssid));
    memset(pass, 0, sizeof(pass));
    memcpy(ssid, &data[1], ssid_len);
    memcpy(pass, &data[1 + ssid_len + 1], pass_len);
    ESP_LOGI(TAG, "WIFI_CRED ssid='%s' (pass %u bytes) — enabling", ssid, pass_len);
    (void)board_wifi_set_credentials(ssid, pass);
    (void)board_wifi_set(true);
}

static void IRAM_ATTR preload_tx_fifo(void)
{
    if (!s_i2c_hw) {
        return;
    }
    uint8_t c = s_cursor;
    if (c >= EYE_I2C_RAM_SIZE) {
        c = 0;
    }
    uint8_t n = (uint8_t)(EYE_I2C_RAM_SIZE - c);
    if (n > SOC_I2C_FIFO_LEN) {
        n = SOC_I2C_FIFO_LEN;
    }
    uint8_t idx = s_snap_idx;
    i2c_ll_slave_disable_tx_it(s_i2c_hw);
    i2c_ll_txfifo_rst(s_i2c_hw);
    i2c_ll_write_txfifo(s_i2c_hw, &s_snap[idx][c], n);
    /* Snapshot what we loaded for task-context logging. */
    s_last_preload[0] = s_snap[idx][c];
    s_last_preload[1] = (n > 1) ? s_snap[idx][c + 1] : 0;
    s_last_preload[2] = (n > 2) ? s_snap[idx][c + 2] : 0;
    s_last_preload[3] = (n > 3) ? s_snap[idx][c + 3] : 0;
}

static bool IRAM_ATTR on_receive_cb(i2c_slave_dev_handle_t i2c_slave,
                                    const i2c_slave_rx_done_event_data_t *evt, void *arg)
{
    (void)i2c_slave;
    (void)arg;
    if (!evt || !evt->buffer || evt->length == 0 || evt->length > EYE_I2C_RX_MAX) {
        return false;
    }

    s_cursor = evt->buffer[0];
    s_last_ptr = s_cursor;

    if (evt->length == 1) {
        /*
         * Register pointer write (start of a split read). RP2040 cannot clock-
         * stretch, so the TX FIFO must already hold the reply before readfrom().
         */
        preload_tx_fifo();
        s_ptr_count++;
        BaseType_t woke = pdFALSE;
        eye_evt_t ev = EVT_PTR;
        xQueueSendFromISR(s_ctx.evt_q, &ev, &woke);
        return woke == pdTRUE;
    }

    eye_rx_pkt_t pkt = {0};
    pkt.len = evt->length;
    memcpy(pkt.data, evt->buffer, evt->length);
    BaseType_t woke = pdFALSE;
    if (xQueueSendFromISR(s_ctx.rx_q, &pkt, &woke) != pdTRUE) {
        return false;
    }
    eye_evt_t ev = EVT_RX;
    xQueueSendFromISR(s_ctx.evt_q, &ev, &woke);
    return woke == pdTRUE;
}

static bool IRAM_ATTR on_request_cb(i2c_slave_dev_handle_t i2c_slave,
                                    const i2c_slave_request_event_data_t *evt, void *arg)
{
    (void)i2c_slave;
    (void)evt;
    (void)arg;
    s_req_count++;
    preload_tx_fifo();
    if (s_i2c_hw) {
        i2c_ll_slave_clear_stretch(s_i2c_hw);
    }
    return false;
}

static esp_err_t slave_hw_start(void)
{
    s_i2c_hw = I2C_LL_GET_HW(I2C_NUM_1);

    i2c_slave_config_t cfg = {
        .i2c_port = I2C_NUM_1,
        .sda_io_num = EYE_I2C_SDA_GPIO,
        .scl_io_num = EYE_I2C_SCL_GPIO,
        .clk_source = I2C_CLK_SRC_DEFAULT,
        .send_buf_depth = 256,
        .receive_buf_depth = 256,
        .slave_addr = EYE_I2C_ADDR,
        .addr_bit_len = I2C_ADDR_BIT_LEN_7,
        .intr_priority = 0,
        .flags =
            {
                .enable_internal_pullup = false,
            },
    };

    esp_err_t err = i2c_new_slave_device(&cfg, &s_ctx.handle);
    if (err != ESP_OK) {
        ESP_LOGE(TAG, "i2c_new_slave_device: %s", esp_err_to_name(err));
        return err;
    }

    /* Pico/RP2040 master I2C does not reliably support slave clock stretching. */
    i2c_ll_slave_enable_scl_stretch(s_i2c_hw, false);
    i2c_ll_slave_clear_stretch(s_i2c_hw);

    i2c_slave_event_callbacks_t cbs = {
        .on_receive = on_receive_cb,
        .on_request = on_request_cb,
    };
    err = i2c_slave_register_event_callbacks(s_ctx.handle, &cbs, NULL);
    if (err != ESP_OK) {
        i2c_del_slave_device(s_ctx.handle);
        s_ctx.handle = NULL;
        return err;
    }

    s_ctx.device_up = true;
    refresh_regs();
    ESP_LOGI(TAG, "v2 slave up addr=0x%02X SDA=%d SCL=%d (no-stretch, preload TX)", EYE_I2C_ADDR,
             EYE_I2C_SDA_GPIO, EYE_I2C_SCL_GPIO);
    return ESP_OK;
}

static void eye_i2c_task(void *arg)
{
    (void)arg;
    while (true) {
        eye_evt_t ev;
        if (xQueueReceive(s_ctx.evt_q, &ev, pdMS_TO_TICKS(40)) != pdTRUE) {
            refresh_regs();
            continue;
        }
        do {
            if (ev == EVT_RX) {
                eye_rx_pkt_t pkt;
                while (xQueueReceive(s_ctx.rx_q, &pkt, 0) == pdTRUE) {
                    if (pkt.len >= 2 && pkt.data[0] == REG_WIFI_CRED) {
                        apply_wifi_cred(&pkt.data[1], pkt.len - 1);
                    } else if (pkt.len >= 2) {
                        apply_write(pkt.data[0], &pkt.data[1], pkt.len - 1);
                    }
                }
            } else if (ev == EVT_PTR) {
                ESP_LOGI(TAG, "PTR=0x%02x preload=%02x %02x %02x %02x (ptr_n=%lu req_n=%lu)",
                         s_last_ptr, s_last_preload[0], s_last_preload[1], s_last_preload[2],
                         s_last_preload[3], (unsigned long)s_ptr_count, (unsigned long)s_req_count);
            }
        } while (xQueueReceive(s_ctx.evt_q, &ev, 0) == pdTRUE);
    }
}

esp_err_t eye_i2c_init(void)
{
    memset(&s_ctx, 0, sizeof(s_ctx));
    memset(s_snap, 0, sizeof(s_snap));
    s_ctx.wr[REG_FLAGS] = FLAG_MORPH_ROI;
    s_ctx.wr[REG_STRIDE] = 4;
    s_ctx.wr[REG_CMD] = 0xFF;
    put_u16(&s_ctx.wr[REG_MIN_AREA_L], 80);
    uint8_t h, s, v, ht, st, vt;
    vision_get_hsv(&h, &s, &v);
    vision_get_tolerance(&ht, &st, &vt);
    s_ctx.wr[REG_H] = h;
    s_ctx.wr[REG_S] = s;
    s_ctx.wr[REG_V] = v;
    s_ctx.wr[REG_H_TOL] = ht;
    s_ctx.wr[REG_S_TOL] = st;
    s_ctx.wr[REG_V_TOL] = vt;

    s_ctx.evt_q = xQueueCreate(16, sizeof(eye_evt_t));
    s_ctx.rx_q = xQueueCreate(8, sizeof(eye_rx_pkt_t));
    if (!s_ctx.evt_q || !s_ctx.rx_q) {
        return ESP_ERR_NO_MEM;
    }

    ESP_RETURN_ON_ERROR(slave_hw_start(), TAG, "slave_hw_start failed");

    BaseType_t ok = xTaskCreate(eye_i2c_task, "eye_i2c", 6144, NULL, 12, NULL);
    if (ok != pdPASS) {
        i2c_del_slave_device(s_ctx.handle);
        return ESP_ERR_NO_MEM;
    }
    return ESP_OK;
}