#include "commands.h"

// TAKE THESE OUT WHEN IMPLEMENTING ON THE QUAD SIDE
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;
}
//------------------------------------------------

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
			}
		}
	},
	
	// CALIBRATION
	{
		// Message Type ID
		0x01,
		
		// Calibration Subtypes
		{
			// yaw setpoint subtype
			{
				// ID
				0x00,
				// Command text
				"setyaw",
				// Type of the command data
				floatType,
				// Function pointer
				&setyaw
			},
			// yaw p constant subtype
			{
				// ID
				0x01,
				// Command text
				"setyawp",
				// Type of the command data
				floatType,
				// Function pointer
				&setyawp
			},
			// yaw d constant subtype
			{
				// ID
				0x02,
				// Command text
				"setyawd",
				// Type of the command data
				floatType,
				// Function pointer
				&setyawd
			},
			// roll setpoint subtype
			{
				// ID
				0x03,
				// Command text
				"setroll",
				// Type of the command data
				floatType,
				// Function pointer
				&setroll
			},
			// roll p constant subtype
			{
				// ID
				0x04,
				// Command text
				"setrollp",
				// Type of the command data
				floatType,
				// Function pointer
				&setrollp
			},
			// roll d constant subtype
			{
				// ID
				0x05,
				// Command text
				"setrolld",
				// Type of the command data
				floatType,
				// Function pointer
				&setrolld
			},
			// pitch setpoint subtype
			{
				// ID
				0x06,
				// Command text
				"setpitch",
				// Type of the command data
				floatType,
				// Function pointer
				&setpitch
			},
			// pitch p constant subtype
			{
				// ID
				0x07,
				// Command text
				"setpitchp",
				// Type of the command data
				floatType,
				// Function pointer
				&setpitchp
			},
			// pitch d constant subtype
			{
				// ID
				0x08,
				// Command text
				"setpitchd",
				// Type of the command data
				floatType,
				// Function pointer
				&setpitchd
			},
			// throttle setpoint subtype
			{
				// ID
				0x09,
				// Command text
				"setthrottle",
				// Type of the command data
				floatType,
				// Function pointer
				&setthrottle
			},
			// throttle p constant subtype
			{
				// ID
				0x0A,
				// Command text
				"setthrottlep",
				// Type of the command data
				floatType,
				// Function pointer
				&setthrottlep
			},
			// throttle i constant subtype
			{
				// ID
				0x0B,
				// Command text
				"setthrottlei",
				// Type of the command data
				floatType,
				// Function pointer
				&setthrottlei
			},
			// throttle d constant subtype
			{
				// ID
				0x0C,
				// Command text
				"setthrottled",
				// Type of the command data
				floatType,
				// Function pointer
				&getthrottled
			}
		}
	},
	
	// REQUEST
	{
		// Message Type ID
		0x02,
		
		// Request Subtypes
		{
			// accelerometer subtype
			{
				// ID 
				0x00,
				// Command text
				"getaccel",
				// Type of the command data
				floatType,
				// Function pointer
				&getaccel
			},
			// gyroscope subtype
			{
				// ID 
				0x01,
				// Command text
				"getgyro",
				// Type of the command data
				floatType,
				// Function pointer
				&getgyro
			},
			// pitch angle subtype
			{
				// ID 
				0x02,
				// Command text
				"getpitchangle",
				// Type of the command data
				floatType,
				// Function pointer
				&getpitchangle
			},
			// roll angle subtype
			{
				// ID 
				0x03,
				// Command text
				"getrollangle",
				// Type of the command data
				floatType,
				// Function pointer
				&getrollangle
			},
			// get yaw setpoint subtype
			{
				// ID
				0x04,
				// Command text
				"getyaw",
				// Type of the command data
				floatType,
				// Function pointer
				&setyaw
			},
			// get yaw p constant subtype
			{
				// ID
				0x05,
				// Command text
				"getyawp",
				// Type of the command data
				floatType,
				// Function pointer
				&getyawp
			},
			// get yaw d constant subtype
			{
				// ID
				0x06,
				// Command text
				"getyawd",
				// Type of the command data
				floatType,
				// Function pointer
				&getyawd
			},
			// get roll setpoint subtype
			{
				// ID
				0x07,
				// Command text
				"getroll",
				// Type of the command data
				floatType,
				// Function pointer
				&getroll
			},
			// get roll p constant subtype
			{
				// ID
				0x08,
				// Command text
				"getrollp",
				// Type of the command data
				floatType,
				// Function pointer
				&getrollp
			},
			// getroll d constant subtype
			{
				// ID
				0x09,
				// Command text
				"getrolld",
				// Type of the command data
				floatType,
				// Function pointer
				&getrolld
			},
			// get pitch setpoint subtype
			{
				// ID
				0x0A,
				// Command text
				"getpitch",
				// Type of the command data
				floatType,
				// Function pointer
				&getpitch
			},
			// get pitch p constant subtype
			{
				// ID
				0x0B,
				// Command text
				"getpitchp",
				// Type of the command data
				floatType,
				// Function pointer
				&getpitchp
			},
			// get pitch d constant subtype
			{
				// ID
				0x0C,
				// Command text
				"getpitchd",
				// Type of the command data
				floatType,
				// Function pointer
				&getpitchd
			},
			// get throttle setpoint subtype
			{
				// ID
				0x0D,
				// Command text
				"getthrottle",
				// Type of the command data
				floatType,
				// Function pointer
				&getthrottle
			},
			// get throttle p constant subtype
			{
				// ID
				0x0E,
				// Command text
				"getthrottlep",
				// Type of the command data
				floatType,
				// Function pointer
				&getthrottlep
			},
			// get throttle i constant subtype
			{
				// ID
				0x0F,
				// Command text
				"getthrottlei",
				// Type of the command data
				floatType,
				// Function pointer
				&getthrottlei
			},
			// get throttle d constant subtype
			{
				// ID
				0x10,
				// Command text
				"getthrottled",
				// Type of the command data
				floatType,
				// Function pointer
				&getthrottled
			}
		}
	},
	
	// RESPONSE
	{
		// Message Type ID
		0x03,
		
		// Response Subtypes
		{
			// accelerometer subtype
			{
				// ID
				0x00,
				// Command text
				"respaccel",
				// Type of the command data
				floatType,
				// Function pointer
				&respaccel
			},
			// gyroscope subtype
			{
				// ID
				0x01,
				// Command text
				"respgyro",
				// Type of the command data
				floatType,
				// Function pointer
				&respgyro
			},
			// pitch angle subtype
			{
				// ID 
				0x02,
				// Command text
				"resppitchangle",
				// Type of the command data
				floatType,
				// Function pointer
				&resppitchangle
			},
			// roll angle subtype
			{
				// ID 
				0x03,
				// Command text
				"resprollangle",
				// Type of the command data
				floatType,
				// Function pointer
				&resprollangle
			}
		}
	},
	
	// UPDATE
	{
		// Message Type ID
		0x04,
		
		// Update Subtypes
		{
			// NONE subtype
			{
				// ID 
				0x00,
				// Command text
				"update",
				// Type of the command data
				stringType,
				// Function pointer
				&update
			}
		}
	},
	
	// 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;
}

