/*
 * cmd_canbus.c
 * CAN bus module — Full command set: sniff, inject, UDS, OBD-II, fuzzing, replay.
 *
 * Frame format streamed to C2: "CAN|<ts_ms>|<id_hex>|<dlc>|<data_hex>"
 */
#include "sdkconfig.h"

#ifdef CONFIG_MODULE_CANBUS

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

#include "esp_log.h"
#include "esp_timer.h"

#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/semphr.h"

#include "utils.h"
#include "canbus_driver.h"
#include "canbus_config.h"

#ifdef CONFIG_CANBUS_UDS
#include "canbus_uds.h"
#endif

#ifdef CONFIG_CANBUS_OBD
#include "canbus_obd.h"
#endif

#ifdef CONFIG_CANBUS_FUZZ
#include "canbus_fuzz.h"
#endif

#ifdef CONFIG_CANBUS_ISO_TP
#include "canbus_isotp.h"
#endif

#define TAG "CAN_CMD"

/* ============================================================
 * Ring Buffer for recording
 * ============================================================ */
#define RECORD_MAX  CONFIG_CANBUS_RECORD_BUFFER

static can_frame_t s_record_buf[RECORD_MAX];
static int         s_record_head  = 0;
static int         s_record_count = 0;
static SemaphoreHandle_t s_record_mutex = NULL;

/* ============================================================
 * Sniff / Record state
 * ============================================================ */
static volatile bool s_sniff_active  = false;
static volatile bool s_record_active = false;
static const char   *s_sniff_req_id  = NULL;    /* Request ID for streaming */

/* Software filters (loaded from NVS at init) */
static uint32_t s_sw_filters[CAN_CFG_MAX_SW_FILTERS];
static int      s_sw_filter_count = 0;

/* ============================================================
 * Helpers
 * ============================================================ */

/* Check if a frame passes software filters (empty = accept all) */
static bool frame_passes_filter(const can_frame_t *frame)
{
    if (s_sw_filter_count == 0) return true;
    for (int i = 0; i < s_sw_filter_count; i++) {
        if (s_sw_filters[i] == frame->id) return true;
    }
    return false;
}

/* Format a CAN frame for C2 transmission */
static int format_frame(const can_frame_t *frame, char *buf, size_t buf_len)
{
    int64_t ts_ms = frame->timestamp_us / 1000;

    int off = snprintf(buf, buf_len, "CAN|%" PRId64 "|%03lX|%u|",
                       ts_ms,
                       (unsigned long)frame->id,
                       frame->dlc);

    for (int i = 0; i < frame->dlc && off < (int)buf_len - 2; i++) {
        off += snprintf(buf + off, buf_len - off, "%02X", frame->data[i]);
    }

    return off;
}

/* Reload software filters from NVS */
static void reload_sw_filters(void)
{
    s_sw_filter_count = can_config_get_filters(
        s_sw_filters, CAN_CFG_MAX_SW_FILTERS
    );
}

/* ============================================================
 * RX Callback — called from driver RX task
 * ============================================================ */

static void can_rx_callback(const can_frame_t *frame, void *ctx)
{
    (void)ctx;

    if (!frame_passes_filter(frame)) return;

    /* Sniff mode: stream to C2 */
    if (s_sniff_active) {
        char line[80];
        format_frame(frame, line, sizeof(line));
        msg_data(TAG, line, strlen(line), false, s_sniff_req_id);
    }

    /* Record mode: store in ring buffer */
    if (s_record_active && s_record_mutex) {
        xSemaphoreTake(s_record_mutex, portMAX_DELAY);
        s_record_buf[s_record_head] = *frame;
        s_record_head = (s_record_head + 1) % RECORD_MAX;
        if (s_record_count < RECORD_MAX) s_record_count++;
        xSemaphoreGive(s_record_mutex);
    }
}

/* ============================================================
 * COMMAND: can_start [bitrate] [mode]
 * ============================================================ */

static int cmd_can_start(int argc, char **argv, const char *req, void *ctx)
{
    (void)ctx;

    int bitrate = can_config_get_bitrate();
    uint8_t osc = can_config_get_osc_mhz();
    can_mode_t mode = CAN_MODE_NORMAL;

    if (argc >= 1) {
        bitrate = atoi(argv[0]);
    }
    if (argc >= 2) {
        if (strcmp(argv[1], "listen") == 0) {
            mode = CAN_MODE_LISTEN_ONLY;
        } else if (strcmp(argv[1], "loopback") == 0) {
            mode = CAN_MODE_LOOPBACK;
        }
    }

    if (can_driver_is_running()) {
        msg_error(TAG, "CAN already running — stop first", req);
        return -1;
    }

    if (!can_driver_init(bitrate, osc)) {
        msg_error(TAG, "MCP2515 init failed — check SPI wiring", req);
        return -1;
    }

    can_driver_set_rx_callback(can_rx_callback, NULL);

#ifdef CONFIG_CANBUS_ISO_TP
    isotp_install_hook();
#endif

    if (!can_driver_start(mode)) {
        msg_error(TAG, "CAN start failed", req);
        can_driver_deinit();
        return -1;
    }

    char resp[128];
    const char *mode_str = (mode == CAN_MODE_LISTEN_ONLY) ? "listen-only"
                         : (mode == CAN_MODE_LOOPBACK)    ? "loopback"
                         :                                   "normal";
    snprintf(resp, sizeof(resp), "CAN started: %d bps, %s mode, osc=%u MHz",
             bitrate, mode_str, osc);
    msg_info(TAG, resp, req);
    return 0;
}

