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

osTimerId_t CLS_HeatbeatTimerId = NULL; // Timer ID
osTimerId_t CLS_VehicleTimerId = NULL; // Timer ID
static CLS_HeatbeatData_t cls_heatbeat_data = {0};
static CLS_VehicleStatus_t cls_vehicle = {0};
static uint8_t EventChangeTypeData[2] ={0};
static CLS_Type_t newType = 0;
static CLS_Position_t newPostion = {0};
static bool writeNewSetting = false;

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

    if (writeNewSetting) {
        CLS_BSP_SetDeviceType(newType);
        CLS_BSP_SetPosition(newPostion);
        writeNewSetting = false;
    }
}

__weak void CLS_VehicleHeatbeat(void *argument) {

}

void CLS_SendEventChangeTypePostion(CanDataId canid, CLS_Type_t newType, CLS_Position_t newPostion) {
        const uint16_t cls_address = GENERATE_CLS_ADDRESS(CLS_CODE_CONFIG,canid,CLS_CHANNEL7);
        EventChangeTypeData[0] = (uint8_t)newType;
        EventChangeTypeData[1] = (newPostion.p0 & 0x0F) | ((newPostion.p1 << 4) & 0xF0);
        CLS_BSP_TxHeaderType cls_hartbeat_header = CREATE_BSP_CAN_HEADER(cls_address, CLS_BSP_DLC_BYTES_2);
        CLS_BSP_CAN_AddMessageToSend(&cls_hartbeat_header, (uint8_t*)&EventChangeTypeData);
}

const CLS_VehicleStatus_t * CLS_GetVehicleStatus(void) {
    return &cls_vehicle;
}

void CLS_OnVehicleStatus(CanDataId canid, uint8_t* data, uint8_t len) {
    if(len == 8) {
        memcpy((uint8_t*)&cls_vehicle, data, 8);
    }
}

void CLS_OnEventChangeTypePostion(CanDataId canid, uint8_t* data, uint8_t len) {

    if(len >= 2) {
        newType = data[0];
        newPostion.p0 = (data[1] & 0x0F);
        newPostion.p1 = ((data[1] >> 4) & 0x0F);
        writeNewSetting = true;
    }
}

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
        // add gCLS_DEVICE_ADDRESS to the interval to avoid collisons
        if (osTimerStart(CLS_HeatbeatTimerId, CLS_HEARTBEAT_INTERVAL_MS + gCLS_DEVICE_ADDRESS) == osOK) {  // Timer started
            // Timer started successfully
        }
    }


    if (cls_heatbeat_data.type == cls_device_Type_MASTER) {
            timerAttr.name = "CLS_Vehicle_Heatbeat";
            CLS_VehicleTimerId = osTimerNew((osTimerFunc_t)CLS_VehicleHeatbeat, osTimerPeriodic, NULL, &timerAttr);
            if (CLS_VehicleTimerId != NULL) {  // Timer object created

                if (osTimerStart(CLS_VehicleTimerId, 100) == osOK) {  // Timer started
                    // Timer started successfully
                
                }

         }
    }

    uint16_t cls_address = GENERATE_CLS_ADDRESS(CLS_CODE_CONFIG,gCLS_DEVICE_ADDRESS,CLS_CHANNEL7);
    //setup endpoint for changing the Device Type/Position
    CanData_regEventMsg(cls_address, CLS_OnEventChangeTypePostion);


    uint16_t cls_vehicle_address = GENERATE_CLS_ADDRESS(CLS_CODE_STATUS,0,CLS_CH_STA_VEHICLE);
    CanData_regEventMsg(cls_vehicle_address, CLS_OnVehicleStatus);
}