/*
 * canbus_driver.c
 * MCP2515 CAN 2.0B controller driver via ESP-IDF SPI master.
 *
 * Architecture:
 *   GPIO ISR (INT pin, active low) → binary semaphore → RX task → callback
 *   TX: direct SPI writes to TX buffer 0, poll for completion.
 */
#include "sdkconfig.h"

#ifdef CONFIG_MODULE_CANBUS

#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/semphr.h"
#include "driver/spi_master.h"
#include "driver/gpio.h"
#include "esp_timer.h"
#include "esp_log.h"

#include "canbus_driver.h"

#define TAG "CAN_DRV"

/* ============================================================
 * MCP2515 SPI Instructions
 * ============================================================ */
#define MCP_RESET       0xC0
#define MCP_READ        0x03
#define MCP_WRITE       0x02
#define MCP_BIT_MODIFY  0x05
#define MCP_READ_STATUS 0xA0
#define MCP_RX_STATUS   0xB0
#define MCP_READ_RX0    0x90    /* Read RX buffer 0 starting at SIDH */
#define MCP_READ_RX1    0x94    /* Read RX buffer 1 starting at SIDH */
#define MCP_LOAD_TX0    0x40    /* Load TX buffer 0 starting at SIDH */
#define MCP_RTS_TX0     0x81    /* Request-To-Send TX buffer 0 */

/* ============================================================
 * MCP2515 Registers
 * ============================================================ */
#define MCP_CANCTRL     0x0F
#define MCP_CANSTAT     0x0E
#define MCP_CNF1        0x2A
#define MCP_CNF2        0x29
#define MCP_CNF3        0x28
#define MCP_CANINTE     0x2B
#define MCP_CANINTF     0x2C
#define MCP_EFLG        0x2D
#define MCP_TEC         0x1C
#define MCP_REC         0x1D

/* RXB0CTRL / RXB1CTRL */
#define MCP_RXB0CTRL    0x60
#define MCP_RXB1CTRL    0x70

/* Filter/mask registers */
#define MCP_RXF0SIDH    0x00
#define MCP_RXF1SIDH    0x04
#define MCP_RXF2SIDH    0x08
#define MCP_RXF3SIDH    0x10
#define MCP_RXF4SIDH    0x14
#define MCP_RXF5SIDH    0x18
#define MCP_RXM0SIDH    0x20
#define MCP_RXM1SIDH    0x24

/* TXB0 registers */
#define MCP_TXB0CTRL    0x30
#define MCP_TXB0SIDH    0x31

/* CANCTRL mode bits */
#define MCP_MODE_NORMAL     0x00
#define MCP_MODE_LISTEN     0x60
#define MCP_MODE_LOOPBACK   0x40
#define MCP_MODE_CONFIG     0x80

/* CANINTF bits */
#define MCP_RX0IF       0x01
#define MCP_RX1IF       0x02
#define MCP_TX0IF       0x04
#define MCP_TX1IF       0x08
#define MCP_TX2IF       0x10
#define MCP_ERRIF       0x20
#define MCP_WAKIF       0x40
#define MCP_MERRF       0x80

/* CANINTE bits */
#define MCP_RX0IE       0x01
#define MCP_RX1IE       0x02
#define MCP_ERRIE       0x20

/* EFLG bits */
#define MCP_EFLG_RX0OVR    0x40
#define MCP_EFLG_RX1OVR    0x80
#define MCP_EFLG_TXBO      0x20
#define MCP_EFLG_RXEP      0x10
#define MCP_EFLG_TXEP      0x08

/* ============================================================
 * Bit Timing Tables
 * ============================================================ */
typedef struct {
    int     bitrate;
    uint8_t cnf1, cnf2, cnf3;
} can_timing_t;

/* 16 MHz oscillator — TQ = 2/Fosc = 125ns */
static const can_timing_t s_timing_16mhz[] = {
    { 1000000, 0x00, 0xCA, 0x01 },  /* 1 Mbps:   SJW=1, BRP=0,  8 TQ  */
    {  500000, 0x00, 0xF0, 0x86 },  /* 500 kbps: SJW=1, BRP=0,  16 TQ */
    {  250000, 0x01, 0xF0, 0x86 },  /* 250 kbps: SJW=1, BRP=1,  16 TQ */
    {  125000, 0x03, 0xF0, 0x86 },  /* 125 kbps: SJW=1, BRP=3,  16 TQ */
    {  100000, 0x04, 0xF0, 0x86 },  /* 100 kbps: SJW=1, BRP=4,  16 TQ */
    { 0, 0, 0, 0 }
};