/* ============================================================
 * COMMAND: can_stop
 * ============================================================ */

static int cmd_can_stop(int argc, char **argv, const char *req, void *ctx)
{
    (void)argc; (void)argv; (void)ctx;

    s_sniff_active  = false;
    s_record_active = false;

    can_driver_stop();
    can_driver_deinit();

    msg_info(TAG, "CAN stopped", req);
    return 0;
}

/* ============================================================
 * COMMAND: can_send <id_hex> <data_hex>
 * ============================================================ */

static int cmd_can_send(int argc, char **argv, const char *req, void *ctx)
{
    (void)ctx;

    if (!can_driver_is_running()) {
        msg_error(TAG, "CAN not running", req);
        return -1;
    }

    can_frame_t frame = { 0 };

    /* Parse ID */
    frame.id = strtoul(argv[0], NULL, 16);
    frame.extended = (frame.id > 0x7FF);

    /* Parse hex data */
    const char *hex = argv[1];
    size_t hex_len = strlen(hex);
    frame.dlc = hex_len / 2;
    if (frame.dlc > 8) frame.dlc = 8;

    for (int i = 0; i < frame.dlc; i++) {
        char byte_str[3] = { hex[i * 2], hex[i * 2 + 1], '\0' };
        frame.data[i] = (uint8_t)strtoul(byte_str, NULL, 16);
    }

    if (!can_driver_send(&frame)) {
        msg_error(TAG, "TX failed", req);
        return -1;
    }

    char resp[64];
    snprintf(resp, sizeof(resp), "TX: 0x%03lX [%u] %s",
             (unsigned long)frame.id, frame.dlc, argv[1]);
    msg_info(TAG, resp, req);
    return 0;
}

/* ============================================================
 * COMMAND: can_filter_add <id_hex> — software filter
 * ============================================================ */

static int cmd_can_filter_add(int argc, char **argv, const char *req, void *ctx)
{
    (void)ctx;

    uint32_t id = strtoul(argv[0], NULL, 16);
    esp_err_t err = can_config_add_filter(id);
    if (err != ESP_OK) {
        msg_error(TAG, "Filter add failed (max reached?)", req);
        return -1;
    }
    reload_sw_filters();

    char resp[64];
    snprintf(resp, sizeof(resp), "Filter added: 0x%03lX (%d total)",
             (unsigned long)id, s_sw_filter_count);
    msg_info(TAG, resp, req);
    return 0;
}

/* ============================================================
 * COMMAND: can_filter_del <id_hex>
 * ============================================================ */

static int cmd_can_filter_del(int argc, char **argv, const char *req, void *ctx)
{
    (void)ctx;

    uint32_t id = strtoul(argv[0], NULL, 16);
    esp_err_t err = can_config_del_filter(id);
    if (err != ESP_OK) {
        msg_error(TAG, "Filter not found", req);
        return -1;
    }
    reload_sw_filters();

    char resp[64];
    snprintf(resp, sizeof(resp), "Filter removed: 0x%03lX (%d remaining)",
             (unsigned long)id, s_sw_filter_count);
    msg_info(TAG, resp, req);
    return 0;
}

/* ============================================================
 * COMMAND: can_filter_list
 * ============================================================ */

static int cmd_can_filter_list(int argc, char **argv, const char *req, void *ctx)
{
    (void)argc; (void)argv; (void)ctx;

    if (s_sw_filter_count == 0) {
        msg_info(TAG, "No software filters (accepting all frames)", req);
        return 0;
    }

    char buf[256];
    int off = snprintf(buf, sizeof(buf), "Filters (%d): ", s_sw_filter_count);
    for (int i = 0; i < s_sw_filter_count && off < (int)sizeof(buf) - 8; i++) {
        off += snprintf(buf + off, sizeof(buf) - off, "0x%03lX ",
                        (unsigned long)s_sw_filters[i]);
    }
    msg_info(TAG, buf, req);
    return 0;
}

/* ============================================================
 * COMMAND: can_filter_clear
 * ============================================================ */

static int cmd_can_filter_clear(int argc, char **argv, const char *req, void *ctx)
{
    (void)argc; (void)argv; (void)ctx;

    can_config_clear_filters();
    reload_sw_filters();
    msg_info(TAG, "All filters cleared (accepting all frames)", req);
    return 0;
}

/* ============================================================
 * COMMAND: can_status
 * ============================================================ */

static int cmd_can_status(int argc, char **argv, const char *req, void *ctx)
{
    (void)argc; (void)argv; (void)ctx;

    can_status_t st;
    can_driver_get_status(&st);

    char buf[384];
    int off = snprintf(buf, sizeof(buf),
        "state=%s rx=%"PRIu32" tx=%"PRIu32" "
        "tec=%"PRIu32" rec=%"PRIu32" bus_err=%"PRIu32" "
        "overflow=%"PRIu32" sniff=%s record=%s (%d/%d)",
        st.state, st.rx_count, st.tx_count,
        st.tx_errors, st.rx_errors, st.bus_errors,
        st.rx_overflow,
        s_sniff_active  ? "on" : "off",
        s_record_active ? "on" : "off",
        s_record_count, RECORD_MAX);

    /* Append NVS config */
    off += snprintf(buf + off, sizeof(buf) - off, "\n");
    can_config_list(buf + off, sizeof(buf) - off);

    msg_info(TAG, buf, req);
    return 0;
}

