/*
 * can_port.c
 *
 *  Created on: Sep 16, 2025
 *      Author: herli
 */
#include "can_port.h"

#include <string.h>

#ifndef CAN_TXQ_CAP
#define CAN_TXQ_CAP 16u
#endif


/* ===== Static state ===== */
static FDCAN_HandleTypeDef *s_hcan = NULL;

typedef struct {
    uint32_t id;
    uint8_t  dlc;
    uint8_t  data[8];
} _CanQueuedMsg;

/* Map 0..8 payload length to FDCAN DLC enum for classic CAN. */
static inline uint32_t _dlc_from_len(uint8_t len)
{
    switch (len) {
        case 0: return FDCAN_DLC_BYTES_0;
        case 1: return FDCAN_DLC_BYTES_1;
        case 2: return FDCAN_DLC_BYTES_2;
        case 3: return FDCAN_DLC_BYTES_3;
        case 4: return FDCAN_DLC_BYTES_4;
        case 5: return FDCAN_DLC_BYTES_5;
        case 6: return FDCAN_DLC_BYTES_6;
        case 7: return FDCAN_DLC_BYTES_7;
        default: return FDCAN_DLC_BYTES_8;
    }
}
static inline bool _try_tx_now(FDCAN_HandleTypeDef *h, uint16_t id, const uint8_t data[8], uint8_t dlc)
{
    FDCAN_TxHeaderTypeDef th = {0};
    th.Identifier          = id;
    th.IdType              = FDCAN_STANDARD_ID;
    th.TxFrameType         = FDCAN_DATA_FRAME;
    th.DataLength          = _dlc_from_len(dlc);
    th.ErrorStateIndicator = FDCAN_ESI_ACTIVE;
    th.BitRateSwitch       = FDCAN_BRS_OFF;
    th.FDFormat            = FDCAN_CLASSIC_CAN;
    th.TxEventFifoControl  = FDCAN_NO_TX_EVENTS;
    th.MessageMarker       = 0;

    return (HAL_FDCAN_AddMessageToTxFifoQ(h, &th, (uint8_t*)data) == HAL_OK);
}

static volatile uint16_t _txq_head = 0; // push
static volatile uint16_t _txq_tail = 0; // pop
static _CanQueuedMsg     _txq_buf[CAN_TXQ_CAP];

static inline bool _txq_full(void)  { return (uint16_t)((_txq_head + 1u) % CAN_TXQ_CAP) == _txq_tail; }
static inline bool _txq_empty(void) { return _txq_head == _txq_tail; }

static bool _txq_push(const _CanQueuedMsg *m)
{
    if (_txq_full()) return false;
    uint16_t h = _txq_head;
    _txq_buf[h] = *m;
    __DMB(); // ensure data visible before advancing head
    _txq_head = (uint16_t)((h + 1u) % CAN_TXQ_CAP);
    return true;
}

static bool _txq_pop(_CanQueuedMsg *m)
{
    if (_txq_empty()) return false;
    uint16_t t = _txq_tail;
    *m = _txq_buf[t];
    __DMB();
    _txq_tail = (uint16_t)((t + 1u) % CAN_TXQ_CAP);
    return true;
}

size_t CAN_TxQueueDepth(void)
{
    int16_t d = (int16_t)_txq_head - (int16_t)_txq_tail;
    if (d < 0) d += (int16_t)CAN_TXQ_CAP;
    return (size_t)d;
}
bool CAN_Service(void)
{
    if (!s_hcan) return false;

    bool sent_any = false;
    for (;;) {
        _CanQueuedMsg m;
        if (!_txq_pop(&m)) break;

        if (!_try_tx_now(s_hcan, (uint16_t)m.id, m.data, m.dlc)) {
            // HW still full -> put back and stop to preserve order
            (void)_txq_push(&m);
            break;
        }
        sent_any = true;
    }
    return sent_any;
}


/* Adapter used by can_manager to actually TX via HAL */
static bool _tx_adapter(uint16_t id, const uint8_t data[8], uint8_t dlc)
{
    if (!s_hcan) return false;

    // 1) Try to send immediately
    if (_try_tx_now(s_hcan, id, data, dlc)) {
        return true;
    }

    // 2) HW busy -> enqueue and report success if queued
    _CanQueuedMsg m;
    m.id  = id;
    m.dlc = dlc;
    memcpy(m.data, data, 8);

    return _txq_push(&m);
}


/* ===== Public API ===== */

void can_port_init(FDCAN_HandleTypeDef *hfdcan)
{
    s_hcan = hfdcan;
    can_manager_init(_tx_adapter);
//falta el register handler

}

