#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;

	int count, rate = 10;
	int forever = 0;


 	while ((c = getopt(argc, argv, "fc:r:")) != -1) {
		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 (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;
}