/*
 * CAN.c

 *
 *  Created on: Dec 04, 2018
 *      Author: v0stap
 */

#include "main.h"
#include "CAN.h"

can_msg_typedef CAN_TX_Msg;					// CAN message for transmit
can_msg_typedef CAN_RX_Msg;					// CAN received message
uint8_t CAN_TX_Rdy = 0;				// CAN hardware ready to transmit a message
uint8_t CAN_RX_Rdy = 0;				// CAN hardware message received
uint32_t CAN_Speed = CAN_500KBS;		// CAN Speed / CAN BTR register
can_buffer_typedef CAN_TX_Buffer, CAN_RX_Buffer;
uint8_t CAN_Filter_Idx = 0;			// CAN filter index

void CAN_Init(void) {
	CAN_TX_Buffer.todo = 0;
	CAN_TX_Buffer.corent = 0;
	CAN_TX_Buffer.done = 0;
	CAN_RX_Buffer.todo = 0;
	CAN_RX_Buffer.corent = 0;
	CAN_RX_Buffer.done = 0;
	CAN_Setup(CAN_500KBS);
	CAN_Write_Filter(0x212, STD_FORMAT);
	CAN_Write_Filter(0x643, STD_FORMAT);
	//CAN_Write_Filter(0x620, STD_FORMAT);
	//CAN_Write_Filter(0x630, STD_FORMAT);
	//CAN_Write_Filter(0x240, STD_FORMAT);
	CAN_Start();
	CAN_Wait_Ready();
}
/*----------------------------------------------------------------------------
 setup CAN interface
 *----------------------------------------------------------------------------*/
void CAN_Setup(uint32_t speed) {

	RCC->AHBENR |= RCC_AHBENR_GPIOBEN;
	RCC->APB1ENR |= RCC_APB1ENR_CANEN;
	GPIOB->MODER &= ~((3 << (8 * 2)) | (3 << (9 * 2)));
	GPIOB->MODER |= ((2 << (8 * 2)) | (2 << (9 * 2)));
	GPIOB->OTYPER &= ~((1 << 8) | (1 << 9));
	GPIOB->OSPEEDR &= ~((3 << (8 * 2)) | (3 << (9 * 2)));
	GPIOB->PUPDR &= ~((3 << (8 * 2)) | (3 << (9 * 2)));
	GPIOB->AFR[1] &= ~((15 << (0 * 4)) | (15 << (1 * 4)));
	GPIOB->AFR[1] |= ((4 << 0) | (4 << (1 * 4)));
	CAN->MCR |= CAN_MCR_INRQ;
	while ((CAN->MSR & CAN_MSR_INAK) != CAN_MSR_INAK) {
		//some code
	}
	CAN->MCR &= ~ CAN_MCR_SLEEP;
	CAN->BTR &= ~(((0x03) << 24) | ((0x07) << 20) | ((0x0F) << 16) | (0x1FF));
	CAN->BTR |= speed;
	NVIC_EnableIRQ(CEC_CAN_IRQn);
	CAN->IER |= CAN_IER_FMPIE0;
}

/*----------------------------------------------------------------------------
 leave initialisation mode
 *----------------------------------------------------------------------------*/
void CAN_Start(void) {
	CAN->MCR &= ~CAN_MCR_INRQ;
	while ((CAN->MSR & CAN_MSR_INAK) == CAN_MSR_INAK) {

	}

}

/*----------------------------------------------------------------------------
 check if transmit mailbox is empty
 *----------------------------------------------------------------------------*/
void CAN_Wait_Ready(void) {
	while ((CAN->TSR & CAN_TSR_TME0) == 0) {      // Transmit mailbox 0 is empty

	}
	CAN_TX_Rdy = 1;
}

