/* Includes ----------------------------------------------------------------- */
#include "fdcan.h"
#include "bsp/fdcan.h"
#include "bsp/bsp.h"
#include "stm32h7xx_hal_gpio.h"
#include <fdcan.h>
#include <cmsis_os2.h>
#include <string.h>

/* Private define ----------------------------------------------------------- */
#define FDCAN_QUEUE_MUTEX_TIMEOUT         100

/* Private typedef ---------------------------------------------------------- */
typedef struct BSP_FDCAN_QueueNode {
    BSP_FDCAN_t fdcan;
    uint32_t can_id;
    osMessageQueueId_t queue;
    uint8_t queue_size;
    struct BSP_FDCAN_QueueNode *next;
} BSP_FDCAN_QueueNode_t;

/* Private variables -------------------------------------------------------- */
//static BSP_FDCAN_QueueNode_t *queue_list = NULL;
//static osMutexId_t queue_mutex = NULL;
//static void (*FDCAN_Callback[BSP_FDCAN_NUM][HAL_FDCAN_CB_NUM])(void);
//static bool inited = false;
//static BSP_FDCAN_IdParser_t id_parser = NULL;
//static BSP_FDCAN_TxQueue_t tx_queues[BSP_FDCAN_NUM];
BSP_FDCAN_QueueNode_t *queue_list = NULL;
osMutexId_t queue_mutex = NULL;
void (*FDCAN_Callback[BSP_FDCAN_NUM][HAL_FDCAN_CB_NUM])(void);
bool inited = false;
BSP_FDCAN_IdParser_t id_parser = NULL;
BSP_FDCAN_TxQueue_t tx_queues[BSP_FDCAN_NUM];
/* Private function prototypes ---------------------------------------------- */
static BSP_FDCAN_t FDCAN_Get(FDCAN_HandleTypeDef *hfdcan);
static osMessageQueueId_t BSP_FDCAN_FindQueue(BSP_FDCAN_t fdcan, uint32_t can_id);
static int8_t BSP_FDCAN_CreateIdQueue(BSP_FDCAN_t fdcan, uint32_t can_id, uint8_t queue_size);
static void BSP_FDCAN_RxFifo0Callback(void);
static void BSP_FDCAN_RxFifo1Callback(void);
static void BSP_FDCAN_TxCompleteCallback(void);
static BSP_FDCAN_FrameType_t BSP_FDCAN_GetFrameType(FDCAN_RxHeaderTypeDef *header);
static uint32_t BSP_FDCAN_DefaultIdParser(uint32_t original_id, BSP_FDCAN_FrameType_t frame_type);
static void BSP_FDCAN_TxQueueInit(BSP_FDCAN_t fdcan);
static bool BSP_FDCAN_TxQueuePush(BSP_FDCAN_t fdcan, BSP_FDCAN_TxMessage_t *msg);
static bool BSP_FDCAN_TxQueuePop(BSP_FDCAN_t fdcan, BSP_FDCAN_TxMessage_t *msg);
static bool BSP_FDCAN_TxQueueIsEmpty(BSP_FDCAN_t fdcan);

/* Private functions -------------------------------------------------------- */
static BSP_FDCAN_t FDCAN_Get(FDCAN_HandleTypeDef *hfdcan) {
    if (hfdcan == NULL) return BSP_FDCAN_ERR;
    if (hfdcan->Instance == FDCAN1) return BSP_FDCAN_1;
    else if (hfdcan->Instance == FDCAN2) return BSP_FDCAN_2;
    else if (hfdcan->Instance == FDCAN3) return BSP_FDCAN_3;
    else return BSP_FDCAN_ERR;
}

static osMessageQueueId_t BSP_FDCAN_FindQueue(BSP_FDCAN_t fdcan, uint32_t can_id) {
    BSP_FDCAN_QueueNode_t *node = queue_list;
    while (node != NULL) {
        if (node->fdcan == fdcan && node->can_id == can_id) return node->queue;
        node = node->next;
    }
    return NULL;
}