/* 8 MHz oscillator — TQ = 2/Fosc = 250ns */
static const can_timing_t s_timing_8mhz[] = {
    {  500000, 0x00, 0x90, 0x02 },  /* 500 kbps: SJW=1, BRP=0,  8 TQ  */
    {  250000, 0x00, 0xF0, 0x86 },  /* 250 kbps: SJW=1, BRP=0,  16 TQ */
    {  125000, 0x01, 0xF0, 0x86 },  /* 125 kbps: SJW=1, BRP=1,  16 TQ */
    {  100000, 0x03, 0xAC, 0x03 },  /* 100 kbps: SJW=1, BRP=3,  10 TQ */
    { 0, 0, 0, 0 }
};

/* ============================================================
 * Driver State
 * ============================================================ */
static spi_device_handle_t s_spi       = NULL;
static TaskHandle_t        s_rx_task   = NULL;
static SemaphoreHandle_t   s_int_sem   = NULL;
static SemaphoreHandle_t   s_tx_mutex  = NULL;
static volatile bool       s_running   = false;

static can_rx_callback_t   s_rx_cb     = NULL;
static void               *s_rx_ctx    = NULL;

/* Counters */
static uint32_t s_rx_count     = 0;
static uint32_t s_tx_count     = 0;
static uint32_t s_bus_errors   = 0;
static uint32_t s_rx_overflow  = 0;
static bool     s_bus_off      = false;
static bool     s_err_passive  = false;

/* ============================================================
 * SPI Low-Level Helpers
 * ============================================================ */

static uint8_t mcp_read_reg(uint8_t addr)
{
    uint8_t tx[3] = { MCP_READ, addr, 0x00 };
    uint8_t rx[3] = { 0 };
    spi_transaction_t t = {
        .length    = 24,
        .tx_buffer = tx,
        .rx_buffer = rx,
    };
    spi_device_transmit(s_spi, &t);
    return rx[2];
}

static void mcp_write_reg(uint8_t addr, uint8_t val)
{
    uint8_t tx[3] = { MCP_WRITE, addr, val };
    spi_transaction_t t = {
        .length    = 24,
        .tx_buffer = tx,
    };
    spi_device_transmit(s_spi, &t);
}

static void mcp_modify_reg(uint8_t addr, uint8_t mask, uint8_t val)
{
    uint8_t tx[4] = { MCP_BIT_MODIFY, addr, mask, val };
    spi_transaction_t t = {
        .length    = 32,
        .tx_buffer = tx,
    };
    spi_device_transmit(s_spi, &t);
}

static void mcp_reset(void)
{
    uint8_t tx[1] = { MCP_RESET };
    spi_transaction_t t = {
        .length    = 8,
        .tx_buffer = tx,
    };
    spi_device_transmit(s_spi, &t);
    vTaskDelay(pdMS_TO_TICKS(10));  /* MCP2515 needs time after reset */
}

static void mcp_set_mode(uint8_t mode)
{
    mcp_modify_reg(MCP_CANCTRL, 0xE0, mode);
    /* Wait for mode change confirmation */
    for (int i = 0; i < 50; i++) {
        uint8_t stat = mcp_read_reg(MCP_CANSTAT);
        if ((stat & 0xE0) == mode) return;
        vTaskDelay(pdMS_TO_TICKS(1));
    }
    ESP_LOGW(TAG, "Mode change to 0x%02X timeout", mode);
}

