/** gui.cpp
 Summary: Convert network data into a visual representation
  - on the console, also takes keyboard input
  - from a web browser, inputs come in the form of GET parameters
 Usage:   ./gui NO_CONSOLE_GUI       (no_console_gui is 1 for true)
**/
using namespace std;
#include <iostream>
#include <stdio.h>
#include <curses.h>
#include <fcntl.h>
#include <gd.h>
#include <gdfonts.h>
#include "settings.h"
#define TIMER_RATE 4 // in Hz
#define MESSAGE_QUEUE 10 // number of messages to keep a queue of
#define PACKETS_QUEUE 3   // number of packets to keep a queue of
void pcap_handler2(u_char *args, const struct pcap_pkthdr *header, const u_char *packet);
void timer_handler(int x);
void write_status(); // write the status to an html file
string ShowImage(char *c_name, double val);
int file_is_png(FILE *fp);
struct pcap *handle; // handle to the pcap object

int s_kb; struct sockaddr_in si_kb;
int s_servo; struct sockaddr_in si_servo;

int screen_width, screen_height; // screen height/width
string message[MESSAGE_QUEUE]; // PORT_GUI_MSG
string packets[PACKETS_QUEUE];   // the last packets pcap saw
s_gps position, waypoints[5];
double screen_lat_top, screen_lat_bot, screen_lon_left, screen_lon_right; // screen min/max for the viewing area
double throttle, steering; // -100 to 100, percent sensed from a SetServo packet
double new_steering, new_throttle; // if manual control, set these
double heading;  // 0-359.999~ 0=north, 90=east
double velocity; // m/s
double rps_motor;
bool   no_console_gui; // true to suppress the curses output

int main(int argc, char *argv[]) {
	if (argc == 1) {
		no_console_gui = false;
	} else if (argc == 2) {
		if (atoi(argv[1]) == 1) {
			no_console_gui = true;
		}
	} else {
		printf("Usage: %s NO_CONSOLE_GUI=1\n", argv[0]);
		exit(-1);
	}
	
	heartbeat_open(PROG_GUI, "gui"); // listen for heartbeat requests

	string ports = "";
	ports = ports + "    udp port " + itoa(PORT_HEARTBEAT_REQ); // required
	ports = ports + " or udp port " + itoa(PORT_QUIT); // required
	ports = ports + " or udp port " + itoa(PORT_GUI_MSG); // raw messages to display
	ports = ports + " or udp port " + itoa(PORT_GPS);
	ports = ports + " or udp port " + itoa(PORT_WAYPOINTS);
	ports = ports + " or udp port " + itoa(PORT_SERVOS);
	ports = ports + " or udp port " + itoa(PORT_ABSDIRECTION);
	ports = ports + " or udp port " + itoa(PORT_FORCES);
	ports = ports + " or udp port " + itoa(PORT_ROTATESPEED);
	pcap_init(&handle, DEVICE, ports); // start listening
	
	s_kb = socket_open(&si_kb, PORT_KEYBOARD); // initialize output socket
	s_servo = socket_open(&si_servo, PORT_SERVOS); // initialize output socket

	// set up timer
	struct itimerval timer1;  // set up the timers
	signal(SIGALRM, timer_handler); // call this function on rollover
	timer1.it_interval.tv_sec  = 0;     // reset val
	timer1.it_interval.tv_usec = (__suseconds_t)((1.0/(double)TIMER_RATE)*1000000); // reset val (converts UPDATE_RATE from Hz into microseconds
	timer1.it_value.tv_sec     = timer1.it_interval.tv_sec;  // initial val
	timer1.it_value.tv_usec    = timer1.it_interval.tv_usec; // initial val
	setitimer(ITIMER_REAL, &timer1, NULL); // apply new settings




	// todo: automate these with the min/max waypoints read
	// current values: from 2004_rddf_qid.txt +- .0002
	screen_lat_top   = 33.69960216;//  34.0903595;
	screen_lat_bot   = 33.69895300;//  34.0885490;
	screen_lon_left  = -117.85956416;//-117.5069387;
	screen_lon_right = -117.85845833;//-117.4947212;

	printf("\ngui: started\n");
	if (!no_console_gui) {
		initscr();
		noecho();
		timeout(1); // allow getch to timeout with an ERR character
		getmaxyx(stdscr, screen_height, screen_width); // get the screen size
		mvprintw(0, 0, "gui: started; press q to quit");
		mvprintw(1, 0, "GPS (lat, lon): ");
		mvprintw(2, 0, "Message: ");
		mvprintw(4, 0, "Last Network: ");

		int i;
		// draw the arena outline, a rectangle filling most of the screen
		for (i = 0; i < screen_width; i++) { // draw top line
			mvprintw(10, i, "#");
		}
		for (i = 0; i < screen_width; i++) { // draw bottom line
			mvprintw(screen_height-1, i, "#");
		}
		for (i = 11; i < screen_height; i++) { // draw left line
			mvprintw(i, 0, "#");
		}
		for (i = 11; i < screen_height; i++) { // draw right line
			mvprintw(i, screen_width-1, "#");
		}
		mvprintw(9, 0, "Upper Left(lat/lon)=%.10lf/%.10lf; Bottom Right=%.10lf/%.10lf", screen_lat_top, screen_lon_left, screen_lat_bot, screen_lon_right);

		refresh();
	}

	char out[255];
	// ** pcap_loop will sit here and block *forever*.  Put all your code in the timer callback
	pcap_loop(handle, -1, pcap_handler2, NULL); // callback pcap_handler2 on new packet

	if (!no_console_gui) {
		endwin();
	}

	close(s_kb);
	pcap_close(handle);
	printf("\ngui: terminated\n");
	return 0;
}