static int8_t BSP_FDCAN_CreateIdQueue(BSP_FDCAN_t fdcan, uint32_t can_id, uint8_t queue_size) {
  if (queue_size == 0) queue_size = BSP_FDCAN_DEFAULT_QUEUE_SIZE;
  if (osMutexAcquire(queue_mutex, FDCAN_QUEUE_MUTEX_TIMEOUT) != osOK) return BSP_ERR_TIMEOUT;
  BSP_FDCAN_QueueNode_t *node = queue_list;
  while (node != NULL) {
    if (node->fdcan == fdcan && node->can_id == can_id) {
      osMutexRelease(queue_mutex);
      return BSP_ERR;
    }
    node = node->next;
  }
  
  BSP_FDCAN_QueueNode_t *new_node = (BSP_FDCAN_QueueNode_t *)BSP_Malloc(sizeof(BSP_FDCAN_QueueNode_t));
  if (new_node == NULL) { osMutexRelease(queue_mutex); return BSP_ERR_NULL; }
  new_node->queue = osMessageQueueNew(queue_size, sizeof(BSP_FDCAN_Message_t), NULL);
  if (new_node->queue == NULL) { BSP_Free(new_node); osMutexRelease(queue_mutex); return BSP_ERR; }
  new_node->fdcan = fdcan;
  new_node->can_id = can_id;
  new_node->queue_size = queue_size;
  new_node->next = queue_list;
  queue_list = new_node;
  osMutexRelease(queue_mutex);
  return BSP_OK;
}

static BSP_FDCAN_FrameType_t BSP_FDCAN_GetFrameType(FDCAN_RxHeaderTypeDef *header) {
    if (header->RxFrameType == FDCAN_REMOTE_FRAME) {
        return (header->IdType == FDCAN_EXTENDED_ID) ? BSP_FDCAN_FRAME_EXT_REMOTE : BSP_FDCAN_FRAME_STD_REMOTE;
    } else {
        return (header->IdType == FDCAN_EXTENDED_ID) ? BSP_FDCAN_FRAME_EXT_DATA : BSP_FDCAN_FRAME_STD_DATA;
    }
}

static uint32_t BSP_FDCAN_DefaultIdParser(uint32_t original_id, BSP_FDCAN_FrameType_t frame_type) {
    (void)frame_type;
    return original_id;
}

static void BSP_FDCAN_TxQueueInit(BSP_FDCAN_t fdcan) {
    if (fdcan >= BSP_FDCAN_NUM) return;
    tx_queues[fdcan].head = 0;
    tx_queues[fdcan].tail = 0;
}

static bool BSP_FDCAN_TxQueuePush(BSP_FDCAN_t fdcan, BSP_FDCAN_TxMessage_t *msg) {
    if (fdcan >= BSP_FDCAN_NUM || msg == NULL) return false;
    BSP_FDCAN_TxQueue_t *queue = &tx_queues[fdcan];
    uint32_t next_head = (queue->head + 1) % BSP_FDCAN_TX_QUEUE_SIZE;
    if (next_head == queue->tail) return false;
    queue->buffer[queue->head] = *msg;
    queue->head = next_head;
    return true;
}

static bool BSP_FDCAN_TxQueuePop(BSP_FDCAN_t fdcan, BSP_FDCAN_TxMessage_t *msg) {
    if (fdcan >= BSP_FDCAN_NUM || msg == NULL) return false;
    BSP_FDCAN_TxQueue_t *queue = &tx_queues[fdcan];
    if (queue->head == queue->tail) return false;
    *msg = queue->buffer[queue->tail];
    queue->tail = (queue->tail + 1) % BSP_FDCAN_TX_QUEUE_SIZE;
    return true;
}

static bool BSP_FDCAN_TxQueueIsEmpty(BSP_FDCAN_t fdcan) {
    if (fdcan >= BSP_FDCAN_NUM) return true;
    return tx_queues[fdcan].head == tx_queues[fdcan].tail;
}
 void BSP_FDCAN_TxCompleteCallback(void) {
//static void BSP_FDCAN_TxCompleteCallback(void) {
    for (int i = 0; i < BSP_FDCAN_NUM; i++) {
        BSP_FDCAN_t fdcan = (BSP_FDCAN_t)i;
        FDCAN_HandleTypeDef *hfdcan = BSP_FDCAN_GetHandle(fdcan);
        if (hfdcan == NULL) continue;
        // 消费所有 TX EVENT FIFO 事件，防止堵塞
        FDCAN_TxEventFifoTypeDef tx_event;
        while (HAL_FDCAN_GetTxEvent(hfdcan, &tx_event) == HAL_OK) {
            // 可在此统计 MessageMarker、ID、时间戳等
        }
        // 续写软件队列到硬件 FIFO
        BSP_FDCAN_TxMessage_t msg;
        while (!BSP_FDCAN_TxQueueIsEmpty(fdcan)) {
            if (HAL_FDCAN_GetTxFifoFreeLevel(hfdcan) == 0) break;
            if (!BSP_FDCAN_TxQueuePop(fdcan, &msg)) break;
            HAL_StatusTypeDef res = HAL_FDCAN_AddMessageToTxFifoQ(hfdcan, &msg.header, msg.data);
            if (res != HAL_OK) {
                break;
            }
        }
    }
}

