#include "communication.h"
#include "commands.h"
#include "type_def.h"
#include "uart.h"

struct MessageType MessageTypes[MAX_TYPE] = 
{
	// DEBUG
	{
		// Message Type ID
		0x00,
		
		// Debug Subtypes
		{
			// NONE subtype
			{
				// ID
				0x00,
				// Command text
				"debug",
				// Type of the command data
				stringType,
				// Function pointer
				&debug
			},

			// Echo subtype
			{
				// ID
				0x01,
				// Command text
				"ack",
				// Type of the command data
				intType,
				// Function pointer
				&debug
			},
			// Log packet receiving
			{
				// ID
				0x02,
				// Command text
				"packetlog",
				// Type of the command data
				stringType,
				// Function pointer
				&packetlog
			},
			// Get packet receiving logs
			{
				// ID
				0x03,
				// Command text
				"getpacketlogs",
				// Type of the command data
				stringType,
				// Function pointer
				&getpacketlogs
			}
		}
	},
	
	// CALIBRATION
	{
		// Message Type ID
		0x01,
		
		// Calibration Subtypes
		{
			// yaw setpoint subtype
			{
				// ID
				0x00,
				// Command text
				"setyaw",
				// Type of the command data
				floatType,
				// Function pointer
				&yawset
			},
			// yaw p constant subtype
			{
				// ID
				0x01,
				// Command text
				"setyawp",
				// Type of the command data
				floatType,
				// Function pointer
				&yawp
			},
			// yaw d constant subtype
			{
				// ID
				0x02,
				// Command text
				"setyawd",
				// Type of the command data
				floatType,
				// Function pointer
				&yawd
			},
			// roll setpoint subtype
			{
				// ID
				0x03,
				// Command text
				"setroll",
				// Type of the command data
				floatType,
				// Function pointer
				&rollset
			},
			// roll p constant subtype
			{
				// ID
				0x04,
				// Command text
				"setrollp",
				// Type of the command data
				floatType,
				// Function pointer
				&rollp
			},
			// roll d constant subtype
			{
				// ID
				0x05,
				// Command text
				"setrolld",
				// Type of the command data
				floatType,
				// Function pointer
				&rolld
			},
			// pitch setpoint subtype
			{
				// ID
				0x06,
				// Command text
				"setpitch",
				// Type of the command data
				floatType,
				// Function pointer
				&pitchset
			},
			// pitch p constant subtype
			{
				// ID
				0x07,
				// Command text
				"setpitchp",
				// Type of the command data
				floatType,
				// Function pointer
				&pitchp
			},
			// pitch d constant subtype
			{
				// ID
				0x08,
				// Command text
				"setpitchd",
				// Type of the command data
				floatType,
				// Function pointer
				&pitchd
			},
			// throttle setpoint subtype
			{
				// ID
				0x09,
				// Command text
				"setthrottle",
				// Type of the command data
				floatType,
				// Function pointer
				&throttleset
			},
			// throttle p constant subtype
			{
				// ID
				0x0A,
				// Command text
				"setthrottlep",
				// Type of the command data
				floatType,
				// Function pointer
				&throttlep
			},
			// throttle i constant subtype
			{
				// ID
				0x0B,
				// Command text
				"setthrottlei",
				// Type of the command data
				floatType,
				// Function pointer
				&throttlei
			},
			// throttle d constant subtype
			{
				// ID
				0x0C,
				// Command text
				"setthrottled",
				// Type of the command data
				floatType,
				// Function pointer
				&throttled
			}
		}
	},
	
	// REQUEST
	{
		// Message Type ID
		0x02,
		
		// Request Subtypes
		{
			// accelerometer subtype
			{
				// ID 
				0x00,
				// Command text
				"accelreq",
				// Type of the command data
				floatType,
				// Function pointer
				&accelreq
			},
			// gyroscope subtype
			{
				// ID 
				0x01,
				// Command text
				"gyroreq",
				// Type of the command data
				floatType,
				// Function pointer
				&gyroreq
			},
			// pitch angle subtype
			{
				// ID 
				0x02,
				// Command text
				"reqpitchangle",
				// Type of the command data
				floatType,
				// Function pointer
				&pitchanglereq
			},
			// roll angle subtype
			{
				// ID 
				0x03,
				// Command text
				"reqrollangle",
				// Type of the command data
				floatType,
				// Function pointer
				&rollanglereq
			}
		}
	},
	
