#define _GNU_SOURCE

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

#include "cli_monitor.h"
#include "frontend_tracker.h"

int cli_monitor(struct backend_conn * conn,	int argc, char **argv) {
	int c, result;
	int countFlag = 0;
	/* getopt_long stores the option index here. */
	int option_index = 0;
	int count, rate = 10;
	int forever = 0;
	static int needHelp = 0;

	static struct option long_options[] = {
 		/* These options don’t set a flag. We distinguish them by their indices. */
 		{"help",	no_argument,	&needHelp,	1},
 		{0, 0, 0, 0}
 	};

 	while (1)
	{
		// If you change this 		  VVV 		please also update the help message 
		c = getopt_long(argc, argv, "fc:r:", long_options, &option_index);

		if (c == -1)
			break;

 		switch(c) {
			case 'c' :
				count = atoi(optarg);
				countFlag = 1;
				break;
			case 'r' :
				rate = atoi(optarg) + 1;
				break;
			case 'f' :
				forever = 1;
				break;
			default :
				break;
		}
	}

	if (needHelp) {
		printf("helping you\n");
		return 0;
	}

	if (forever) {
		for (;;) {
			struct timespec req;
			if (rate == 1) {
				req.tv_sec = 1;
				req.tv_nsec = 0;
			} else { 
				req.tv_sec = 0;
				req.tv_nsec = 1000000000 / rate;
			}
			nanosleep(&req, NULL);
			monitor(conn);
		}
	} else if (countFlag) {
		for (int i = 0; i < count; i++) {
			result = monitor(conn);

			struct timespec req;
			if (rate == 1) {
				req.tv_sec = 1;
				req.tv_nsec = 0;
			} else { 
				req.tv_sec = 0;
				req.tv_nsec = 1000000000 / rate;
			}
			nanosleep(&req, NULL);
		}
	} else {
		return monitor(conn);
	}

	return result;
}

int monitor(struct backend_conn * conn) {
	/* Get tracker data */
	struct frontend_tracker_data td;
	if (frontend_track(conn, &td)) {
		errx(1, "Error reading tracker data");
	}

	/* TODO: Get PID constants and status */
	/* It might be a good idea to only read the pid constants
	 * every few seconds, so count iterations and only do it if 
	 * this is every (rate * 2 or 3) pass through the loop
	 */

	/* Print stuff on screen */
	/* Assuming a tab width of 8 columns */
	printf("\033[2J");
	printf("STATUS: NA\n");
	printf("CTRLR :\tP\tR\tY\tP_V\tR_V\tY_V\tH\tLAT\tLON\n");
	printf("  P   :\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\n",
			0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
	printf("  I   :\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\n",
			0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
	printf("  D   :\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\n",
			0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
	printf("PosAtt:\tH\tLAT\tLON\tP\tR\tY\n");
	printf("      :\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\n",
			td.height, td.lateral, td.longitudinal,
			td.pitch, td.roll, td.yaw);

	return 0;
}