#include "parse_packet.h"

// returns the length of the data in bytes (datalen from packet) and filles data and metadata with the packet information
// use as follows:
//
//		packet is the entire packet message (formatted) 
//		data is an unallocated (char *) (pass it to this function as &data) 
//		meta_data is a pointer to an instance of metadata_t
//
int parse_packet(unsigned char * packet, unsigned char ** data, metadata_t * meta_data)
{	
	//----------------------------------------------------------------------------------------------
	//     index||     0    |     1    |      2      |  3 & 4 |      5 & 6       |  7+  |   end    |
	//---------------------------------------------------------------------------------------------|
	// msg param|| beg char | msg type | msg subtype | msg id | data len (bytes) | data | checksum |
	//-------------------------------------------------------------------------------------------- |
	//     bytes||     1    |     1    |      1      |    2   |        2         | var  |    1     |
	//----------------------------------------------------------------------------------------------	
	
	// first byte must be the begin char
	if(packet[0] != 0xBE)
		return -1;

	// receive metadata
	meta_data->begin_char = packet[0];
	meta_data->msg_type = packet[1];
	meta_data->msg_subtype = packet[2];
	meta_data->msg_id = (packet[4] << 8) | (packet[3]);
	meta_data->data_len = (packet[6] << 8) | (packet[5]);
	meta_data->checksum = packet[7+meta_data->data_len];

	int i;
	
	// receive data
	*data = malloc(meta_data->data_len);
	for(i = 0; i < meta_data->data_len; i++)
	{
		(*data)[i] = packet[7+i];
	}

	// calculate checksum
	char data_checksum = 0;
	for(i = 0; i < meta_data->data_len + 7; i++)
	{
		data_checksum ^= packet[i];
	}

	// compare checksum
	if(meta_data->checksum != data_checksum)
		fprintf(stderr, "Checksums did not match (Quadlog): 0x%02x\t0x%02x\n", meta_data->checksum, data_checksum);

	return meta_data->data_len;
}

float getFloat(unsigned char* str, int pos) {
	union {
		float f;
		int i;
	} x;
	x.i = ((str[pos+3] << 24) | (str[pos+2] << 16) | (str[pos+1] << 8) | (str[pos]));
	return x.f;
}

int getInt(unsigned char* str, int pos) {
	int i = ((str[pos+3] << 24) | (str[pos+2] << 16) | (str[pos+1] << 8) | (str[pos]));
	return i;
}