#include "Mezz_send_via_CAN.h"

uint32_t validity_can = 8;
uint32_t mailBoxTx;
volatile int32_t throttle_sent_via_CAN_Left = 0;
volatile int32_t throttle_sent_via_CAN_Right = 0;

extern CAN_HandleTypeDef hcan1;
uint8_t initCAN_flag = 1;

uint8_t Mezz_send_via_CAN(int32_t throttle_request_Left, int32_t throttle_request_Right)
{
#ifdef MACRON

	if(initCAN_flag == 1)
	{
		init_CAN();
		initCAN_flag = 0;
	}

	throttle_sent_via_CAN_Left = throttle_request_Left;
	throttle_sent_via_CAN_Right = throttle_request_Right;
	CAN_send_throttle( throttle_request_Left, 0x000);
	CAN_send_throttle( throttle_request_Right, 0x001);
#endif

return validity_can;
}
#ifdef MACRON


/*
 * Sending function of the throttle
 */
void CAN_send_throttle(int32_t throttle, uint32_t esc_ID)
{
	CAN_TxHeaderTypeDef TxMessage;
	uint8_t data[8] = {0,0,0,0,0,0};
	TxMessage.ExtId = esc_ID;
	TxMessage.DLC = 8;
	TxMessage.RTR = CAN_RTR_DATA;
	TxMessage.IDE = CAN_ID_EXT;
	TxMessage.TransmitGlobalTime = DISABLE;


	data[7] = 0;
	data[6] = 0;
	data[5] = 0;
	data[4] = 0;
	data[3] = (uint8_t) throttle;
	data[2] = (uint8_t) (throttle >> 8);
	data[1] = (uint8_t) (throttle >> 16);
	data[0] = (uint8_t) (throttle >> 24);

HAL_CAN_AddTxMessage(&hcan1,&TxMessage,data,&mailBoxTx);

}


/*
 * Initialisation of the can filters
 */
void init_CAN(void)
{
	  CAN_FilterTypeDef sFilterConfig;

	  sFilterConfig.FilterBank = 0;
	  sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK;
		sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT;
		sFilterConfig.FilterIdHigh = 0x0000;
		sFilterConfig.FilterIdLow = 0x0000;
		sFilterConfig.FilterMaskIdHigh = 0x0000;
		sFilterConfig.FilterMaskIdLow = 0x0000;
		sFilterConfig.FilterFIFOAssignment = CAN_RX_FIFO0;
		sFilterConfig.FilterActivation = ENABLE;
		sFilterConfig.SlaveStartFilterBank = 14;

	    if(HAL_CAN_ConfigFilter(&hcan1, &sFilterConfig) != HAL_OK)
	    {
	      Error_Handler();
	    }

	    HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 1, 0);
	    HAL_NVIC_EnableIRQ(CAN1_RX0_IRQn);
	    HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING);

	    if(HAL_CAN_Start(&hcan1) != HAL_OK)
		{
		  Error_Handler();
		}
}
#endif /* MACRO */