/* ============================================================
 * COMMAND: can_sniff [duration_s] — async, streams frames to C2
 * ============================================================ */

static int cmd_can_sniff(int argc, char **argv, const char *req, void *ctx)
{
    (void)ctx;

    if (!can_driver_is_running()) {
        msg_error(TAG, "CAN not running", req);
        return -1;
    }

    int duration = 10;  /* default 10 seconds */
    if (argc >= 1) {
        duration = atoi(argv[0]);
        if (duration <= 0 || duration > 3600) duration = 10;
    }

    s_sniff_req_id = req;
    s_sniff_active = true;

    char start_msg[64];
    snprintf(start_msg, sizeof(start_msg), "Sniffing for %d seconds...", duration);
    msg_info(TAG, start_msg, req);

    vTaskDelay(pdMS_TO_TICKS(duration * 1000));

    s_sniff_active = false;
    s_sniff_req_id = NULL;

    msg_data(TAG, "SNIFF_END", 9, true, req);
    return 0;
}

/* ============================================================
 * COMMAND: can_record [duration_s] — async, stores in ring buffer
 * ============================================================ */

static int cmd_can_record(int argc, char **argv, const char *req, void *ctx)
{
    (void)ctx;

    if (!can_driver_is_running()) {
        msg_error(TAG, "CAN not running", req);
        return -1;
    }

    int duration = 10;
    if (argc >= 1) {
        duration = atoi(argv[0]);
        if (duration <= 0 || duration > 3600) duration = 10;
    }

    /* Reset buffer */
    if (s_record_mutex) xSemaphoreTake(s_record_mutex, portMAX_DELAY);
    s_record_head  = 0;
    s_record_count = 0;
    if (s_record_mutex) xSemaphoreGive(s_record_mutex);

    s_record_active = true;

    char start_msg[64];
    snprintf(start_msg, sizeof(start_msg), "Recording for %d seconds...", duration);
    msg_info(TAG, start_msg, req);

    vTaskDelay(pdMS_TO_TICKS(duration * 1000));

    s_record_active = false;

    char end_msg[64];
    snprintf(end_msg, sizeof(end_msg), "Recording complete: %d frames captured", s_record_count);
    msg_info(TAG, end_msg, req);

    return 0;
}

/* ============================================================
 * COMMAND: can_dump — async, sends recorded buffer to C2
 * ============================================================ */

static int cmd_can_dump(int argc, char **argv, const char *req, void *ctx)
{
    (void)argc; (void)argv; (void)ctx;

    if (s_record_count == 0) {
        msg_info(TAG, "No recorded frames", req);
        return 0;
    }

    if (s_record_mutex) xSemaphoreTake(s_record_mutex, portMAX_DELAY);

    int count = s_record_count;
    /* Calculate start index for ring buffer */
    int start = (count >= RECORD_MAX)
                ? s_record_head
                : 0;

    char header[64];
    snprintf(header, sizeof(header), "DUMP_START|%d", count);
    msg_data(TAG, header, strlen(header), false, req);

    char line[80];
    for (int i = 0; i < count; i++) {
        int idx = (start + i) % RECORD_MAX;
        format_frame(&s_record_buf[idx], line, sizeof(line));
        msg_data(TAG, line, strlen(line), false, req);

        /* Yield every 32 frames to avoid watchdog */
        if ((i & 0x1F) == 0x1F) {
            vTaskDelay(pdMS_TO_TICKS(1));
        }
    }

    if (s_record_mutex) xSemaphoreGive(s_record_mutex);

    msg_data(TAG, "DUMP_END", 8, true, req);
    return 0;
}

/* ============================================================
 * COMMAND: can_replay [speed_pct] — async, replays recorded buffer
 * ============================================================ */

static int cmd_can_replay(int argc, char **argv, const char *req, void *ctx)
{
    (void)ctx;

    if (!can_driver_is_running()) {
        msg_error(TAG, "CAN not running", req);
        return -1;
    }

    if (s_record_count == 0) {
        msg_info(TAG, "No recorded frames to replay", req);
        return 0;
    }

    int speed = 100;
    if (argc >= 1) {
        speed = atoi(argv[0]);
        if (speed < 0) speed = 0;
        if (speed > 1000) speed = 1000;
    }

    char start_msg[64];
    snprintf(start_msg, sizeof(start_msg), "Replaying %d frames at %d%% speed",
             s_record_count, speed);
    msg_info(TAG, start_msg, req);

    /* Build ordered frame array from ring buffer */
    int count = s_record_count;
    int start = (count >= RECORD_MAX) ? s_record_head : 0;

    /* Replay directly from the ring buffer (read-only during replay) */
    can_frame_t *ordered = malloc(count * sizeof(can_frame_t));
    if (!ordered) {
        msg_error(TAG, "Out of memory for replay", req);
        return -1;
    }

    if (s_record_mutex) xSemaphoreTake(s_record_mutex, portMAX_DELAY);
    for (int i = 0; i < count; i++) {
        ordered[i] = s_record_buf[(start + i) % RECORD_MAX];
    }
    if (s_record_mutex) xSemaphoreGive(s_record_mutex);

    can_driver_replay(ordered, count, speed);
    free(ordered);

    msg_info(TAG, "Replay complete", req);
    return 0;
}

/* ============================================================
 * COMMANDS: UDS (Phase 3) — guarded by CONFIG_CANBUS_UDS
 * ============================================================ */

#ifdef CONFIG_CANBUS_UDS

