/* CAN bus driver (Issues #597, #676, #674, #694) */
#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;
static volatile can_wdog_t      s_wdog;

#ifdef TEST_HOST
static volatile uint32_t s_test_esr = 0u;
void can_driver_inject_esr(uint32_t v) { s_test_esr = v; }
static uint32_t _read_esr(void) { return s_test_esr; }
static HAL_StatusTypeDef _can_restart(void) {
    HAL_CAN_Stop(&s_can); s_test_esr = 0u; return HAL_CAN_Start(&s_can);
}
#else
static uint32_t _read_esr(void) { return s_can.Instance->ESR; }
static HAL_StatusTypeDef _can_restart(void) {
    HAL_CAN_Stop(&s_can); return HAL_CAN_Start(&s_can);
}
#endif

void can_driver_init(void)
{
    __HAL_RCC_CAN1_CLK_ENABLE();
    __HAL_RCC_GPIOB_CLK_ENABLE();
    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);
    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;
    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; }
    CAN_FilterTypeDef flt = {0};
    flt.FilterBank = 0u; flt.FilterMode = CAN_FILTERMODE_IDMASK;
    flt.FilterScale = CAN_FILTERSCALE_32BIT;
    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; }
    CAN_FilterTypeDef flt2 = {0};
    flt2.FilterBank = 15u; flt2.FilterMode = CAN_FILTERMODE_IDMASK;
    flt2.FilterScale = CAN_FILTERSCALE_32BIT;
    flt2.FilterIdLow = 0x0004u; flt2.FilterMaskIdLow = 0x0004u;
    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);
#ifndef TEST_HOST
    HAL_NVIC_SetPriority(CAN1_SCE_IRQn, 5u, 0u);
    HAL_NVIC_EnableIRQ(CAN1_SCE_IRQn);
    HAL_CAN_ActivateNotification(&s_can, CAN_IT_BUSOFF | CAN_IT_ERROR_PASSIVE | CAN_IT_ERROR_WARNING);
#endif
    memset((void *)s_feedback, 0, sizeof(s_feedback));
    memset((void *)&s_stats, 0, sizeof(s_stats));
    memset((void *)&s_wdog, 0, sizeof(s_wdog));
}

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;
    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)
{
    if (_read_esr() & CAN_ESR_BOFF) { s_stats.bus_off = 1u; return; }
    s_stats.bus_off = 0u;
    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;
        if (s_std_cb != NULL) s_std_cb((uint16_t)rxhdr.StdId, rxdata, (uint8_t)rxhdr.DLC);
        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++;
    }
    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 || 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));
}

/* SCE interrupt handler -- Issue #694 */
#ifndef TEST_HOST
void CAN1_SCE_IRQHandler(void)
{
    uint32_t esr = s_can.Instance->ESR;
    if (esr & CAN_ESR_BOFF) {
        if (s_wdog.error_state != CAN_ERR_BUS_OFF) s_wdog.busoff_count++;
        s_wdog.error_state = CAN_ERR_BUS_OFF; s_stats.bus_off = 1u;
    } else if (esr & CAN_ESR_EPVF) {
        if (s_wdog.error_state < CAN_ERR_ERROR_PASSIVE) {
            s_wdog.errpassive_count++; s_wdog.error_state = CAN_ERR_ERROR_PASSIVE;
        }
    } else if (esr & CAN_ESR_EWGF) {
        if (s_wdog.error_state < CAN_ERR_WARNING) {
            s_wdog.errwarn_count++; s_wdog.error_state = CAN_ERR_WARNING;
        }
    }
    __HAL_CAN_CLEAR_FLAG(&s_can, CAN_FLAG_ERRI);
    HAL_CAN_IRQHandler(&s_can);
}
#endif

/* Watchdog tick -- Issue #694 */
can_error_state_t can_driver_watchdog_tick(uint32_t now_ms)
{
    uint32_t esr = _read_esr();
    if (esr & CAN_ESR_BOFF) {
        if (s_wdog.error_state != CAN_ERR_BUS_OFF) {
            s_wdog.busoff_count++; s_wdog.busoff_ms = now_ms; s_wdog.busoff_pending = 1u;
        }
        s_wdog.error_state = CAN_ERR_BUS_OFF; s_stats.bus_off = 1u;
    } else if (esr & CAN_ESR_EPVF) {
        if (s_wdog.error_state < CAN_ERR_ERROR_PASSIVE) {
            s_wdog.errpassive_count++; s_wdog.error_state = CAN_ERR_ERROR_PASSIVE;
        }
    } else if (esr & CAN_ESR_EWGF) {
        if (s_wdog.error_state < CAN_ERR_WARNING) {
            s_wdog.errwarn_count++; s_wdog.error_state = CAN_ERR_WARNING;
        }
    } else {
        if (s_wdog.error_state > CAN_ERR_NOMINAL && s_wdog.error_state < CAN_ERR_BUS_OFF)
            s_wdog.error_state = CAN_ERR_NOMINAL;
    }
    if (s_wdog.busoff_pending && s_wdog.error_state == CAN_ERR_BUS_OFF &&
        (now_ms - s_wdog.busoff_ms) >= CAN_WDOG_RESTART_MS) {
        if (_can_restart() == HAL_OK) {
            s_wdog.restart_count++; s_wdog.error_state = CAN_ERR_NOMINAL;
            s_wdog.busoff_pending = 0u; s_stats.bus_off = 0u;
        } else {
            s_wdog.busoff_ms = now_ms;
        }
    }
    s_wdog.tec = (uint8_t)((esr >> CAN_ESR_TEC_Pos) & 0xFFu);
    s_wdog.rec = (uint8_t)((esr >> CAN_ESR_REC_Pos) & 0xFFu);
    return s_wdog.error_state;
}

void can_driver_get_wdog(can_wdog_t *out)
{
    if (out == NULL) return;
    memcpy(out, (const void *)&s_wdog, sizeof(can_wdog_t));
}