void timer_handler(int x) {
	signal(SIGALRM, timer_handler); // reset, so it timers more than once
	// handle timer event
	int i;
	char out[255];

	if (!no_console_gui) {
		char ch;
		ch = getch();
		if (ch != ERR) {
			sprintf(out, "Keyboard,%d,%s,%c", PROG_GUI, timestamp().c_str(), ch);
			socket_send(s_kb, out, si_kb);
			switch (ch) {
			case 'w': case ',': // throttle up
				new_throttle = throttle + 10;
				if (new_throttle > 100) { new_throttle = 100; }
				mvprintw(4, 50, "Throttle Up to %.1lf     ", new_throttle);
				break;
			case 's': case 'o': // throttle down
				new_throttle = throttle - 10;
				if (new_throttle < -100) { new_throttle = -100; }
				mvprintw(4, 50, "Throttle Down to %.1lf   ", new_throttle);
				break;
			case 'a': // left
				new_steering = steering - 10;
				if (new_steering < -100) { new_steering = -100; }
				mvprintw(4, 50, "Steer Left to %.1lf      ", new_steering);
				break;
			case 'd': case 'e': // right
				new_steering = steering + 10;
				if (new_steering > 100) { new_steering = 100; }
				mvprintw(4, 50, "Steer Right to %.1lf     ", new_steering);
				break;
			case 'q': case ' ': // stop
				new_throttle = 0;
				new_steering = 0;
				mvprintw(4, 50, "Stop                     ");

				int s_quit; struct sockaddr_in si_quit;
				s_quit = socket_open(&si_quit, PORT_QUIT); // initialize output socket (make multiple of these to send on multiple ports)
				char buf2[] = "::quit::"; // send a couple times just to make sure
				socket_send(s_quit, buf2, si_quit); // send quit message
				socket_send(s_quit, buf2, si_quit); // send quit message
				socket_send(s_quit, buf2, si_quit); // send quit message
				close(s_quit);
				break;
			}
		}
	}

	
	
	if (new_throttle != throttle) {
		sprintf(out, "SetServo,%d,%s,%d,%.3lf", PROG_GUI, timestamp().c_str(), SERVO_THROTTLE, new_throttle);
//		socket_send(s_servo, out, si_servo);
	}	
	if (new_steering != steering) {
		sprintf(out, "SetServo,%d,%s,%d,%.3lf", PROG_GUI, timestamp().c_str(), SERVO_STEERING, new_steering);
//		socket_send(s_servo, out, si_servo);
	}	

	if (!no_console_gui) {
		// print status messages
		for (i = 16; i < screen_width; i++) { mvprintw(1, i, " "); }
		mvprintw(1, 16, "%.10lf, %.10lf -> %.10lf, %.10lf", position.lat, position.lon, waypoints[0].lat, waypoints[0].lon);
		for (i = 9; i < screen_width; i++) { mvprintw(2, i, " "); }
		mvprintw(2, 9, "%s", message[0].c_str());
		for (i = 13; i < screen_width; i++) { mvprintw(4, i, " "); }
		mvprintw(4, 14, "%s", packets[0].c_str());


		double distance = gps2distance(position.lat, position.lon, waypoints[0].lat, waypoints[0].lon);
		mvprintw(3, 0, "Steering: %.1lf  Throttle: %.1lf  Heading: %.2lf  Dist: %.3lf  Speed: %.1lfm/s (%.1lfmph) @%.1lfrpm      ", steering, throttle, heading, distance, velocity, (meters2feet(velocity) * 5280 / 60 / 60), (rps_motor * 60));


		int row, col;
		// print bot position
		row = (int)(11+(position.lat - screen_lat_top)  / (screen_lat_bot   - screen_lat_top)  * (screen_height - 12));
		col = (int)(1 +(position.lon - screen_lon_left) / (screen_lon_right - screen_lon_left) * (screen_width  - 2));
		mvprintw(row, col, "*");

		// print the waypoints
		for (i=4; i >= 0; i--) { // go backwards so lowest number overwrites higher on duplicates
			row = (int)(11+(waypoints[i].lat - screen_lat_top)  / (screen_lat_bot   - screen_lat_top)  * (screen_height - 12));
			col = (int)(1 +(waypoints[i].lon - screen_lon_left) / (screen_lon_right - screen_lon_left) * (screen_width  - 2));
			mvprintw(row, col, "%d", i+1);
		}
		refresh();
	}
}