int update(unsigned char *packet, int dataLen,	modular_structs_t *structs)
{
	unsigned char update[28];
	memcpy(update, ((float *)packet), 28);
	
	int packetId = getInt(update, 0);
	float y_pos = getFloat(update, 4);
	float x_pos = getFloat(update, 8);
	float alt_pos = getFloat(update, 12);
	float roll = getFloat(update, 16);
	float pitch = getFloat(update, 20);
	float yaw = getFloat(update, 24);

	structs->log_struct.currentQuadPosition.packetId = packetId;
	structs->log_struct.currentQuadPosition.y_pos = y_pos;
	structs->log_struct.currentQuadPosition.x_pos = x_pos;
	structs->log_struct.currentQuadPosition.alt_pos = alt_pos;
	structs->log_struct.currentQuadPosition.roll = roll;
	structs->log_struct.currentQuadPosition.pitch = pitch;
	structs->log_struct.currentQuadPosition.yaw = yaw;
	
	printf("QUAD: VRPN Packet:");
	printf("Packet ID: %d\n", packetId);
	printf("Y Position: %f\n", y_pos);
	printf("X Position: %f\n", x_pos);
	printf("Altitude Position: %f\n", alt_pos);
	printf("Roll: %f\n", roll);
	printf("Pitch: %f\n", pitch);
	printf("Yaw: %f\n", yaw);
	
	printf("function for update\n");
	return 0;
}

// Why is this here?
// This should be on the ground station side
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;
}

// ------------------------------------------------------------------

int setyaw(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
	float value;
	
	memcpy(&value, ((float *)packet), dataLen);
	
	printf("%f\n", value);
	
	structs->setpoint_struct.desiredQuadPosition.yaw = value;

	printf("function for setyaw: %f\n", structs->setpoint_struct.desiredQuadPosition.yaw);
	
	return 0;
}

int setyawp(unsigned char *packet, int dataLen,  modular_structs_t *structs)
{
	float value;
	
	memcpy(&value, ((float *)packet), dataLen);
	structs->parameter_struct.yaw_angle_pid.Kp = value;
	
	printf("function for setyawp: %f\n", structs->parameter_struct.yaw_angle_pid.Kp);
	
	return 0;
}

int setyawd(unsigned char *packet, int dataLen,  modular_structs_t *structs)
{
	float value;
	
	memcpy(&value, ((float *)packet), dataLen);
	structs->parameter_struct.yaw_angle_pid.Kd = value;
	
	printf("function for setyawd: %f\n", structs->parameter_struct.yaw_angle_pid.Kd);
	
	return 0;
}

