#include "track.h"
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <unistd.h>	// read, close
#include <fcntl.h>
#include <string.h>
#include <termios.h>
#include <errno.h>

coordinate_t *attention;
int count;

serial_t * open_serial(char *device, int baud)
{
	serial_t *serial;
	struct termios options;

	serial = malloc(sizeof(serial_t));
	if (errno == ENOMEM)
	{
		fprintf(stderr, "malloc returned ENOMEM while allocating " \
				"a new serial_t structure.\n");
		return NULL;
	}

	serial->device = device;
	serial->baud = baud;

	serial->buffer = malloc(255 * sizeof(unsigned char));

	serial->dev_handle = open(device, O_RDWR | O_NOCTTY | O_NDELAY);
	if (serial->dev_handle == -1) 
	{
		fprintf(stderr, "An error occurred while opening %s.\n", device);
		if ((strcmp(device, "/dev/ttyS0") != 0) && 
			(strcmp(device, "/dev/ttyS1") != 0))
		{
			fprintf(stderr, "The common device choices are /dev/ttyS0 " \
							"and /dev/ttyS1.\n");
		}
		free(serial);
		return NULL;
	}

	fcntl(serial->dev_handle, F_SETFL, 0);

	tcgetattr(serial->dev_handle, &options);

	switch(baud)
	{
		case 4800:
			cfsetispeed(&options, B4800);
			cfsetospeed(&options, B4800);
			break;
		case 9600:
			cfsetispeed(&options, B9600);
			cfsetospeed(&options, B9600);
			break;
		case 19200:
			cfsetispeed(&options, B19200);
			cfsetospeed(&options, B19200);
			break;
		case 38400:
			cfsetispeed(&options, B38400);
			cfsetospeed(&options, B38400);
			break;
		case 57600:
			cfsetispeed(&options, B57600);
			cfsetospeed(&options, B57600);
			break;
		case 115200:
			cfsetispeed(&options, B115200);
			cfsetospeed(&options, B115200);
			break;
		default:
			fprintf(stderr, "The serial baud rate must be 4800, 9600 " \
					"19200, 38400, 57600, or 115200.\n");
			close(serial->dev_handle);
			free(serial->buffer);
			free(serial);
			return NULL;
	}

	// configure serial port options
	options.c_cflag &= ~CSIZE;
	options.c_cflag |= CRTSCTS | CS8 | CLOCAL | CREAD;
	options.c_iflag = IGNPAR | ICRNL;
	options.c_oflag = 0;

	tcflush(serial->dev_handle, TCIFLUSH);
	tcsetattr(serial->dev_handle, TCSANOW, &options);

	return serial;
}

void close_serial(serial_t *serial)
{
	close(serial->dev_handle);
	free(serial->buffer);
	free(serial);
	serial = NULL;
}

int read_serial(serial_t *serial, coordinate_t *coordinate)
{
	size_t bytes;
	int offset = 0;
	unsigned char tempbuf[1];
	char *token_ptr;

	do
	{
		// read one character from the serial port
		bytes = read(serial->dev_handle, tempbuf, 1);
		if (bytes != 1)
		{
			fprintf(stderr, "Expected 1 byte from the serial port.\n" \
					"%d bytes received.\n", bytes);
			return -1;
		}

		// If it is not '\n' then append the character
		// to the serial buffer line.  The eye tracker
		// sends strings with one x-y coordinate pair
		// per line.  Rebuild these lines for parsing.
		if (tempbuf[bytes-1] != '\n')
		{
			serial->buffer[offset++] = tempbuf[bytes-1];
		}
		else
		{
			// c-strings require an EOL character 
			serial->buffer[offset] = '\0';
			break;
		}
	}
	while (bytes > 0);

	// Extract x-y coordiates for gaze fixation.
	// On the PAL system x ranges between [0,511]
	// and y is in the interval [0,308].  The
	// upper left corner is (0,0).

	// The string format has three spaces followed
	// by a tap character and the coordinate value.
	token_ptr = strtok(serial->buffer, " ");
	if (token_ptr == NULL)
	{
		return -2;
	}
	coordinate->x = atof(token_ptr);

	token_ptr = strtok(NULL, " ");
	if (token_ptr == NULL)
	{
		return -3;
	}
	coordinate->y = atof(token_ptr);

	return 1;
}

