#include <hwo/utils.h>

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

#include <rb2/regbus.h>
#include <sys/errno.h>

int32_t	dbg_rb2_router_ident;


void rb2_route(RB2_TELEGRAM *request) {

	rb2_telegram_buffer_t *txtele = rb2_pool_pop();
	
	if (txtele) {
		list_init( &txtele->list );
		txtele->telegram = *request;
		rb2_route_listed( txtele );
	};	
};

void rb2_route_listed(rb2_telegram_buffer_t *txtele) {
	cpustate_t state;
	
	spinlock_lock_irq( &(&__rb2_instance)->queues.sl_router, &state );
	list_append( &(txtele->list), (list_t*) &((&__rb2_instance)->queues.router) );
	spinlock_release_irq( &(&__rb2_instance)->queues.sl_router, &state );

	thread_wake( (&__rb2_instance)->threads.router );
};


uint8_t	rb2_router_handle_ident(rb2_telegram_buffer_t *telegram){
	
	if (!telegram->link){
		rb2_pool_push(telegram);
		return -EFAIL;
	};
	
	dbg_rb2_router_ident--;
	
	telegram->link->identrx = 3;
	
	if (telegram->link->ring) {					
		if ((&__rb2_instance)->ringmaster) {
			(&__rb2_instance)->axes = telegram->telegram.ax;
			rb2_pool_push(telegram);
			return ESUCCESS;
		} else {
			(&__rb2_instance)->ax = (++telegram->telegram.ax);
			telegram->link->send( telegram->link, telegram );
			return ESUCCESS;
		};
	} else {
		if (telegram->telegram.sender.ax){
			(&__rb2_instance)->ax = telegram->telegram.sender.ax;
		};
		
		telegram->link->idmask = telegram->telegram.endpoints;
		rb2_pool_push(telegram);
		return ESUCCESS;
	};
};

uint8_t	rb2_router_handle_foreign(rb2_telegram_buffer_t *telegram){
	list_t	*i;
	uint8_t temp;

	for_each_list_entry(i,&((&__rb2_instance)->links.list)){
		RB2_LINK *link = list_entry(i,RB2_LINK,list);
		
		if (link->send == NULL){
			continue;
		};

		// Eigenes Achsensystem ist Ziel...
		if (	
			(telegram->telegram.receiver.ax == RB_AX_LOCAL) ||
			(telegram->telegram.receiver.ax == (&__rb2_instance)->ax)
		) {	

			if (link->idmask & (1<<telegram->telegram.receiver.node)) {
				link->send( link, telegram );
				return 0;
			};
		} else {	// Fremde Achse
			if (link->ring) {
				if ((&__rb2_instance)->ringmaster && ((&__rb2_instance)->axes < telegram->telegram.receiver.ax) ) {
					break;
				} else {
					link->send( link, telegram );
					return 0;
				};
			} else if (link->idmask & (0x01)) {
					link->send( link, telegram );
					return 0;
			};
		};
	};
	
	/* Keine gültige Schnittstelle -> FAIL senden */
	if (telegram->telegram.flags.response) {	// FAIL für Response ist unmöglich
		rb2_pool_push(telegram);
		return 0;
	} else {
		temp = telegram->telegram.sender.addr;
		telegram->telegram.sender.addr = telegram->telegram.receiver.addr;
		telegram->telegram.receiver.addr = temp;
		telegram->telegram.flags.response = 1;
		telegram->telegram.flags.fail = 1;
		telegram->telegram.i32 = 0x99880000L | ((telegram->telegram.sender.addr<<8) & 0xFF00) | ((__rb2_instance.ax << 4) & 0xF0) | (rb2_first_id() & 0x0F);
		
		rb2_route_listed( telegram );
		return 0;
	};
};

/**
 * @brief Der Router Thread des RegBUS Systems.
 * 
 * Wartet auf Telegramme zum weiterleiten und verteilen an die entsprechenden Empfänger (z.B. lokale API, Schnittstellen)
 * @param arg
 * \callgraph
 * \callergraph
 */
void rb2_router(void *arg) {
	list_t *i,*tmp;
	cpustate_t state;
	int n;
		
	while (1)
	{
		yield();
		
		rb2_loop_ident();
		
		yield();
		
		rb2_loop_rx();
		
		spinlock_lock_irq( &(&__rb2_instance)->queues.sl_router, &state );

		n = 0;

		for_each_list_entry_save(i,tmp,&((&__rb2_instance)->queues.router)){
			rb2_telegram_buffer_t *toroute = list_entry(i,rb2_telegram_buffer_t,list);
			list_remove(i);
	
			spinlock_release_irq( &(&__rb2_instance)->queues.sl_router, &state );

			if (toroute->telegram.flags.ident){
				rb2_router_handle_ident(toroute);
			} else if (rb2_request_is_local( &(toroute->telegram) )){
				rb2_api_rx( toroute );
			} else {
				rb2_router_handle_foreign(toroute);
			};
			
			spinlock_lock_irq( &(&__rb2_instance)->queues.sl_router, &state );
			
			n++;
			if (n>5){
				break;
			};
		};
		
		spinlock_release_irq( &(&__rb2_instance)->queues.sl_router, &state );		

	};
};