static void BSP_FDCAN_RxFifo0Callback(void) {
    FDCAN_RxHeaderTypeDef rx_header;
    uint8_t rx_data[BSP_FDCAN_MAX_DLC];
    for (int fdcan_idx = 0; fdcan_idx < BSP_FDCAN_NUM; fdcan_idx++) {
        FDCAN_HandleTypeDef *hfdcan = BSP_FDCAN_GetHandle((BSP_FDCAN_t)fdcan_idx);
        if (hfdcan == NULL) continue;
        while (HAL_FDCAN_GetRxFifoFillLevel(hfdcan, FDCAN_RX_FIFO0) > 0) {
            if (HAL_FDCAN_GetRxMessage(hfdcan, FDCAN_RX_FIFO0, &rx_header, rx_data) == HAL_OK) {
                uint32_t original_id = (rx_header.IdType == FDCAN_STANDARD_ID) ? rx_header.Identifier&0x7ff : rx_header.Identifier&0x1fffffff;
                BSP_FDCAN_FrameType_t frame_type = BSP_FDCAN_GetFrameType(&rx_header);
                uint32_t parsed_id = BSP_FDCAN_ParseId(original_id, frame_type);
                osMessageQueueId_t queue = BSP_FDCAN_FindQueue((BSP_FDCAN_t)fdcan_idx, parsed_id);
                if (queue != NULL) {
                    BSP_FDCAN_Message_t msg;
                    msg.frame_type = frame_type;
                    msg.original_id = original_id;
                    msg.parsed_id = parsed_id;
                    msg.dlc = rx_header.DataLength;
                    if (msg.dlc > BSP_FDCAN_MAX_DLC) msg.dlc = BSP_FDCAN_MAX_DLC;
                    memcpy(msg.data, rx_data, msg.dlc);
                    osMessageQueuePut(queue, &msg, 0, 0);
                }
            } else {
                break;
            }
        }
    }
}

static void BSP_FDCAN_RxFifo1Callback(void) {
    FDCAN_RxHeaderTypeDef rx_header;
    uint8_t rx_data[BSP_FDCAN_MAX_DLC];
    for (int fdcan_idx = 0; fdcan_idx < BSP_FDCAN_NUM; fdcan_idx++) {
        FDCAN_HandleTypeDef *hfdcan = BSP_FDCAN_GetHandle((BSP_FDCAN_t)fdcan_idx);
        if (hfdcan == NULL) continue;
        while (HAL_FDCAN_GetRxFifoFillLevel(hfdcan, FDCAN_RX_FIFO1) > 0) {
            if (HAL_FDCAN_GetRxMessage(hfdcan, FDCAN_RX_FIFO1, &rx_header, rx_data) == HAL_OK) {
                uint32_t original_id = (rx_header.IdType == FDCAN_STANDARD_ID) ? rx_header.Identifier&0x7ff : rx_header.Identifier&0x1fffffff;
                BSP_FDCAN_FrameType_t frame_type = BSP_FDCAN_GetFrameType(&rx_header);
                uint32_t parsed_id = BSP_FDCAN_ParseId(original_id, frame_type);
                osMessageQueueId_t queue = BSP_FDCAN_FindQueue((BSP_FDCAN_t)fdcan_idx, parsed_id);
                if (queue != NULL) {
                    BSP_FDCAN_Message_t msg;
                    msg.frame_type = frame_type;
                    msg.original_id = original_id;
                    msg.parsed_id = parsed_id;
                    msg.dlc = rx_header.DataLength;
                    if (msg.dlc > BSP_FDCAN_MAX_DLC) msg.dlc = BSP_FDCAN_MAX_DLC;
                    memcpy(msg.data, rx_data, msg.dlc);
                    osMessageQueuePut(queue, &msg, 0, 0);
                }
            } else {
                break;
            }
        }
    }
}

/* HAL Callback Stubs (map HAL FDCAN callbacks to user callbacks) */
void HAL_FDCAN_TxEventFifoCallback(FDCAN_HandleTypeDef *hfdcan, uint32_t TxEventFifoITs) {
    BSP_FDCAN_t bsp_fdcan = FDCAN_Get(hfdcan);
    if (bsp_fdcan != BSP_FDCAN_ERR) {
        if (FDCAN_Callback[bsp_fdcan][HAL_FDCAN_TX_EVENT_FIFO_CB])
            FDCAN_Callback[bsp_fdcan][HAL_FDCAN_TX_EVENT_FIFO_CB]();
    }
}

void HAL_FDCAN_TxBufferCompleteCallback(FDCAN_HandleTypeDef *hfdcan, uint32_t BufferIndex) {
    BSP_FDCAN_t bsp_fdcan = FDCAN_Get(hfdcan);
    if (bsp_fdcan != BSP_FDCAN_ERR) {
        if (FDCAN_Callback[bsp_fdcan][HAL_FDCAN_TX_BUFFER_COMPLETE_CB])
            FDCAN_Callback[bsp_fdcan][HAL_FDCAN_TX_BUFFER_COMPLETE_CB]();
    }
}

