#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <err.h>

#include "frontend_getpid.h"
#include "pid_common.h"

/* Get a specified PID.
 *
 * Example:
 *
 * struct frontend_pid_data pid_data;
 * pid_data.pid = PID_PITCH;
 * if (frontend_getpid(conn, &pid_data)) {
 * 		error
 * } else {
 * 		pid_data.p, pid_data.i, and pid_data.d are filled
 * }
 *
 * Returns 0 on success, 1 on error
 */
int frontend_getpid(
		struct backend_conn * conn, struct frontend_pid_data * pid_data) {
	
	char line[100] = "";
	switch (pid_data->controller) {
		case PID_PITCH :
			strncpy(line, "getpitchp\ngetpitchi\ngetpitchd\n", 30);
			break;
		case PID_ROLL :
			strncpy(line, "getrollp\ngetrolli\ngetrolld\n", 27);
			break;
		case PID_YAW :
			strncpy(line, "getyawp\ngetyawi\ngetyawd\n", 24);
			break;
		case PID_PITCH_RATE :
			strncpy(line, "getpitchratep\ngetpitchratei\ngetpitchrated\n", 42);
			break;
		case PID_ROLL_RATE :
			strncpy(line, "getrollratep\ngetrollratei\ngetrollrated\n", 39);
			break;
		case PID_YAW_RATE :
			strncpy(line, "getyawratep\ngetyawratei\ngetyawrated\n", 36);
			break;
		case PID_HEIGHT :
			strncpy(line, "getheightp\ngetheighti\ngetheightd\n", 33);
			break;
		case PID_LAT :
			strncpy(line, "getlatp\ngetlati\ngetlatd\n", 24);
			break;
		case PID_LONG :
			strncpy(line, "getlongp\ngetlongi\ngetlongd\n", 27);
			break;
		default :
			return 1;
	}

	int size;
	if((size = ucart_backendWrite(conn, line)) < 0 ) {
		return 1;
	}

	char * response;
	char tmpresponse[64];
	char * cmdString;
	size_t pendingResponses = 3;
	float value;
	while (pendingResponses) {
		response = ucart_backendGetline(conn);
		if (response == NULL) {
			warnx("Line not returned from backend");
			return 1;
		}
		strncpy(tmpresponse, response, 64);
		if (strncmp(tmpresponse, "get", 3) == 0) {
			cmdString = strtok(tmpresponse, " ");
			value = strtof(&response[strlen(cmdString)],NULL);
			
			if(cmdString[strlen(cmdString)-1] == 'p') {
				pid_data->p = value;
			} else if(cmdString[strlen(cmdString)-1] == 'i') {
				pid_data->i = value;
			} else if(cmdString[strlen(cmdString)-1] == 'd') {
				pid_data->d = value;
			}
			pendingResponses--;
		}
	}
	return 0;
}