/**
 * candevice.c
 * @param config
 */

#include <can/can.h>
#include <sys/errno.h>
#include <sys/cpu.h>
#include <sys/systick.h>

#include <hwo/bits.h>

#include <avr/io.h>

#include <string.h>
#include <stdlib.h>


// How many mailboxes to use for RX
#define CAN_RX_MAILBOXES	12


static _can_device_t
				device;

int can_prepare_mailbox_rx(int mailbox);

uint32_t	can_get_device_config(void){
	return device.setup;
};

int can_init_device(uint32_t config,int rxpool_length) {
	uint8_t n,i;

	memset( &device, 0x00, sizeof(device) );

	list_init( &(device.tx_queue) );

	for (n=0;n<16;n++)
	{
		CANPAGE	= (n << 4);

		CANCDMOB= 0x00;
		CANSTMOB= 0x00;
		CANIDT1	= 0x00;
		CANIDT2	= 0x00;
		CANIDT3	= 0x00;
		CANIDT4	= 0x00;
		CANIDM1	= 0x00;
		CANIDM2	= 0x00;
		CANIDM3	= 0x00;
		CANIDM4	= 0x00;
		CANSTMH	= 0x00;
		CANSTML	= 0x00;

		for (i=0;i<8;i++)
			CANMSG = 0x00;
	};

	CANGCON = _BV(SWRES);

	device.setup = config;
	device.rxpool = malloc( sizeof(canframe_t) * rxpool_length );
	if (device.rxpool == NULL){
		return -ENOMEM;
	};
	memset( device.rxpool, 0x00, sizeof(canframe_t) * rxpool_length );
	device.rxpool_length = rxpool_length;

	switch (__freq_cpu)
	{
		case 16000000:
			switch (config & CAN_SPEED_MASK)
			{
				case CAN_1MBIT:
					CANBT1 = 0x02;
					CANBT2 = 0x04;
					CANBT3 = 0x13;
					break;
				case CAN_125KBIT:
					CANBT1 = 0x0E;
					CANBT2 = 0x0C;
					CANBT3 = 0x37;
					break;
			};
		break;
		case 8000000:
			switch (config & CAN_SPEED_MASK)
			{
				case CAN_1MBIT:
					CANBT1 = 0x00;
					CANBT2 = 0x04;// 0x04;
					CANBT3 = 0x12; //0x12;
					break;
				case CAN_125KBIT:
					CANBT1 = 0x0E; //0x06;
					CANBT2 = 0x04; //0x0c;
					CANBT3 = 0x13; //0x37;
					break;
			};
		break;
	};

	CANTCON = CANBT1;

	CANGIE = _BV(ENIT) | _BV(ENRX) | _BV(ENTX) | _BV(ENERR);
	CANIE1 = 0x7F;
	CANIE2 = 0xFF;

	CANGCON |= _BV(ENASTB);

	{
		ATOMIC

		for (n=0; n < CAN_RX_MAILBOXES; n++){
			can_prepare_mailbox_rx(n);
		}

	}

	CANPAGE = 0;

	return ESUCCESS;
};



static inline uint32_t canid_to_register(canid_t* cid){
	bits32_t raw;

	raw.ui32 = cid->addr;

	if (cid->ide){
		raw.ui32 <<= 3;
	} else
	{
		raw.ui32 <<= 21;
	};

	if (cid->rtr) {
		raw.ui8[0] |= _BV(RTRTAG);
	};

	return raw.ui32;
};

static inline void canid_from_register(canid_t* cid){
	bits32_t raw;

	raw.ui32 = CANIDT;

	cid->ide		= (CANCDMOB & _BV(IDE)) ? 1 : 0;
	if (cid->ide){
		cid->addr	= raw.ui32 >> 3;
	} else {
		cid->addr	= raw.ui16[1] >> 5;
	};
	cid->rtr		= (raw.ui8[0] & _BV(RTRTAG)) ? 1 : 0;
	cid->reserved	= 0;
};

/**
 * @brief test canframe for matching canfilter criteria
 * @param filter	canfilter to be used for test
 * @param frame		frame to be tested
 * @return 1 if filter matches frame, 0 if no match
 */
static inline int can_filter_match(canfilter_t *filter,canframe_t *frame){
	canid_t id1,id2;

	id1.value = filter->id.value	& filter->mask.value;
	id2.value = frame->id.value	& filter->mask.value;

	return (id1.value == id2.value) ? 1 : 0;
};

int can_prepare_mailbox_rx(int mailbox) {

	ATOMIC

	if (mailbox < 15)
	{
		CANPAGE = mailbox << 4;

		CANSTMOB = 0x00;
		CANCDMOB = 0x00;

		CANIDT = 0;
		CANIDM = 0;

		CANCDMOB = _BV(CONMOB1);

	} else {
		return -EPARAM;
	};

	return ESUCCESS;
};