void pcap_handler2(u_char *args, const struct pcap_pkthdr *header, const u_char *packet) {
	s_pcap_packet pcap_packet = pcap_decode(packet);

	int i;

	if (pcap_packet.port == PORT_HEARTBEAT_REQ) { // required
//		cout << "port=" << pcap_packet.port << "; hb_payload=" << pcap_packet.payload << endl << "\r";
		heartbeat(pcap_packet.payload); // reply to request
	} else if (pcap_packet.port == PORT_QUIT) { // quit on ::quit:: message
		if (strlen(pcap_packet.payload) == 8) { // ::quit::
			if (pcap_packet.payload[0] == ':' && pcap_packet.payload[1] == ':' && pcap_packet.payload[2] == 'q' && pcap_packet.payload[3] == 'u' && pcap_packet.payload[4] == 'i' && pcap_packet.payload[5] == 't' && pcap_packet.payload[6] == ':' && pcap_packet.payload[7] == ':') {
				pcap_breakloop(handle);
			}
		}
	} else {
		// packet received
		// pcap_packet.port     // the port we received the data on
		// pcap_packet.payload  // the data passed
		for (i = PACKETS_QUEUE-1; i > 0; i--) {
			packets[i] = packets[i-1];
		}
		packets[0] = pcap_packet.payload;

		string name, timestamp;
		int id, i, count, servo_num, sensor_num;
		double lat, lon, angle, percent;
		switch (pcap_packet.port) {
		case PORT_GUI_MSG:
			for (i = MESSAGE_QUEUE-1; i > 0; i--) {
				message[i] = message[i-1];
			}
			message[0] = pcap_packet.payload;
			break;
		case PORT_ABSDIRECTION:
			name      = strtok(pcap_packet.payload, ",");
			id        = atoi(strtok(NULL, ","));
			timestamp = strtok(NULL, ",");
			sensor_num= atoi(strtok(NULL, ","));
			angle = strtod(strtok(NULL, ","), NULL);
			if (name == "AbsDirection") {
				if (sensor_num == DEV_COMPASS) {
					heading = angle;
				}
			}
			break;
		case PORT_GPS:
			name      = strtok(pcap_packet.payload, ",");
			id        = atoi(strtok(NULL, ","));
			timestamp = strtok(NULL, ",");
			lat       = strtod(strtok(NULL, ","), NULL);
			lon       = strtod(strtok(NULL, ","), NULL);
			if (name == "GPS") {
				position.lat = lat;
				position.lon = lon;
			}
			break;
		case PORT_WAYPOINTS:
			count = 0; // count the number of commas
			for (i = 0; i < strlen(pcap_packet.payload); i++) {
				if (pcap_packet.payload[i] == ',') {
					count++;
				}
			}
			name      = strtok(pcap_packet.payload, ",");
			id        = atoi(strtok(NULL, ","));
			timestamp = strtok(NULL, ",");
			if (name == "Waypoints") {
				switch(count) {
					case 5:  count = 1; break;
					case 8:  count = 2; break;
					case 11: count = 3; break;
					case 14: count = 4; break;
					case 17: count = 5; break;
					case 20: count = 6; break;
					default: count = 0; break; // empty/invalid packet
				}
				for (i = 0; i < count; i++) {
					waypoints[i].lat       = strtod(strtok(NULL, ","), NULL);
					waypoints[i].lon       = strtod(strtok(NULL, ","), NULL);
					waypoints[i].max_speed = strtod(strtok(NULL, ","), NULL);
				}
			}
			break;
		case PORT_SERVOS:
			name      = strtok(pcap_packet.payload, ",");
			id        = atoi(strtok(NULL, ","));
			timestamp = strtok(NULL, ",");
			servo_num = atoi(strtok(NULL, ","));
			percent   = strtod(strtok(NULL, ","), NULL);
			if (name == "SetServo") {
				switch (servo_num) {
				case SERVO_THROTTLE:
					throttle = percent;
					break;
				case SERVO_STEERING:
					steering = percent;
					break;
				}
			}
			break;
		case PORT_FORCES:
			name      = strtok(pcap_packet.payload, ",");
			id        = atoi(strtok(NULL, ","));
			timestamp = strtok(NULL, ",");
			if (name == "Forces") {
				velocity  = strtod(strtok(NULL, ","), NULL);
			}
			break;
		case PORT_ROTATESPEED:
			name      = strtok(pcap_packet.payload, ",");
			id        = atoi(strtok(NULL, ","));
			timestamp = strtok(NULL, ",");
			if (name == "RotateSpeed") {
				rps_motor = strtod(strtok(NULL, ","), NULL);
			}
			break;
		}
	}
	write_status();
}




