#include <c8051f040.h>
#include "sysinit.h"
#include "uart0.h"
#include <stdio.h>
#include "can.h"


static char buf[40]; // for sprintf( )

void init_xbar( ) {
	char save = SFRPAGE;

	SFRPAGE = CONFIG_PAGE;
	XBR0 = 0x04; // enable UART0
	XBR1 = 0x00; // enable nothing
	XBR2 = 0x40; // enable the crossbar
	XBR3 = 0x80; // enable CAN

	P0MDOUT &= ~0x02; // RX pin input
	P0MDOUT |= 0x01; // TX

	// write "0xdead" to the watchdog register, disabling the watchdog
	WDTCN = 0xde;
	WDTCN = 0xad; 

	SFRPAGE = save;
}


void boot_system( ) {
	char SFRPAGE_SAVE = SFRPAGE;        // Save Current SFR page
	SFRPAGE = CONFIG_PAGE;
	
	init_sysclk( );
	init_xbar();
	uart0_init( );
	can_init( );
	printf("Hello World!\r\n");

	EA = 1; /* enable interrupts now that everything is initialized */

	SFRPAGE = SFRPAGE_SAVE;
}

void main( ) {
	CAN_BUFFER canbuf;
	boot_system( );


	while (1) {
		printf("getting transmit buffer\r\n");
		canbuf = can_get_tx_buf( );
		printf("setting message ID\r\n");
		//can_set_address_ext(canbuf, 0x12345678);
		can_set_address_std(canbuf,0x7df);
		printf("setting data\r\n");
		can_set_buffer_data(canbuf, "\x01\x02\x03\x04\x05\x06\x07\x08", 8);
		printf("sending buffer\r\n");
		can_send_tx_buf(canbuf);		
		printf("message sent successfully\r\n");

		if (canbuf = can_get_rx_msg( )) {
			printf("got CAN message in!\r\n");
			printf("ID=%04lx\r\n", can_get_address(canbuf));
			printf("DLC=%d\r\n", can_get_dlc(canbuf));
			printf("data0=%02x\r\n", can_get_data_byte(canbuf, 0));
			printf("data1=%02x\r\n", can_get_data_byte(canbuf, 1));
			printf("data2=%02x\r\n", can_get_data_byte(canbuf, 2));
			printf("data3=%02x\r\n", can_get_data_byte(canbuf, 3));
			printf("data4=%02x\r\n", can_get_data_byte(canbuf, 4));
			printf("data5=%02x\r\n", can_get_data_byte(canbuf, 5));
			printf("data6=%02x\r\n", can_get_data_byte(canbuf, 6));
			printf("data7=%02x\r\n", can_get_data_byte(canbuf, 7));
			can_free_rx_buf(canbuf);
		}
	}

}