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

#include "frontend_setpid.h"
#include "pid_common.h"
#include "frontend_common.h"

int frontend_setpid(
		struct backend_conn * conn,
		struct frontend_pid_data * pid_data,
		int mask)
{
	if (conn == NULL) {
		return -1;
	}

	char * controller;
	switch (pid_data->controller) {
		case PID_PITCH:
			controller = "pitch";
			break;
		case PID_ROLL:
			controller = "roll";
			break;
		case PID_YAW:
			controller = "yaw";
			break;
		case PID_PITCH_RATE:
			controller = "pitchrate";
			break;
		case PID_ROLL_RATE:
			controller = "rollrate";
			break;
		case PID_YAW_RATE:
			controller = "yawrate";
			break;
		case PID_HEIGHT:
			controller = "height";
			break;
		case PID_LAT:
			controller = "lat";
			break;
		case PID_LONG:
			controller = "long";
			break;
		default:
			warnx("Unsupported PID constant");
			return -1;
	}

	char buffer[2048];
	/* Set the P, I, and D */
	if (mask & SET_P) {
		if (snprintf(buffer, 2048,
					"set%sp %f\n", 
					controller, 
					pid_data->p) >= 2048) {
			errx(0, "Buffer to small to format!");
		}
		ucart_backendWrite(conn, buffer);
	}
	if (mask & SET_I) {
		if (snprintf(buffer, 2048,
					"set%si %f\n", 
					controller, 
					pid_data->i) >= 2048) {
			errx(0, "Buffer to small to format!");
		}
		ucart_backendWrite(conn, buffer);
	}
	if (mask & SET_D) {
		if (snprintf(buffer, 2048,
					"set%sd %f\n", 
					controller, 
					pid_data->d) >= 2048) {
			errx(0, "Buffer to small to format!");
		}
		ucart_backendWrite(conn, buffer);
	}

	return 0;
}