void HAL_FDCAN_TxBufferAbortCallback(FDCAN_HandleTypeDef *hfdcan, uint32_t BufferIndex) {
    BSP_FDCAN_t bsp_fdcan = FDCAN_Get(hfdcan);
    if (bsp_fdcan != BSP_FDCAN_ERR) {
        if (FDCAN_Callback[bsp_fdcan][HAL_FDCAN_TX_BUFFER_ABORT_CB])
            FDCAN_Callback[bsp_fdcan][HAL_FDCAN_TX_BUFFER_ABORT_CB]();
    }
}

void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo0ITs) {
    BSP_FDCAN_t bsp_fdcan = FDCAN_Get(hfdcan);
    if (bsp_fdcan != BSP_FDCAN_ERR) {
        if (FDCAN_Callback[bsp_fdcan][HAL_FDCAN_RX_FIFO0_MSG_PENDING_CB])
            FDCAN_Callback[bsp_fdcan][HAL_FDCAN_RX_FIFO0_MSG_PENDING_CB]();
    }
}

void HAL_FDCAN_RxFifo1Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo1ITs) {
    BSP_FDCAN_t bsp_fdcan = FDCAN_Get(hfdcan);
    if (bsp_fdcan != BSP_FDCAN_ERR) {
        if (FDCAN_Callback[bsp_fdcan][HAL_FDCAN_RX_FIFO1_MSG_PENDING_CB])
            FDCAN_Callback[bsp_fdcan][HAL_FDCAN_RX_FIFO1_MSG_PENDING_CB]();
    }
}

void HAL_FDCAN_ErrorCallback(FDCAN_HandleTypeDef *hfdcan) {
    BSP_FDCAN_t bsp_fdcan = FDCAN_Get(hfdcan);
    if (bsp_fdcan != BSP_FDCAN_ERR) {
        if (FDCAN_Callback[bsp_fdcan][HAL_FDCAN_ERROR_CB])
            FDCAN_Callback[bsp_fdcan][HAL_FDCAN_ERROR_CB]();
    }
}

