#include <cstdint>
#include "hardware/5400tp194.h"
#include "can_test.h"

void basiccan_test() {
	//enable GPIO alternative function
	GPIOA->altf1 = 0xF00;

	//enable clock
	CMM->apb0clken |= CMM_APB0CLKEN_CAN0 | CMM_APB0CLKEN_CAN1;


	//Mode - Basic CAN
	CAN0->basic.clkDivider = 0x0;
	CAN0->basic.clkDivider = 0x0;

	//Prescaler = 3, sync jump width = 1
	CAN0->basic.busTiming0 = 0x83;

	//Time segment 1 = 5, Time segment 2 = 2, SAM = 1
	CAN0->basic.busTiming1 = 0xA5;


	//Mode - Basic CAN
	CAN1->basic.clkDivider = 0x0;

	//prescaler = 3, sync jump width = 1
	CAN1->basic.busTiming0 = 0x83;

	//Time segment 1 = 5, Time segment 2 = 2, SAM = 1
	CAN1->basic.busTiming1 = 0xA5;

	//Acceptance code
	CAN1->basic.acceptCode = 0xF1;

	//Acceptance mask
	CAN1->basic.acceptMask = 0x00;

	//Clear receiver buffer
	CAN1->basic.command = 4;


	//Leave reset mode
	CAN1->basic.control = 0;
	CAN0->basic.control = 0;


	//ID bytes 10-3 = 0xF1, ID bytes 2-0 = 0b000
	//RTR bit = 0, DLC = 8
	CAN0->basic.txId[0] = 0xF1;
	CAN0->basic.txId[1] = 0x08;


	//Data to TX
	CAN0->basic.txData[0] = 0xB1;
	CAN0->basic.txData[1] = 0xA4;
	CAN0->basic.txData[2] = 0xD6;
	CAN0->basic.txData[3] = 0xAE;
	CAN0->basic.txData[4] = 0xCA;
	CAN0->basic.txData[5] = 0x35;
	CAN0->basic.txData[6] = 0x65;
	CAN0->basic.txData[7] = 0x2F;


	//Transmission request
	CAN0->basic.command = 0x1;


	//Check for transmission completion
	while ((CAN0->basic.status & 0x08 )!= 0x08);


	//Check for data in receiver
	while ((CAN1->basic.status & 0x01) == 0)


	//Check data in receiver
	if (CAN1->basic.rxData[0] != 0xB1) while(1);
	if (CAN1->basic.rxData[1] != 0xA4) while(1);
	if (CAN1->basic.rxData[2] != 0xD6) while(1);
	if (CAN1->basic.rxData[3] != 0xAE) while(1);
	if (CAN1->basic.rxData[4] != 0xCA) while(1);
	if (CAN1->basic.rxData[5] != 0x35) while(1);
	if (CAN1->basic.rxData[6] != 0x65) while(1);
	if (CAN1->basic.rxData[7] != 0x2F) while(1);


	//Switch to reset mode
	CAN0->basic.control = 1;
	CAN1->basic.control = 1;


	//Clear receiver buffer
	CAN1->basic.command = 4;


	//New acceptance code and mask
	CAN1->basic.acceptCode = 0xEA;
	CAN1->basic.acceptMask = 0x00;


	//Leave reset mode
	CAN0->basic.control = 0;
	CAN1->basic.control = 0;


	//ID bytes 10-3 = 0xEA, ID bytes 2-0 = 0b111
	//RTR bit = 0, DLC = 8
	CAN0->basic.txId[0] = 0xEA;
	CAN0->basic.txId[1] = 0xE8;


	//Data to TX
	CAN0->basic.txData[0] = 0xAA;
	CAN0->basic.txData[1] = 0xBB;
	CAN0->basic.txData[2] = 0xCC;
	CAN0->basic.txData[3] = 0xDD;
	CAN0->basic.txData[4] = 0xEE;
	CAN0->basic.txData[5] = 0x02;
	CAN0->basic.txData[6] = 0xF2;
	CAN0->basic.txData[7] = 0x92;


	//Transmission request
	CAN0->basic.command = 0;
	CAN0->basic.command = 0x1;


	//Check for transmission completion
	while ((CAN0->basic.status & 0x08) != 0x08);


	//Check for data in receiver
	while ((CAN1->basic.status & 0x01) != 0x01);


	//Check data in receiver
	if (CAN1->basic.rxData[0] != 0xAA) while(1);
	if (CAN1->basic.rxData[1] != 0xBB) while(1);
	if (CAN1->basic.rxData[2] != 0xCC) while(1);
	if (CAN1->basic.rxData[3] != 0xDD) while(1);
	if (CAN1->basic.rxData[4] != 0xEE) while(1);
	if (CAN1->basic.rxData[5] != 0x02) while(1);
	if (CAN1->basic.rxData[6] != 0xF2) while(1);
	if (CAN1->basic.rxData[7] != 0x92) while(1);


	//Clear receiver buffer
	CAN1->basic.command = 0x4;


	//Switch to reset mode
	CAN0->basic.control = 1;
	CAN1->basic.control = 1;


	//Disable GPIO alternative function and clock
	GPIOA->altf1 = 0x0;
	CMM->apb0clken &= ~(1 << 21 | 1 << 22);
}