	// RESPONSE
	{
		// Message Type ID
		0x03,
		
		// Response Subtypes
		{
			// accelerometer subtype
			{
				// ID
				0x00,
				// Command text
				"respaccel",
				// Type of the command data
				floatType,
				// Function pointer
				&accelresp
			},
			// gyroscope subtype
			{
				// ID
				0x01,
				// Command text
				"respgyro",
				// Type of the command data
				floatType,
				// Function pointer
				&gyroresp
			},
			// pitch angle subtype
			{
				// ID 
				0x02,
				// Command text
				"resppitchangle",
				// Type of the command data
				floatType,
				// Function pointer
				&pitchangleresp
			},
			// roll angle subtype
			{
				// ID 
				0x03,
				// Command text
				"resprollangle",
				// Type of the command data
				floatType,
				// Function pointer
				&rollangleresp
			}
		}
	},
	
	// UPDATE
	{
		// Message Type ID
		0x04,
		
		// Update Subtypes
		{
			// NONE subtype
			{
				// ID 
				0x00,
				// Command text
				"update",
				// Type of the command data
				stringType,
				// Function pointer
				&update
			},
			// BEGIN UPDATE subtype
			{
				// ID
				0x01,
				// Command text
				"beginupdate",
				// Type of the command data
				stringType,
				// Function pointer
				&beginupdate
			}
		}
	},
	
	// LOG
	{
		// Message Type ID
		0x05,
		
		// Log Subtypes
		{
			// NONE subtype
			{
				// ID
				0x00,
				// Command text
				"log",
				// Type of the command data
				stringType,
				// Function pointer
				&logdata
			},
			// Response subtype
			{
				// ID
				0x01,
				// Command text
				"response",
				// Type of the command data
				stringType,
				// Function pointer
				&response
			}
		}
	},
	
};

int debug(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
	printf("function for debug\n");
	return 0;
}

static int n_msg_received = 0;
static size_t total_payload_received = 0;

int packetlog(unsigned char* packet, int dataLen, modular_structs_t* structs) {
	n_msg_received += 1;
	total_payload_received += dataLen;
	return 0;
}

int getpacketlogs(unsigned char* packet, int dataLen, modular_structs_t* structs) {
	char buf[255];

	// Message logging number of messages received and size of payload received
	int length = snprintf(buf, sizeof buf, "%d,%d", n_msg_received, total_payload_received);

	send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
	return 0;
}

/* Handles receiving new location updates */
int update(unsigned char *packet, int dataLen,	modular_structs_t *structs)
{
	//processUpdate(packet, &(structs->raw_sensor_struct.currentQuadPosition));

	quadPosition_t* currentQuadPosition = &(structs->raw_sensor_struct.currentQuadPosition);
	// Packet must come as [NEARPY], 4 bytes each
	int packetId = getInt(packet, 0);
//	printf("Packet ID: %d\n", packetId);
	float y_pos = getFloat(packet, 4);
//	printf("y_pos: %f\n", y_pos);
	float x_pos = getFloat(packet, 8);
//	printf("x_pos: %f\n", x_pos);
	float alt_pos = getFloat(packet, 12);
//	printf("alt_pos: %f\n", alt_pos);
	float roll = getFloat(packet, 16);
//	printf("roll: %f\n", roll);
	float pitch = getFloat(packet, 20);
//	printf("pitch: %f\n", pitch);
	float yaw = getFloat(packet, 24);
//	printf("yaw: %f\n", yaw);

	currentQuadPosition->packetId = packetId;
	currentQuadPosition->y_pos = y_pos;
	currentQuadPosition->x_pos = x_pos;
	currentQuadPosition->alt_pos = alt_pos;
	currentQuadPosition->roll = roll;
	currentQuadPosition->pitch = pitch;
	currentQuadPosition->yaw = yaw;

	// Make location as fresh
	structs->user_input_struct.locationFresh = 1;

	return 0;
}

// This is called on the ground station to begin sending VRPN to the quad
int beginupdate(unsigned char *c, int dataLen, modular_structs_t *structs) {
	structs->user_input_struct.receivedBeginUpdate = 1;
	return 0;
}

int logdata(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
	printf("Logging: %s\n", packet);
	return 0;
}

int response(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
	printf("This is the response: %s\n", packet);
	
	return 0;
}

// ------------------------------------------------------------------
// Quad side implementation

// TODO: Erase memory leaks