void write_status() {
	// write the status to the html file
	int i;
	char filename[255] = "";
	strcpy(filename, HTTP_FILE_STATUS);
	strcat(filename, ".tmp"); // add .tmp so we can just rename when it's done writing
	FILE *fp = fopen(filename, "w");

	fprintf(fp, "<html><head><title>Robot Status</title>\n");
	fprintf(fp, "<meta http-equiv=\"refresh\" content=\"1\">\n");
	fprintf(fp, "</head><body>\n");
	fprintf(fp, "<table border=\"1\" cellpadding=\"2\" cellspacing=\"0\" width=\"100%\">\n");
	fprintf(fp, "<tr><td nowrap>Timestamp</td><td>%s</td></tr>\n", timestamp().c_str());
	fprintf(fp, "<tr><td nowrap>Lat, Lon</td><td>%.10lf, %.10lf</td></tr>\n", position.lat, position.lon);
	
	fprintf(fp, "<tr><td nowrap>Next 5 Waypoints</td><td>\n");
	for (i = 0; i < 5; i++) {
		fprintf(fp, "%d. %.10lf, %.10lf<br>\n", i+1, waypoints[i].lat, waypoints[i].lon);
	}
	fprintf(fp, "</td></tr>\n");

	fprintf(fp, "<tr><td nowrap>Last %d Messages</td><td>\n", MESSAGE_QUEUE+1);
	for (i = 0; i < MESSAGE_QUEUE; i++) {
		fprintf(fp, "%d. %s<br>\n", i+1, message[i].c_str());
	}
	fprintf(fp, "</td></tr>\n");

	fprintf(fp, "<tr><td nowrap>Last %d Packets</td><td>\n", PACKETS_QUEUE+1);
	for (i = 0; i < PACKETS_QUEUE; i++) {
		fprintf(fp, "%d. %s<br>", i+1, packets[i].c_str());
	}
	fprintf(fp, "</td></tr>\n");
		
	double distance = gps2distance(position.lat, position.lon, waypoints[0].lat, waypoints[0].lon);
	fprintf(fp, "<tr><td>Steering</td><td>%.1lf &nbsp; %s</td></tr>\n", steering, ShowImage("steering", steering).c_str());
	fprintf(fp, "<tr><td>Throttle</td><td>%.1lf &nbsp; %s</td></tr>\n", throttle, ShowImage("throttle", throttle).c_str());
	fprintf(fp, "<tr><td>Heading </td><td>%.2lf</td></tr>\n", heading);
	fprintf(fp, "<tr><td>Dist    </td><td>%.3lf</td></tr>\n", distance);
	fprintf(fp, "<tr><td>Speed   </td><td>%.1lfm/s (%.1lfmph) @%.1lfrpm</td></tr>\n", velocity, (meters2feet(velocity) * 5280 / 60 / 60), (rps_motor * 60));
	fprintf(fp, "</table>\n");
	fprintf(fp, "</body></html>\n");

	fclose(fp);

	
	rename(filename, HTTP_FILE_STATUS);
}

