#include "can.h"

CAN_HandleTypeDef can_handle;

CAN_FilterConfTypeDef can_filter;

CanRxMsgTypeDef can_rx_msg;

CanTxMsgTypeDef can_tx_msg;

bool can_silenced;

uint32_t can_broadcast_id;

uint32_t can_rx_id;

void can_init(uint32_t rx_id, uint32_t broadcast_id)

{

    can_handle.Instance = CAN;

    can_handle.pRxMsg = &can_rx_msg;

    can_handle.pTxMsg = &can_tx_msg;

    can_handle.Init.Mode = CAN_MODE_NORMAL;

    can_handle.Init.Prescaler = 6;

    can_handle.Init.SJW = CAN_SJW_1TQ;

    can_handle.Init.BS1 = CAN_BS1_11TQ;

    can_handle.Init.BS2 = CAN_BS2_4TQ;

    can_handle.Init.TTCM = DISABLE;

    can_handle.Init.ABOM = ENABLE;

    can_handle.Init.AWUM = DISABLE;

    can_handle.Init.NART = DISABLE;

    can_handle.Init.RFLM = DISABLE;

    can_handle.Init.TXFP = DISABLE;

    HAL_CAN_Init(&can_handle);

    can_filter.FilterNumber = 0;

    can_filter.BankNumber = 0;

    can_filter.FilterMode = CAN_FILTERMODE_IDMASK;

    can_filter.FilterScale = CAN_FILTERSCALE_16BIT;

    uint32_t mask = 0x00000000;

    uint32_t id = 0x00000000;

    can_filter.FilterIdHigh = (id >> 11) & 0xFFFF;

    can_filter.FilterIdLow = (id << 5) & 0xFFFF;

    can_filter.FilterMaskIdHigh = (mask >> 11) & 0xFFFF;

    can_filter.FilterMaskIdLow = (mask << 5) & 0xFFFF;

    can_filter.FilterFIFOAssignment = CAN_FILTER_FIFO0;

    can_filter.FilterActivation = ENABLE;

    HAL_CAN_ConfigFilter(&can_handle, &can_filter);

    HAL_NVIC_SetPriority(CEC_CAN_IRQn, 1, 0);

    HAL_NVIC_EnableIRQ(CEC_CAN_IRQn);

    HAL_CAN_Receive_IT(&can_handle, CAN_FIFO0);

    can_silenced = false;

    can_rx_id = rx_id;

    can_broadcast_id = broadcast_id;

}

void can_send_test(uint16_t id)

{

    can_handle.pTxMsg->StdId = id;

    can_handle.pTxMsg->IDE = CAN_ID_STD;

    can_handle.pTxMsg->RTR = CAN_RTR_DATA;

    can_handle.pTxMsg->DLC = 4;

    can_handle.pTxMsg->Data[0] = 'T'; //0x54

    can_handle.pTxMsg->Data[1] = 'E'; //0x45

    can_handle.pTxMsg->Data[2] = 'S'; //0x53

    can_handle.pTxMsg->Data[3] = 'T'; //0x54

    HAL_CAN_Transmit_IT(&can_handle);

}

bool can_send(uint32_t id, uint32_t ide, uint8_t dlc, uint8_t data[8])

{

    bool result = true;

    if (!can_silenced)

    {

        can_handle.pTxMsg->StdId = id;

        can_handle.pTxMsg->ExtId = id;

        can_handle.pTxMsg->IDE = ide;

        can_handle.pTxMsg->RTR = CAN_RTR_DATA;

        can_handle.pTxMsg->DLC = dlc;

        memcpy(can_handle.pTxMsg->Data, data, 8);

        HAL_CAN_Transmit_IT(&can_handle);

    }

    else

    {

        result = false;

    }

    return result;

}

void can_set_receive_mask(uint32_t mask)

{

    can_filter.FilterMode = CAN_FILTERMODE_IDMASK;

    can_filter.FilterScale = CAN_FILTERSCALE_32BIT;

    can_filter.FilterMaskIdHigh = (mask >> 16) & 0xFFFF;

    can_filter.FilterMaskIdLow = mask & 0xFFFF;

    can_filter.FilterActivation = ENABLE;

    HAL_CAN_ConfigFilter(&can_handle, &can_filter);

}

void can_set_receive_id(uint32_t id)

