#include <can/cannode.h>
#include <can/can.h>

#include <sys/errno.h>
#include <sys/assert.h>

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

cannode_t*	can_node_create(int identity,register_node_proc node_proc){
	cannode_t* cn = malloc(sizeof(cannode_t));
	if (cn){
		memset(cn,0x00,sizeof(cannode_t));
		
		cn->identity = identity & 0x0F;
		cn->node_proc = node_proc;
		cn->rxfilter = (canfilter_t){
				id: {
					addr: ((int32_t)cn->identity) << 20,
					ide: 1
					},
				mask: {
					addr: 0x1FFF0000l,
					ide: 1
				}
		};
	};
	return cn;
};

int can_node_received(cannode_t *cn,canframe_t *frame){
	unsigned int regno = frame->id.addr & 0xffff;
	
	if (frame->id.rtr){
		
		if (!cn->node_proc( RNPOP_READ, regno, &(frame->payload.bytes[4]), &(frame->payload.dwords[0]) )){
		
			frame->id.rtr = 0;
			frame->flags.len = 5;
			
			assert( can_send( frame ) );
		
		};
	} else if (frame->flags.len == 2){
		int i;
		
		for (i=regno;i < (regno+frame->payload.words[0]); i++){
			
			if (!cn->node_proc( RNPOP_READ, regno, &(frame->payload.bytes[4]), &(frame->payload.dwords[0]) )){
			
				frame->id.rtr = 0;
				frame->id.addr = (((int32_t)cn->identity) << 20)|(regno);
				frame->flags.len = 5;
				
				assert( can_send( frame ) );
			};
		};
	} else if (frame->flags.len == 6){
		cn->node_proc( RNPOP_WRITE, regno, &(frame->payload.bytes[4]), &(frame->payload.dwords[0]) );
		
		frame->id.rtr = 0;
		frame->flags.len = 5;
		
		assert( can_send( frame ) );
	};
	
	
	return ESUCCESS;
};

int can_node_handler(cannode_t *cn){
	canframe_t frame;
	
	if (cn == NULL){
		return -ENULLPTR;
	};
	
	while (!can_recv(&(cn->rxfilter),&frame)){
		assert( can_node_received( cn, &frame ) );
	};
	
	return ESUCCESS;
};


int			can_node_read	(int node,unsigned int reg,uint8_t *type,void *buffer){
	canframe_t frame;
	canfilter_t f;
	
	node &= 0x0f;
	
	memset( &frame, 0x00, sizeof(frame) );
	frame.id.addr = (((uint32_t)node)<<20) | (reg);
	frame.id.ide = 1;
	frame.id.rtr = 1;
	frame.flags.len = 1;
	frame.flags.tx = 1;
	
	assert( can_send( &frame ) );
	
	f = (canfilter_t){
		id: {
			addr: (((int32_t)node) << 20) | (reg),
			ide: 1
			},
		mask: {
			addr: 0x1FFFFFFFl,
			ide: 1,
			rtr: 1
		}
	};
	
	*(int32_t*)buffer = 0;
	
	assert( can_recv_timeout( &f, &frame, 10 ));
		
	*(int32_t*)buffer	= frame.payload.dwords[0];
	*type				= frame.payload.bytes[4];
	
	return ESUCCESS;
};

int			can_node_write	(int node,unsigned int reg,uint8_t type,void *buffer){
	canframe_t frame;
	
	memset( &frame, 0x00, sizeof(frame) );
	frame.id.addr = (((int32_t)node)<<20) | (reg);
	frame.id.ide = 1;
	frame.id.rtr = 0;
	frame.flags.len = 6;
	frame.payload.dwords[0] = *(uint32_t*)buffer;
	frame.payload.bytes[4] = type;
	
	can_send( &frame );
	
	return ESUCCESS;
};