#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
				&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
			}
		}
	},
	
	// 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 yawset(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 yawset: %f\n", structs->setpoint_struct.desiredQuadPosition.yaw);
	
	return 0;
}

int yawp(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 yawp: %f\n", structs->parameter_struct.yaw_angle_pid.Kp);
	
	return 0;
}

int yawd(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 yawd: %f\n", structs->parameter_struct.yaw_angle_pid.Kd);
	
	return 0;
}

int rollset(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 rollset: %f\n", structs->setpoint_struct.desiredQuadPosition.roll);
	
	return 0;
}

int rollp(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 rollp: %f\n", structs->parameter_struct.local_y_pid.Kp);
	
	return 0;
}

int rolld(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 rolld: %f\n", structs->parameter_struct.local_y_pid.Kd);
	
	return 0;
}

int pitchset(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 pitchset: %f\n", structs->setpoint_struct.desiredQuadPosition.pitch);
	
	return 0;
}

int pitchp(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 pitchp: %f\n", structs->parameter_struct.local_x_pid.Kp);
	
	return 0;
}

int pitchd(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 pitchd: %f\n", structs->parameter_struct.local_x_pid.Kd);
	
	return 0;
}

// ------------------------------------------------------------
// These should be renamed to altitude!
int throttleset(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 throttleset: %f\n", structs->setpoint_struct.desiredQuadPosition.alt_pos);
	
	return 0;
}

int throttlep(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 throttlep: %f\n", structs->parameter_struct.alt_pid.Kp);
	
	return 0;
}

int throttlei(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 throttlei: %f\n", structs->parameter_struct.alt_pid.Ki);
	
	return 0;
}

int throttled(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 throttled: %f\n", structs->parameter_struct.alt_pid.Kd);
	
	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;
}