/* CAN bus driver for BLDC motor controllers (Issue #597)
 * CAN1 on PB8 (RX, AF9) / PB9 (TX, AF9) at 500 kbps   (Issue #676 remap)
 * Filter bank 0 (CAN1 master; SlaveStartFilterBank=14)
 *
 * NOTE: Mamba F722S MK2 does not expose PB12/PB13 externally.
 * Waveshare CAN module wired to SCL pad (PB8 = CAN1_RX) and
 * SDA pad (PB9 = CAN1_TX).  I2C1 is free (BME280 moved to I2C2).
 * CAN1 uses AF9 on PB8/PB9 — no conflict with other active peripherals.
 */

#include "can_driver.h"
#include "stm32f7xx_hal.h"
#include <string.h>

static CAN_HandleTypeDef        s_can;
static volatile can_feedback_t  s_feedback[CAN_NUM_MOTORS];
static volatile can_stats_t     s_stats;
static can_ext_frame_cb_t       s_ext_cb = NULL;
static can_std_frame_cb_t       s_std_cb = NULL;

void can_driver_init(void)
{
    __HAL_RCC_CAN1_CLK_ENABLE();
    __HAL_RCC_GPIOB_CLK_ENABLE();

    /* PB8 = CAN1_RX, PB9 = CAN1_TX, AF9 (Issue #676) */
    GPIO_InitTypeDef gpio = {0};
    gpio.Pin       = GPIO_PIN_8 | GPIO_PIN_9;
    gpio.Mode      = GPIO_MODE_AF_PP;
    gpio.Pull      = GPIO_NOPULL;
    gpio.Speed     = GPIO_SPEED_FREQ_HIGH;
    gpio.Alternate = GPIO_AF9_CAN1;
    HAL_GPIO_Init(GPIOB, &gpio);

    /* 500 kbps @ APB1=54 MHz: PSC=6, BS1=13tq, BS2=4tq, SJW=1tq
     * bit_time = 6 × (1+13+4) / 54000000 = 2 µs → 500 kbps
     * sample point = (1+13)/18 = 77.8% */
    s_can.Instance                  = CAN1;
    s_can.Init.Prescaler            = CAN_PRESCALER;
    s_can.Init.Mode                 = CAN_MODE_NORMAL;
    s_can.Init.SyncJumpWidth        = CAN_SJW_1TQ;
    s_can.Init.TimeSeg1             = CAN_BS1_13TQ;
    s_can.Init.TimeSeg2             = CAN_BS2_4TQ;
    s_can.Init.TimeTriggeredMode    = DISABLE;
    s_can.Init.AutoBusOff           = ENABLE;   /* HW recovery after 128×11 bits */
    s_can.Init.AutoWakeUp           = DISABLE;
    s_can.Init.AutoRetransmission   = ENABLE;
    s_can.Init.ReceiveFifoLocked    = DISABLE;
    s_can.Init.TransmitFifoPriority = DISABLE;

    if (HAL_CAN_Init(&s_can) != HAL_OK) {
        s_stats.bus_off = 1u;
        return;
    }

    /* Filter bank 0: 32-bit mask, FIFO0, accept ALL standard 11-bit frames.
     * CAN1 is master (SlaveStartFilterBank=14). FilterIdHigh=0, MaskHigh=0
     * → mask=0 passes every standard ID. Orin std-frame commands land here. */
    CAN_FilterTypeDef flt = {0};
    flt.FilterBank           = 0u;
    flt.FilterMode           = CAN_FILTERMODE_IDMASK;
    flt.FilterScale          = CAN_FILTERSCALE_32BIT;
    flt.FilterIdHigh         = 0u;
    flt.FilterIdLow          = 0u;
    flt.FilterMaskIdHigh     = 0u;
    flt.FilterMaskIdLow      = 0u;
    flt.FilterFIFOAssignment = CAN_RX_FIFO0;
    flt.FilterActivation     = CAN_FILTER_ENABLE;
    flt.SlaveStartFilterBank = 14u;

    if (HAL_CAN_ConfigFilter(&s_can, &flt) != HAL_OK) {
        s_stats.bus_off = 1u;
        return;
    }

    /* Filter bank 15: 32-bit mask, FIFO1, accept ALL extended 29-bit frames.
     * For extended frames, the IDE bit sits at FilterIdLow[2].
     * FilterIdLow  = 0x0004 (IDE=1) → match extended frames.
     * FilterMaskIdLow = 0x0004 → only the IDE bit is checked; all ext IDs pass.
     * This routes every VESC STATUS / STATUS_4 / STATUS_5 frame to FIFO1. */
    CAN_FilterTypeDef flt2 = {0};
    flt2.FilterBank           = 15u;
    flt2.FilterMode           = CAN_FILTERMODE_IDMASK;
    flt2.FilterScale          = CAN_FILTERSCALE_32BIT;
    flt2.FilterIdHigh         = 0u;
    flt2.FilterIdLow          = 0x0004u;  /* IDE = 1 */
    flt2.FilterMaskIdHigh     = 0u;
    flt2.FilterMaskIdLow      = 0x0004u;  /* only check IDE bit */
    flt2.FilterFIFOAssignment = CAN_RX_FIFO1;
    flt2.FilterActivation     = CAN_FILTER_ENABLE;
    flt2.SlaveStartFilterBank = 14u;

    if (HAL_CAN_ConfigFilter(&s_can, &flt2) != HAL_OK) {
        s_stats.bus_off = 1u;
        return;
    }

    HAL_CAN_Start(&s_can);

    memset((void *)s_feedback, 0, sizeof(s_feedback));
    memset((void *)&s_stats,   0, sizeof(s_stats));
}