/* Read a complete frame from RX buffer (0 or 1) */
static void mcp_read_rx_buffer(int buf, can_frame_t *frame)
{
    /* Use READ_RX instruction for auto-clear of interrupt flag */
    uint8_t cmd = (buf == 0) ? MCP_READ_RX0 : MCP_READ_RX1;

    /* Read: cmd + SIDH + SIDL + EID8 + EID0 + DLC + 8 data = 14 bytes */
    uint8_t tx[14] = { 0 };
    uint8_t rx[14] = { 0 };
    tx[0] = cmd;

    spi_transaction_t t = {
        .length    = 14 * 8,
        .tx_buffer = tx,
        .rx_buffer = rx,
    };
    spi_device_transmit(s_spi, &t);

    /* Parse — offsets relative to rx[1] (SIDH is byte 1) */
    uint8_t sidh = rx[1];
    uint8_t sidl = rx[2];
    uint8_t eid8 = rx[3];
    uint8_t eid0 = rx[4];
    uint8_t dlc  = rx[5];

    frame->extended = (sidl & 0x08) != 0;
    frame->rtr      = false;

    if (frame->extended) {
        frame->id = ((uint32_t)sidh << 21)
                  | ((uint32_t)(sidl & 0xE0) << 13)
                  | ((uint32_t)(sidl & 0x03) << 16)
                  | ((uint32_t)eid8 << 8)
                  | (uint32_t)eid0;
        frame->rtr = (dlc & 0x40) != 0;
    } else {
        frame->id = ((uint32_t)sidh << 3) | ((uint32_t)(sidl >> 5) & 0x07);
        frame->rtr = (sidl & 0x10) != 0;
    }

    frame->dlc = dlc & 0x0F;
    if (frame->dlc > 8) frame->dlc = 8;

    memcpy(frame->data, &rx[6], 8);
    frame->timestamp_us = 0;  /* Caller sets timestamp */
}

/* Write a frame to TX buffer 0 and request send */
static bool mcp_write_tx_buffer(const can_frame_t *frame)
{
    /* Check if TX buffer 0 is free */
    uint8_t ctrl = mcp_read_reg(MCP_TXB0CTRL);
    if (ctrl & 0x08) {
        /* TXREQ still set — previous TX pending */
        return false;
    }

    /* Build TX buffer content: SIDH + SIDL + EID8 + EID0 + DLC + data */
    uint8_t tx[14] = { 0 };
    tx[0] = MCP_LOAD_TX0;

    if (frame->extended) {
        tx[1] = (uint8_t)(frame->id >> 21);                        /* SIDH */
        tx[2] = (uint8_t)((frame->id >> 13) & 0xE0)               /* SIDL high bits */
               | 0x08                                               /* EXIDE = 1 */
               | (uint8_t)((frame->id >> 16) & 0x03);             /* SIDL low bits */
        tx[3] = (uint8_t)(frame->id >> 8);                         /* EID8 */
        tx[4] = (uint8_t)(frame->id);                              /* EID0 */
        tx[5] = frame->dlc | (frame->rtr ? 0x40 : 0x00);          /* DLC + RTR */
    } else {
        tx[1] = (uint8_t)(frame->id >> 3);                        /* SIDH */
        tx[2] = (uint8_t)((frame->id & 0x07) << 5)               /* SIDL */
               | (frame->rtr ? 0x10 : 0x00);
        tx[3] = 0;
        tx[4] = 0;
        tx[5] = frame->dlc;
    }

    memcpy(&tx[6], frame->data, 8);

    spi_transaction_t t = {
        .length    = 14 * 8,
        .tx_buffer = tx,
    };
    spi_device_transmit(s_spi, &t);

    /* Request to send */
    uint8_t rts = MCP_RTS_TX0;
    spi_transaction_t rts_t = {
        .length    = 8,
        .tx_buffer = &rts,
    };
    spi_device_transmit(s_spi, &rts_t);

    return true;
}

/* ============================================================
 * GPIO ISR — INT pin (active low)
 * ============================================================ */

static void IRAM_ATTR gpio_isr_handler(void *arg)
{
    BaseType_t woken = pdFALSE;
    xSemaphoreGiveFromISR(s_int_sem, &woken);
    if (woken) portYIELD_FROM_ISR();
}

/* ============================================================
 * RX Task
 * ============================================================ */

