#include <rb2/regbus.h>
#include <hwo/threads.h>

#include <can/can.h>

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


#if 0

int32_t	dbg_rb2_can_tx;
int32_t	dbg_rb2_can_rx;
int32_t	dbg_rb2_can_rx_err;
int16_t	dbg_rb2_can_rx_ptr[2];

RB2_CAN_LINK	*rb2_can_link;

void	rb2_can_prepare_rx(CANREQUEST *can);
void	rb2_can_irq_tx(CANREQUEST *request);

rb2_telegram_buffer_t*	rb2_can_receive	(RB2_LINK *link);
uint8_t					rb2_can_send	(RB2_LINK *link,rb2_telegram_buffer_t *telegram);

void rb2_can_irq_rx(CANREQUEST *request);

RB2_LINK*	rb2_create_can_link(void) {
	if (rb2_can_link)
		return &(rb2_can_link->link);
		
	rb2_can_link = malloc( sizeof( RB2_CAN_LINK ) );
	
	if (rb2_can_link) {
		uint8_t n;
		
		memset( rb2_can_link, 0x00, sizeof( RB2_CAN_LINK ) );
	
		rb2_can_link->link.receive	= NULL;
		rb2_can_link->link.send		= rb2_can_send;	
	
		for (n=0;n<2;n++) {
			rb2_can_link->canrx[n] = malloc( sizeof(CANREQUEST) );
			if (rb2_can_link->canrx[n]) {
				rb2_can_prepare_rx( rb2_can_link->canrx[n] );
				can_enqueue( rb2_can_link->canrx[n] );
			};
		};

		list_append( &(rb2_can_link->link.list), &((&__rb2_instance)->links.list) );
	
		return &(rb2_can_link->link);
	};
	return NULL;
};

void	rb2_can_prepare_rx(CANREQUEST *can) {
	if (can) {
		memset( can, 0x00, sizeof( CANREQUEST ) );
		
		can->ide 		= 1;
		can->idmask.id	= 0x1FFF0000L;
		can->id.id		= 0x00080000L;
		can->irq_notify = rb2_can_irq_rx;
	};
};

uint8_t	rb2_can_send		(RB2_LINK *link,rb2_telegram_buffer_t *telegram) {
	CANREQUEST *can = malloc( sizeof(CANREQUEST) );
	uint8_t n;
	
	if (can) {
		memset( can, 0x00, sizeof( CANREQUEST ) );
		
		can->ide		= 1;
		can->tx			= 1;
		can->id.id		= 0x00080000L |
							( ((uint32_t)(&__rb2_instance)->ax) << 12) |
							( ((uint32_t)rb2_first_id()) << 8) |
							telegram->telegram.flags.byte;
		
		can->len = 8;
		can->data.bytes[0]	= telegram->telegram.receiver.addr;
		can->data.bytes[1]	= telegram->telegram.sender.addr;
		can->data.words[1]	= telegram->telegram.regno;
		can->data.dwords[1]	= telegram->telegram.ui32;
				
		if (telegram->telegram.flags.ident) {
			for (n=0;n<(RB2_CANLINK_IDDEPTH-1);n++){
				RB2_CAN_LINK(link)->idmasks[ n ] = RB2_CAN_LINK(link)->idmasks[ n + 1 ];
			};
			RB2_CAN_LINK(link)->idmasks[n] = 0;
		};
		
		can->irq_notify = rb2_can_irq_tx;
		
		if (can_enqueue( can )){
			free( can );
			return -1;
		} else {
			if (_dbg_trace_rb2_can_tx)
				_dbg_trace_rb2_can_tx( telegram );
		};
	};
	free(telegram);
	return 0;
};

void rb2_can_irq_tx(CANREQUEST *request){
	dbg_rb2_can_tx++;

	free(request);
};

void rb2_can_irq_rx(CANREQUEST *request){
	RB2_CAN_LINK		*clink	= rb2_can_link;
	rb2_telegram_buffer_t *t 		= NULL;
	uint8_t				n;
	
	dbg_rb2_can_rx_ptr[ 1 ] = dbg_rb2_can_rx_ptr[ 0 ];
	dbg_rb2_can_rx_ptr[ 0 ] = (int16_t)request;
	
	if (!request->err) {
	
		t = malloc( sizeof(rb2_telegram_buffer_t) );
		
		if (t) {
			memset( t, 0x00, sizeof(rb2_telegram_buffer_t) );
			
			t->telegram.flags.byte		= request->id.id & 0x000000FF;
			t->telegram.receiver.addr	= request->data.bytes[0];
			t->telegram.sender.addr		= request->data.bytes[1];
			t->telegram.regno			= request->data.words[1];
			t->telegram.ui32			= request->data.dwords[1];
									
			if (t->telegram.flags.ident) {
				for (n=0;n<RB2_CANLINK_IDDEPTH;n++){
					clink->idmasks[n] |= t->telegram.endpoints;	
				};				
				t->telegram.endpoints = clink->idmasks[0];
			} else {
				if (
					((&__rb2_instance)->ax == t->telegram.receiver.ax) &&
					(clink->link.idmask & (1<<t->telegram.receiver.node))
					){
						free( t );
						t = NULL;
				};
			};
			
			if (t){
				dbg_rb2_can_rx++;
				
				t->link = &(rb2_can_link->link);
				
				if (_dbg_trace_rb2_can_rx)
					_dbg_trace_rb2_can_rx( t );

				rb2_route_listed( t );
			};
		};
	} else {
		dbg_rb2_can_rx_err++;
	};
	
	rb2_can_prepare_rx( request );
	
	if (!can_enqueue( request )){
		
	};
};


#endif