void can_driver_send_cmd(uint8_t node_id, const can_cmd_t *cmd)
{
    if (node_id >= CAN_NUM_MOTORS || s_stats.bus_off) {
        return;
    }

    /* Payload: [vel_lo, vel_hi, torque_lo, torque_hi] — little-endian */
    uint8_t data[4];
    data[0] = (uint8_t)((uint16_t)cmd->velocity_rpm & 0xFFu);
    data[1] = (uint8_t)((uint16_t)cmd->velocity_rpm >> 8u);
    data[2] = (uint8_t)((uint16_t)cmd->torque_x100  & 0xFFu);
    data[3] = (uint8_t)((uint16_t)cmd->torque_x100  >> 8u);

    CAN_TxHeaderTypeDef hdr = {0};
    hdr.StdId = CAN_ID_VEL_CMD_BASE + (uint32_t)node_id;
    hdr.IDE   = CAN_ID_STD;
    hdr.RTR   = CAN_RTR_DATA;
    hdr.DLC   = 4u;

    uint32_t mailbox;
    if (HAL_CAN_GetTxMailboxesFreeLevel(&s_can) > 0u) {
        if (HAL_CAN_AddTxMessage(&s_can, &hdr, data, &mailbox) == HAL_OK) {
            s_stats.tx_count++;
        } else {
            s_stats.err_count++;
        }
    }
}

void can_driver_send_enable(uint8_t node_id, bool enable)
{
    if (node_id >= CAN_NUM_MOTORS || s_stats.bus_off) {
        return;
    }

    uint8_t data[1] = { enable ? 1u : 0u };

    CAN_TxHeaderTypeDef hdr = {0};
    hdr.StdId = CAN_ID_ENABLE_CMD_BASE + (uint32_t)node_id;
    hdr.IDE   = CAN_ID_STD;
    hdr.RTR   = CAN_RTR_DATA;
    hdr.DLC   = 1u;

    uint32_t mailbox;
    if (HAL_CAN_GetTxMailboxesFreeLevel(&s_can) > 0u) {
        if (HAL_CAN_AddTxMessage(&s_can, &hdr, data, &mailbox) == HAL_OK) {
            s_stats.tx_count++;
        } else {
            s_stats.err_count++;
        }
    }
}