{

    can_filter.FilterMode = CAN_FILTERMODE_IDMASK;

    can_filter.FilterScale = CAN_FILTERSCALE_32BIT;

    can_filter.FilterIdHigh = (id >> 16) & 0xFFFF;

    can_filter.FilterIdLow = id & 0xFFFF;

    can_filter.FilterActivation = ENABLE;

    HAL_CAN_ConfigFilter(&can_handle, &can_filter);

}

bool can_silence_bus(bool value)

{

    bool result = true;

    can_silenced = value;

    return result;

}

void HAL_CAN_RxCpltCallback(CAN_HandleTypeDef* hcan)

{

    if ((hcan->pRxMsg->StdId == can_rx_id) | (hcan->pRxMsg->StdId == can_broadcast_id))

    {

        protocol_receive_message(hcan->pRxMsg);

        HAL_CAN_Receive_IT(&can_handle, CAN_FIFO0);

    }

}

void HAL_CAN_TxCpltCallback(CAN_HandleTypeDef* hcan) {}

void HAL_CAN_ErrorCallback(CAN_HandleTypeDef* hcan)

{

    led_set(LED_ERROR, 1);

}

#include "protocol.h"

protocol_device_t protocol_device;

flash_settings_t protocol_settings;

void protocol_init(protocol_device_t device)

{

    protocol_device = device;

    flash_restore(&protocol_settings);

    can_init(protocol_settings.val.can_id, DEFAULT_BROADCAST_ID);

}

bool protocol_send_test()

{

    bool result = true;

    can_send_test(protocol_settings.val.can_id | 0x00000001);

    return result;

}

bool protocol_receive_message(CanRxMsgTypeDef* can_message)

{

    bool result = true;

    protocol_message_t message;

    message.command = can_message->Data[0] & 0x80;

    message.id = can_message->Data[0] & 0x7F;

    message.sensor = can_message->Data[3];

    message.data.byte_data[0] = can_message->Data[4];

    message.data.byte_data[1] = can_message->Data[5];

    message.data.byte_data[2] = can_message->Data[6];

    message.data.byte_data[3] = can_message->Data[7];

    protocol_process_message(&message);

    return result;

}

bool protocol_send_message(protocol_message_t* message)

{

    bool result = true;

    uint8_t data[8];

    data[0] = (message->command & 0x80) | (message->id & 0x7F);

    data[1] = (message->key >> 8) & 0xFF;

    data[2] = (message->key >> 0) & 0xFF;

    data[3] = message->sensor;

    data[4] = message->data.byte_data[0];

    data[5] = message->data.byte_data[1];

    data[6] = message->data.byte_data[2];

    data[7] = message->data.byte_data[3];

    can_send(protocol_settings.val.can_id | 0x00000001, CAN_ID_STD, 8, data);

    return result;

}

bool protocol_process_message(protocol_message_t* message)

{

    bool result = true;

    if (message->command)

    {

        switch(message->id)

        {

            case ESTOP:

                {

                    //call estop weak function

                    result = protocol_estop(message->data.float_data!=0.0f);

                }

                break;

            case SILENCE_BUS:

                {

                    //silence can bus

                    result = can_silence_bus(message->data.float_data!=0.0f);

                }

                break;

            case SET_OUTPUT:

                {

                    //call set output weak function

                    result = protocol_set_output(message);

                }

                break;

            case GET_DATA:

                {

                    //call get data weak function

                    result = protocol_get_data(message);

                }

                break;

            case CONFIG:

                {

                    //call config weak function

                    result = protocol_config(message);

                }

                break;

            default:

                result = false;

                break;

        }

    }

    return result;

}

bool protocol_send_data(protocol_data_key_t key, uint8_t sensor, float data)

{

    bool result = true;

    protocol_message_t message;

    message.command = false;

    message.id = protocol_device;

    message.key = key;

    message.sensor = sensor;

    message.data.float_data = data;

    result = protocol_send_message(&message);

    return result;

}

__weak bool protocol_estop(bool value)

{

    return false;

}

__weak bool protocol_set_output(protocol_message_t* message)

{

    return false;

}

__weak bool protocol_get_data(protocol_message_t* message)

{

    return false;

}

__weak bool protocol_config(protocol_message_t* message)

{

    return false;

}