int yawset(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
	float value;
	char buf[255] = {};
	
	// Copy the data into the value variable
	memcpy(&value, ((float *)packet), dataLen);
	
	// Update the struct
	structs->setpoint_struct.desiredQuadPosition.yaw = value;

	// Debug print statement
	//printf("function for yawset: %f\n", structs->setpoint_struct.desiredQuadPosition.yaw);
	
	// Send a reply to the ground station
	int length = snprintf(buf, sizeof(buf), "Successfully set desired yaw to %.2f radians\r\n", structs->setpoint_struct.desiredQuadPosition.yaw);

	send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
	return 0;
}

int yawp(unsigned char *packet, int dataLen,  modular_structs_t *structs)
{
	float value;
	char buf[255] = {0};
	
	memcpy(&value, ((float *)packet), dataLen);
	structs->parameter_struct.yaw_angle_pid.Kp = value;
	
	printf("function for yawp: %f\n", structs->parameter_struct.yaw_angle_pid.Kp);
	
	// Send a reply to the ground station
	int length = snprintf(buf, sizeof(buf), "Successfully set yaw Kp to %.2f\r\n", structs->parameter_struct.yaw_angle_pid.Kp);

	send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
	return 0;
}

int yawd(unsigned char *packet, int dataLen,  modular_structs_t *structs)
{
	float value;
	char buf[255] = {};
	
	memcpy(&value, ((float *)packet), dataLen);
	structs->parameter_struct.yaw_angle_pid.Kd = value;
	
	printf("function for yawd: %f\n", structs->parameter_struct.yaw_angle_pid.Kd);
	
	// Send a reply to the ground station
	int length = snprintf(buf, sizeof(buf), "Successfully set yaw Kd to %.2f\r\n", structs->parameter_struct.yaw_angle_pid.Kd);

	send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
	return 0;
}

int rollset(unsigned char *packet, int dataLen,	 modular_structs_t *structs)
{
	float value;
	char buf[255] = {};
	
	memcpy(&value, ((float *)packet), dataLen);
	structs->setpoint_struct.desiredQuadPosition.roll = value;
	
	printf("function for rollset: %f\n", structs->setpoint_struct.desiredQuadPosition.roll);
	
	// Send a reply to the ground station
	int length = snprintf(buf, sizeof(buf), "Successfully set desired roll to %.2f radians\r\n", structs->setpoint_struct.desiredQuadPosition.roll);

	send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
	return 0;
}

int rollp(unsigned char *packet, int dataLen,  modular_structs_t *structs)
{
	float value;
	char buf[255] = {};
	memcpy(&value, ((float *)packet), dataLen);
	structs->parameter_struct.roll_angle_pid.Kp = value;
	
	printf("function for rollp: %f\n", structs->parameter_struct.roll_angle_pid.Kp);
	
	// Send a reply to the ground station
	size_t length = snprintf(buf, sizeof(buf), "Successfully set roll Kp to %.2f\r\n", structs->parameter_struct.roll_angle_pid.Kp);

	send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);

	return 0;
}

int rolld(unsigned char *packet, int dataLen,  modular_structs_t *structs)
{
	float value;
	char buf[255] = {};
	
	memcpy(&value, ((float *)packet), dataLen);
	structs->parameter_struct.roll_angle_pid.Kd = value;

	printf("function for rolld: %f\n", structs->parameter_struct.roll_angle_pid.Kd);
	
	// Send a reply to the ground station
	size_t length = snprintf(buf, sizeof(buf), "Successfully set roll Kd to %.2f\r\n", structs->parameter_struct.roll_angle_pid.Kd);

	send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
	return 0;
}

int pitchset(unsigned char *packet, int dataLen,  modular_structs_t *structs)
{
	float value;
	char buf[255] = {};
	
	memcpy(&value, ((float *)packet), dataLen);
	structs->setpoint_struct.desiredQuadPosition.pitch = value;
	
	printf("function for pitchset: %f\n", structs->setpoint_struct.desiredQuadPosition.pitch);
	
	// Send a reply to the ground station
	int length = snprintf(buf, sizeof(buf), "Successfully set desired pitch to %.2f radians\r\n", structs->setpoint_struct.desiredQuadPosition.pitch);

	send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
	return 0;
}

int pitchp(unsigned char *packet, int dataLen,	modular_structs_t *structs)
{
	float value;
	char buf[255] = {};
	
	memcpy(&value, ((float *)packet), dataLen);
	structs->parameter_struct.pitch_angle_pid.Kp = value;
	
	printf("function for pitchp: %f\n", structs->parameter_struct.pitch_angle_pid.Kp);
	
	// Send a reply to the ground station
	int length = snprintf(buf, sizeof(buf), "Successfully set pitch Kp to %.2f\r\n", structs->parameter_struct.pitch_angle_pid.Kp);

	send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
	return 0;
}