string ShowImage(char *c_name, double val) {
	// 1) Read in overlay graphic
	// 2) Copy to new file, for pixels with color REPLACE_COLOR replace with its gradiant based on the width/height
	// http://www.boutell.com/gd/manual2.0.33.html#fonts
	string name = c_name;
	string file_original;
	string file_output;
	string ret;
	bool is_horiz = false;

	if (name == "steering") {
		file_original = "steering_original.png";
		file_output   = "steering.png";
		is_horiz = true;
	} else if (name == "throttle") {
		file_original = "throttle_original.png";
		file_output   = "throttle.png";
		is_horiz = false;
	} else {
		return "Invalid Image Specifier";
	}
	

	FILE *fp = fopen(file_output.c_str(), "wb");
	if (!fp) {
			printf("Error: unable to open %s\n", file_output.c_str()); exit(-1);
	}

	int width;
	int height;

	if (is_horiz) {
		width  = 90;
		height = 20;
	} else {
		width  = 20;
		height = 90;
	}
	gdImagePtr im; // declare the image
//	im = gdImageCreateFromPng(file_original.c_str());
	im = gdImageCreate(width, height); // create blank image
	// declare color intexes
	int bg    = gdImageColorAllocate(im, 255, 255, 255); // bg
	int white = gdImageColorAllocate(im, 255, 255, 255);
	int black = gdImageColorAllocate(im, 0,   0,   0);
	int red   = gdImageColorAllocate(im, 255, 0,   0);


	int h = (int)(height * (val/100.0));
//cout << "\n\rheight=" << height << "; val=" << val << "; h=" << h << "\n\r";
	if (h > 0) {
		if (is_horiz) {
			gdImageFilledRectangle(im, width/2, 0, width - h, height, red); // positive side
		} else { // is vert
			gdImageFilledRectangle(im, 0, height/2, width, height/2 - h, red); // positive side
		}
	}
	if (h < 0) {
		if (is_horiz) {
			gdImageFilledRectangle(im, width/2, 0, width - h, height, red); // positive side
		} else { // is vert
			gdImageFilledRectangle(im, 0, height/2, width, height/2 - h, red); // positive side
		}
	}

	if (is_horiz) {
		gdImageLine(im, width/2, 0, width/2, height, black); // midline
	} else { // is vert
		gdImageLine(im, 0, height/2, width, height/2, black); // midline
	}
/*
	width  = gdImageSX(im);
	height = gdImageSY(im);

	int c, r, g, b;
	for (int x = 0; x < width; x++) {
		for (int y = 0; y < height; y++) {
			c = gdImageGetPixel(im, x, y);
			r = im->red[c];
			g = im->green[c];
			b = im->blue[c];

			if (r == ) {
				im->red[c] = something
			}
		}
	}
*/

	char temp[50];
	sprintf(temp, "%lf", val);
	if (is_horiz) {
		gdImageString(im, gdFontGetSmall(), width/2+5, 5, (unsigned char*)temp, black);
	}



	gdImagePng(im, fp);
	fclose(fp);
	gdImageDestroy(im);

	ret = "<img src=\"" + file_output + "\">"; // width=\"" << width << "\" height=\"" << height << "\">";
	return ret;
}