/* can_scan_ecu — discover ECUs on the bus */
static int cmd_can_scan_ecu(int argc, char **argv, const char *req, void *ctx)
{
    (void)argc; (void)argv; (void)ctx;

    if (!can_driver_is_running()) {
        msg_error(TAG, "CAN not running", req);
        return -1;
    }

    msg_info(TAG, "Scanning for ECUs...", req);

    uint32_t found[32];
    int count = uds_scan_ecus(found, 32);

    if (count == 0) {
        msg_info(TAG, "No ECUs found", req);
        return 0;
    }

    char buf[256];
    int off = snprintf(buf, sizeof(buf), "Found %d ECU(s): ", count);
    for (int i = 0; i < count && off < (int)sizeof(buf) - 12; i++) {
        off += snprintf(buf + off, sizeof(buf) - off, "0x%03lX ",
                        (unsigned long)found[i]);
        can_config_add_ecu(found[i]);
    }
    msg_info(TAG, buf, req);
    return 0;
}

/* can_uds <tx_id_hex> <service_hex> [data_hex] — raw UDS request */
static int cmd_can_uds(int argc, char **argv, const char *req, void *ctx)
{
    (void)ctx;

    if (!can_driver_is_running()) {
        msg_error(TAG, "CAN not running", req);
        return -1;
    }

    uint32_t tx_id = strtoul(argv[0], NULL, 16);
    uint32_t rx_id = tx_id + 0x08;

    /* Parse service + optional data as hex */
    const char *hex = argv[1];
    size_t hex_len = strlen(hex);
    uint8_t uds_req[64];
    size_t uds_len = hex_len / 2;
    if (uds_len > sizeof(uds_req)) uds_len = sizeof(uds_req);

    for (size_t i = 0; i < uds_len; i++) {
        char b[3] = { hex[i * 2], hex[i * 2 + 1], '\0' };
        uds_req[i] = (uint8_t)strtoul(b, NULL, 16);
    }

    /* Append extra data arg if present */
    if (argc >= 3) {
        const char *data_hex = argv[2];
        size_t dlen = strlen(data_hex) / 2;
        for (size_t i = 0; i < dlen && uds_len < sizeof(uds_req); i++) {
            char b[3] = { data_hex[i * 2], data_hex[i * 2 + 1], '\0' };
            uds_req[uds_len++] = (uint8_t)strtoul(b, NULL, 16);
        }
    }

    uds_ctx_t uctx = {
        .tx_id = tx_id, .rx_id = rx_id,
        .timeout_ms = 2000,
        .session = UDS_SESSION_DEFAULT,
    };

    uint8_t resp[512];
    size_t resp_len = 0;
    int ret = uds_raw_request(&uctx, uds_req, uds_len, resp, sizeof(resp), &resp_len);

    if (ret < 0) {
        msg_error(TAG, "UDS request failed or negative response", req);
        return -1;
    }

    /* Format response as hex */
    char out[256];
    int off = snprintf(out, sizeof(out), "UDS_RSP|0x%03lX|", (unsigned long)rx_id);
    for (size_t i = 0; i < resp_len && off < (int)sizeof(out) - 2; i++) {
        off += snprintf(out + off, sizeof(out) - off, "%02X", resp[i]);
    }
    msg_data(TAG, out, strlen(out), true, req);
    return 0;
}

/* can_uds_session <tx_id_hex> <session_type> */
static int cmd_can_uds_session(int argc, char **argv, const char *req, void *ctx)
{
    (void)ctx;

    uint32_t tx_id = strtoul(argv[0], NULL, 16);
    uint8_t session = (uint8_t)strtoul(argv[1], NULL, 16);

    uds_ctx_t uctx = {
        .tx_id = tx_id, .rx_id = tx_id + 0x08,
        .timeout_ms = 2000,
    };

    if (uds_diagnostic_session(&uctx, session) == 0) {
        char buf[64];
        snprintf(buf, sizeof(buf), "Session 0x%02X active on 0x%03lX",
                 session, (unsigned long)tx_id);
        msg_info(TAG, buf, req);
        return 0;
    }
    msg_error(TAG, "DiagnosticSessionControl failed", req);
    return -1;
}

/* can_uds_read <tx_id_hex> <did_hex> */
static int cmd_can_uds_read(int argc, char **argv, const char *req, void *ctx)
{
    (void)ctx;

    uint32_t tx_id = strtoul(argv[0], NULL, 16);
    uint16_t did = (uint16_t)strtoul(argv[1], NULL, 16);

    uds_ctx_t uctx = {
        .tx_id = tx_id, .rx_id = tx_id + 0x08,
        .timeout_ms = 2000,
    };

    uint8_t data[256];
    int len = uds_read_data_by_id(&uctx, did, data, sizeof(data));

    if (len < 0) {
        msg_error(TAG, "ReadDataByIdentifier failed", req);
        return -1;
    }

    char out[256];
    int off = snprintf(out, sizeof(out), "DID_%04X|", did);
    for (int i = 0; i < len && off < (int)sizeof(out) - 2; i++) {
        off += snprintf(out + off, sizeof(out) - off, "%02X", data[i]);
    }
    msg_data(TAG, out, strlen(out), true, req);
    return 0;
}

