#include "commands.h"

/* New stuff - this is nice and clean */

/* Override any callbacks here */


/****** LEGACY CODE BE VERY AFRAID ********/

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

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

int cb_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 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 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 respaccel(unsigned char *packet, int dataLen,  modular_structs_t *structs)
{
	printf("function for respaccel\n");
	return 0;
}

int respyaw(unsigned char *packet, int dataLen, modular_structs_t *structs){
	return 0;
}
int respyawp(unsigned char *packet, int dataLen, modular_structs_t *structs){
	return 0;
}
int respyawd(unsigned char *packet, int dataLen, modular_structs_t *structs) {
	return 0;
}
int resproll(unsigned char *packet, int dataLen, modular_structs_t *structs){
	return 0;
}
int resprollp(unsigned char *packet, int dataLen, modular_structs_t *structs){
	return 0;
}
int resprolld(unsigned char *packet, int dataLen, modular_structs_t *structs){
	return 0;
}
int resppitch(unsigned char *packet, int dataLen, modular_structs_t *structs){
	return 0;
}
int resppitchp(unsigned char *packet, int dataLen, modular_structs_t *structs){
	return 0;
}
int resppitchd(unsigned char *packet, int dataLen, modular_structs_t *structs){
	return 0;
}
int respthrottle(unsigned char *packet, int dataLen, modular_structs_t *structs){
	return 0;
}
int respthrottlep(unsigned char *packet, int dataLen, modular_structs_t *structs){
	return 0;
}
int respthrottlei(unsigned char *packet, int dataLen, modular_structs_t *structs){
	return 0;
}
int respthrottled(unsigned char *packet, int dataLen, modular_structs_t *structs){
	return 0;
}