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

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

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

#include <avr/eeprom.h>

#include <sys/runtime.h>

int32_t	_dbg_jetcan_requests,
		_dbg_jetcan_misses,
		_dbg_jetcan_timeouts,
		_dbg_jetcan_rtt,
		_dbg_jetcan_rtt_min = 0x7FFFFFFFl,
		_dbg_jetcan_rtt_max;


struct _jetcan jetcan;

static canfilter_t
		cf_jetcan_reply = 
		{
			id: {
				addr:	0x100
			},
			mask: {
				addr:	0x7FF,
				ide:	1
			}
		};

#define IS_1MBIT	((can_get_device_config()&CAN_SPEED_MASK)==CAN_1MBIT)
#define IS_125KBIT	((can_get_device_config()&CAN_SPEED_MASK)==CAN_125KBIT)


int	jcan_lock(void){
	mutex_lock( &jetcan.lock );
	return ESUCCESS;
};
int	jcan_unlock(void){
	mutex_release( &jetcan.lock );
	return ESUCCESS;
};

JSlave*	jcan_slave(uint8_t num)
{
	if (num < jetcan.nSlaves){
		return &jetcan.slaves[num];
	};
	return NULL;
};

int	jcan_num_slaves(void)
{
	return jetcan.nSlaves;
};


int	jcan_setup_bus(jSetCModeCallback cmodecallback)
{
	canfilter_t	rxfilter = {
		id: {
			addr:	0x160
		},
		mask: {
			addr:	0x7FF,
			ide:	1
		}
	};
	
	canframe_t	*mid_frame,
				*stp_frame;

	mid_frame = malloc( sizeof(canframe_t) );
	if (!mid_frame){
		return -ENOMEM;
	};
	stp_frame = malloc( sizeof(canframe_t) );
	if (!stp_frame){
		free(mid_frame);
		return -ENOMEM;
	};

	jcan_lock();
	
	jetcan.nSlaves = 0;
	
	cmodecallback(0x00);
	wait_ms(250);
	
	if (IS_1MBIT){
		cmodecallback(0x01);
	} else if (IS_125KBIT){
		cmodecallback(0x02);
	};

	do
	{
		if (!can_recv_timeout(&rxfilter, stp_frame, 100)){

			memset( &jetcan.slaves[jetcan.nSlaves],
					0x00,
					sizeof(JSlave));
					
			jetcan.slaves[ jetcan.nSlaves ].refID =		(0x0161 + jetcan.nSlaves);
			jetcan.slaves[ jetcan.nSlaves ].modcode =	stp_frame->payload.bytes[0];
			
			*mid_frame = (canframe_t){
				flags: {
					tx:		1,
					len:	2
				},
				id: {
					addr: 0x0161,
				},
				payload: {
					words: {
						jetcan.slaves[ jetcan.nSlaves ].refID,
						0,
						0,
						0
					}
				}
			};
			
			can_send( mid_frame );
			
			jetcan.nSlaves++;
			
			continue;
		};
		
	} while (0);
	
	cmodecallback(0x03);

	jcan_unlock();
	
	free( mid_frame );
	free( stp_frame );
	
	return ESUCCESS;
};

int	jReadRegister32Ex(uint8_t slave,uint16_t parm,void *pValue,uint8_t *type)
{
	JSlave		*sl;
	uint32_t 	*pui32 = (uint32_t*)pValue;
	int 		retries = 3;
	canframe_t	req,reply;
	systick_t	st_start;
	
	MUTEXED( &(jetcan.lock) );

	if (!parm){
		return -EPARAM;
	};

	_dbg_jetcan_requests++;
	st_start = systick_ticks();

	*pui32 = 0;
	
	while (!can_recv(&cf_jetcan_reply, &reply));
	
	while (retries--){
		sl = jcan_slave(slave);	
		if (sl){

			req = (canframe_t){
					flags: { tx: 1, len: 3 },
					id: { addr: sl->refID },
					payload: {
						words: {
							parm,
							*type
						}
					}
				};
				
			can_send( &req );
			
			if (can_recv_timeout( &cf_jetcan_reply, &reply, 25 ) == 0){
				*pui32 = reply.payload.dwords[0];
				*type = reply.payload.bytes[4];
				
				st_start = systick_ticks() - st_start;
				_dbg_jetcan_rtt += st_start;
				_dbg_jetcan_rtt -= _dbg_jetcan_rtt >> 4;
				
				if (_dbg_jetcan_rtt_min > st_start)
					_dbg_jetcan_rtt_min = st_start;
				
				if (_dbg_jetcan_rtt_max < st_start)
					_dbg_jetcan_rtt_max = st_start;
					
				return ESUCCESS;
			};
			_dbg_jetcan_misses++;
		};
	};
	
	_dbg_jetcan_timeouts++;
	
	return -EBUSY;
};