/* can_uds_dump <tx_id_hex> <addr_hex> <size_dec> — read memory */
static int cmd_can_uds_dump(int argc, char **argv, const char *req, void *ctx)
{
    (void)ctx;

    uint32_t tx_id = strtoul(argv[0], NULL, 16);
    uint32_t addr  = strtoul(argv[1], NULL, 16);
    uint16_t size  = (uint16_t)atoi(argv[2]);

    if (size == 0 || size > 4096) {
        msg_error(TAG, "Size must be 1-4096", req);
        return -1;
    }

    uds_ctx_t uctx = {
        .tx_id = tx_id, .rx_id = tx_id + 0x08,
        .timeout_ms = 3000,
    };

    /* Read in chunks of 256 bytes */
    char header[48];
    snprintf(header, sizeof(header), "MEM_DUMP|0x%08lX|%u", (unsigned long)addr, size);
    msg_data(TAG, header, strlen(header), false, req);

    uint32_t off = 0;
    while (off < size) {
        uint16_t chunk = (size - off > 256) ? 256 : (uint16_t)(size - off);
        uint8_t data[256];

        int len = uds_read_memory(&uctx, addr + off, chunk, data);
        if (len < 0) {
            char err_msg[48];
            snprintf(err_msg, sizeof(err_msg), "ReadMemory failed at 0x%08lX",
                     (unsigned long)(addr + off));
            msg_error(TAG, err_msg, req);
            return -1;
        }

        /* Send chunk as hex */
        char line[600];
        int loff = snprintf(line, sizeof(line), "MEM|%08lX|",
                            (unsigned long)(addr + off));
        for (int i = 0; i < len && loff < (int)sizeof(line) - 2; i++) {
            loff += snprintf(line + loff, sizeof(line) - loff, "%02X", data[i]);
        }
        msg_data(TAG, line, strlen(line), false, req);

        off += len;
        vTaskDelay(pdMS_TO_TICKS(10));
    }

    msg_data(TAG, "MEM_DUMP_END", 12, true, req);
    return 0;
}

/* can_uds_auth <tx_id_hex> [level] — SecurityAccess seed request */
static int cmd_can_uds_auth(int argc, char **argv, const char *req, void *ctx)
{
    (void)ctx;

    uint32_t tx_id = strtoul(argv[0], NULL, 16);
    uint8_t level = (argc >= 2) ? (uint8_t)strtoul(argv[1], NULL, 16) : 0x01;

    uds_ctx_t uctx = {
        .tx_id = tx_id, .rx_id = tx_id + 0x08,
        .timeout_ms = 2000,
    };

    uint8_t seed[32];
    size_t seed_len = 0;

    if (uds_security_access_seed(&uctx, level, seed, &seed_len) < 0) {
        msg_error(TAG, "SecurityAccess seed request failed", req);
        return -1;
    }

    char out[96];
    int off = snprintf(out, sizeof(out), "SA_SEED|L%02X|", level);
    for (size_t i = 0; i < seed_len && off < (int)sizeof(out) - 2; i++) {
        off += snprintf(out + off, sizeof(out) - off, "%02X", seed[i]);
    }
    msg_data(TAG, out, strlen(out), true, req);
    return 0;
}

#endif /* CONFIG_CANBUS_UDS */

/* ============================================================
 * COMMANDS: OBD-II (Phase 4) — guarded by CONFIG_CANBUS_OBD
 * ============================================================ */

#ifdef CONFIG_CANBUS_OBD

/* can_obd <pid_hex> — query a single OBD-II PID */
static int cmd_can_obd(int argc, char **argv, const char *req, void *ctx)
{
    (void)ctx;

    if (!can_driver_is_running()) {
        msg_error(TAG, "CAN not running", req);
        return -1;
    }

    uint8_t pid = (uint8_t)strtoul(argv[0], NULL, 16);
    obd_result_t result;

    if (obd_query_pid(0x01, pid, &result) < 0) {
        msg_error(TAG, "OBD query failed", req);
        return -1;
    }

    char out[96];
    snprintf(out, sizeof(out), "OBD|%s|%.1f|%s",
             result.name, result.value, result.unit);
    msg_data(TAG, out, strlen(out), true, req);
    return 0;
}

/* can_obd_vin — read VIN */
static int cmd_can_obd_vin(int argc, char **argv, const char *req, void *ctx)
{
    (void)argc; (void)argv; (void)ctx;

    if (!can_driver_is_running()) {
        msg_error(TAG, "CAN not running", req);
        return -1;
    }

    char vin[20] = { 0 };
    if (obd_read_vin(vin, sizeof(vin)) < 0) {
        msg_error(TAG, "VIN read failed", req);
        return -1;
    }

    char out[48];
    snprintf(out, sizeof(out), "VIN|%s", vin);
    msg_data(TAG, out, strlen(out), true, req);
    return 0;
}

/* can_obd_dtc — read diagnostic trouble codes */
static int cmd_can_obd_dtc(int argc, char **argv, const char *req, void *ctx)
{
    (void)argc; (void)argv; (void)ctx;

    if (!can_driver_is_running()) {
        msg_error(TAG, "CAN not running", req);
        return -1;
    }

    char dtc_buf[256];
    obd_read_dtcs(dtc_buf, sizeof(dtc_buf));
    msg_data(TAG, dtc_buf, strlen(dtc_buf), true, req);
    return 0;
}