static void rx_task(void *arg)
{
    ESP_LOGI(TAG, "RX task started");

    while (s_running) {
        /* Wait for interrupt or timeout (poll every 100ms as fallback) */
        if (xSemaphoreTake(s_int_sem, pdMS_TO_TICKS(100)) != pdTRUE) {
            continue;
        }

        /* Read interrupt flags */
        uint8_t intf = mcp_read_reg(MCP_CANINTF);

        /* RX buffer 0 full */
        if (intf & MCP_RX0IF) {
            can_frame_t frame;
            mcp_read_rx_buffer(0, &frame);  /* READ_RX auto-clears RX0IF */
            frame.timestamp_us = esp_timer_get_time();
            s_rx_count++;
            if (s_rx_cb) s_rx_cb(&frame, s_rx_ctx);
        }

        /* RX buffer 1 full */
        if (intf & MCP_RX1IF) {
            can_frame_t frame;
            mcp_read_rx_buffer(1, &frame);  /* READ_RX auto-clears RX1IF */
            frame.timestamp_us = esp_timer_get_time();
            s_rx_count++;
            if (s_rx_cb) s_rx_cb(&frame, s_rx_ctx);
        }

        /* Error interrupt */
        if (intf & MCP_ERRIF) {
            uint8_t eflg = mcp_read_reg(MCP_EFLG);
            s_bus_errors++;

            if (eflg & MCP_EFLG_TXBO) {
                s_bus_off = true;
                ESP_LOGW(TAG, "Bus-off detected");
            }
            if (eflg & (MCP_EFLG_RXEP | MCP_EFLG_TXEP)) {
                s_err_passive = true;
            }
            if (eflg & (MCP_EFLG_RX0OVR | MCP_EFLG_RX1OVR)) {
                s_rx_overflow++;
            }

            /* Clear error flags */
            mcp_modify_reg(MCP_EFLG, 0xFF, 0x00);
            mcp_modify_reg(MCP_CANINTF, MCP_ERRIF, 0x00);
        }

        /* TX complete — clear flags */
        if (intf & (MCP_TX0IF | MCP_TX1IF | MCP_TX2IF)) {
            mcp_modify_reg(MCP_CANINTF, MCP_TX0IF | MCP_TX1IF | MCP_TX2IF, 0x00);
        }
    }

    ESP_LOGI(TAG, "RX task stopped");
    s_rx_task = NULL;
    vTaskDelete(NULL);
}

/* ============================================================
 * Public API — Lifecycle
 * ============================================================ */