/* Exported functions ------------------------------------------------------- */
int8_t BSP_FDCAN_Init(void) {
    if (inited) return BSP_ERR_INITED;

    memset(FDCAN_Callback, 0, sizeof(FDCAN_Callback));

    for (int i = 0; i < BSP_FDCAN_NUM; i++) BSP_FDCAN_TxQueueInit((BSP_FDCAN_t)i);

    id_parser = BSP_FDCAN_DefaultIdParser;

    queue_mutex = osMutexNew(NULL);

    if (queue_mutex == NULL) return BSP_ERR;

    inited = true;

  /* 配置并启动 FDCAN 实例，绑定中断/回调 */

  //  过滤器编号：相对于每个（相当于经典can过滤器的bank）
  //  sFilterConfig.FilterIndex = 0 to 127(标准ID) or 0 to 63(扩展ID);  
  //  关于过滤器索引的说明：
  //  由stm32h7xx_hal_fdcan.c的第1874行代码可知滤波器地址计算方式如下：
  //  StandardFilterSA（字节） = SRAMCAN_BASE + (MessageRAMOffset * 4U)
  //  标准滤波器物理地址（字节） = StandardFilterSA + (FilterIndex * 4U)（每个标准滤波器占 4 字节 = 1 word,扩展的则是8个字节）
  //  
  //
  //  标识符类型:
  //  sFilterConfig.IdType = FDCAN_STANDARD_ID or FDCAN_EXTENDED_ID;
  //  过滤器类型: （仅介绍掩码模式）
  //  sFilterConfig.FilterType = FDCAN_FILTER_MASK;(掩码模式）
  //  过滤器配置:
  //  sFilterConfig.FilterConfig = FDCAN_FILTER_DISABLE; (禁用该滤波器条目)
  //                               FDCAN_FILTER_TO_RXFIFO0; (将匹配的消息放入 FIFO 0（普通优先级）)
  //                               FDCAN_FILTER_TO_RXFIFO1; (将匹配的消息放入 FIFO 1（高优先级）)
  //                               FDCAN_FILTER_TO_RXBUFFER; (将匹配的消息放入 指定的接收缓冲区)
  //                               FDCAN_FILTER_REJECT; (拒绝接收该标识符对应的报文)
  //                               FDCAN_FILTER_ACCEPT; (接受所有消息)
  //                               FDCAN_FILTER_HP            (过滤器匹配时，将报文标记为高优先级)                            
  //                               FDCAN_FILTER_TO_RXFIFO0_HP (过滤器匹配时，将报文标记为高优先级并存储至接收FIFO 0)          
  //                               FDCAN_FILTER_TO_RXFIFO1_HP (过滤器匹配时，将报文标记为高优先级并存储至接收FIFO 1)          
  //                               FDCAN_FILTER_TO_RXBUFFER   (将报文存储至接收缓冲区，过滤器类型（FilterType）配置项失效 )
  //  过滤器ID与掩码(FilterType掩码模式下)
  //  比较值（要匹配的 ID 的参考位）
  //  sFilterConfig.FilterID1 = 0 to 0x7FF; 标准ID
  //                            0 to 0x1FFFFFFF 扩展ID
  //  掩码（1=比较该位，0=忽略该位）
  //  sFilterConfig.FilterID2 = 0 to 0x7FF; 标准ID
  //                            0 to 0x1FFFFFFF 扩展ID
  //  接收缓冲区索引
  //  FilterConfig == FDCAN_FILTER_TO_RXBUFFER 时有效;必须小于RxBuffersNbr配置的实际Rx buffer数量
  //  sFilterConfig.RxBufferIndex = 0 to (RxBuffersNbr - 1);
  //  标记校准信息（用于 FDCAN 校准/时钟相关单元作特殊处理或统计）
  //  仅在FilterConfig 设为 FDCAN_FILTER_TO_RXBUFFER 时才有意义，通常设置为0
  //  IsCalibrationMsg = 0 or 1; 

  /* 依据上述说明，配置过滤器 */
  FDCAN_FilterTypeDef sFilterConfig;
  /* 配置 FDCAN1 过滤器 */
  /* 索引0;标准ID;掩码模式;存入 FIFO 0（普通优先级）;不过滤; */
  sFilterConfig.FilterIndex = 0;
  sFilterConfig.IdType = FDCAN_STANDARD_ID; 
  sFilterConfig.FilterType = FDCAN_FILTER_MASK; 
  sFilterConfig.FilterConfig = FDCAN_FILTER_TO_RXFIFO0;
  sFilterConfig.FilterID1 = 0x000; 
  sFilterConfig.FilterID2 = 0x000; 
  sFilterConfig.RxBufferIndex = 0;
  HAL_FDCAN_ConfigFilter(&hfdcan1, &sFilterConfig);
  HAL_FDCAN_ConfigGlobalFilter(&hfdcan1,FDCAN_REJECT, FDCAN_REJECT, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE);

  sFilterConfig.FilterIndex = 1;
  sFilterConfig.IdType = FDCAN_STANDARD_ID; 
  sFilterConfig.FilterType = FDCAN_FILTER_MASK; 
  sFilterConfig.FilterConfig = FDCAN_FILTER_TO_RXFIFO0;
  sFilterConfig.FilterID1 = 0x000; 
  sFilterConfig.FilterID2 = 0x000; 
  sFilterConfig.RxBufferIndex = 0;
  HAL_FDCAN_ConfigFilter(&hfdcan1, &sFilterConfig);
  HAL_FDCAN_ConfigGlobalFilter(&hfdcan1,FDCAN_REJECT, FDCAN_REJECT, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE);
  /* 配置 FDCAN2 过滤器 */
  /* 索引0;标准ID;掩码模式;存入 FIFO 1（普通优先级）;不过滤; */
  sFilterConfig.FilterIndex = 2;  
  sFilterConfig.IdType = FDCAN_STANDARD_ID;
  sFilterConfig.FilterType = FDCAN_FILTER_MASK; 
  sFilterConfig.FilterConfig = FDCAN_FILTER_TO_RXFIFO1;
  sFilterConfig.FilterID1 = 0x000; 
  sFilterConfig.FilterID2 = 0x000; 
  sFilterConfig.RxBufferIndex = 0;
  HAL_FDCAN_ConfigFilter(&hfdcan2, &sFilterConfig);
  HAL_FDCAN_ConfigGlobalFilter(&hfdcan2,FDCAN_REJECT, FDCAN_REJECT, DISABLE, DISABLE);
    
  sFilterConfig.FilterIndex = 3;  
  sFilterConfig.IdType = FDCAN_STANDARD_ID;
  sFilterConfig.FilterType = FDCAN_FILTER_MASK; 
  sFilterConfig.FilterConfig = FDCAN_FILTER_TO_RXFIFO1;
  sFilterConfig.FilterID1 = 0x000; 
  sFilterConfig.FilterID2 = 0x000; 
  sFilterConfig.RxBufferIndex = 0;
  HAL_FDCAN_ConfigFilter(&hfdcan2, &sFilterConfig);
  HAL_FDCAN_ConfigGlobalFilter(&hfdcan2,FDCAN_REJECT, FDCAN_REJECT, DISABLE, DISABLE);
  /* 配置 FDCAN3 过滤器 */
  /* 索引0;标准ID;掩码模式;存入 FIFO 1（普通优先级）;不过滤; */
  sFilterConfig.FilterIndex = 4;  
  sFilterConfig.IdType = FDCAN_STANDARD_ID;
  sFilterConfig.FilterType = FDCAN_FILTER_MASK; 
  sFilterConfig.FilterConfig = FDCAN_FILTER_TO_RXFIFO1;
  sFilterConfig.FilterID1 = 0x000; 
  sFilterConfig.FilterID2 = 0x000; 
  sFilterConfig.RxBufferIndex = 0;
  HAL_FDCAN_ConfigFilter(&hfdcan3, &sFilterConfig);
  HAL_FDCAN_ConfigGlobalFilter(&hfdcan3,FDCAN_REJECT, FDCAN_REJECT, DISABLE, DISABLE);

  sFilterConfig.FilterIndex = 5;  
  sFilterConfig.IdType = FDCAN_STANDARD_ID;
  sFilterConfig.FilterType = FDCAN_FILTER_MASK; 
  sFilterConfig.FilterConfig = FDCAN_FILTER_TO_RXFIFO1;
  sFilterConfig.FilterID1 = 0x000; 
  sFilterConfig.FilterID2 = 0x000; 
  sFilterConfig.RxBufferIndex = 0;
  HAL_FDCAN_ConfigFilter(&hfdcan3, &sFilterConfig);
  HAL_FDCAN_ConfigGlobalFilter(&hfdcan3,FDCAN_REJECT, FDCAN_REJECT, DISABLE, DISABLE);


//  HAL_FDCAN_ActivateNotification(&hfdcan1, FDCAN_IT_RX_FIFO0_NEW_MESSAGE, 0);
//  HAL_FDCAN_ActivateNotification(&hfdcan1, FDCAN_IT_TX_FIFO_EMPTY, 0);
//  HAL_FDCAN_ActivateNotification(&hfdcan2, FDCAN_IT_RX_FIFO1_NEW_MESSAGE, 0);
//  HAL_FDCAN_ActivateNotification(&hfdcan2, FDCAN_IT_TX_FIFO_EMPTY, 0);
//    HAL_FDCAN_ActivateNotification(&hfdcan3, FDCAN_IT_RX_FIFO1_NEW_MESSAGE, 0);
//  HAL_FDCAN_ActivateNotification(&hfdcan3, FDCAN_IT_TX_FIFO_EMPTY, 0);
  HAL_FDCAN_ActivateNotification(&hfdcan1, FDCAN_IT_RX_FIFO0_NEW_MESSAGE 
                                                              | FDCAN_IT_TX_EVT_FIFO_NEW_DATA 
                                                              | FDCAN_IT_RAM_ACCESS_FAILURE, 0);//加一个RAM访问失败中断
  HAL_FDCAN_ActivateNotification(&hfdcan2, FDCAN_IT_RX_FIFO1_NEW_MESSAGE 
                                                              | FDCAN_IT_TX_EVT_FIFO_NEW_DATA
                                                              | FDCAN_IT_RAM_ACCESS_FAILURE, 0);
  HAL_FDCAN_ActivateNotification(&hfdcan3, FDCAN_IT_RX_FIFO1_NEW_MESSAGE 
                                                              | FDCAN_IT_TX_EVT_FIFO_NEW_DATA
                                                              | FDCAN_IT_RAM_ACCESS_FAILURE, 0);
  
  /* __ ,启动！ */
  HAL_FDCAN_Start(&hfdcan1);
  HAL_FDCAN_Start(&hfdcan2);
  HAL_FDCAN_Start(&hfdcan3);
  
  BSP_FDCAN_RegisterCallback(BSP_FDCAN_1, HAL_FDCAN_RX_FIFO0_MSG_PENDING_CB, BSP_FDCAN_RxFifo0Callback);
  BSP_FDCAN_RegisterCallback(BSP_FDCAN_1, HAL_FDCAN_TX_EVENT_FIFO_CB, BSP_FDCAN_TxCompleteCallback);
  
  BSP_FDCAN_RegisterCallback(BSP_FDCAN_2, HAL_FDCAN_RX_FIFO1_MSG_PENDING_CB, BSP_FDCAN_RxFifo1Callback);
  BSP_FDCAN_RegisterCallback(BSP_FDCAN_2, HAL_FDCAN_TX_EVENT_FIFO_CB, BSP_FDCAN_TxCompleteCallback);

  BSP_FDCAN_RegisterCallback(BSP_FDCAN_3, HAL_FDCAN_RX_FIFO1_MSG_PENDING_CB, BSP_FDCAN_RxFifo1Callback);
  BSP_FDCAN_RegisterCallback(BSP_FDCAN_3, HAL_FDCAN_TX_EVENT_FIFO_CB, BSP_FDCAN_TxCompleteCallback);
  

  
  //POWER ENABLE
  // HAL_GPIO_WritePin(GPIO_TypeDef *GPIOx, uint16_t GPIO_Pin, GPIO_PinState PinState);

  return BSP_OK;
}