int setroll(unsigned char *packet, int dataLen,	 modular_structs_t *structs)
{
	float value;
	
	memcpy(&value, ((float *)packet), dataLen);
	structs->setpoint_struct.desiredQuadPosition.roll = value;
	
	printf("function for setroll: %f\n", structs->setpoint_struct.desiredQuadPosition.roll);
	
	return 0;
}

int setrollp(unsigned char *packet, int dataLen,  modular_structs_t *structs)
{
	float value;
	
	memcpy(&value, ((float *)packet), dataLen);
	structs->parameter_struct.local_y_pid.Kp = value;
	
	printf("function for setrollp: %f\n", structs->parameter_struct.local_y_pid.Kp);
	
	return 0;
}

int setrolld(unsigned char *packet, int dataLen,  modular_structs_t *structs)
{
	float value;
	
	memcpy(&value, ((float *)packet), dataLen);
	structs->parameter_struct.local_y_pid.Kd = value;
	
	printf("function for setrolld: %f\n", structs->parameter_struct.local_y_pid.Kd);
	
	return 0;
}

int setpitch(unsigned char *packet, int dataLen,  modular_structs_t *structs)
{
	float value;
	
	memcpy(&value, ((float *)packet), dataLen);
	structs->setpoint_struct.desiredQuadPosition.pitch = value;
	
	printf("function for setpitch: %f\n", structs->setpoint_struct.desiredQuadPosition.pitch);
	
	return 0;
}

int setpitchp(unsigned char *packet, int dataLen,	modular_structs_t *structs)
{
	float value;
	
	memcpy(&value, ((float *)packet), dataLen);
	structs->parameter_struct.local_x_pid.Kp = value;
	
	printf("function for setpitchp: %f\n", structs->parameter_struct.local_x_pid.Kp);
	
	return 0;
}

int setpitchd(unsigned char *packet, int dataLen,	modular_structs_t *structs)
{
	float value;
	
	memcpy(&value, ((float *)packet), dataLen);
	structs->parameter_struct.local_x_pid.Kd = value;
	
	printf("function for setpitchd: %f\n", structs->parameter_struct.local_x_pid.Kd);
	
	return 0;
}

// ------------------------------------------------------------
// These should be renamed to altitude!
int setthrottle(unsigned char *packet, int dataLen,	 modular_structs_t *structs)
{
	float value;
	
	memcpy(&value, ((float *)packet), dataLen);
	structs->setpoint_struct.desiredQuadPosition.alt_pos = value;
	
	printf("function for setthrottle: %f\n", structs->setpoint_struct.desiredQuadPosition.alt_pos);
	
	return 0;
}

int setthrottlep(unsigned char *packet, int dataLen,  modular_structs_t *structs)
{
	float value;
	
	memcpy(&value, ((float *)packet), dataLen);
	structs->parameter_struct.alt_pid.Kp = value;
	
	printf("function for setthrottlep: %f\n", structs->parameter_struct.alt_pid.Kp);
	
	return 0;
}

int setthrottlei(unsigned char *packet, int dataLen,  modular_structs_t *structs)
{
	float value;
	
	memcpy(&value, ((float *)packet), dataLen);
	structs->parameter_struct.alt_pid.Ki = value;
	
	printf("function for setthrottlei: %f\n", structs->parameter_struct.alt_pid.Ki);
	
	return 0;
}

int setthrottled(unsigned char *packet, int dataLen,  modular_structs_t *structs)
{
	float value;
	
	memcpy(&value, ((float *)packet), dataLen);
	structs->parameter_struct.alt_pid.Kd = value;
	
	printf("function for setthrottled: %f\n", structs->parameter_struct.alt_pid.Kd);
	
	return 0;
}
int getyaw(unsigned char *packet, int dataLen, modular_structs_t *structs){
	return 0;
}
int getyawp(unsigned char *packet, int dataLen, modular_structs_t *structs){
	return 0;
}
int getyawd(unsigned char *packet, int dataLen, modular_structs_t *structs) {
	return 0;
}
int getroll(unsigned char *packet, int dataLen, modular_structs_t *structs){
	return 0;
}
int getrollp(unsigned char *packet, int dataLen, modular_structs_t *structs){
	return 0;
}
int getrolld(unsigned char *packet, int dataLen, modular_structs_t *structs){
	return 0;
}
int getpitch(unsigned char *packet, int dataLen, modular_structs_t *structs){
	return 0;
}
int getpitchp(unsigned char *packet, int dataLen, modular_structs_t *structs){
	return 0;
}
int getpitchd(unsigned char *packet, int dataLen, modular_structs_t *structs){
	return 0;
}
int getthrottle(unsigned char *packet, int dataLen, modular_structs_t *structs){
	return 0;
}
int getthrottlep(unsigned char *packet, int dataLen, modular_structs_t *structs){
	return 0;
}
int getthrottlei(unsigned char *packet, int dataLen, modular_structs_t *structs){
	return 0;
}
int getthrottled(unsigned char *packet, int dataLen, modular_structs_t *structs){
	return 0;
}

// These should be renamed to altitude!
// ------------------------------------------------------------

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

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

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

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

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

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

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

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