#include "cmsis_os2.h"  // CMSIS-RTOS2 header file
#include "CanDataTask.h"
#include "fdcan.h"
#include "CanDataHandler.h"
#include "FreeRTOS.h"


// Define thread flags
#define FLAG_FDCAN_RX_FIFO0  (1<<0)
#define FLAG_FDCAN_RX_FIFO1  (1<<1)


// Memory for the task
StaticTask_t CanDataTask_cb;
uint32_t CanDataTask_stk[512];
// Attributes for the task
osThreadId_t CanDataTask_id;
const osThreadAttr_t CanDataTask_attr = {
  .name = "CanDataTask",
  .attr_bits = 0U,
  .cb_mem = &CanDataTask_cb,
  .cb_size = sizeof(CanDataTask_cb),
  .stack_mem = CanDataTask_stk,
  .stack_size = sizeof(CanDataTask_stk),
  .priority = osPriorityNormal,
  .tz_module = 0U,
  .reserved = 0U
};


uint32_t dlcDecode(uint32_t dlcCode) {
    switch(dlcCode) {
        case FDCAN_DLC_BYTES_0:  return 0;
        case FDCAN_DLC_BYTES_1:  return 1;
        case FDCAN_DLC_BYTES_2:  return 2;
        case FDCAN_DLC_BYTES_3:  return 3;
        case FDCAN_DLC_BYTES_4:  return 4;
        case FDCAN_DLC_BYTES_5:  return 5;
        case FDCAN_DLC_BYTES_6:  return 6;
        case FDCAN_DLC_BYTES_7:  return 7;
        case FDCAN_DLC_BYTES_8:  return 8;
        case FDCAN_DLC_BYTES_12: return 12;
        case FDCAN_DLC_BYTES_16: return 16;
        case FDCAN_DLC_BYTES_20: return 20;
        case FDCAN_DLC_BYTES_24: return 24;
        case FDCAN_DLC_BYTES_32: return 32;
        case FDCAN_DLC_BYTES_48: return 48;
        case FDCAN_DLC_BYTES_64: return 64;
        default: return 0; // Return 0 for unknown dlc
    }
}


void CanDataTask_func(void *argument);

void CanDataTask_start() {
  // Task functionality here
   CanDataTask_id = osThreadNew(CanDataTask_func, NULL, &CanDataTask_attr);
}

static FDCAN_RxHeaderTypeDef RxHeader;
static uint8_t RxData[8];
void CanDataTask_HandleFifo(uint32_t fifo) {
    while (HAL_FDCAN_GetRxFifoFillLevel(&hfdcan1, FDCAN_RX_FIFO0) > 0 ) {
        if (HAL_FDCAN_GetRxMessage(&hfdcan1, fifo, &RxHeader, RxData) != HAL_OK) {
            Error_Handler();
        } else {
            CanData_canFifo0RxCallback(RxHeader.Identifier,RxData, dlcDecode(RxHeader.DataLength));
        }
    }
}

// Function for the task
void CanDataTask_func(void *argument) {
    /* Configure global filter on FDCAN instanc:
       Filter all remote frames with STD and EXT ID
       Reject non matching frames with STD ID and EXT ID */
    if (HAL_FDCAN_ConfigGlobalFilter(&hfdcan1, FDCAN_REJECT, FDCAN_REJECT, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE) != HAL_OK) {
        Error_Handler();
    }

    /* Start the FDCAN module */
    if (HAL_FDCAN_Start(&hfdcan1) != HAL_OK){
        Error_Handler();
    }


    for(;;) {
        // wait for interrupt event on any fifo
        osThreadFlagsWait(FLAG_FDCAN_RX_FIFO0 | FLAG_FDCAN_RX_FIFO1, osFlagsWaitAny, osWaitForever);
        // check the fifos for data and handle it if nessessay
        CanDataTask_HandleFifo(FDCAN_RX_FIFO0);
        CanDataTask_HandleFifo(FDCAN_RX_FIFO1);
    }   

}


void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo0ITs) {
    // Notify the thread
    osThreadFlagsSet(CanDataTask_id, FLAG_FDCAN_RX_FIFO0);
}

void HAL_FDCAN_RxFifo1Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo1ITs) {
    // Notify the thread
    osThreadFlagsSet(CanDataTask_id, FLAG_FDCAN_RX_FIFO1);
}