#include <stdlib.h>

#include "cmHandler.h"

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

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


const char * cmToString(int msgType, const struct controller_message *cm) 
{
	size_t index = (cm->id * MAX_CONTROL_PARAM_ID)  + cm->value_id;
	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)
{
	char st[128];
	strncpy(st, string, strlen(string));
	size_t i;
	int result;
	float cmdValue;
	char * cmdString = strtok(st, " ");
	cm->value = strtof(&string[strlen(cmdString)],NULL);
	int index = -1;

	for(i = 0; i < MAX_PARAM_COMMANDS; ++i) {
		if((result = strncmp(cmdString, respParamStrings[i], strlen(respParamStrings[i]))) == 0) {
			index = i;
			break;
		} else if ((result = strncmp(cmdString, setParamStrings[i], strlen(setParamStrings[i]))) == 0) {
			index = i;
			break;	
		}
	}

	/* Error, nothing found */
	if (index == -1) {
		return NULL;
	} else {
		cm->value_id = i % MAX_CONTROL_PARAM_ID;
		if (i <= PARAM_ROLL) {

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

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

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

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

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

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

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

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

			cm->id = ALT_ID;
		}
	}
	return cm;
}