int	jReadRegister32Exl(uint8_t slave,uint16_t parm,int32_t *value)
{
	uint8_t type = J_LONG;
	int r;
	
	r = jReadRegister32Ex(slave,parm,value,&type);
	if (r)
		return r;
		
	if (type != J_LONG)
		*value = (int32_t)(*(float*)value);

	return ESUCCESS;
};

int	jReadRegister32Exd(uint8_t slave,uint16_t parm,float *value)
{
	uint8_t type = J_FLOAT;
	int r;
	
	r = jReadRegister32Ex(slave,parm,value,&type);
	if (r)
		return r;
		
	if (type != J_FLOAT)
		*value = (float)*(int32_t*)value;

	return ESUCCESS;
};

int	jSetRegister32Ex(uint8_t slave,uint16_t parm,void *pValue,uint8_t type){
	JSlave		*sl;
	uint16_t 	*pw16 = (uint16_t*)pValue;
	int 		retries = 3;
	canframe_t	req,reply;
	systick_t	st_start;
	MUTEXED( &(jetcan.lock) );

	if (!parm){
		return -EPARAM;
	};

	_dbg_jetcan_requests++;
	st_start = systick_ticks();

	while (!can_recv(&cf_jetcan_reply, &reply));

	while (retries--){
		sl = jcan_slave(slave);	
		if (sl){

			req = (canframe_t){
					flags: { tx: 1, len: 7 },
					id: { addr: sl->refID },
					payload: {
						words: {
							parm,
							pw16[0],
							pw16[1],
							type
						}
					}
				};
				
			can_send( &req );
			
			if (can_recv_timeout( &cf_jetcan_reply, &reply, 10 ) == 0){

				st_start = systick_ticks() - st_start;
				_dbg_jetcan_rtt += st_start;
				_dbg_jetcan_rtt -= _dbg_jetcan_rtt >> 4;
				
				if (_dbg_jetcan_rtt_min > st_start)
					_dbg_jetcan_rtt_min = st_start;
				
				if (_dbg_jetcan_rtt_max < (int32_t)st_start)
					_dbg_jetcan_rtt_max = st_start;
					
				return ESUCCESS;
			};
			_dbg_jetcan_misses++;
		};
	};
	
	_dbg_jetcan_timeouts++;
	
	return -EBUSY;
};

int	jSetRegister32Exl(uint8_t slave,uint16_t parm,int32_t value)
{
	return jSetRegister32Ex(slave,parm,&value,J_LONG);
};
int	jSetRegister32Exd(uint8_t slave,uint16_t parm,float value)
{
	return jSetRegister32Ex(slave,parm,&value,J_FLOAT);
};

/* Bit Manipulationen */
void	jSetRegisterBit(uint8_t slave,uint16_t parm,uint8_t bit)
{
	int32_t v;
	
	if (jReadRegister32Exl(slave, parm, &v))
	{
		v |= (1<<bit);
		jSetRegister32Exl(slave,parm,v);
	};
};

void	jClearRegisterBit(uint8_t slave,uint16_t parm,uint8_t bit)
{
	int32_t v;
	
	if (jReadRegister32Exl(slave, parm, &v))
	{
		v &= ~(1<<bit);
		jSetRegister32Exl(slave,parm,v);
	};
};

int jReadRegisterBit(uint8_t slave,uint16_t parm,uint8_t bit)
{
	int32_t v;
	
	if (jReadRegister32Exl(slave, parm, &v))
	{
		if (v & (1<<bit))
			return 1;
		else
			return 0;
	};
	return -1;
};