/**
 * @brief copy frame to frame_t* from hardware registers
 * @param frame target frame_t
 * @return 0 on success
 */
int can_frame_from_mailbox(canframe_t *frame){
	uint8_t n;

	frame->frame_id = device.frame_id_rx++;
	frame->flags.len = (CANCDMOB & 0x0F);
	canid_from_register( &(frame->id) );


	for (n=0;n<8;n++){
		frame->payload.bytes[n] = CANMSG;
	};

	return ESUCCESS;
};

int can_next_frame_tx(void){
	canframe_t *frame = list_first_entry( &(device.tx_queue), canframe_t, list );

	uint8_t tmp = CANPAGE;
	CANPAGE = (14 << 4);

	CANSTMOB = 0x00;
	CANCDMOB = 0x00;

	if (frame){
		uint8_t n;

		CANCDMOB = (frame->flags.len & 0x0f) | (frame->id.ide ? _BV(IDE) : 0);

		CANIDT = canid_to_register( &frame->id );
		for (n=0;n<8;n++){
			CANMSG = frame->payload.bytes[n];
		};

		CANCDMOB |= _BV(CONMOB0);
	};

	CANPAGE = tmp;

	return ESUCCESS;
};


void can_vect(void){
	uint16_t sit = CANSIT2 | (CANSIT1 << 8);
	uint8_t n;

	uint8_t	cptemp = CANPAGE;

	if (sit & 0x7FFF)
	{
		for (n=0;n<15;n++)
		{
			if (sit & _BV(n))
			{
				CANPAGE = (n << 4);
				CANCDMOB &= ~(_BV(CONMOB0) | _BV(CONMOB1));

				if (n < CAN_RX_MAILBOXES){
					if (CANSTMOB & 0x1F) {
						// Ein Fehler trat auf

					} else {
						// Empfangen
						can_frame_from_mailbox( &(device.rxpool[ device.rxpool_next++ ]) );

						if (device.rxpool_next >= device.rxpool_length){
							device.rxpool_next = 0;
						};
					};
					can_prepare_mailbox_rx(n);

				} else if (n == 14) {

					canframe_t* tx = list_first_entry( &(device.tx_queue), canframe_t, list);
					if (tx){
						tx->frame_id = device.frame_id_tx++;
						tx->flags.error = (CANSTMOB & 0x1F) ? 1 : 0;
						tx->flags.tx = 0;
						list_remove( &(tx->list) );
						thread_wake( tx->notify );
					};
					can_next_frame_tx();

				};
			};
		};
	};

	CANPAGE = cptemp;
};


VECT(CANIT_vect){
	can_vect();
};



/* USER API */

int can_send(canframe_t *frame){
	if (frame){
		ATOMIC

		int empty = list_is_empty( &(device.tx_queue) );

		frame->notify = current_thread();
		list_append( &(frame->list ), &(device.tx_queue) );

		if (empty){
			can_next_frame_tx();
		};

		wait_ms( 10 );

		if (!list_is_empty( &(frame->list) ) ){
			list_remove( &(frame->list) );
		};

		if (frame->flags.tx || frame->flags.error){
			return -EFAIL;
		};

		return ESUCCESS;
	};
	return -EFAIL;
};

int			can_send_simple(canid_t cid,uint8_t len,canpayload_t payload){
	canframe_t	*frame = malloc(sizeof(canframe_t));
	int r;

	if (frame){
		memset( frame, 0x00, sizeof(canframe_t) );
		frame->flags.tx = 1;
		frame->flags.len = len;
		frame->id = cid;
		frame->payload = payload;

		r = can_send( frame );

		free(frame);

		return r;
	} else {
		return -ENOMEM;
	}
};

int can_recv(canfilter_t *filter,canframe_t *frame){
	ATOMIC

	int n;

	if (!frame){
		return -ENULLPTR;
	}

	n = device.rxpool_next;
	do {
		if ((device.rxpool[n].frame_id > filter->last_seen_frame_id) || (n < device.rxpool_next)){

			if (can_filter_match( filter, &(device.rxpool[n]) )){
				*frame = device.rxpool[n];

				filter->last_seen_frame_id = frame->frame_id;

				memset( &device.rxpool[n], 0x00, sizeof(canframe_t) );

				return ESUCCESS;
			};

		};

		filter->last_seen_frame_id = device.rxpool[n].frame_id;

		n++;
		if (n == device.rxpool_length){
			n = 0;
		};

	} while (device.rxpool_next != n);

	return -EFAIL;
};

int can_recv_timeout(canfilter_t *filter,canframe_t *frame,int ms){
	int n,r;

	for (n = 0; n < ms; n++){
		r = can_recv( filter, frame );
		if (r == ESUCCESS){
			return ESUCCESS;
		};
		wait_ms(1);
	};

	return -ETIMEOUT;
};