/* can_obd_supported — list supported PIDs */
static int cmd_can_obd_supported(int argc, char **argv, const char *req, void *ctx)
{
    (void)argc; (void)argv; (void)ctx;

    if (!can_driver_is_running()) {
        msg_error(TAG, "CAN not running", req);
        return -1;
    }

    uint8_t pids[128];
    int count = obd_query_supported(pids, 128);

    if (count <= 0) {
        msg_info(TAG, "No supported PIDs found", req);
        return 0;
    }

    char buf[512];
    int off = snprintf(buf, sizeof(buf), "Supported PIDs (%d): ", count);
    for (int i = 0; i < count && off < (int)sizeof(buf) - 6; i++) {
        off += snprintf(buf + off, sizeof(buf) - off, "%02X ", pids[i]);
    }
    msg_data(TAG, buf, strlen(buf), true, req);
    return 0;
}

/* can_obd_monitor <pid_hex,...> [interval_ms] — continuous monitoring */
static int cmd_can_obd_monitor(int argc, char **argv, const char *req, void *ctx)
{
    (void)ctx;

    if (!can_driver_is_running()) {
        msg_error(TAG, "CAN not running", req);
        return -1;
    }

    /* Parse comma-separated PID list */
    uint8_t pids[16];
    int pid_count = 0;

    char pid_arg[64];
    strncpy(pid_arg, argv[0], sizeof(pid_arg) - 1);
    pid_arg[sizeof(pid_arg) - 1] = '\0';

    char *saveptr = NULL;
    char *token = strtok_r(pid_arg, ",", &saveptr);
    while (token && pid_count < 16) {
        pids[pid_count++] = (uint8_t)strtoul(token, NULL, 16);
        token = strtok_r(NULL, ",", &saveptr);
    }

    int interval = (argc >= 2) ? atoi(argv[1]) : 1000;
    if (interval < 100) interval = 100;

    obd_monitor_start(pids, pid_count, interval, req);

    char start_msg[64];
    snprintf(start_msg, sizeof(start_msg), "OBD monitor: %d PIDs, %d ms interval",
             pid_count, interval);
    msg_info(TAG, start_msg, req);

    /* Block until monitor stops (or we could let it run indefinitely) */
    while (obd_monitor_is_running()) {
        vTaskDelay(pdMS_TO_TICKS(1000));
    }

    return 0;
}

/* can_obd_monitor_stop — stop continuous monitoring */
static int cmd_can_obd_monitor_stop(int argc, char **argv, const char *req, void *ctx)
{
    (void)argc; (void)argv; (void)ctx;
    obd_monitor_stop();
    msg_info(TAG, "OBD monitor stopped", req);
    return 0;
}

#endif /* CONFIG_CANBUS_OBD */

/* ============================================================
 * COMMANDS: Fuzzing (Phase 5) — guarded by CONFIG_CANBUS_FUZZ
 * ============================================================ */

#ifdef CONFIG_CANBUS_FUZZ

/* can_fuzz_id [start_hex] [end_hex] [delay_ms] */
static int cmd_can_fuzz_id(int argc, char **argv, const char *req, void *ctx)
{
    (void)ctx;

    if (!can_driver_is_running()) {
        msg_error(TAG, "CAN not running", req);
        return -1;
    }

    fuzz_config_t cfg = {
        .mode = FUZZ_MODE_ID_SCAN,
        .id_start = (argc >= 1) ? strtoul(argv[0], NULL, 16) : 0x000,
        .id_end   = (argc >= 2) ? strtoul(argv[1], NULL, 16) : 0x7FF,
        .delay_ms = (argc >= 3) ? atoi(argv[2]) : 5,
        .seed_dlc = 8,
    };
    memset(cfg.seed_data, 0x00, 8);

    if (!can_fuzz_start(&cfg, req)) {
        msg_error(TAG, "Failed to start ID fuzz", req);
        return -1;
    }

    msg_info(TAG, "ID scan fuzz started", req);
    while (can_fuzz_is_running()) {
        vTaskDelay(pdMS_TO_TICKS(500));
    }
    return 0;
}

/* can_fuzz_data <id_hex> [seed_hex] [delay_ms] */
static int cmd_can_fuzz_data(int argc, char **argv, const char *req, void *ctx)
{
    (void)ctx;

    if (!can_driver_is_running()) {
        msg_error(TAG, "CAN not running", req);
        return -1;
    }

    fuzz_config_t cfg = {
        .mode = FUZZ_MODE_DATA_MUTATE,
        .target_id = strtoul(argv[0], NULL, 16),
        .delay_ms  = (argc >= 3) ? atoi(argv[2]) : 2,
        .seed_dlc  = 8,
    };
    memset(cfg.seed_data, 0x00, 8);

    if (argc >= 2) {
        const char *hex = argv[1];
        size_t hlen = strlen(hex);
        cfg.seed_dlc = hlen / 2;
        if (cfg.seed_dlc > 8) cfg.seed_dlc = 8;
        for (int i = 0; i < cfg.seed_dlc; i++) {
            char b[3] = { hex[i * 2], hex[i * 2 + 1], '\0' };
            cfg.seed_data[i] = (uint8_t)strtoul(b, NULL, 16);
        }
    }

    if (!can_fuzz_start(&cfg, req)) {
        msg_error(TAG, "Failed to start data fuzz", req);
        return -1;
    }

    msg_info(TAG, "Data mutation fuzz started", req);
    while (can_fuzz_is_running()) {
        vTaskDelay(pdMS_TO_TICKS(500));
    }
    return 0;
}

