/* CAN bus driver for BLDC motor controllers (Issue #597)
 * CAN2 on PB12 (RX, AF9) / PB13 (TX, AF9) at 500 kbps
 * Filter bank 14 (first CAN2 bank; SlaveStartFilterBank=14)
 */

#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;

void can_driver_init(void)
{
    /* CAN2 requires CAN1 master clock for shared filter RAM */
    __HAL_RCC_CAN1_CLK_ENABLE();
    __HAL_RCC_CAN2_CLK_ENABLE();
    __HAL_RCC_GPIOB_CLK_ENABLE();

    /* PB12 = CAN2_RX, PB13 = CAN2_TX, AF9 */
    GPIO_InitTypeDef gpio = {0};
    gpio.Pin       = GPIO_PIN_12 | GPIO_PIN_13;
    gpio.Mode      = GPIO_MODE_AF_PP;
    gpio.Pull      = GPIO_NOPULL;
    gpio.Speed     = GPIO_SPEED_FREQ_HIGH;
    gpio.Alternate = GPIO_AF9_CAN2;
    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                  = CAN2;
    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 14: 32-bit mask, FIFO0, accept std IDs 0x200–0x21F
     * FilterIdHigh   = 0x200 << 5 = 0x4000  (base ID shifted to bit[15:5])
     * FilterMaskHigh = 0x7E0 << 5 = 0xFC00  (mask: top 6 bits must match) */
    CAN_FilterTypeDef flt = {0};
    flt.FilterBank           = 14u;
    flt.FilterMode           = CAN_FILTERMODE_IDMASK;
    flt.FilterScale          = CAN_FILTERSCALE_32BIT;
    flt.FilterIdHigh         = (uint16_t)(CAN_FILTER_STDID << 5u);
    flt.FilterIdLow          = 0u;
    flt.FilterMaskIdHigh     = (uint16_t)(CAN_FILTER_MASK  << 5u);
    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;
    }

    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 */
    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;
        }

        /* Only process data frames with standard IDs */
        if (rxhdr.IDE != CAN_ID_STD || rxhdr.RTR != CAN_RTR_DATA) {
            continue;
        }

        /* Decode feedback frames: base 0x200, one per node */
        uint32_t nid_u = rxhdr.StdId - CAN_ID_FEEDBACK_BASE;
        if (nid_u >= CAN_NUM_MOTORS || rxhdr.DLC < 8u) {
            continue;
        }

        uint8_t nid = (uint8_t)nid_u;

        /* Layout: [0-1] vel_rpm  [2-3] current_ma  [4-5] pos_x100  [6] temp_c  [7] fault */
        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++;
    }
}

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));
}