FDCAN_HandleTypeDef *BSP_FDCAN_GetHandle(BSP_FDCAN_t fdcan) {
    if (fdcan >= BSP_FDCAN_NUM) return NULL;
    switch (fdcan) {
        case BSP_FDCAN_1: return &hfdcan1;
        case BSP_FDCAN_2: return &hfdcan2;
        case BSP_FDCAN_3: return &hfdcan3;
        default: return NULL;
    }
}

int8_t BSP_FDCAN_RegisterCallback(BSP_FDCAN_t fdcan, BSP_FDCAN_Callback_t type, void (*callback)(void)) {
    if (!inited) return BSP_ERR_INITED;
    if (callback == NULL) return BSP_ERR_NULL;
    if (fdcan >= BSP_FDCAN_NUM) return BSP_ERR;
    if (type >= HAL_FDCAN_CB_NUM) return BSP_ERR;
    FDCAN_Callback[fdcan][type] = callback;
    return BSP_OK;
}
uint32_t fifolevel=0;
int8_t BSP_FDCAN_Transmit(BSP_FDCAN_t fdcan, BSP_FDCAN_Format_t format, uint32_t id, uint8_t *data, uint8_t dlc) {
    if (!inited) return BSP_ERR_INITED;
    if (fdcan >= BSP_FDCAN_NUM) return BSP_ERR;
    if (data == NULL && format != BSP_FDCAN_FORMAT_STD_REMOTE && format != BSP_FDCAN_FORMAT_EXT_REMOTE) return BSP_ERR_NULL;
    if (dlc > BSP_FDCAN_MAX_DLC) return BSP_ERR;
    FDCAN_HandleTypeDef *hfdcan = BSP_FDCAN_GetHandle(fdcan);
    if (hfdcan == NULL) return BSP_ERR_NULL;

    BSP_FDCAN_TxMessage_t tx_msg = {0};
    switch (format) {
        case BSP_FDCAN_FORMAT_STD_DATA:
            tx_msg.header.Identifier = id;
            tx_msg.header.IdType = FDCAN_STANDARD_ID;
            tx_msg.header.TxFrameType = FDCAN_DATA_FRAME;
            break;
        case BSP_FDCAN_FORMAT_EXT_DATA:
            tx_msg.header.Identifier = id;
            tx_msg.header.IdType = FDCAN_EXTENDED_ID;
            tx_msg.header.TxFrameType = FDCAN_DATA_FRAME;
            break;
        case BSP_FDCAN_FORMAT_STD_REMOTE:
            tx_msg.header.Identifier = id;
            tx_msg.header.IdType = FDCAN_STANDARD_ID;
            tx_msg.header.TxFrameType = FDCAN_REMOTE_FRAME;
            break;
        case BSP_FDCAN_FORMAT_EXT_REMOTE:
            tx_msg.header.Identifier = id;
            tx_msg.header.IdType = FDCAN_EXTENDED_ID;
            tx_msg.header.TxFrameType = FDCAN_REMOTE_FRAME;
            break;
        default:
            return BSP_ERR;
    }
	tx_msg.header.ErrorStateIndicator = FDCAN_ESI_ACTIVE;
	tx_msg.header.BitRateSwitch = FDCAN_BRS_OFF;
	tx_msg.header.FDFormat = FDCAN_CLASSIC_CAN;
    tx_msg.header.TxEventFifoControl = FDCAN_STORE_TX_EVENTS;
	tx_msg.header.MessageMarker = 0x01;
//    tx_msg.header.DataLength = FDCAN_DLC_BYTES_64;
	if (dlc <= 8) {
    tx_msg.header.DataLength = FDCAN_DLC_BYTES_8;
  } else if (dlc <= 24) {
    tx_msg.header.DataLength = FDCAN_DLC_BYTES_24;
  } else if (dlc < 32) {
    tx_msg.header.DataLength = FDCAN_DLC_BYTES_32;
  } else if (dlc < 48) {
    tx_msg.header.DataLength = FDCAN_DLC_BYTES_48;
  } else {
    tx_msg.header.DataLength = FDCAN_DLC_BYTES_64;
  }
    if (data != NULL && dlc > 0) 
        memcpy(tx_msg.data, data, dlc);
	fifolevel=HAL_FDCAN_GetTxFifoFreeLevel(hfdcan);
    if (HAL_FDCAN_GetTxFifoFreeLevel(hfdcan) > 0) {
        if (HAL_FDCAN_AddMessageToTxFifoQ(hfdcan, &tx_msg.header, tx_msg.data) == HAL_OK) return BSP_OK;
    }
    if (BSP_FDCAN_TxQueuePush(fdcan, &tx_msg)) return BSP_OK;
    return BSP_ERR;
}

