#include "cmsis_os2.h"  // CMSIS-RTOS2 header file
#include "CanDataTask.h"
#include "fdcan.h"
#include "CanDataHandler.h"
#include "FreeRTOS.h"
#include "CLSAddress.h"
#include "firmware.pb.h"
#include "cls_device.pb.h"
#include "usb.pb.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,
};


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

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

    if(HAL_FDCAN_ActivateNotification(&hfdcan1,FDCAN_IT_RX_FIFO0_NEW_MESSAGE | FDCAN_IT_RX_FIFO1_NEW_MESSAGE, 0) != HAL_OK) {
        Error_Handler();
    }


    // setup listening for heartbeats 
    for (size_t i = 0; i < 16; i++)
    {
        CanData_regDataMsg(GENERATE_CLS_ADDRESS(CLS_CODE_STATUS, i, CLS_CH_STA_HEATBEAT));
    }
    


    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
        while (HAL_FDCAN_GetRxFifoFillLevel(&hfdcan1, FDCAN_RX_FIFO0) > 0 ) {
        if (HAL_FDCAN_GetRxMessage(&hfdcan1, FDCAN_RX_FIFO0, &RxHeader, RxData) != HAL_OK) {
            Error_Handler();
        } else {
            CanData_canFifo0RxCallback(RxHeader.Identifier,RxData, dlcDecode(RxHeader.DataLength));
        }
    }
         while (HAL_FDCAN_GetRxFifoFillLevel(&hfdcan1, FDCAN_RX_FIFO1) > 0 ) {
        if (HAL_FDCAN_GetRxMessage(&hfdcan1, FDCAN_RX_FIFO1, &RxHeader, RxData) != HAL_OK) {
            Error_Handler();
        } else {
            CanData_canFifo1RxCallback(RxHeader.Identifier,RxData, dlcDecode(RxHeader.DataLength));
        }
    }
    }   
}


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


void DataClbk_cls_device_ResponseList(void* msg, uint32_t length) {
    // igored
}


#include "UsbDataHandler.h"
cls_device_ResponseList list;
extern uint8_t gCLS_DEVICE_ADDRESS;
void DataClbk_cls_device_RequestList(void* msg, uint32_t length) {
    memset(&list,0,sizeof(list));
    // add yourself
    list.devices[list.devices_count].available = true;
    list.devices[list.devices_count].canid = GENERATE_CLS_ADDRESS(CLS_CODE_STATUS, gCLS_DEVICE_ADDRESS, CLS_CH_STA_HEATBEAT);
    list.devices[list.devices_count].device = gCLS_DEVICE_ADDRESS;
    list.devices[list.devices_count].counter = (osKernelGetTickCount()/500)%256;
    list.devices_count++;

    for (size_t i = 0; i < 16; i++)
    {
        uint16_t canid = (GENERATE_CLS_ADDRESS(CLS_CODE_STATUS, i, CLS_CH_STA_HEATBEAT));
        const CanDataMessage * msg =CanData_getDataMessage(canid);
        if(msg) {
            if(msg->data_length > 0) {
                list.devices[list.devices_count].available = true;
                list.devices[list.devices_count].canid = canid;
                list.devices[list.devices_count].device = i;
                list.devices[list.devices_count].counter = msg->data[0];
                list.devices_count++;
            }
        }
    }
    
    USBDataResonse(&list, cls_device_ResponseList_fields, cls_usb_PackageType_RESPONSE_DEVICE_LIST);

}