#include <stdio.h>
#include <unistd.h>
#include <getopt.h>

#include "cli_getpid.h"

int cli_getpid(struct backend_conn * conn,	int argc, char **argv) {
	int c;
	static int getRoll = 0, getPitch = 0, getYaw = 0, getAll = 0;
	static int getRollV = 0, getPitchV = 0, getYawV = 0;
	static int getHeight = 0, getLat = 0, getLong = 0;
	static int needHelp = 0;
	struct frontend_pid_data pid_data;
	static struct option long_options[] = {
 		/* These options don’t set a flag. We distinguish them by their indices. */
 		{"roll",	no_argument,	&getRoll,	1},
 		{"pitch",   no_argument,   	&getPitch, 	1},
 		{"yaw",   no_argument,   	&getYaw, 	1},
 		{"rollv",   no_argument,   	&getRollV, 	1},
 		{"pitchv",   no_argument,   &getPitchV, 1},
 		{"yawv",   no_argument,   	&getYawV, 	1},
 		{"height",   no_argument,   &getHeight,	1},
 		{"lat",   no_argument,   	&getLat, 	1},
 		{"long",   no_argument,   	&getLong, 	1},
 		{"help",	no_argument,	&needHelp,	1},
 		{0, 0, 0, 0}
 	};

 	while (1)
	{
		/* getopt_long stores the option index here. */
		int option_index = 0;

		c = getopt_long(argc, argv, "a", long_options, &option_index);

		/* Detect the end of the options. */
		if (c == -1)
			break;

		if (c == 'a') {
			getAll = 1;
		}
	}

	if (needHelp) {
		printf("Getpid gets the p, i , or d constant values of any single controller\n");
		printf("Usage Syntax : \n\t./Cli getpid controller [options...]\n");
		printf("Symlink Usage Syntax : \n\t./getpid controller [options...]\n\n");
		printf("Available 'controllers' include the following\n");
		printf("\t[--pitch] : Pitch\n\t[--roll] : Roll\n\t[--yaw] : Yaw\n");
		printf("\t[--pitchv] : Pitch Velocity\n\t[--rollv] : Roll Velocity\n\t[--yawv] : Yaw Velocity\n");
		printf("\t[--height] : Z\n\t[--lat] : X\n\t[--long] : Y\n\n");
		printf("Available 'controller' options include the following\n");
		printf("\t[-p] 'val' : Gets the p constant of the 'controller\n");
		printf("\t[-i] 'val' : Gets the i constant of the 'controller'\n");
		printf("\t[-d] 'val' : Gets the d constant of the 'controller'\n");
		return 0;
	}

	if (argc < 2) {
		printf("Incorrect Usage, run './cli getpid --help' for correct usage.\n");
		return 1;
	}
	
	int result;
	if(getAll) {
		for(int i = 0; i < PID_NUM_PIDS; ++i) {
			pid_data.controller = i;
			if ((result = getValues(conn, &pid_data))) {
				return result;
			}
		}
	} else {
		if(getPitch) {
			pid_data.controller = PID_PITCH;
			if ((result = getValues(conn, &pid_data))) {
				return result;
			}
		}
		if(getRoll) {
			pid_data.controller = PID_ROLL;
			if ((result = getValues(conn, &pid_data))) {
				return result;
			}
		}
		if(getYaw) {
			pid_data.controller = PID_YAW;
			if ((result = getValues(conn, &pid_data))) {
				return result;
			}
		}
		if(getPitchV) {
			pid_data.controller = PID_PITCH_RATE;
			if ((result = getValues(conn, &pid_data))) {
				return result;
			}
		}
		if(getRollV) {
			pid_data.controller = PID_ROLL_RATE;
			if ((result = getValues(conn, &pid_data))) {
				return result;
			}
		}
		if(getYawV) {
			pid_data.controller = PID_YAW_RATE;
			if ((result = getValues(conn, &pid_data))) {
				return result;
			}
		}
		if(getHeight) {
			pid_data.controller = PID_HEIGHT;
			if ((result = getValues(conn, &pid_data))) {
				return result;
			}
		}
		if(getLat) {
			pid_data.controller = PID_LAT;
			if ((result = getValues(conn, &pid_data))) {
				return result;
			}
		}
		if(getLong) {
			pid_data.controller = PID_LONG;
			if ((result = getValues(conn, &pid_data))) {
				return result;
			}
		}

	}

	return 0;
}

int getValues(struct backend_conn * conn, struct frontend_pid_data * pid_data) {
	if(frontend_getpid(conn, pid_data)) {
		return 1;
	}

	switch(pid_data->controller) {
		case PID_PITCH :
			printf("Pitch Constants: P = %f\tI = %f\tD = %f\n",
				pid_data->p, pid_data->i, pid_data->d);
			break;
		case PID_ROLL :
			printf("Roll Constants: P = %f\tI = %f\tD = %f\n",
				pid_data->p, pid_data->i, pid_data->d);
			break;
		case PID_YAW :
			printf("Yaw Constants: P = %f\tI = %f\tD = %f\n",
				pid_data->p, pid_data->i, pid_data->d);
			break;
		case PID_PITCH_RATE :
			printf("Pitch Rate Constants: P = %f\tI = %f\tD = %f\n",
				pid_data->p, pid_data->i, pid_data->d);
			break;
		case PID_ROLL_RATE :
			printf("Roll Rate Constants: P = %f\tI = %f\tD = %f\n",
				pid_data->p, pid_data->i, pid_data->d);
			break;
		case PID_YAW_RATE :
			printf("Yaw Rate Constants: P = %f\tI = %f\tD = %f\n",
				pid_data->p, pid_data->i, pid_data->d);
			break;
		case PID_HEIGHT :
			printf("Height Constants: P = %f\tI = %f\tD = %f\n",
				pid_data->p, pid_data->i, pid_data->d);
			break;
		case PID_LAT :
			printf("Latitude Constants: P = %f\tI = %f\tD = %f\n",
				pid_data->p, pid_data->i, pid_data->d);
			break;
		case PID_LONG :
			printf("Longitude Constants: P = %f\tI = %f\tD = %f\n",
				pid_data->p, pid_data->i, pid_data->d);
			break;
		default :
			break;
	}
	return 0;
}