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

static void timespec_diff(struct timespec *start,  struct timespec *result);

int cli_monitor(struct backend_conn * conn,	int argc, char **argv) {
	int c, result;
	int timeFlag = 0;

	static struct timespec startTime;
	static struct timespec elapsedTime;
	static struct timespec lastChecked;
	static int nsecs, rate = 10;
	static int forever = 0;


 	while ((c = getopt(argc, argv, "ft:r:")) != -1) {
		switch(c) {
			case 't' :
				nsecs = atoi(optarg);
				timeFlag = 1;
				break;
			case 'r' :
				rate = atoi(optarg) + 1;
				break;
			case 'f' :
				forever = 1;
				break;
			default :
				break;
		}
	}

	if (forever) {
		for (;;) {
			monitor(conn);
		}
	} else if (timeFlag) {
		clock_gettime(CLOCK_REALTIME, &startTime);
		clock_gettime(CLOCK_REALTIME, &lastChecked);
		while((result = monitor(conn)) == 0) {
			timespec_diff(&startTime, &elapsedTime);
			if(elapsedTime.tv_sec > nsecs) {
				break;
			}
			while(1) {
				timespec_diff(&lastChecked, &elapsedTime);
				if(elapsedTime.tv_nsec >= (long) ((SECOND_IN_NANO * 2) / rate)) {
					clock_gettime(CLOCK_REALTIME, &lastChecked);
					break;
				}
			}
		}
	} 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 */

	/* 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%7lf\t%7lf\t%7lf\t%7lf\t%7lf\t%7lf\t%7lf\t%7lf\t%7lf\n",
			0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
	printf("  I   :\t%7lf\t%7lf\t%7lf\t%7lf\t%7lf\t%7lf\t%7lf\t%7lf\t%7lf\n",
			0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
	printf("  D   :\t%7lf\t%7lf\t%7lf\t%7lf\t%7lf\t%7lf\t%7lf\t%7lf\t%7lf\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%7lf\t%7lf\t%7lf\t%7lf\t%7lf\t%7lf\n",
			td.height, td.lateral, td.longitudinal,
			td.pitch, td.roll, td.yaw);

	return 0;
}

static void timespec_diff(struct timespec *start, struct timespec *result)
{
	struct timespec stop;
	clock_gettime(CLOCK_REALTIME, &stop);
    if ((stop.tv_nsec - start->tv_nsec) < 0) {
        result->tv_sec = stop.tv_sec - start->tv_sec - 1;
        result->tv_nsec = stop.tv_nsec - start->tv_nsec + 1000000000;
    } else {
        result->tv_sec = stop.tv_sec - start->tv_sec;
        result->tv_nsec = stop.tv_nsec - start->tv_nsec;
    }

    return;
}