bool can_driver_init(int bitrate, uint8_t osc_mhz)
{
    if (s_spi) {
        ESP_LOGW(TAG, "Already initialized");
        return false;
    }

    /* Select timing table */
    const can_timing_t *table = NULL;
    if (osc_mhz == 16) {
        table = s_timing_16mhz;
    } else if (osc_mhz == 8) {
        table = s_timing_8mhz;
    } else {
        ESP_LOGE(TAG, "Unsupported oscillator: %u MHz", osc_mhz);
        return false;
    }

    /* Find matching bitrate */
    const can_timing_t *timing = NULL;
    for (int i = 0; table[i].bitrate != 0; i++) {
        if (table[i].bitrate == bitrate) {
            timing = &table[i];
            break;
        }
    }
    if (!timing) {
        ESP_LOGE(TAG, "Unsupported bitrate %d for %u MHz osc", bitrate, osc_mhz);
        return false;
    }

    /* Init SPI bus */
    spi_bus_config_t bus_cfg = {
        .mosi_io_num     = CONFIG_CANBUS_PIN_MOSI,
        .miso_io_num     = CONFIG_CANBUS_PIN_MISO,
        .sclk_io_num     = CONFIG_CANBUS_PIN_SCK,
        .quadwp_io_num   = -1,
        .quadhd_io_num   = -1,
        .max_transfer_sz = 32,
    };

    esp_err_t ret = spi_bus_initialize(CONFIG_CANBUS_SPI_HOST, &bus_cfg, SPI_DMA_DISABLED);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "SPI bus init failed: %s", esp_err_to_name(ret));
        return false;
    }

    /* Add MCP2515 as SPI device */
    spi_device_interface_config_t dev_cfg = {
        .mode           = 0,                        /* SPI mode 0 (CPOL=0, CPHA=0) */
        .clock_speed_hz = CONFIG_CANBUS_SPI_CLOCK_HZ,
        .spics_io_num   = CONFIG_CANBUS_PIN_CS,
        .queue_size     = 4,
    };

    ret = spi_bus_add_device(CONFIG_CANBUS_SPI_HOST, &dev_cfg, &s_spi);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "SPI device add failed: %s", esp_err_to_name(ret));
        spi_bus_free(CONFIG_CANBUS_SPI_HOST);
        return false;
    }

    /* Reset MCP2515 (enters CONFIG mode automatically) */
    mcp_reset();

    /* Verify we can read CANSTAT — should be in CONFIG mode (0x80) */
    uint8_t stat = mcp_read_reg(MCP_CANSTAT);
    if ((stat & 0xE0) != MCP_MODE_CONFIG) {
        ESP_LOGE(TAG, "MCP2515 not responding (CANSTAT=0x%02X)", stat);
        spi_bus_remove_device(s_spi);
        spi_bus_free(CONFIG_CANBUS_SPI_HOST);
        s_spi = NULL;
        return false;
    }

    ESP_LOGI(TAG, "MCP2515 detected (CANSTAT=0x%02X)", stat);

    /* Set bit timing */
    mcp_write_reg(MCP_CNF1, timing->cnf1);
    mcp_write_reg(MCP_CNF2, timing->cnf2);
    mcp_write_reg(MCP_CNF3, timing->cnf3);

    /* Enable interrupts: RX0, RX1, Error */
    mcp_write_reg(MCP_CANINTE, MCP_RX0IE | MCP_RX1IE | MCP_ERRIE);

    /* Clear all interrupt flags */
    mcp_write_reg(MCP_CANINTF, 0x00);

    /* RXB0CTRL: rollover to RXB1 if RXB0 full, receive all valid messages */
    mcp_write_reg(MCP_RXB0CTRL, 0x64);  /* BUKT=1, RXM=11 (turn mask/filter off) */
    mcp_write_reg(MCP_RXB1CTRL, 0x60);  /* RXM=11 (turn mask/filter off) */

    /* Create semaphores */
    s_int_sem  = xSemaphoreCreateBinary();
    s_tx_mutex = xSemaphoreCreateMutex();

    /* Reset counters */
    s_rx_count    = 0;
    s_tx_count    = 0;
    s_bus_errors  = 0;
    s_rx_overflow = 0;
    s_bus_off     = false;
    s_err_passive = false;

    /* Configure INT pin as input with pull-up, falling edge interrupt */
    gpio_config_t io_cfg = {
        .pin_bit_mask = (1ULL << CONFIG_CANBUS_PIN_INT),
        .mode         = GPIO_MODE_INPUT,
        .pull_up_en   = GPIO_PULLUP_ENABLE,
        .pull_down_en = GPIO_PULLDOWN_DISABLE,
        .intr_type    = GPIO_INTR_NEGEDGE,
    };
    gpio_config(&io_cfg);
    gpio_install_isr_service(0);
    gpio_isr_handler_add(CONFIG_CANBUS_PIN_INT, gpio_isr_handler, NULL);

    ESP_LOGI(TAG, "Initialized: %d bps, %u MHz osc, SPI@%d Hz",
             bitrate, osc_mhz, CONFIG_CANBUS_SPI_CLOCK_HZ);
    return true;
}

bool can_driver_start(can_mode_t mode)
{
    if (!s_spi) {
        ESP_LOGE(TAG, "Not initialized");
        return false;
    }
    if (s_running) {
        ESP_LOGW(TAG, "Already running");
        return false;
    }

    /* Map mode enum to MCP2515 mode register value */
    uint8_t mcp_mode;
    const char *mode_str;
    switch (mode) {
        case CAN_MODE_LISTEN_ONLY:
            mcp_mode = MCP_MODE_LISTEN;
            mode_str = "listen-only";
            break;
        case CAN_MODE_LOOPBACK:
            mcp_mode = MCP_MODE_LOOPBACK;
            mode_str = "loopback";
            break;
        default:
            mcp_mode = MCP_MODE_NORMAL;
            mode_str = "normal";
            break;
    }

    /* Set operational mode */
    mcp_set_mode(mcp_mode);

    /* Verify mode */
    uint8_t stat = mcp_read_reg(MCP_CANSTAT);
    if ((stat & 0xE0) != mcp_mode) {
        ESP_LOGE(TAG, "Failed to enter %s mode (CANSTAT=0x%02X)", mode_str, stat);
        return false;
    }

    s_running = true;

    /* Start RX task on Core 1, priority 5 (above normal) */
    BaseType_t ret = xTaskCreatePinnedToCore(
        rx_task, "can_rx", 4096, NULL, 5, &s_rx_task, 1
    );
    if (ret != pdPASS) {
        ESP_LOGE(TAG, "Failed to create RX task");
        s_running = false;
        mcp_set_mode(MCP_MODE_CONFIG);
        return false;
    }

    ESP_LOGI(TAG, "Started in %s mode", mode_str);
    return true;
}