// CAN send data
void CAN_Send_Msg(can_msg_typedef *msg) {
	CAN_TX_Rdy = 0;
	CAN->sTxMailBox[0].TIR = (uint32_t) 0;
	if (msg->format == STD_FORMAT) {
		CAN->sTxMailBox[0].TIR |= (uint32_t) (msg->id << 21) | CAN_ID_STD;
	} else {
		CAN->sTxMailBox[0].TIR |= (uint32_t) (msg->id << 3) | CAN_ID_EXT;
	}
	if (msg->frame == DATA_FRAME) {
		CAN->sTxMailBox[0].TIR |= CAN_RTR_DATA;
	} else {
		CAN->sTxMailBox[0].TIR |= CAN_RTR_REMOTE;
	}
	CAN->sTxMailBox[0].TDLR = (((uint32_t) msg->data[3] << 24)
			| ((uint32_t) msg->data[2] << 16) | ((uint32_t) msg->data[1] << 8)
			| ((uint32_t) msg->data[0]));
	CAN->sTxMailBox[0].TDHR = (((uint32_t) msg->data[7] << 24)
			| ((uint32_t) msg->data[6] << 16) | ((uint32_t) msg->data[5] << 8)
			| ((uint32_t) msg->data[4]));
	CAN->sTxMailBox[0].TDTR &= ~CAN_TDT0R_DLC;
	CAN->sTxMailBox[0].TDTR |= (msg->lenght & CAN_TDT0R_DLC);
	CAN->IER |= CAN_IER_TMEIE;
	CAN->sTxMailBox[0].TIR |= CAN_TI0R_TXRQ;
}
// Read CAN message
void CAN_Recive_Msg(can_msg_typedef *msg) {
	if ((CAN->sFIFOMailBox[0].RIR & CAN_ID_EXT) == 0) {
		msg->format = STD_FORMAT;
		msg->id = (uint32_t) 0x000007FF & (CAN->sFIFOMailBox[0].RIR >> 21);
	} else {
		msg->format = EXTD_FORMAT;
		msg->id = (uint32_t) 0x0003FFFF & (CAN->sFIFOMailBox[0].RIR >> 3);
	}
	if ((CAN->sFIFOMailBox[0].RIR & CAN_RTR_REMOTE) == 0) {
		msg->frame = DATA_FRAME;
	} else {
		msg->frame = REMOTE_FRAME;
	}
	msg->lenght = (uint8_t) 0x0000000F & CAN->sFIFOMailBox[0].RDTR;
	msg->data[0] = (uint32_t) 0x000000FF & (CAN->sFIFOMailBox[0].RDLR);
	msg->data[1] = (uint32_t) 0x000000FF & (CAN->sFIFOMailBox[0].RDLR >> 8);
	msg->data[2] = (uint32_t) 0x000000FF & (CAN->sFIFOMailBox[0].RDLR >> 16);
	msg->data[3] = (uint32_t) 0x000000FF & (CAN->sFIFOMailBox[0].RDLR >> 24);
	msg->data[4] = (uint32_t) 0x000000FF & (CAN->sFIFOMailBox[0].RDHR);
	msg->data[5] = (uint32_t) 0x000000FF & (CAN->sFIFOMailBox[0].RDHR >> 8);
	msg->data[6] = (uint32_t) 0x000000FF & (CAN->sFIFOMailBox[0].RDHR >> 16);
	msg->data[7] = (uint32_t) 0x000000FF & (CAN->sFIFOMailBox[0].RDHR >> 24);
	CAN->RF0R |= CAN_RF0R_RFOM0;

	CAN_RX_Rdy = 0;
//	if (msg->id == 0x643)
//		printf(
//				"CAN ID: 0x%X%X Data: 0x%X 0x%X 0x%X 0x%X 0x%X 0x%X 0x%X 0x%X \n",
//				(uint16_t) (msg->id >> 16), (uint16_t) msg->id, msg->data[0],
//				msg->data[1], msg->data[2], msg->data[3], msg->data[4],
//				msg->data[5], msg->data[6], msg->data[7]);
//	//(&canRxMsg); //TO DO not realy working
}
//Write CAN Filter
void CAN_Write_Filter(uint32_t id, uint8_t format) {
	uint32_t CAN_msg_typedefId = 0;
	if (CAN_Filter_Idx > 13) {
		return;
	}
	if (format == STD_FORMAT) {
		CAN_msg_typedefId |= (uint32_t) (id << 21) | CAN_ID_STD;
	} else {
		CAN_msg_typedefId |= (uint32_t) (id << 3) | CAN_ID_EXT;
	}
	CAN->FMR |= CAN_FMR_FINIT;
	CAN->FA1R &= ~(uint32_t) (1 << CAN_Filter_Idx);
	CAN->FS1R |= (uint32_t) (1 << CAN_Filter_Idx);
	CAN->FM1R |= (uint32_t) (1 << CAN_Filter_Idx);
	//Comment next to lines for no filter
	if (id != 0) {
		CAN->sFilterRegister[CAN_Filter_Idx].FR1 = CAN_msg_typedefId;
		CAN->sFilterRegister[CAN_Filter_Idx].FR2 = CAN_msg_typedefId;
		//Uncomment next to line for no filter
	} else {
		CAN->sFilterRegister[CAN_Filter_Idx].FR1 = 0; //  32-bit identifier
		CAN->sFilterRegister[CAN_Filter_Idx].FR2 = 0; //  32-bit identifier
	}
	CAN->FFA1R &= ~(uint32_t) (1 << CAN_Filter_Idx);
	CAN->FA1R |= (uint32_t) (1 << CAN_Filter_Idx);
	CAN->FMR &= ~CAN_FMR_FINIT;
	CAN_Filter_Idx++;
}
//CAN recive/transmit irq handler
void CEC_CAN_IRQHandler(void) {
	if ((CAN->TSR & CAN_TSR_TME0) == CAN_TSR_TME0) {
		CAN->TSR |= CAN_TSR_RQCP0;
		CAN->IER &= ~CAN_IER_TMEIE;
		CAN_TX_Rdy = 1;
	}
	if ((CAN->RF0R & CAN_RF0R_FMP0) != 0) {
		CAN_Add_RX_Buffer();
		// TO DO
		CAN->RF0R |= CAN_RF0R_RFOM0;
		CAN_RX_Rdy = 1;
	}
}
void CAN_Send_TX_Buffer(void) {

	if (CAN_TX_Rdy) {
		//HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_1);
		if (CAN_TX_Buffer.todo > 0) {
			CAN_TX_Buffer.todo--;
			//HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_1);
			CAN_Send_Msg(&CAN_TX_Buffer.data[CAN_TX_Buffer.done]);
			if ((CAN_TX_Buffer.done++) > 14) {
				CAN_TX_Buffer.done = 0;
			}
		}
	}
}
void CAN_Add_TX_Buffer(can_msg_typedef *data) {
	if ((CAN_TX_Buffer.todo++) < 15) {
		if ((CAN_TX_Buffer.corent++) > 14) {
			CAN_TX_Buffer.corent = 0;
		}
		//memcpy((void*) &canTX_buffer.data[canTX_buffer.corent], (void*) &data, sizeof(CAN_msg_typedef));
		CAN_TX_Buffer.data[CAN_TX_Buffer.corent].id = data->id;
		CAN_TX_Buffer.data[CAN_TX_Buffer.corent].format = data->format;
		CAN_TX_Buffer.data[CAN_TX_Buffer.corent].frame = data->frame;
		CAN_TX_Buffer.data[CAN_TX_Buffer.corent].lenght = data->lenght;
		CAN_TX_Buffer.data[CAN_TX_Buffer.corent].data[0] = data->data[0];
		CAN_TX_Buffer.data[CAN_TX_Buffer.corent].data[1] = data->data[1];
		CAN_TX_Buffer.data[CAN_TX_Buffer.corent].data[2] = data->data[2];
		CAN_TX_Buffer.data[CAN_TX_Buffer.corent].data[3] = data->data[3];
		CAN_TX_Buffer.data[CAN_TX_Buffer.corent].data[4] = data->data[4];
		CAN_TX_Buffer.data[CAN_TX_Buffer.corent].data[5] = data->data[5];
		CAN_TX_Buffer.data[CAN_TX_Buffer.corent].data[6] = data->data[6];
		CAN_TX_Buffer.data[CAN_TX_Buffer.corent].data[7] = data->data[7];
	}
}
void CAN_Add_RX_Buffer(void) {
	if ((CAN_RX_Buffer.todo) < 16) {
		CAN_RX_Buffer.todo++;
		if ((CAN->sFIFOMailBox[0].RIR & CAN_ID_EXT) == 0) {
			CAN_RX_Buffer.data[CAN_RX_Buffer.corent].format = STD_FORMAT;
			CAN_RX_Buffer.data[CAN_RX_Buffer.corent].id = (uint32_t) 0x000007FF
					& (CAN->sFIFOMailBox[0].RIR >> 21);
		} else {
			CAN_RX_Buffer.data[CAN_RX_Buffer.corent].format = EXTD_FORMAT;
			CAN_RX_Buffer.data[CAN_RX_Buffer.corent].id = (uint32_t) 0x0003FFFF
					& (CAN->sFIFOMailBox[0].RIR >> 3);
		}
		if ((CAN->sFIFOMailBox[0].RIR & CAN_RTR_REMOTE) == 0) {
			CAN_RX_Buffer.data[CAN_RX_Buffer.corent].frame = DATA_FRAME;
		} else {
			CAN_RX_Buffer.data[CAN_RX_Buffer.corent].frame = REMOTE_FRAME;
		}
		CAN_RX_Buffer.data[CAN_RX_Buffer.corent].lenght = (uint8_t) 0x0000000F
				& CAN->sFIFOMailBox[0].RDTR;
		CAN_RX_Buffer.data[CAN_RX_Buffer.corent].data[0] = (uint32_t) 0x000000FF
				& (CAN->sFIFOMailBox[0].RDLR);
		CAN_RX_Buffer.data[CAN_RX_Buffer.corent].data[1] = (uint32_t) 0x000000FF
				& (CAN->sFIFOMailBox[0].RDLR >> 8);
		CAN_RX_Buffer.data[CAN_RX_Buffer.corent].data[2] = (uint32_t) 0x000000FF
				& (CAN->sFIFOMailBox[0].RDLR >> 16);
		CAN_RX_Buffer.data[CAN_RX_Buffer.corent].data[3] = (uint32_t) 0x000000FF
				& (CAN->sFIFOMailBox[0].RDLR >> 24);
		CAN_RX_Buffer.data[CAN_RX_Buffer.corent].data[4] = (uint32_t) 0x000000FF
				& (CAN->sFIFOMailBox[0].RDHR);
		CAN_RX_Buffer.data[CAN_RX_Buffer.corent].data[5] = (uint32_t) 0x000000FF
				& (CAN->sFIFOMailBox[0].RDHR >> 8);
		CAN_RX_Buffer.data[CAN_RX_Buffer.corent].data[6] = (uint32_t) 0x000000FF
				& (CAN->sFIFOMailBox[0].RDHR >> 16);
		CAN_RX_Buffer.data[CAN_RX_Buffer.corent].data[7] = (uint32_t) 0x000000FF
				& (CAN->sFIFOMailBox[0].RDHR >> 24);
		if ((CAN_RX_Buffer.corent++) > 15) {
			CAN_RX_Buffer.corent = 0;
		}

		//canAddRXBuffer(&canRxMsg); //TO DO not realy working
	}
	CAN->RF0R |= CAN_RF0R_RFOM0;
}
void CAN_Read_RX_Buffer(can_msg_typedef *data) {
	if (CAN_RX_Buffer.todo > 0) {
		//memcpy((void*) &data, (void*) &canRX_buffer.data[canRX_buffer.done], sizeof(CAN_msg_typedef));
		data->id = CAN_RX_Buffer.data[CAN_RX_Buffer.done].id;
		data->format = CAN_RX_Buffer.data[CAN_RX_Buffer.done].format;
		data->frame = CAN_RX_Buffer.data[CAN_RX_Buffer.done].frame;
		data->lenght = CAN_RX_Buffer.data[CAN_RX_Buffer.done].lenght;
		data->data[0] = CAN_RX_Buffer.data[CAN_RX_Buffer.done].data[0];
		data->data[1] = CAN_RX_Buffer.data[CAN_RX_Buffer.done].data[1];
		data->data[2] = CAN_RX_Buffer.data[CAN_RX_Buffer.done].data[2];
		data->data[3] = CAN_RX_Buffer.data[CAN_RX_Buffer.done].data[3];
		data->data[4] = CAN_RX_Buffer.data[CAN_RX_Buffer.done].data[4];
		data->data[5] = CAN_RX_Buffer.data[CAN_RX_Buffer.done].data[5];
		data->data[6] = CAN_RX_Buffer.data[CAN_RX_Buffer.done].data[6];
		data->data[7] = CAN_RX_Buffer.data[CAN_RX_Buffer.done].data[7];
		if ((CAN_RX_Buffer.done++) > 15) {
			CAN_RX_Buffer.done = 0;
		}
		CAN_RX_Buffer.todo--;
	}
}