#include <rb2/regbus.h>
#include <sys/threads.h>
#include <sys/mutex.h>
#include <sys/atomic.h>

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

#include <util/delay.h>

#define RB2_USART_MAX_LINKS	2

int32_t	_dbg_rb2_usart_sync,
		_dbg_rb2_usart_sync_min = 0x7FFFFFFFl,
		_dbg_rb2_usart_sync_max;
int32_t	_dbg_rb2_usart_txtele,
		_dbg_rb2_usart_rxtele;


rb2_telegram_buffer_t*
		rb2_usart_recv	(RB2_LINK *link);
uint8_t	rb2_usart_send	(RB2_LINK *link,rb2_telegram_buffer_t *telegram);

RB2_LINK*	rb2_create_usart_link(uint8_t _usart) {
	ATOMIC
	
	RB2_USART_LINK *link = malloc( sizeof(RB2_USART_LINK) );
	if (link){
		memset(link, 0x00, sizeof(RB2_USART_LINK));
		
		link->usart = _usart;
		
		list_init( &link->received );
		list_init( &link->send );
				
		link->link.send		= rb2_usart_send;
		link->link.receive	= rb2_usart_recv;
		
		rb2_link_add( &(link->link) );
	};
	return &link->link;
};

#if defined(__USART_BUFFER__)	// buffer based i/o

int rb2_usart_buffers(RB2_USART_LINK *link){
	
	if (link->rx && (usart_rx_done(link->usart) == 0)){
		int n = 0;
		CHKSUM chk;

		if (link->rx->bytes[0] != RB2_MAGIC){
			n = 1;
		} else {
			chksum( &chk, link->rx->bytes, sizeof(rb2_telegram_t) - 2 );
			if ( (chk.combined != link->rx->telegram.chksum.combined)){
				n = 1;
			};
		};
		
		if (n)
		while ( (n < sizeof(rb2_telegram_t)) && (link->rx->bytes[n] != RB2_MAGIC) ){
			n++;
		};
		
		if (n){
			uint8_t l = 0; //sizeof(rb2_telegram_t) - n;
			
			_dbg_rb2_usart_sync = *(int32_t*)&link->rx->telegram;
			
			while (n < sizeof(rb2_telegram_t)){
				link->rx->bytes[l] = link->rx->bytes[n];
				l++;	n++;
			};
			
			usart_rx( link->usart, &(link->rx->telegram), n);
		} else {
			usart_rx( link->usart, NULL, 0 );
			list_append( &(link->rx->list), &(link->received) );
			link->rx = NULL;
		};
	};
	
	if (!link->rx){
		link->rx = rb2_pool_pop();
		if (link->rx){
			usart_rx( link->usart, &(link->rx->telegram), sizeof(rb2_telegram_t) );
		};
	};
	
	if (link->tx && (usart_tx_done( link->usart ) <= 0)){
		usart_tx( link->usart, NULL, 0 );
		rb2_pool_push(link->tx);
		link->tx = NULL;
	};
	
	if (!link->tx){
		link->tx = list_first_entry( &(link->send), rb2_telegram_buffer_t, list );
		if (link->tx){
			list_remove( &(link->tx->list) );
			usart_tx( link->usart, &(link->tx->telegram), sizeof(rb2_telegram_t) );
		};
	};
	
	return ESUCCESS;
};

uint8_t	rb2_usart_send(RB2_LINK *link,rb2_telegram_buffer_t *telegram) {
	telegram->telegram.magic = RB2_MAGIC;
	chksum( &(telegram->telegram.chksum), &(telegram->telegram), sizeof(RB2_TELEGRAM) - 2 );
	
	{
		ATOMIC
		list_append( &telegram->list, &(RB2_USART_LINK(link)->send) );
	}

	return rb2_usart_buffers( RB2_USART_LINK(link) );
};

rb2_telegram_buffer_t* rb2_usart_recv(RB2_LINK *l){
	ATOMIC
	RB2_USART_LINK *link = RB2_USART_LINK(l);
	rb2_telegram_buffer_t *tele;

	rb2_usart_buffers(link);

	tele = list_first_entry(&(link->received),rb2_telegram_buffer_t,list);
	if (tele){
		list_remove(&tele->list);
		return tele;
	};

	return NULL;
};