/* can_fuzz_random [delay_ms] [count] */
static int cmd_can_fuzz_random(int argc, char **argv, const char *req, void *ctx)
{
    (void)ctx;

    if (!can_driver_is_running()) {
        msg_error(TAG, "CAN not running", req);
        return -1;
    }

    fuzz_config_t cfg = {
        .mode = FUZZ_MODE_RANDOM,
        .delay_ms = (argc >= 1) ? atoi(argv[0]) : 5,
        .max_iterations = (argc >= 2) ? atoi(argv[1]) : 10000,
    };

    if (!can_fuzz_start(&cfg, req)) {
        msg_error(TAG, "Failed to start random fuzz", req);
        return -1;
    }

    msg_info(TAG, "Random fuzz started", req);
    while (can_fuzz_is_running()) {
        vTaskDelay(pdMS_TO_TICKS(500));
    }
    return 0;
}

/* can_fuzz_stop */
static int cmd_can_fuzz_stop_cmd(int argc, char **argv, const char *req, void *ctx)
{
    (void)argc; (void)argv; (void)ctx;
    can_fuzz_stop();
    msg_info(TAG, "Fuzzing stopped", req);
    return 0;
}

#endif /* CONFIG_CANBUS_FUZZ */

/* ============================================================
 * Command Table
 * ============================================================ */

