#include "msg.h"

/*

Protocol description:

p[0] 		= route length	(N bytes)
p[1] 		= jump		(points to next byte/node to jump, 0 for the first...)
p[2]		= payload len	(X bytes)
p[3..n+3]	= route		(the route itself, in order, 1 byte per node)
p[n+4..n+x]	= payload


*/



uint8_t *msg_make(uint8_t *route, uint8_t routeLen, uint8_t *payload, uint8_t payloadLen) {
uint8_t *data  = NULL;
int i = 0;

	if (!route || !payload) return NULL;

	data = malloc((3+routeLen+payloadLen)*sizeof(uint8_t));

	data[0] = routeLen;
	data[1] = 0;
	data[2] = payloadLen;

	for (i=0; i<routeLen; i++)	data[3+i] = route[i];

	for (i=0; i<payloadLen; i++)	data[3+routeLen+i] = payload[i];

return data;
}

uint8_t *msg_process(uint8_t *d, uint8_t thisNode) {
netpacket_send_args_t npsargs;
uint8_t to = 0, routeLen = 0, payloadLen = 0, jump = 0;


	if (!d) return NULL;

	/*Get the length of the route and the payload and the jump value*/
	routeLen = d[MSG_ROUTELEN];
	payloadLen = d[MSG_PAYLOADLEN];
	jump = d[MSG_JUMP] + 1;

	/*Send to...*/
	to = d[3+jump];

	/*Next node !*/
	d[MSG_JUMP] = jump;

	/*If the message is for us, return it*/
	if (d[3+jump-1] == thisNode && jump == routeLen) return d+(3+routeLen);

	/*If not, we send it*/
	else {

		DEBUGM("message to : %d\n", to);

		Net_sendArgsInit(&npsargs);
		npsargs.netheader.to = to;
		npsargs.netheader.type = USERDEFINED_PACKET+MESSAGE;
		npsargs.netheader.flags = 0;
		npsargs.payload[0].buffer = (uint8_t*) d;
		npsargs.payload[0].size = 3 + routeLen + payloadLen;
		Net_send(&npsargs);


	}

return NULL;
}


uint8_t msg_len(uint8_t *d) {
	
	if (!d) return 0;

	return d[MSG_PAYLOADLEN];

}


uint8_t *msg_get_message(uint8_t *d) {
uint8_t routeLen = 0;

	if (!d) return NULL;

	routeLen = d[MSG_ROUTELEN];

return d+(3+routeLen);
}

void msg_free(uint8_t *d) {

	if (d) free(d);
	d = NULL;

}

