#include "CLS.h"
#include "CLSAddress.h"
#include "cmsis_os2.h"
#include "fdcan.h"

osTimerId_t CLS_HeatbeatTimerId; // Timer ID


static uint8_t cls_hartbeat_counter = 0;
static FDCAN_TxHeaderTypeDef cls_hartbeat_header = {
    .IdType = FDCAN_STANDARD_ID,
    .Identifier = GENERATE_CLS_ADDRESS(CLS_CODE_STATUS,CLS_DEVICE,CLS_CH_STA_HEATBEAT),
    .TxFrameType = FDCAN_DATA_FRAME,
    .DataLength = FDCAN_DLC_BYTES_1,
    .ErrorStateIndicator = FDCAN_ESI_PASSIVE,
    .BitRateSwitch = FDCAN_BRS_OFF,
    .FDFormat = FDCAN_CLASSIC_CAN,
    .TxEventFifoControl = FDCAN_NO_TX_EVENTS,
    .MessageMarker = 0xCC,
};

uint8_t TxData[8];
    FDCAN_TxHeaderTypeDef TxHeader;

void CLS_Heatbeat(void *argument) {
    // Code to be executed every 500ms
    cls_hartbeat_counter++;
    if (HAL_FDCAN_GetTxFifoFreeLevel(&hfdcan1) > 1){
        HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1, &cls_hartbeat_header, &cls_hartbeat_counter);
    } 
}



void CLS_Init(void) {
    osTimerAttr_t timerAttr;
    timerAttr.name = "CLS_Heatbeat";
    timerAttr.attr_bits = 0U;
    timerAttr.cb_mem = NULL;
    timerAttr.cb_size = 0U;


    CLS_HeatbeatTimerId = osTimerNew((osTimerFunc_t)CLS_Heatbeat, osTimerPeriodic, NULL, &timerAttr);
    if (CLS_HeatbeatTimerId != NULL) {  // Timer object created
        if (osTimerStart(CLS_HeatbeatTimerId, 500) == osOK) {  // Timer started
            // Timer started successfully
        }
    }
}