static const command_t can_cmds[] = {
    {
        .name     = "can_start",
        .sub      = NULL,
        .help     = "Init MCP2515 + start CAN: [bitrate] [normal|listen|loopback]",
        .min_args = 0,
        .max_args = 2,
        .handler  = (command_handler_t)cmd_can_start,
        .ctx      = NULL,
        .async    = false,
    },
    {
        .name     = "can_stop",
        .sub      = NULL,
        .help     = "Stop CAN bus + deinit MCP2515",
        .min_args = 0,
        .max_args = 0,
        .handler  = (command_handler_t)cmd_can_stop,
        .ctx      = NULL,
        .async    = false,
    },
    {
        .name     = "can_send",
        .sub      = NULL,
        .help     = "Send CAN frame: <id_hex> <data_hex>",
        .min_args = 2,
        .max_args = 2,
        .handler  = (command_handler_t)cmd_can_send,
        .ctx      = NULL,
        .async    = false,
    },
    {
        .name     = "can_filter_add",
        .sub      = NULL,
        .help     = "Add software filter: <id_hex>",
        .min_args = 1,
        .max_args = 1,
        .handler  = (command_handler_t)cmd_can_filter_add,
        .ctx      = NULL,
        .async    = false,
    },
    {
        .name     = "can_filter_del",
        .sub      = NULL,
        .help     = "Remove software filter: <id_hex>",
        .min_args = 1,
        .max_args = 1,
        .handler  = (command_handler_t)cmd_can_filter_del,
        .ctx      = NULL,
        .async    = false,
    },
    {
        .name     = "can_filter_list",
        .sub      = NULL,
        .help     = "List active software filters",
        .min_args = 0,
        .max_args = 0,
        .handler  = (command_handler_t)cmd_can_filter_list,
        .ctx      = NULL,
        .async    = false,
    },
    {
        .name     = "can_filter_clear",
        .sub      = NULL,
        .help     = "Clear all software filters (accept all)",
        .min_args = 0,
        .max_args = 0,
        .handler  = (command_handler_t)cmd_can_filter_clear,
        .ctx      = NULL,
        .async    = false,
    },
    {
        .name     = "can_status",
        .sub      = NULL,
        .help     = "CAN bus status + config",
        .min_args = 0,
        .max_args = 0,
        .handler  = (command_handler_t)cmd_can_status,
        .ctx      = NULL,
        .async    = false,
    },
    {
        .name     = "can_sniff",
        .sub      = NULL,
        .help     = "Stream CAN frames to C2: [duration_s]",
        .min_args = 0,
        .max_args = 1,
        .handler  = (command_handler_t)cmd_can_sniff,
        .ctx      = NULL,
        .async    = true,
    },
    {
        .name     = "can_record",
        .sub      = NULL,
        .help     = "Record CAN frames locally: [duration_s]",
        .min_args = 0,
        .max_args = 1,
        .handler  = (command_handler_t)cmd_can_record,
        .ctx      = NULL,
        .async    = true,
    },
    {
        .name     = "can_dump",
        .sub      = NULL,
        .help     = "Send recorded CAN frames to C2",
        .min_args = 0,
        .max_args = 0,
        .handler  = (command_handler_t)cmd_can_dump,
        .ctx      = NULL,
        .async    = true,
    },
    {
        .name     = "can_replay",
        .sub      = NULL,
        .help     = "Replay recorded frames on bus: [speed_pct]",
        .min_args = 0,
        .max_args = 1,
        .handler  = (command_handler_t)cmd_can_replay,
        .ctx      = NULL,
        .async    = true,
    },

    /* --- UDS commands (Phase 3) --- */
#ifdef CONFIG_CANBUS_UDS
    {
        .name     = "can_scan_ecu",
        .sub      = NULL,
        .help     = "Scan for UDS ECUs (0x7E0-0x7EF + 0x700-0x7DF)",
        .min_args = 0,
        .max_args = 0,
        .handler  = (command_handler_t)cmd_can_scan_ecu,
        .ctx      = NULL,
        .async    = true,
    },
    {
        .name     = "can_uds",
        .sub      = NULL,
        .help     = "Raw UDS request: <tx_id> <service_hex> [data_hex]",
        .min_args = 2,
        .max_args = 3,
        .handler  = (command_handler_t)cmd_can_uds,
        .ctx      = NULL,
        .async    = true,
    },
    {
        .name     = "can_uds_session",
        .sub      = NULL,
        .help     = "DiagnosticSessionControl: <tx_id> <session_type>",
        .min_args = 2,
        .max_args = 2,
        .handler  = (command_handler_t)cmd_can_uds_session,
        .ctx      = NULL,
        .async    = false,
    },
    {
        .name     = "can_uds_read",
        .sub      = NULL,
        .help     = "ReadDataByIdentifier: <tx_id> <did_hex>",
        .min_args = 2,
        .max_args = 2,
        .handler  = (command_handler_t)cmd_can_uds_read,
        .ctx      = NULL,
        .async    = true,
    },
    {
        .name     = "can_uds_dump",
        .sub      = NULL,
        .help     = "ReadMemoryByAddress: <tx_id> <addr_hex> <size>",
        .min_args = 3,
        .max_args = 3,
        .handler  = (command_handler_t)cmd_can_uds_dump,
        .ctx      = NULL,
        .async    = true,
    },
    {
        .name     = "can_uds_auth",
        .sub      = NULL,
        .help     = "SecurityAccess seed: <tx_id> [level]",
        .min_args = 1,
        .max_args = 2,
        .handler  = (command_handler_t)cmd_can_uds_auth,
        .ctx      = NULL,
        .async    = true,
    },
#endif /* CONFIG_CANBUS_UDS */

    /* --- OBD-II commands (Phase 4) --- */
#ifdef CONFIG_CANBUS_OBD
    {
        .name     = "can_obd",
        .sub      = NULL,
        .help     = "Query OBD-II PID: <pid_hex>",
        .min_args = 1,
        .max_args = 1,
        .handler  = (command_handler_t)cmd_can_obd,
        .ctx      = NULL,
        .async    = true,
    },
    {
        .name     = "can_obd_vin",
        .sub      = NULL,
        .help     = "Read Vehicle Identification Number",
        .min_args = 0,
        .max_args = 0,
        .handler  = (command_handler_t)cmd_can_obd_vin,
        .ctx      = NULL,
        .async    = true,
    },
    {
        .name     = "can_obd_dtc",
        .sub      = NULL,
        .help     = "Read Diagnostic Trouble Codes",
        .min_args = 0,
        .max_args = 0,
        .handler  = (command_handler_t)cmd_can_obd_dtc,
        .ctx      = NULL,
        .async    = true,
    },
    {
        .name     = "can_obd_supported",
        .sub      = NULL,
        .help     = "List supported OBD-II PIDs",
        .min_args = 0,
        .max_args = 0,
        .handler  = (command_handler_t)cmd_can_obd_supported,
        .ctx      = NULL,
        .async    = true,
    },
    {
        .name     = "can_obd_monitor",
        .sub      = NULL,
        .help     = "Stream PIDs to C2: <pid_hex,...> [interval_ms]",
        .min_args = 1,
        .max_args = 2,
        .handler  = (command_handler_t)cmd_can_obd_monitor,
        .ctx      = NULL,
        .async    = true,
    },
    {
        .name     = "can_obd_monitor_stop",
        .sub      = NULL,
        .help     = "Stop OBD-II monitoring",
        .min_args = 0,
        .max_args = 0,
        .handler  = (command_handler_t)cmd_can_obd_monitor_stop,
        .ctx      = NULL,
        .async    = false,
    },
#endif /* CONFIG_CANBUS_OBD */

    /* --- Fuzzing commands (Phase 5) --- */
#ifdef CONFIG_CANBUS_FUZZ
    {
        .name     = "can_fuzz_id",
        .sub      = NULL,
        .help     = "ID scan fuzz: [start_hex] [end_hex] [delay_ms]",
        .min_args = 0,
        .max_args = 3,
        .handler  = (command_handler_t)cmd_can_fuzz_id,
        .ctx      = NULL,
        .async    = true,
    },
    {
        .name     = "can_fuzz_data",
        .sub      = NULL,
        .help     = "Data mutation fuzz: <id_hex> [seed_hex] [delay_ms]",
        .min_args = 1,
        .max_args = 3,
        .handler  = (command_handler_t)cmd_can_fuzz_data,
        .ctx      = NULL,
        .async    = true,
    },
    {
        .name     = "can_fuzz_random",
        .sub      = NULL,
        .help     = "Random fuzz: [delay_ms] [count]",
        .min_args = 0,
        .max_args = 2,
        .handler  = (command_handler_t)cmd_can_fuzz_random,
        .ctx      = NULL,
        .async    = true,
    },
    {
        .name     = "can_fuzz_stop",
        .sub      = NULL,
        .help     = "Stop fuzzing",
        .min_args = 0,
        .max_args = 0,
        .handler  = (command_handler_t)cmd_can_fuzz_stop_cmd,
        .ctx      = NULL,
        .async    = false,
    },
#endif /* CONFIG_CANBUS_FUZZ */
};

/* ============================================================
 * Registration
 * ============================================================ */

void mod_canbus_register_commands(void)
{
    ESPILON_LOGI_PURPLE(TAG, "Registering CAN bus commands");

    /* Init NVS config */
    can_config_init();

    /* Load software filters */
    reload_sw_filters();

    /* Create record mutex */
    if (!s_record_mutex) {
        s_record_mutex = xSemaphoreCreateMutex();
    }

    for (size_t i = 0; i < sizeof(can_cmds) / sizeof(can_cmds[0]); i++) {
        command_register(&can_cmds[i]);
    }
}

#endif /* CONFIG_MODULE_CANBUS */