void can_port_boot(void)
{
    can_manager_boot();

    // Send the first EMPF3 immediately
    //can_port_send_msg_def(&MSG_ID_EMPF3);

    // Schedule the remaining two, 40 ms apart, via timeout index 18
    //Timeout_ResetByIndex(18, TIM16->CNT);  // first callback in 40 ms
}

void can_port_handle_rx_fifo(uint32_t fifo_index)
{
    if (!s_hcan) return;

    const uint32_t fifo = (fifo_index == 1) ? FDCAN_RX_FIFO1 : FDCAN_RX_FIFO0;

    while (HAL_FDCAN_GetRxFifoFillLevel(s_hcan, fifo) > 0U) {
        FDCAN_RxHeaderTypeDef rh;
        uint8_t data[8];
        if (HAL_FDCAN_GetRxMessage(s_hcan, fifo, &rh, data) != HAL_OK) {
            /* If needed: handle error or break */
            break;
        }
        /* Pass to the generic manager; it will decode symbols and/or call handlers. */
        can_manager_on_rx((uint16_t)rh.Identifier, data, 8);
    }
}

bool can_port_send_msg(uint16_t can_id)
{
    return can_manager_send(can_id);
}

bool can_port_send_raw(uint16_t id, const uint8_t data[8], uint8_t dlc)
{
    return _tx_adapter(id, data, dlc);
}

static int _is_std_id(uint32_t id) { return (id <= 0x7FFu); }

size_t can_port_required_std_filters(void)
{
    size_t count = 0, n = 0;
    const CanMessageDef* const* all = can_db_all_ptr(&n);
    if (!all) return 0;

    // simple dedup set (tiny nets) — 64 IDs buffer; grow if you need more
    uint16_t seen[64];
    size_t seen_n = 0;

    for (size_t i = 0; i < n; ++i) {
        const CanMessageDef *m = all[i];
        if (!m || m->dir != CAN_DIR_RX) continue;
        if (!_is_std_id(m->can_id))     continue;

        uint16_t id = (uint16_t)m->can_id;
        int dup = 0;
        for (size_t k = 0; k < seen_n; ++k) { if (seen[k] == id) { dup = 1; break; } }
        if (!dup) {
            if (seen_n < (sizeof(seen)/sizeof(seen[0]))) seen[seen_n++] = id;
            count++;
        }
    }
    return count;
}

HAL_StatusTypeDef can_port_apply_rx_filters(FDCAN_HandleTypeDef *hfdcan)
{
    if (!hfdcan) return HAL_ERROR;

    // 1) Reject everything by default (we’ll explicitly allow only DB RX IDs)
    HAL_StatusTypeDef st =
        HAL_FDCAN_ConfigGlobalFilter(hfdcan,
                                     FDCAN_REJECT,        // non-matching STD
                                     FDCAN_REJECT,        // non-matching EXT
                                     FDCAN_REJECT_REMOTE, // STD remote
                                     FDCAN_REJECT_REMOTE);// EXT remote
    if (st != HAL_OK) return st;

    // 2) Build exact-match STANDARD filters to FIFO0
    size_t n=0, idx=0;
    const CanMessageDef* const* all = can_db_all_ptr(&n);
    if (!all) return HAL_OK;

    // Dedup pass (same as above, but we’ll store all unique IDs)
    uint16_t ids[64];
    size_t ids_n = 0;

    for (size_t i = 0; i < n; ++i) {
        const CanMessageDef *m = all[i];
        if (!m || m->dir != CAN_DIR_RX) continue;
        if (!_is_std_id(m->can_id))     continue;

        uint16_t id = (uint16_t)m->can_id;
        int dup = 0;
        for (size_t k = 0; k < ids_n; ++k) { if (ids[k] == id) { dup = 1; break; } }
        if (!dup && ids_n < (sizeof(ids)/sizeof(ids[0]))) ids[ids_n++] = id;
    }

    // Guard: hardware must have enough filter elements (StdFiltersNbr >= ids_n)
    // If not, we still program as many as possible.
    for (idx = 0; idx < ids_n; ++idx) {
        FDCAN_FilterTypeDef f = {0};
        f.IdType       = FDCAN_STANDARD_ID;
        f.FilterIndex  = (uint32_t)idx;
        f.FilterType   = FDCAN_FILTER_MASK;             // mask-based exact match
        f.FilterConfig = FDCAN_FILTER_TO_RXFIFO0;       // route to FIFO0
        f.FilterID1    = ids[idx];                      // ID to match
        f.FilterID2    = 0x7FF;                         // mask: all 11 bits must match

        st = HAL_FDCAN_ConfigFilter(hfdcan, &f);
        if (st != HAL_OK) return st;
    }

    return HAL_OK;
}