void can_driver_stop(void)
{
    if (!s_running) return;

    s_running = false;

    /* Give semaphore to wake RX task so it exits */
    if (s_int_sem) xSemaphoreGive(s_int_sem);

    /* Wait for RX task to die */
    for (int i = 0; i < 20 && s_rx_task != NULL; i++) {
        vTaskDelay(pdMS_TO_TICKS(50));
    }

    /* Put MCP2515 back to CONFIG mode */
    if (s_spi) {
        mcp_set_mode(MCP_MODE_CONFIG);
    }

    ESP_LOGI(TAG, "Stopped");
}

void can_driver_deinit(void)
{
    can_driver_stop();

    /* Remove ISR */
    gpio_isr_handler_remove(CONFIG_CANBUS_PIN_INT);

    /* Free SPI */
    if (s_spi) {
        spi_bus_remove_device(s_spi);
        spi_bus_free(CONFIG_CANBUS_SPI_HOST);
        s_spi = NULL;
    }

    /* Free semaphores */
    if (s_int_sem)  { vSemaphoreDelete(s_int_sem);  s_int_sem  = NULL; }
    if (s_tx_mutex) { vSemaphoreDelete(s_tx_mutex); s_tx_mutex = NULL; }

    ESP_LOGI(TAG, "Deinitialized");
}

bool can_driver_is_running(void)
{
    return s_running;
}

/* ============================================================
 * Public API — TX / RX
 * ============================================================ */

bool can_driver_send(const can_frame_t *frame)
{
    if (!s_running || !s_spi) return false;

    xSemaphoreTake(s_tx_mutex, portMAX_DELAY);

    /* Try to load into TX buffer, with retries for busy buffer */
    bool ok = false;
    for (int i = 0; i < 10; i++) {
        if (mcp_write_tx_buffer(frame)) {
            ok = true;
            break;
        }
        vTaskDelay(pdMS_TO_TICKS(1));
    }

    if (ok) {
        /* Wait for TX complete (TX0IF) or timeout */
        for (int i = 0; i < 100; i++) {
            uint8_t intf = mcp_read_reg(MCP_CANINTF);
            if (intf & MCP_TX0IF) {
                mcp_modify_reg(MCP_CANINTF, MCP_TX0IF, 0x00);
                s_tx_count++;
                break;
            }
            vTaskDelay(pdMS_TO_TICKS(1));
        }
    }

    xSemaphoreGive(s_tx_mutex);
    return ok;
}

void can_driver_set_rx_callback(can_rx_callback_t cb, void *ctx)
{
    s_rx_cb  = cb;
    s_rx_ctx = ctx;
}

void can_driver_get_rx_callback(can_rx_callback_t *cb, void **ctx)
{
    if (cb)  *cb  = s_rx_cb;
    if (ctx) *ctx = s_rx_ctx;
}

/* ============================================================
 * Public API — Hardware Filters
 * ============================================================ */

/* Filter register base addresses (SIDH of each filter) */
static const uint8_t s_filter_addrs[6] = {
    MCP_RXF0SIDH, MCP_RXF1SIDH, MCP_RXF2SIDH,
    MCP_RXF3SIDH, MCP_RXF4SIDH, MCP_RXF5SIDH,
};

static const uint8_t s_mask_addrs[2] = {
    MCP_RXM0SIDH, MCP_RXM1SIDH,
};

/* Write ID to filter/mask register set (4 bytes: SIDH, SIDL, EID8, EID0) */
static void write_id_regs(uint8_t base_addr, uint32_t id, bool extended)
{
    uint8_t sidh, sidl, eid8, eid0;

    if (extended) {
        sidh = (uint8_t)(id >> 21);
        sidl = (uint8_t)((id >> 13) & 0xE0) | 0x08 | (uint8_t)((id >> 16) & 0x03);
        eid8 = (uint8_t)(id >> 8);
        eid0 = (uint8_t)(id);
    } else {
        sidh = (uint8_t)(id >> 3);
        sidl = (uint8_t)((id & 0x07) << 5);
        eid8 = 0;
        eid0 = 0;
    }

    mcp_write_reg(base_addr,     sidh);
    mcp_write_reg(base_addr + 1, sidl);
    mcp_write_reg(base_addr + 2, eid8);
    mcp_write_reg(base_addr + 3, eid0);
}