#else	// fifo based i/o

	uint8_t	rb2_usart_send(RB2_LINK *l,rb2_telegram_buffer_t *telegram) {
		int		n = 0;
		RB2_USART_LINK
				*link = RB2_USART_LINK(l);
		
		telegram->telegram.magic = RB2_MAGIC;
		chksum( &(telegram->telegram.chksum), &(telegram->telegram), sizeof(RB2_TELEGRAM) - 2 );
		
		while (n < sizeof(rb2_telegram_t)){
			if (usart_write( link->usart, telegram->bytes[ n ] ) < 0){
				wait_ms(4);
			} else {
				n++;
			};
		};
		
		_dbg_rb2_usart_txtele++;
		
		rb2_pool_push( telegram );
		
		return -EFAIL;
	};

	rb2_telegram_buffer_t* rb2_usart_recv(RB2_LINK *l){
		ATOMIC
		RB2_USART_LINK *link = RB2_USART_LINK(l);
		
		if (!link->rx){
			link->rx = rb2_pool_pop();
			link->rxptr = 0;
		};

		if (link->rx){
			int ch;
			CHKSUM chk;
			
			while ( (ch = usart_read( link->usart )) >= 0 ){
				link->rx->bytes[ link->rxptr++ ] = ch;
				
				if (link->rx->bytes[ 0 ] != RB2_MAGIC){
					link->rxptr = 0;
				} else if (link->rxptr == sizeof(rb2_telegram_t)){
					chksum( &chk, link->rx->bytes, sizeof(rb2_telegram_t) - 2 );
					if (chk.combined == link->rx->telegram.chksum.combined){
						rb2_telegram_buffer_t *rt = link->rx;
						link->rx = NULL;
						
						_dbg_rb2_usart_rxtele++;
						
						return rt;
					} else {
						uint8_t p = 0;
						link->rxptr = 0;
						while (((++p) < sizeof(rb2_telegram_t)) && (link->rx->bytes[p] != RB2_MAGIC) );
						
						_dbg_rb2_usart_sync += p;
						_dbg_rb2_usart_sync -= (_dbg_rb2_usart_sync >> 3);
						
						if (_dbg_rb2_usart_sync_min > p){
							_dbg_rb2_usart_sync_min = p;
						};
						if (_dbg_rb2_usart_sync_max < p){
							_dbg_rb2_usart_sync_max = p;
						};
						
						while (p < sizeof(rb2_telegram_t)){
							link->rx->bytes[link->rxptr++] = link->rx->bytes[p++];
						};
					};
				};
			};
			
		};
		return NULL;
	};
	
#endif

/*
int	rb2_usart_irq_recvd_char(USART *usart,int ch){
	RB2_USART_LINK *link = link_for_usart(usart);
	
	if (link){
		
		if (link->rx == NULL){
			link->rx = rb2_pool_pop_irq();
			link->rxptr = 0;			
		};
		
		if (link->rx){
			link->rx->bytes[ link->rxptr++ ] = ch;
			
			if (link->rxptr == 1){
				if (link->rx->bytes[0] != RB2_MAGIC){
					link->rxptr = 0;
				};
			} else if (link->rxptr == sizeof(RB2_TELEGRAM)){
				CHKSUM chk;
				chksum( &chk, link->rx->bytes, sizeof(RB2_TELEGRAM) - 2 );				
				
				if (chk.combined == link->rx->telegram.chksum.combined){
					link->rx->link = &link->link;
					
					if (_dbg_trace_rb2_usart_rx)
						_dbg_trace_rb2_usart_rx(link->rx);
					
					list_append( &(link->rx->list), &(link->received) );
					link->rx = NULL;
				};
				link->rxptr = 0;
			};
		};
	};
	
	return 0;
};

int	rb2_usart_irq_needs_char(USART *usart){
	RB2_USART_LINK *link = link_for_usart(usart);
		
	if (link){
		if (!link->tx){
			link->tx = list_first_entry(&(link->send),rb2_telegram_buffer_t,list);
			if (link->tx){
				list_remove(&(link->tx->list));
			};
			link->txptr = 0;
		};
		
		if (link->tx){
			if (link->txptr >= sizeof(RB2_TELEGRAM)){	// Telegram versendet, Puffer freimachen

				if (_dbg_trace_rb2_usart_tx)
					_dbg_trace_rb2_usart_tx(link->tx);

				rb2_pool_push( link->tx );
				
				link->txptr = 0;
				link->tx = list_first_entry(&link->send,rb2_telegram_buffer_t,list);
				if (link->tx){
					list_remove(&link->tx->list);
				} else {
					return -1;
				}
			};
			return link->tx->bytes[link->txptr++];
		};
	};
	return -1;
};

*/