int8_t BSP_FDCAN_TransmitStdDataFrame(BSP_FDCAN_t fdcan, BSP_FDCAN_StdDataFrame_t *frame) {
    if (frame == NULL) return BSP_ERR_NULL;
    return BSP_FDCAN_Transmit(fdcan, BSP_FDCAN_FORMAT_STD_DATA, frame->id, frame->data, frame->dlc);
}

int8_t BSP_FDCAN_TransmitExtDataFrame(BSP_FDCAN_t fdcan, BSP_FDCAN_ExtDataFrame_t *frame) {
    if (frame == NULL) return BSP_ERR_NULL;
    return BSP_FDCAN_Transmit(fdcan, BSP_FDCAN_FORMAT_EXT_DATA, frame->id, frame->data, frame->dlc);
}

int8_t BSP_FDCAN_TransmitRemoteFrame(BSP_FDCAN_t fdcan, BSP_FDCAN_RemoteFrame_t *frame) {
    if (frame == NULL) return BSP_ERR_NULL;
    BSP_FDCAN_Format_t format = frame->is_extended ? BSP_FDCAN_FORMAT_EXT_REMOTE : BSP_FDCAN_FORMAT_STD_REMOTE;
    return BSP_FDCAN_Transmit(fdcan, format, frame->id, NULL, frame->dlc);
}