void pelican_test() {
	//Enable GPIO alternative function
	GPIOA->altf0 = 0;
	GPIOA->altf1 = 0xF00;

	//Enable clock
	CMM->apb0clken |= CMM_APB0CLKEN_CAN0 | CMM_APB0CLKEN_CAN1;


	//Mode - PeliCAN
	CAN1->peli.clkDivider = 0x80;
	CAN1->peli.clkDivider = 0x80;


	//Prescaler = 3, sync jump width = 1
	CAN1->peli.busTiming0 = 0x83;

	//Time segment 1 = 5, Time segment 2 = 2, SAM = 1
	CAN1->peli.busTiming1 = 0xA5;


	//Mode - PeliCAN
	CAN0->peli.clkDivider = 0x80;

	//prescaler = 3, sync jump width = 1
	CAN0->peli.busTiming0 = 0x83;

	//Time segment 1 = 5, Time segment 2 = 2, SAM = 1
	CAN0->peli.busTiming1 = 0xA5;


	//Error warning limit = 1
	CAN0->peli.errorWarningLimit = 0x1;


	//Acceptance code 0 = 0xAA
	CAN1->peli.txFiSff = 0xAA;

	//Acceptance code 1 = 0xE0
	CAN1->peli.txData[0] = 0xE0;

	//Acceptance code 2 = 0xAA
	CAN1->peli.txData[1] = 0xAA;

	//Acceptance code 3 = 0xE0
	CAN1->peli.txData[2] = 0xE0;


	//Leave reset mode, double filter
	CAN0->peli.mode = 0;
	CAN1->peli.mode = 0;


	//TX frame information, EFF, DLC = 8
	CAN0->peli.txFiSff = 0x88;


	//TX ID0 = 0xAA
	CAN0->peli.txData[0] = 0xAA;

	//TX ID1 = 0xE0
	CAN0->peli.txData[1] = 0xE0;

	//TX ID2 = 0xEA
	CAN0->peli.txData[2] = 0xEA;

	//TX ID3 = 0xFF
	CAN0->peli.txData[3] = 0xFF;


	//TX Data
	CAN0->peli.txData[4] = 0x3A;
	CAN0->peli.txData[5] = 0x8D;
	CAN0->peli.txData[6] = 0xCC;
	CAN0->peli.txData[7] = 0xE3;
	CAN0->peli.txData[8] = 0x0A;
	CAN0->peli.txData[9] = 0xFF;
	CAN0->peli.txData[10] = 0xBA;
	CAN0->peli.txData[11] = 0x72;


	//Transmission request
	CAN0->peli.command = 0x01;


	//Check for transmission completion
	while((CAN0->peli.status & 0x08) != 0x08);


	//Check for data in receiver
	while((CAN1->peli.status & 0x01) != 0x01);


	//Check data in receiver
	if(CAN1->peli.rxData[4] != 0x3A) while(1);
	if(CAN1->peli.rxData[5] != 0x8D) while(1);
	if(CAN1->peli.rxData[6] != 0xCC) while(1);
	if(CAN1->peli.rxData[7] != 0xE3) while(1);
	if(CAN1->peli.rxData[8] != 0x0A) while(1);
	if(CAN1->peli.rxData[9] != 0xFF) while(1);
	if(CAN1->peli.rxData[10] != 0xBA) while(1);
	if(CAN1->peli.rxData[11] != 0x72) while(1);


	//Switch to reset mode
	CAN0->peli.mode = 1;
	CAN1->peli.mode = 1;


	//Disable GPIO alternative function and clock
	GPIOA->altf1 = 0x0;
	CMM->apb0clken &= ~(1 << 21 | 1 << 22);
}

void can_test() {
	basiccan_test();
	pelican_test();
}
