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

volatile CanBuffer can_rx_buffer;

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;

    can_buffer_init(&can_rx_buffer);
}

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 can_receive(void)
{
    if ((can_handle.Instance->RF0R & CAN_RF0R_FMP0) != 0)
    {
        HAL_StatusTypeDef status = HAL_CAN_Receive(&can_handle, 0, 0);

        if ((status == HAL_OK) & ((can_handle.pRxMsg->StdId == can_rx_id) | (can_handle.pRxMsg->StdId == can_broadcast_id)))
        {
            can_buffer_add(&can_rx_buffer, can_handle.pRxMsg);
        }
        //can_handle.Instance->RF0R |= CAN_RF0R_RFOM0;

        led_blink_once(LED_CAN, 250);
    }
}

void can_process_receive_buffer(void)
{
    CanRxMsgTypeDef msg;
    uint8_t size = can_buffer_size(&can_rx_buffer);

    for (uint8_t i = 0; i < size; i++)
    {
        msg = *can_buffer_remove(&can_rx_buffer);
        protocol_receive_message(&msg);
    }
}

void HAL_CAN_RxCpltCallback(CAN_HandleTypeDef* hcan)
{
    if ((hcan->pRxMsg->StdId == can_rx_id) | (hcan->pRxMsg->StdId == can_broadcast_id))
    {
        can_buffer_add(&can_rx_buffer, hcan->pRxMsg);
    }

    led_blink_once(LED_ERROR, 500);
    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);
}