int8_t BSP_FDCAN_RegisterId(BSP_FDCAN_t fdcan, uint32_t can_id, uint8_t queue_size) {
    if (!inited) return BSP_ERR_INITED;
    return BSP_FDCAN_CreateIdQueue(fdcan, can_id, queue_size);
}

int8_t BSP_FDCAN_GetMessage(BSP_FDCAN_t fdcan, uint32_t can_id, BSP_FDCAN_Message_t *msg, uint32_t timeout) {
    if (!inited) return BSP_ERR_INITED;
    if (msg == NULL) return BSP_ERR_NULL;
    if (osMutexAcquire(queue_mutex, FDCAN_QUEUE_MUTEX_TIMEOUT) != osOK) return BSP_ERR_TIMEOUT;
    osMessageQueueId_t queue = BSP_FDCAN_FindQueue(fdcan, can_id);
    osMutexRelease(queue_mutex);
    if (queue == NULL) return BSP_ERR_NO_DEV;
    osStatus_t res = osMessageQueueGet(queue, msg, NULL, timeout);
    return (res == osOK) ? BSP_OK : BSP_ERR;
}

int32_t BSP_FDCAN_GetQueueCount(BSP_FDCAN_t fdcan, uint32_t can_id) {
    if (!inited) return -1;
    if (osMutexAcquire(queue_mutex, FDCAN_QUEUE_MUTEX_TIMEOUT) != osOK) return -1;
    osMessageQueueId_t queue = BSP_FDCAN_FindQueue(fdcan, can_id);
    osMutexRelease(queue_mutex);
    if (queue == NULL) return -1;
    return (int32_t)osMessageQueueGetCount(queue);
}

int8_t BSP_FDCAN_FlushQueue(BSP_FDCAN_t fdcan, uint32_t can_id) {
    if (!inited) return BSP_ERR_INITED;
    if (osMutexAcquire(queue_mutex, FDCAN_QUEUE_MUTEX_TIMEOUT) != osOK) return BSP_ERR_TIMEOUT;
    osMessageQueueId_t queue = BSP_FDCAN_FindQueue(fdcan, can_id);
    osMutexRelease(queue_mutex);
    if (queue == NULL) return BSP_ERR_NO_DEV;
    BSP_FDCAN_Message_t tmp;
    while (osMessageQueueGet(queue, &tmp, NULL, BSP_FDCAN_TIMEOUT_IMMEDIATE) == osOK) { }
    return BSP_OK;
}

int8_t BSP_FDCAN_RegisterIdParser(BSP_FDCAN_IdParser_t parser) {
    if (!inited) return BSP_ERR_INITED;
    if (parser == NULL) return BSP_ERR_NULL;
    id_parser = parser;
    return BSP_OK;
}

uint32_t BSP_FDCAN_ParseId(uint32_t original_id, BSP_FDCAN_FrameType_t frame_type) {
    if (id_parser != NULL) return id_parser(original_id, frame_type);
    return BSP_FDCAN_DefaultIdParser(original_id, frame_type);
}
/* */