#include <stdlib.h>

#include "cmHandler.h"

const char * respParamStrings[MAX_PARAM_COMMANDS] = {
	"getrollp",
	"getrolli",
	"getrolld",
	"getpitchp",
	"getpitchi",
	"getpitchd",
	"getyawp",
	"getyawi",
	"getyawd",
	"getrollratep",
	"getrollratei",
	"getrollrated",
	"getpitchratep",
	"getpitchratei",
	"getpitchrated",
	"getyawratep",
	"getyawratei",
	"getyawrated",
	"getlatp",
	"getlati",
	"getlatd",
	"getlat",
	"getlongp",
	"getlongi",
	"getlongd",
	"getlong",
	"getheightp",
	"getheighti",
	"getheightd",
	"getheight"
};

const char * setParamStrings[MAX_PARAM_COMMANDS] = {
	"setrollp",
	"setrolli",
	"setrolld",
	"setpitchp",
	"setpitchi",
	"setpitchd",
	"setyawp",
	"setyawi",
	"setyawd",
	"setrollratep",
	"setrollratei",
	"setrollrated",
	"setpitchratep",
	"setpitchratei",
	"setpitchrated",
	"setyawratep",
	"setyawratei",
	"setyawrated",
	"setlatp",
	"setlati",
	"setlatd",
	"setlongp",
	"setlongi",
	"setlongd",
	"setheightp",
	"setheighti",
	"setheightd",
	"setlat",
	"setlong",
	"setheight",
};

const char * cmToString(int msgType, const struct controller_message *cm) 
{
	size_t index;
	if (cm->id == X_SETPOINT_ID) {
		index =  PARAM_LOCAL_X; // This will change when we make node structure changes
	} else if (cm->id == X_SETPOINT_ID) {
		index =  PARAM_LOCAL_Y; // This will change when we make node structure changes
	} else if (cm->id == X_SETPOINT_ID) {
		index =  PARMA_ALT; // This will change when we make node structure changes
	} else {
		index = (cm->id * MAX_CONTROL_PARAM_ID)  + cm->value_id;
		printf("index = %lu\n", index);
	}	
	switch (msgType) {
		case DEBUG_ID :
		case PACKETLOG_ID :
		case GETPACKETLOGS_ID :
		case UPDATE_ID :
		case BEGINUPDATE_ID :
		case LOG_ID:
		case SETPARAM_ID:
			return setParamStrings[index];
		case GETPARAM_ID:
			return NULL;
			break;
		case RESPONSE_ID:
			return NULL;
		case RESPPARAM_ID:
			return respParamStrings[index];
		default:
			return NULL;
			break;
	}
}

struct controller_message * stringToCm(const char * string, struct controller_message *cm)
{
	int result;
	size_t i;
	char cmdString[strlen(string)];
	strncpy(cmdString, string, strlen(string));

	float cmdValue;
	for (i = 0; i < strlen(string) + 1; ++i) {
		if (string[i] == ' ' || string[i] == '\n' || string[i] == '\0') {
			cmdString[i] = '\0';
			break;
		}
	}

	int index = -1;

	for(i = 0; i < MAX_PARAM_COMMANDS; ++i) {

		if((result = strncmp(cmdString, respParamStrings[i], strlen(respParamStrings[i]))) == 0 &&
			strlen(cmdString) == strlen(respParamStrings[i])) {
			index = i;
			break;
		} else if ((result = strncmp(cmdString, setParamStrings[i], strlen(setParamStrings[i]))) == 0 &&
			strlen(cmdString) == strlen(setParamStrings[i])) {
			cm->value = strtof(&string[strlen(cmdString)],NULL);
			index = i;
			break;	
		}
	}

	/* Error, nothing found */
	if (index == -1) {
		return NULL;
	} else {
		cm->value_id = i % MAX_CONTROL_PARAM_ID; // Default assuming not a setpoint command
		if (i <= PARAM_ROLL_D) {

			cm->id = ROLL_ID;
		} else if (i <= PARAM_PITCH_D) {

			cm->id = PITCH_ID;
		} else if (i <= PARAM_YAW_D) {

			cm->id = YAW_ID;
		} else if (i <= PARAM_ROLL_RATE_D) {

			cm->id = ROLL_RATE_ID;
		} else if (i <= PARAM_PITCH_RATE_D) {

			cm->id = PITCH_RATE_ID;
		} else if (i <= PARAM_YAW_RATE_D) {

			cm->id = YAW_RATE_ID;
		} else if (i <= PARAM_LOCAL_X_D) {

			cm->id = LOCAL_X_ID;
		} else if (i <= PARAM_LOCAL_Y_D) {

			cm->id = LOCAL_Y_ID;
		} else if (i <= PARAM_ALT_D) {

			cm->id = ALT_ID;
		} else if ( i == PARAM_LOCAL_X) {

			cm->id = X_SETPOINT_ID;
			cm->value_id = 0;
		} else if ( i == PARAM_LOCAL_Y) {
			
			cm->id = Y_SETPOINT_ID;
			cm->value_id = 0;
		} else if ( i == PARAM_ALT) {
			
			cm->id = ALT_SETPOINT_ID;
			cm->value_id = 0;
		}
	}
	// printf("cm->id = %d\ncm->value_id = %d\n", cm->id, cm->value_id);
	return cm;
}