int pitchd(unsigned char *packet, int dataLen,	modular_structs_t *structs)
{
	float value;
	char buf[255] = {};
	
	memcpy(&value, ((float *)packet), dataLen);
	structs->parameter_struct.pitch_angle_pid.Kd = value;
	
	printf("function for pitchd: %f\n", structs->parameter_struct.pitch_angle_pid.Kd);
	
	// Send a reply to the ground station
	int length = snprintf(buf, sizeof(buf), "Successfully set desired yaw to %.2f\r\n", structs->parameter_struct.pitch_angle_pid.Kd);

	send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
	return 0;
}

// ------------------------------------------------------------
// These should be renamed to altitude!
int throttleset(unsigned char *packet, int dataLen,	 modular_structs_t *structs)
{
	float value;
	char buf[255] = {};
	
	memcpy(&value, ((float *)packet), dataLen);
	structs->setpoint_struct.desiredQuadPosition.alt_pos = value;
	
	printf("function for throttleset: %f\n", structs->setpoint_struct.desiredQuadPosition.alt_pos);
	
	// Send a reply to the ground station
	int length = snprintf(buf, sizeof(buf), "Successfully set desired altitude to %.2f meters\r\n", structs->setpoint_struct.desiredQuadPosition.alt_pos);

	send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
	return 0;
}

int throttlep(unsigned char *packet, int dataLen,  modular_structs_t *structs)
{
	float value;
	char buf[255] = {};
	
	memcpy(&value, ((float *)packet), dataLen);
	structs->parameter_struct.alt_pid.Kp = value;
	
	printf("function for throttlep: %f\n", structs->parameter_struct.alt_pid.Kp);
	
	// Send a reply to the ground station
	int length = snprintf(buf, sizeof(buf), "Successfully set alt Kp to %.2f\r\n", structs->parameter_struct.alt_pid.Kp);

	send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
	return 0;
}

int throttlei(unsigned char *packet, int dataLen,  modular_structs_t *structs)
{
	float value;
	char buf[255] = {};
	
	memcpy(&value, ((float *)packet), dataLen);
	structs->parameter_struct.alt_pid.Ki = value;
	
	printf("function for throttlei: %f\n", structs->parameter_struct.alt_pid.Ki);
	
	// Send a reply to the ground station
	int length = snprintf(buf, sizeof(buf), "Successfully set alt Ki to %.2f\r\n", structs->parameter_struct.alt_pid.Ki);

	send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
	return 0;
}

int throttled(unsigned char *packet, int dataLen,  modular_structs_t *structs)
{
	float value;
	char buf[255] = {};
	
	memcpy(&value, ((float *)packet), dataLen);
	structs->parameter_struct.alt_pid.Kd = value;
	
	printf("function for throttled: %f\n", structs->parameter_struct.alt_pid.Kd);
	
	// Send a reply to the ground station
	int length = snprintf(buf, sizeof(buf), "Successfully set alt Kd to %.2f\r\n", structs->parameter_struct.alt_pid.Kd);

	send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
	return 0;
}
// These should be renamed to altitude!
// ------------------------------------------------------------

int accelreq(unsigned char *packet, int dataLen,  modular_structs_t *structs)
{
	printf("function for accelreq\n");
	return 0;
}

int gyroresp(unsigned char *packet, int dataLen,  modular_structs_t *structs)
{
	printf("function for accelreq\n");
	return 0;
}

int pitchangleresp(unsigned char *packet, int dataLen,	modular_structs_t *structs)
{
	printf("function for accelreq\n");
	return 0;
}

int rollangleresp(unsigned char *packet, int dataLen,  modular_structs_t *structs)
{
	printf("function for accelreq\n");
	return 0;
}

int gyroreq(unsigned char *packet, int dataLen,	 modular_structs_t *structs)
{
	printf("function for accelreq\n");
	return 0;
}

int pitchanglereq(unsigned char *packet, int dataLen,  modular_structs_t *structs)
{
	printf("function for accelreq\n");
	return 0;
}

int rollanglereq(unsigned char *packet, int dataLen,  modular_structs_t *structs)
{
	printf("function for accelreq\n");
	return 0;
}

int accelresp(unsigned char *packet, int dataLen,  modular_structs_t *structs)
{
	printf("function for accelreq\n");
	return 0;
}