#include <ScatterWeb.System.h>
#include <ScatterWeb.CC1020.h>
#include "constants.h"
#include "dsrProcess.h"
#include "dsr.h"
#include "types.h"

uint8_t reqNum = 0;
uint8_t sentNum = 0;
uint8_t sent[MAX_SENT];



/*Cleans the sent list*/
void clean() {

	sentNum = 0;
	reqNum = 0;

}

/*Checks if a number is in a list*/
int inList(uint8_t search, uint8_t *in, uint8_t listLen) {
int i = 0;
	
	for(i=0; i<listLen; i++) {
		if (in[i] == search) return 1;
	
	}

return 0;
}



uint8_t dsrStartRouteRequest(uint8_t thisNode, uint8_t dest, uint8_t id) {
uint8_t source = 0, requestId = 0;
uint8_t *packet = NULL;
char data[1];
_dsr_packet p;
netpacket_send_args_t npsargs;
int i = 0;


	/*A maximum number of requests ID can be stored*/
	if (sentNum == MAX_SENT) return BUFFER_FULL;

	/*Now, time to build the packet of the route request*/
	source = (uint8_t) thisNode;
	data[0] = source;

	/*If id is 0 we are resending*/
	if (!id) {
		srand(reqNum++);
	
		do {
			requestId = 1 + (rand() % 255);
		} while(inList(requestId, sent, sentNum));
	
	
		requestId = (uint8_t) reqNum++;
	
		/*And let's add the new reqid to the "black list"*/
		sent[sentNum] = requestId;
		sentNum++;
	}
	else requestId = id;

	p = _dsrMakePacket(_dsrMakeHeader(source, dest, requestId, ROUTE_REQUEST, 0, 0, 1), _dsrMakePayload((uint8_t *)data));
	packet  = _dsrMakeFrame(p);

	/*Packet maid, we send it*/
	Net_sendArgsInit(&npsargs);
	npsargs.netheader.to = BCAST;
	npsargs.netheader.type = USERDEFINED_PACKET+ROUTE;
	npsargs.netheader.flags = 0;
	npsargs.payload[0].buffer = (uint8_t*) packet;
	npsargs.payload[0].size = HEADER_LEN + 1;
	Net_send(&npsargs);
	/*sndPacketNode(neighbours, numNeighbours, (char *)packet, HEADER_LEN + p.h.payload_len);*/

	/*Clean in X seconds*/
	Timers_add(REFRESH_T*1024, clean, 0xFFFF);

	DEBUGM("to = %d, len = %d, type = %X\n", BCAST, HEADER_LEN+1, npsargs.netheader.type);


	//for(i = 0; i< HEADER_LEN; i++) printf(">[%d] %X\n", i, packet[i]);


	/*Free the mallocs !*/
	_dsrFreeFrame(packet);

return 0;
}




/*
 This function is called everytime a broadcast
 message is received.
*/
uint8_t dsrProcessRouteRequest(uint8_t *data, uint8_t thisNode) {
_dsr_packet p;
uint8_t source, dest, jump, requestId, payloadLen;
uint8_t *payload=NULL, *packet=NULL;
int i = 0;
netpacket_send_args_t npsargs;


//	for (i=0; i<HEADER_LEN + data[POS_PAYLOAD_LEN]; i++) printf("[%d] %X\n", i, data[i]);

	/*Get the request id from the received packet*/
	requestId = data[POS_REQ_ID];

	DEBUGM("reqId = %d - inList = %d - reqNum = %d - sentNum = %d\n", requestId, inList(requestId, sent, sentNum), reqNum, sentNum);

//	for (i=0; i<sentNum; i++) printf("sent[%d] = %d\n", i, sent[i]);

	/*RReq*/
	if (data[POS_TYPE] == ROUTE_REQUEST && !inList(requestId, sent, sentNum)) {


//		printf("RReq received\n");


		/*First let's add the new reqid to the "black list"*/
		sent[sentNum] = requestId;
		sentNum++;

		/*Clean in X seconds*/
		Timers_add(REFRESH_T*1024, clean, 0xFFFF);

		/*Past X seconds the buffer will be cleaned*/
		if (sentNum == MAX_SENT) return BUFFER_FULL;

		source = thisNode;
		dest = data[POS_DST];
		payloadLen = data[POS_PAYLOAD_LEN] + 1;

		/*Let's generate the payload, we first copy the existing one*/		
		payload = malloc((payloadLen)*sizeof(uint8_t));
		for (i = 0; i < payloadLen; i++) {
			payload[i] = data[HEADER_LEN+i];
		}
		/*And then add the new node (this one !) to the list*/
		payload[payloadLen-1] = source;

		/*If the final destination is this node, start a RReply*/
		if (data[POS_DST] == thisNode) {
			DEBUGM("Destination reached, starting route reply...\n");

			/*We create the packet structure*/
			jump = payloadLen - 2;
			dest = payload[0];
			p = _dsrMakePacket(_dsrMakeHeader(source, dest, requestId, ROUTE_REPLY, jump, 0, payloadLen),\
			_dsrMakePayload((uint8_t *)payload));

			/*Then the packet array*/
			packet = _dsrMakeFrame(p);

			/*Wait for eveything to resettle...*/
			/*sleep(5);*/

//			for (i=0; i<HEADER_LEN + payloadLen; i++) printf(">>[%d] %X\n", i, packet[i]);

			/*And send, finally, but just to one node !*/
			Net_sendArgsInit(&npsargs);
			npsargs.netheader.to = payload[jump];
			npsargs.netheader.type = USERDEFINED_PACKET+ROUTE;
			npsargs.netheader.flags = 0;
			npsargs.payload[0].buffer = (uint8_t*) packet;
			npsargs.payload[0].size = HEADER_LEN + payloadLen;
			Net_send(&npsargs);

		}
		/*If not, just rebroadcast*/
		else {
			/*We created the packet structure*/
			p = _dsrMakePacket(_dsrMakeHeader(source, dest, requestId, ROUTE_REQUEST, 0, 0, payloadLen),\
			_dsrMakePayload((uint8_t *)payload));
	
			/*Then the packet array*/
			packet = _dsrMakeFrame(p);

			/*Eliminate noise and wait for the UDP stack to calm down...*/
			/*sleep(1);*/

			/*And send, finally*/
			Net_sendArgsInit(&npsargs);
			npsargs.netheader.to = BCAST;
			npsargs.netheader.type = USERDEFINED_PACKET+ROUTE;
			npsargs.netheader.flags = 0;
			npsargs.payload[0].buffer = (uint8_t*) packet;
			npsargs.payload[0].size = HEADER_LEN + payloadLen;
			Net_send(&npsargs);
			/*sndPacketNode(neighbours, numNeighbours, (char *)packet, HEADER_LEN + payloadLen);*/

//			printf("Rebroadcasted !\n");

		}
	
		free(payload);
		_dsrFreeFrame(packet);
		
	}

return 0;
}