int main(int argc, char **argv)
{
	int i;

	coordinate_t *curr_xy;
	buffer_t *buffer;
	double dist1, dist2;

	int num_points;
	char *points_file;
	char *line;
	int n;
	buffer_t *points_buffer;
	coordinate_t *point;
	coordinate_t *nearest;
	char *token_ptr;

	// The eye tracker uses a 512x512 coordinate scale
	double x_scale = 0.75;
	double y_scale = 0.5;
	FILE *fd;
	int ret;

	const char *usage = "usage: track [[-n|--number] num_points] " \
						"[[-f|--file] file]";

	num_points = 8;

	for (i = 0; i < argc; i++)
	{
		if (strcmp(argv[i], "-f") == 0 || strcmp(argv[i], "--file") == 0)
		{
			if (i + 1 <= argc - 1)
			{
				i++;
				points_file = argv[i];
			}
			else
			{
				fprintf(stderr, "%s\n\n", usage);
				exit(EXIT_FAILURE);
			}
		}
		else
			points_file = "";

		if (strcmp(argv[i], "-n") == 0 || strcmp(argv[i], "--number") == 0)
		{
			if (i + 1 <= argc - 1)
			{
				i++;
				num_points = atoi(argv[i]);
			}
			else
			{
				fprintf(stderr, "%s\n\n", usage);
				exit(EXIT_FAILURE);
			}
		}
	}

	// A points file was specified.  Make sure it
	// exists and then extract the coordiantes.
	if (strcmp(points_file, "") != 0)
	{
		fd = fopen(points_file, "r");
		if (fd == NULL)
		{
			fprintf(stderr, "Error opening the file %s for read.\n",
					points_file);
			exit(EXIT_FAILURE);
		}

		// Buffer of allowable fixation points.
		// There should not be more than 100 or
		// else the first ones will be lost.  Really 
		// in any case there should not be more
		// than a handfull anyway.
		points_buffer = create_buffer(100);

		line = NULL;
		n = 0;
		while (getline(&line, &n, fd) != -1)
		{
			point = malloc(sizeof(coordinate_t));
			printf("%s", line);

			// Points are with respect to the 384x256 pixel
			// system image and so must be rescaled for
			// comparison to the 512x512 eye tracker data.
			token_ptr = strtok(line, " ");
			if (token_ptr == NULL)
			{
				fprintf(stderr, "Error parsing x\n");
			}
			// Rescale x as described
			point->x = atof(token_ptr) / x_scale;

			token_ptr = strtok(NULL, " ");
			if (token_ptr == NULL)
			{
				fprintf(stderr, "Error parsing y\n");
			}
			// Rescale y as described
			point->y = atof(token_ptr) / y_scale;

			push_buffer(points_buffer, point);
		}

		print_buffer(points_buffer);

		free(line);

		fclose(fd);
	}

	attention = malloc(num_points * sizeof(coordinate_t));

	buffer = create_buffer(num_points);

	serial_t *serial;
	if ((serial = open_serial("/dev/ttyS1", 4800)) == NULL)
	{
		fprintf(stderr, "Error while opening the serial device.\n");
		return 0;
	}

	count = 0;
	while (1)
	{
		// Note for your memory deallocation sanity.
		// If memory is added to a buffer it will
		// automagically be deallocated when popped
		// off the front.  Only when not pushed onto
		// the buffer must it be freed manually.
		curr_xy = malloc(sizeof(coordinate_t));
		if (errno == ENOMEM)
		{
			fprintf(stderr, "malloc returned ENOMEM while allocating " \
					"a new coordinate_t structure.\n");
			close_serial(serial);
			return 0;
		}

		// Read the next coordinate pair and
		// store in a coordinate_t structure 
		if ((ret = read_serial(serial, curr_xy)) < 0)
		{
			continue;
		}

		// Skip any points of (0,0).  This is likely an
		// error introduced by a bad calibration or blink.
		if (curr_xy->x == 0 && curr_xy->y == 0)
		{
			free(curr_xy);
			continue;
		}

		// Initialize with the first fixation point.
		if (count == 0)
		{
			nearest = nearest_neighbor(curr_xy, points_buffer);
			attention[count].x = nearest->x;
			attention[count].y = nearest->y;
			count++;
		}

		// Push the position onto the buffer
		push_buffer(buffer, curr_xy);

		// Average euclidian distance between fixation
		// points and the previous attention location
		dist1 = average_buffer_distance(buffer, &attention[count-1]);

		// Average euclidian distance between the
		// most recent points in the buffer.
		dist2 = average_buffer_distance(buffer, NULL);

		// Movement between attention points
		if (dist1 > 50)
		{
			// Focus on a single point
			if (dist2 < 15)
			{
				nearest = nearest_neighbor(curr_xy, points_buffer);

				// Do not allow successive identical fixation points
				if (nearest->x == attention[count-1].x &&
					nearest->y == attention[count-1].y)
					continue;

				attention[count].x = nearest->x;
				attention[count].y = nearest->y;
				count++;

				printf("x = %f\ty = %f\n", curr_xy->x, curr_xy->y);
				printf("dist1 = %f\tdist2 = %f\n", dist1, dist2);
				printf("nearest.x = %f\tnearest.y = %f\n",
						nearest->x, nearest->y);
			}
		}

		if (count == num_points)
			break;
	}

	// Write fixation points to a text file
	fd = fopen("attention.txt", "wt");
	for (count = 0; count < num_points; count++)
	{
		fprintf(fd, "%f %f\n",
				// Points are with respect to the 512x512 pixel
				// eye tracker data and so must be rescaled back
				// to the 384x256 pixel system data;
				floor(attention[count].x * x_scale),
				floor(attention[count].y * y_scale));
		fflush(fd);
	}
	fclose(fd);

	buffer_destroy(points_buffer);
	buffer_destroy(buffer);
	free(attention);
	close_serial(serial);

	return 1;
}