#include "CLS_BSP.h"
#include "CLS.h"
#include "CLSAddress.h"
#include "cmsis_os2.h"
#include "version_info.h"

osTimerId_t CLS_HeatbeatTimerId; // Timer ID

static CLS_HeatbeatData_t cls_heatbeat_data = {0};

_Static_assert(sizeof(cls_heatbeat_data) == 8, "CLS_HeatbeatData_t is not 8 bytes");

void CLS_Heatbeat(void *argument) {
    // Code to be executed every CLS_HEARTBEAT_INTERVAL_MS


    CLS_BSP_TxHeaderType cls_hartbeat_header = CREATE_BSP_CAN_HEADER(GENERATE_CLS_ADDRESS(CLS_CODE_STATUS,gCLS_DEVICE_ADDRESS,CLS_CH_STA_HEATBEAT), CLS_BSP_DLC_BYTES_8);
    cls_heatbeat_data.counter++;
    CLS_BSP_CAN_AddMessageToSend(&cls_hartbeat_header, (uint8_t*)&cls_heatbeat_data);
}


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

    cls_heatbeat_data.firmware_version.major = VERSION_INFO.major;
    cls_heatbeat_data.firmware_version.minor = VERSION_INFO.minor;
    cls_heatbeat_data.firmware_version.patch = VERSION_INFO.patch;
    cls_heatbeat_data.firmware_version.count = VERSION_INFO.count;
    cls_heatbeat_data.id = gCLS_DEVICE_ADDRESS;
    cls_heatbeat_data.type = CLS_BSP_GetDeviceType();
    cls_heatbeat_data.counter = 0;
    cls_heatbeat_data.position = CLS_BSP_GetPosition();

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