uint8_t *dsrProcessRouteReply(uint8_t *data, uint8_t thisNode) {
uint8_t source, payloadLen, jump, dest, requestId;
uint8_t *payload=NULL, *packet=NULL, *ret=NULL;
_dsr_packet p;
unsigned int i = 0;
netpacket_send_args_t npsargs;

//	for (i=0; i<HEADER_LEN + data[POS_PAYLOAD_LEN]; i++) printf("[%d] %X\n", i, data[i]);


	if (data[POS_TYPE] == ROUTE_REPLY) {


//		printf("RReply received !\n");
		


		/*Build a new packet*/
		source = thisNode;
		payloadLen = data[POS_PAYLOAD_LEN];
	
		/*Let's generate the payload, we first copy the existing one*/
		payload = malloc((payloadLen)*sizeof(uint8_t));
		for (i = 0; i < payloadLen; i++) {
			payload[i] = data[HEADER_LEN+i];
		}
				
		/*Next position in the payload to send to*/
		jump = !(data[POS_JUMP]) ? 0 : data[POS_JUMP] - 1;
			
		/*If it's the end of the reply... great !*/
		if (jump == 0 && payload[jump] == source) {
			DEBUGM("Route complete : ");
			
			/*We reserve memory for the route , sotre it and return it, first pos will be the route length*/
			ret = malloc((1+payloadLen)*sizeof(uint8_t));
			ret[0] = payloadLen;
			for (i = 1; i < payloadLen+1; i++) ret[i] = payload[i-1];

			for (i = 0; i < payloadLen; i++) printf("%d%s", payload[i], (i==payloadLen-1)?"\n":" -> ");

			/*Free and exit so we can put it in listen mode*/
			free(data);
			return ret;

		}
		else {
			dest = payload[jump];
			requestId = data[POS_REQ_ID];
	
			/*We create the packet structure*/
			p = _dsrMakePacket(_dsrMakeHeader(source, dest, requestId, ROUTE_REPLY, jump, 0, payloadLen),\
			_dsrMakePayload((uint8_t *)payload));

			/*Then the packet array*/
			packet = _dsrMakeFrame(p);
					
			/*Nice wait (eliminate noise)*/
			/*sleep(1);*/

			/*And send, finally*/
			Net_sendArgsInit(&npsargs);
			npsargs.netheader.to = dest;
			npsargs.netheader.type = USERDEFINED_PACKET+ROUTE;
			npsargs.netheader.flags = 0;
			npsargs.payload[0].buffer = (uint8_t*) packet;
			npsargs.payload[0].size = HEADER_LEN + payloadLen;
			Net_send(&npsargs);
			/*sndPacketNode(reply, 1, (char *)packet, HEADER_LEN + payloadLen);*/
			DEBUGM("Bouncing Route Reply : %d -> [%d] -> %d\n", payload[jump+2], source, dest);

		}
	}

/*All ok, but no route still*/
return NULL;
}

