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

#include "cli_getsetpoint.h"

int cli_getsetpoint(struct backend_conn * conn,	int argc, char **argv) {
	int c;
	static int getheight = 0, getlat = 0, getlong = 0;
	static int getpitch = 0, getroll = 0, getyaw = 0;
	struct frontend_setpoint_data setpoint_data;
	static int needHelp = 0;
	static int mask;
	static struct option long_options[] = {
 		/* These options don’t set a flag. We distinguish them by their indices. */
 		{"help",	no_argument,	&needHelp,	1},
 		{"height",  no_argument,	&getheight, 1},
 		{"long",	no_argument,	&getlong,	1},
 		{"lat",		no_argument, 	&getlat,	1},
 		{"pitch",   no_argument,	&getpitch,  1},
 		{"roll",	no_argument,	&getroll,	1},
 		{"yaw",		no_argument, 	&getyaw,	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') {
			getheight = 1;
			getlat = 1;
			getlong = 1;
			getpitch = 1;
			getroll = 1;
			getyaw = 1;
		}
	}

	if (needHelp) {
		printf("Getsetpoint gets the height, lat, long, pitch, roll and yaw set point values for the quad.\n");
		printf("Usage Syntax : \n\t./Cli getsetpoint [options...]\n");
		printf("Symlink Usage Syntax : \n\t./getsetpoint [options...]\n\n");
		printf("Available options include the following\n");
		printf("\t[-a] 		 : Gets all of the following setpoints\n");
		printf("\t[--height] : Gets the height setpoint\n");
		printf("\t[--lat]    : Gets the latitude setpoint\n");
		printf("\t[--long]   : Gets the longitude setpoint\n");
		printf("\t[--pitch]  : Gets the pitch setpoint\n");
		printf("\t[--roll]   : Gets the roll setpoint\n");
		printf("\t[--yaw]    : Gets the yaw setpoint\n");
		return 0;
	}

	if (argc < 2) {
		printf("Incorrect Usage, run './cli getsetpoint --help' for correct usage.\n");
		return 1;
	}
	int result;
	if(getheight) {
		if ((result = getSetPointValues(conn, &setpoint_data, HEIGHT))) {
			return result;
		}
	}
	if(getlat) {
		if ((result = getSetPointValues(conn, &setpoint_data, LAT))) {
			return result;
		}
	}
	if(getlong) {
		if ((result = getSetPointValues(conn, &setpoint_data, LONGG))) {
			return result;
		}
	}
	if(getpitch) {
		if ((result = getSetPointValues(conn, &setpoint_data, PITCH))) {
			return result;
		}
	}
	if(getroll) {
		if ((result = getSetPointValues(conn, &setpoint_data, ROLL))) {
			return result;
		}
	}
	if(getyaw) {
		if ((result = getSetPointValues(conn, &setpoint_data, YAW))) {
			return result;
		}
	}

	return 0;
}

int getSetPointValues(struct backend_conn * conn, struct frontend_setpoint_data * setpoint_data, int type) {
	if(frontend_getsetpoint(conn, setpoint_data, type)) {
		return 1;
	}

	switch(type) {
		case HEIGHT :
			printf("Height: %f\n",
				setpoint_data->height);
			break;
		case LAT :
			printf("Latitude: %f\n",
				setpoint_data->lat);
		case LONGG :
			printf("Longitude: %f\n",
				setpoint_data->longg);
			break;
		case PITCH :
			printf("Pitch: %f\n",
				setpoint_data->pitch);
			break;
		case ROLL :
			printf("Roll: %f\n",
				setpoint_data->roll);
			break;
		case YAW :
			printf("Yaw: %f\n",
				setpoint_data->yaw);
			break;
		default :
			break;
	}
	return 0;
}