void can_driver_process(void)
{
    /* Check for bus-off */
    if (s_can.Instance->ESR & CAN_ESR_BOFF) {
        s_stats.bus_off = 1u;
        return;
    }
    s_stats.bus_off = 0u;

    /* Drain FIFO0 (standard-ID frames: Orin commands + legacy feedback) */
    while (HAL_CAN_GetRxFifoFillLevel(&s_can, CAN_RX_FIFO0) > 0u) {
        CAN_RxHeaderTypeDef rxhdr;
        uint8_t rxdata[8];

        if (HAL_CAN_GetRxMessage(&s_can, CAN_RX_FIFO0, &rxhdr, rxdata) != HAL_OK) {
            s_stats.err_count++;
            break;
        }

        if (rxhdr.IDE != CAN_ID_STD || rxhdr.RTR != CAN_RTR_DATA) {
            continue;
        }

        /* Dispatch to registered standard-frame callback (Orin CAN protocol) */
        if (s_std_cb != NULL) {
            s_std_cb((uint16_t)rxhdr.StdId, rxdata, (uint8_t)rxhdr.DLC);
        }

        /* Legacy: decode feedback frames at 0x200-0x21F (Issue #597) */
        uint32_t nid_u = rxhdr.StdId - CAN_ID_FEEDBACK_BASE;
        if (nid_u < CAN_NUM_MOTORS && rxhdr.DLC >= 8u) {
            uint8_t nid = (uint8_t)nid_u;
            s_feedback[nid].velocity_rpm  = (int16_t)((uint16_t)rxdata[0] | ((uint16_t)rxdata[1] << 8u));
            s_feedback[nid].current_ma    = (int16_t)((uint16_t)rxdata[2] | ((uint16_t)rxdata[3] << 8u));
            s_feedback[nid].position_x100 = (int16_t)((uint16_t)rxdata[4] | ((uint16_t)rxdata[5] << 8u));
            s_feedback[nid].temperature_c = (int8_t)rxdata[6];
            s_feedback[nid].fault         = rxdata[7];
            s_feedback[nid].last_rx_ms    = HAL_GetTick();
        }

        s_stats.rx_count++;
    }

    /* Drain FIFO1 (extended-ID frames: VESC STATUS/STATUS_4/STATUS_5) */
    while (HAL_CAN_GetRxFifoFillLevel(&s_can, CAN_RX_FIFO1) > 0u) {
        CAN_RxHeaderTypeDef rxhdr;
        uint8_t rxdata[8];

        if (HAL_CAN_GetRxMessage(&s_can, CAN_RX_FIFO1, &rxhdr, rxdata) != HAL_OK) {
            s_stats.err_count++;
            break;
        }

        if (rxhdr.IDE != CAN_ID_EXT || rxhdr.RTR != CAN_RTR_DATA) {
            continue;
        }

        if (s_ext_cb != NULL) {
            s_ext_cb(rxhdr.ExtId, rxdata, (uint8_t)rxhdr.DLC);
        }

        s_stats.rx_count++;
    }
}

void can_driver_set_ext_cb(can_ext_frame_cb_t cb)
{
    s_ext_cb = cb;
}

void can_driver_set_std_cb(can_std_frame_cb_t cb)
{
    s_std_cb = cb;
}

void can_driver_send_ext(uint32_t ext_id, const uint8_t *data, uint8_t len)
{
    if (s_stats.bus_off || len > 8u) {
        return;
    }

    CAN_TxHeaderTypeDef hdr = {0};
    hdr.ExtId = ext_id;
    hdr.IDE   = CAN_ID_EXT;
    hdr.RTR   = CAN_RTR_DATA;
    hdr.DLC   = len;

    uint32_t mailbox;
    if (HAL_CAN_GetTxMailboxesFreeLevel(&s_can) > 0u) {
        if (HAL_CAN_AddTxMessage(&s_can, &hdr, (uint8_t *)data, &mailbox) == HAL_OK) {
            s_stats.tx_count++;
        } else {
            s_stats.err_count++;
        }
    }
}

void can_driver_send_std(uint16_t std_id, const uint8_t *data, uint8_t len)
{
    if (s_stats.bus_off || len > 8u) {
        return;
    }

    CAN_TxHeaderTypeDef hdr = {0};
    hdr.StdId = std_id;
    hdr.IDE   = CAN_ID_STD;
    hdr.RTR   = CAN_RTR_DATA;
    hdr.DLC   = len;

    uint32_t mailbox;
    if (HAL_CAN_GetTxMailboxesFreeLevel(&s_can) > 0u) {
        if (HAL_CAN_AddTxMessage(&s_can, &hdr, (uint8_t *)data, &mailbox) == HAL_OK) {
            s_stats.tx_count++;
        } else {
            s_stats.err_count++;
        }
    }
}

bool can_driver_get_feedback(uint8_t node_id, can_feedback_t *out)
{
    if (node_id >= CAN_NUM_MOTORS || out == NULL) {
        return false;
    }
    if (s_feedback[node_id].last_rx_ms == 0u) {
        return false;
    }
    memcpy(out, (const void *)&s_feedback[node_id], sizeof(can_feedback_t));
    return true;
}

bool can_driver_is_alive(uint8_t node_id, uint32_t now_ms)
{
    if (node_id >= CAN_NUM_MOTORS) {
        return false;
    }
    if (s_feedback[node_id].last_rx_ms == 0u) {
        return false;
    }
    return (now_ms - s_feedback[node_id].last_rx_ms) < CAN_NODE_TIMEOUT_MS;
}

void can_driver_get_stats(can_stats_t *out)
{
    if (out == NULL) {
        return;
    }
    memcpy(out, (const void *)&s_stats, sizeof(can_stats_t));
}