#include "communication.h"
#include "commands.h"
#include <string.h>
#include <ctype.h>

static int msgNum = 0;
//--------------------------------
// Ground Station
//--------------------------------

// Formatting commands from ground station CLI
int formatCommand(char *command, unsigned char *formattedCommand) {
	//fprintf(stderr, "length = %li , received '%s'\n", strlen(command), command);
	char cmd[512];
	strncpy(cmd, command, 512);

	char * cmdText = strtok(cmd, " ");

	metadata_t metadata;

	// ----------------------------------------------
	if(cmdText != NULL) {
		for(int type = 0; type < MAX_TYPE_ID; type++)
		{
			if(strcmp(cmdText, MessageTypes[type].cmdText) == 0)
			{
				printf("Sending\n\ttype: %d, \n\tcommand: %s\n", type, MessageTypes[type].cmdText);
				metadata.begin_char = (char) BEGIN_CHAR;
				metadata.msg_type = type;
				metadata.msg_id = msgNum++;
				metadata.data_len = 0;


				/* TODO: Format data correctly
				 * - Implement a case for every type
				 * 	- Format the tokens into data as appropriate for the type,
				 *   	  based on the quad's callbacks.c.
				 * - Pass the data and metadata into
				 *   	  formatPacket
				 * - Purge cmdDataType from existence
				 */

				/* Static data buffer */
				char data[256];
				switch (type) {
					// In each case, us strtok(NULL, " ") to tokenize,
					// format,
					// and append to data buffer (increase
					// metadat.data_len appropriately).
					case DEBUG_ID:
						break;
					case PACKETLOG_ID:
						break;
					case GETPACKETLOGS_ID:
						break;
					case UPDATE_ID:
						break;
					case BEGINUPDATE_ID:
						break;
					case LOG_ID:
						break;
					case RESPONSE_ID:
						break;
					case SETCONTROL_ID:
						break;
					case GETCONTROL_ID:
						break;
					case RESPCONTROL_ID:
						break;
					default:
						break;

				}
				return formatPacket(&metadata, data, formattedCommand);
			}
		}
	}
	// Only gets here if the command does not exist
	return -1;	
}
// QUAD & Ground Station
// Format the log data from log_message
int formatPacket(metadata_t *metadata, void *data, unsigned char *formattedCommand)
{
	//----------------------------------------------------------------------------------------------
	//	   index||	   0	|	  1	   |	  2		 |	3 & 4 |		 5 & 6		 |	7+	|	end	   |
	//---------------------------------------------------------------------------------------------|
	// msg param|| beg char |               msg type       | 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;
	formattedCommand[2] = ((metadata->msg_type >> 8) & 0x000000ff);

	//Msg id (msgNum is 2 bytes)
	formattedCommand[3] = (metadata->msg_id & 0x000000ff);
	formattedCommand[4] = ((metadata->msg_id >> 8) & 0x000000ff);

	// 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;

	memcpy(&(formattedCommand[7]), data, metadata->data_len);

	// Checksum
	// receive data and calculate checksum
	int i;
	char data_checksum = 0;
	for(i = 0; i < 7 + metadata->data_len; i++)
	{
		data_checksum ^= formattedCommand[i];
	}

	formattedCommand[7 + metadata->data_len] = data_checksum;

	/* Return data length */
	return metadata->data_len + 7;
}

// 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 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[2] << 8 | packet[1];
	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];
	//fprintf(stderr, "datalen: %d\n", meta_data->data_len);

	int i;

	// receive data
	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 - 1; i++)
	{
		calculated_checksum ^= packet[i];
	}

	// compare checksum
	if(packet_checksum != calculated_checksum) {
		return -2;
	}

	return 0;
}

// Process the command received
int processCommand(unsigned char *packet, modular_structs_t *structs) {
	int validPacket;
	unsigned char data[256];
	metadata_t metadata;

	// Validate the message is correctly formatted
	validPacket = parse_packet(packet, data, &metadata);
	if(validPacket != 0) {
		return -1;
	}

	if(metadata.data_len >= 0) {
		(* (MessageTypes[(unsigned char)metadata.msg_type].functionPtr))();

		return 0;
	}

	// Only gets here if there is an error
	return -1;
}