bool can_driver_set_filter(int idx, uint32_t id, bool extended)
{
    if (!s_spi || idx < 0 || idx > 5) return false;

    /* Filters can only be set in CONFIG mode */
    bool was_running = s_running;
    if (was_running) can_driver_stop();

    mcp_set_mode(MCP_MODE_CONFIG);
    write_id_regs(s_filter_addrs[idx], id, extended);

    /* Enable filtering on the relevant RX buffer */
    if (idx < 2) {
        mcp_write_reg(MCP_RXB0CTRL, 0x04);  /* BUKT=1, RXM=00 (use filter) */
    } else {
        mcp_write_reg(MCP_RXB1CTRL, 0x00);  /* RXM=00 (use filter) */
    }

    if (was_running) can_driver_start(CAN_MODE_NORMAL);
    return true;
}

bool can_driver_set_mask(int idx, uint32_t mask, bool extended)
{
    if (!s_spi || idx < 0 || idx > 1) return false;

    bool was_running = s_running;
    if (was_running) can_driver_stop();

    mcp_set_mode(MCP_MODE_CONFIG);
    write_id_regs(s_mask_addrs[idx], mask, extended);

    if (was_running) can_driver_start(CAN_MODE_NORMAL);
    return true;
}

void can_driver_clear_filters(void)
{
    if (!s_spi) return;

    bool was_running = s_running;
    if (was_running) can_driver_stop();

    mcp_set_mode(MCP_MODE_CONFIG);

    /* Set masks to 0 (match anything) */
    for (int i = 0; i < 2; i++) {
        write_id_regs(s_mask_addrs[i], 0, false);
    }

    /* RXM=11 → turn off mask/filter, receive all */
    mcp_write_reg(MCP_RXB0CTRL, 0x64);
    mcp_write_reg(MCP_RXB1CTRL, 0x60);

    if (was_running) can_driver_start(CAN_MODE_NORMAL);
}

/* ============================================================
 * Public API — Status
 * ============================================================ */

void can_driver_get_status(can_status_t *out)
{
    memset(out, 0, sizeof(*out));

    out->rx_count    = s_rx_count;
    out->tx_count    = s_tx_count;
    out->bus_errors  = s_bus_errors;
    out->rx_overflow = s_rx_overflow;
    out->bus_off     = s_bus_off;

    if (s_spi) {
        out->tx_errors    = mcp_read_reg(MCP_TEC);
        out->rx_errors    = mcp_read_reg(MCP_REC);
        out->error_passive = (out->tx_errors > 127) || (out->rx_errors > 127);
    }

    if (!s_spi)             out->state = "not_initialized";
    else if (!s_running)    out->state = "stopped";
    else if (out->bus_off)  out->state = "bus_off";
    else if (out->error_passive) out->state = "error_passive";
    else                    out->state = "running";
}

/* ============================================================
 * Public API — Replay
 * ============================================================ */

bool can_driver_replay(const can_frame_t *frames, int count, int speed_pct)
{
    if (!s_running || !frames || count <= 0) return false;

    ESP_LOGI(TAG, "Replaying %d frames at %d%% speed", count, speed_pct);

    int64_t base_ts = frames[0].timestamp_us;

    for (int i = 0; i < count && s_running; i++) {
        /* Wait for inter-frame delay */
        if (i > 0 && speed_pct > 0) {
            int64_t delta_us = frames[i].timestamp_us - frames[i - 1].timestamp_us;
            if (delta_us > 0) {
                int64_t wait_us = (delta_us * 100) / speed_pct;
                if (wait_us > 1000) {
                    vTaskDelay(pdMS_TO_TICKS(wait_us / 1000));
                }
            }
        }

        can_frame_t tx = frames[i];
        if (!can_driver_send(&tx)) {
            ESP_LOGW(TAG, "Replay: send failed at frame %d", i);
        }
    }

    ESP_LOGI(TAG, "Replay complete (%d frames)", count);
    return true;
}

#endif /* CONFIG_MODULE_CANBUS */