#include "communication.h"

// QUAD & Ground Station
// Format the log data from log_message
//int formatData(unsigned char *log_msg, unsigned char *formattedCommand)
int formatPacket(metadata_t *metadata, void *data, unsigned char **formattedCommand)
{
	*formattedCommand = malloc(sizeof(char) * metadata->data_len + 8);
	/*if (formattedCommand == NULL)
	{
		return -1;
	}*/
	
	//----------------------------------------------------------------------------------------------
	//	   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	   |
	//----------------------------------------------------------------------------------------------
					
	// Begin Char:
	(*formattedCommand)[0] = metadata->begin_char;

	// Msg type:
	(*formattedCommand)[1] = metadata->msg_type;
	
	// Msg subtype
	(*formattedCommand)[2] = metadata->msg_subtype;
	
	//Msg id (msgNum is 2 bytes)
	(*formattedCommand)[3] = metadata->msg_id;
	
	// Data length and data - bytes 5&6 for len, 7+ for data
	(*formattedCommand)[5] = metadata->data_len & 0x000000ff;
	(*formattedCommand)[6] = (metadata->data_len >> 8) & 0x000000ff;
	
//	printf("data length %d\n", metadata->data_len);
//	printf("data length %x\n", (*formattedCommand)[5]);
//	printf("data length %x\n", (*formattedCommand)[6]);

	memcpy(&((*formattedCommand)[7]), data, metadata->data_len);
	
	// Checksum
	// receive data and calculate checksum
	int i;
	unsigned char packet_checksum = 0;
	for(i = 0; i < 7 + metadata->data_len; i++)
	{
		packet_checksum ^= (*formattedCommand)[i];
	}
	
//	printf("Packet checksum: 0x%02x\n", packet_checksum);

	(*formattedCommand)[7 + metadata->data_len] = packet_checksum;
	
	return 0;
}

// returns the length of the data in bytes (datalen from packet) and fills 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) {
		printf("The first packet byte is not the begin char.\n");
		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]);
	unsigned char packet_checksum = packet[7+meta_data->data_len];

//	printf("msg_type: %x\n", meta_data->msg_type);
//	printf("msg_subtype: %x\n", meta_data->msg_subtype);
//	printf("msg_type: %d\n", 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
	unsigned char calculated_checksum = 0;
	for(i = 0; i < meta_data->data_len + 7; i++)
	{
		calculated_checksum ^= packet[i];
	}

	// compare checksum
	if(packet_checksum != calculated_checksum)
		printf("Checksums did not match (Quadlog): 0x%02x\t0x%02x\n", packet_checksum, calculated_checksum);

	return 0;
}

// QUAD & Ground Station
// Process the command received
int processCommand(unsigned char *packet, modular_structs_t *structs) {
	int validPacket;
	unsigned char *data;
	metadata_t metadata;
	
	printf("Process Command.\n");

	// Validate the message is correctly formatted
	validPacket = parse_packet(packet, &data, &metadata);
	if(validPacket != 0) {
		printf("Packet is not valid.\n");
		return -1;
	}
	
	if(metadata.data_len >= 0) {
		// Call the appropriate subtype function
		(* (MessageTypes[metadata.msg_type].subtypes[metadata.msg_subtype].functionPtr))(data, metadata.data_len, structs);
		
//		printf("%s\n", MessageTypes[metadata.msg_type].subtypes[metadata.msg_subtype].cmdText);

		return 0;
	} else {
		printf("Data length is less than 0.\n");
	}
	
	// Only gets here if there is an error
	return -1;
}