#include "control.h"


#include <sys/socket.h>
#include <netdb.h>
#include <stdio.h>
#include <errno.h>
#include <string.h>
#include <stdlib.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <poll.h>

static int opensocket(const char *hostname, const char *port) {
	struct addrinfo *res = NULL, pref;
	int ret, s, val;
	
	pref.ai_flags = 0;
	pref.ai_family = PF_UNSPEC;
	pref.ai_socktype = SOCK_STREAM;
	pref.ai_protocol = 0;
	pref.ai_addrlen = 0;
	pref.ai_addr = NULL;
	pref.ai_canonname = NULL;
	pref.ai_next = NULL;
	
	if (0 != (ret = getaddrinfo(hostname, port, &pref, &res)) || !res) {
		fprintf(stderr, "Couldn't find host/port '%s:%s': %s\n", hostname, port, strerror(errno));
		return -1;
	}

	if (-1 == (s = socket(res->ai_family, res->ai_socktype, res->ai_protocol))) {
		fprintf(stderr, "Couldn't create socket: %s\n", strerror(errno));
		freeaddrinfo(res);
		return -1;
	}
	
	if (0 != connect(s, res->ai_addr, res->ai_addrlen)) {
		fprintf(stderr, "Couldn't connect socket to '%s:%s': %s\n", hostname, port, strerror(errno));
		freeaddrinfo(res);
		close(s);
		return -1;
	}
	
	val = 1;
	if (0 != setsockopt(s, IPPROTO_TCP, TCP_NODELAY, &val, sizeof(val))) {
		fprintf(stderr, "Couldn't set socket option TCP_NODELAY: %s\n", strerror(errno));
		freeaddrinfo(res);
		close(s);
		return -1;
	}
	
	freeaddrinfo(res);
	return s;
}



static void telemetry_free(gpointer _te, gpointer x) {
	telemetry *te = (telemetry*) _te;
	UNUSED(x);
	g_array_free(te->objects, TRUE);
	g_slice_free(telemetry, te);
}

telemetry* telemetry_new() {
	telemetry *te = g_slice_new0(telemetry);
	te->objects = g_array_new(FALSE, FALSE, sizeof(object));
	return te;
}

void telemetry_print(telemetry *t){
	fprintf(stderr,"Telemetry object:\n");
	fprintf(stderr,"\tTimestamp: %d Number of Objects: %d Vehicle:\n",t->ts,t->objects->len)	;
	vehicle_print(&t->vehicle);
}

guint object_hash(gconstpointer _o) {
	object *o = (object*) _o;
	const gchar *p = (const gchar*) (&o->x);
	gsize n = 2 * sizeof(double);
	guint h = 0;
	while (n--) { h = (h << 5) - h + *p; p++; }
	return h;
}

gboolean object_equal(gconstpointer _a, gconstpointer _b) {
	object *a = (object*) _a, *b = (object*) _b;
	return (a->type == b->type && a->x == b->x && a->y == b->y && a->rad == b->rad);
}

object* object_new(object_t type, double x, double y, double rad, double dir, double speed) {
	object *o = g_slice_new(object);
	o->type = type; o->x = x; o->y = y; o->rad = rad; o->dir = dir; o->speed = speed;
	return o;
}

void _object_free(gpointer p) {
	g_slice_free(object, (object*) p);
}

trial *trial_new(const char *hostname, const char *port) {
	trial *t;
	int sock;
	
	if (-1 == (sock = opensocket(hostname, port))) return NULL;
	
	t = g_slice_new0(trial);
	t->socket = sock;
	g_queue_init(&t->telemetry);
	t->map.solid_objects = g_hash_table_new_full(object_hash, object_equal, NULL, _object_free);
	t->sim.tm.objects = g_array_new(FALSE, FALSE, sizeof(object));
	control_parser_new(t);
	t->runcnt = 0;
	return t;
}

void trial_reset_run(trial *t) {
	g_queue_foreach(&t->telemetry, telemetry_free, NULL);
	g_queue_clear(&t->telemetry);
	t->last_ts = 0;
	t->runcnt++;
	control_parser_reset(t);
}

int trial_wait_for_start(trial *t) {
	while (!t->finished && !t->alive) {
		if (-1 == trial_wait_for_input(t)) return -1;
	}
	return 0;
}

static char buffer[16*1024];

int trial_check_input(trial *t) {
	ssize_t len;
	struct pollfd p;
	
	p.fd = t->socket;
	p.events = POLLIN;
	switch (poll(&p, 1, 0)) {
		case -1:
			switch (errno) {
				case EAGAIN:
				case EINTR:
					return 0;
			}
			fprintf(stderr, "poll error: %s\n", strerror(errno));
			return -1;
		case 0:
			return 0;
	}
	len = read(t->socket, buffer, sizeof(buffer));
	if (len < 0) {
		fprintf(stderr, "read error: %s\n", strerror(errno));
		return -1;
	} else if (len == 0) {
		t->finished = 1;
		if (t->alive) {
			fprintf(stderr, "unexpected connection close\n");
			return -1;
		}
		return 0;
	}
	g_string_append_len(t->parse_ctx->buffer, buffer, len);
	return (RUN_ERROR != control_parse(t)) ? 0 : -1;
}

int trial_wait_for_input(trial *t) {
	if (t->parse_ctx->buffer->len) return trial_check_input(t);
	ssize_t len = read(t->socket, buffer, sizeof(buffer));
	if (len < 0) {
		if (errno == ECONNRESET) {
			len = 0; /* treat as simply connection close */
		} else {
			fprintf(stderr, "read error: %s\n", strerror(errno));
			return -1;
		}
	}
	if (len == 0) {
		if (t->alive) {
			fprintf(stderr, "unexpected connection close\n");
			return -1;
		}
		t->finished = 1;
		return 0;
	}
	g_string_append_len(t->parse_ctx->buffer, buffer, len);
	return (RUN_ERROR != control_parse(t)) ? 0 : -1;
}

void trial_free(trial *t) {
	shutdown(t->socket, SHUT_RDWR);
	close(t->socket);
	g_queue_foreach(&t->telemetry, telemetry_free, NULL);
	g_queue_clear(&t->telemetry);
	g_hash_table_destroy(t->map.solid_objects);
	g_array_free(t->sim.tm.objects, TRUE);
	control_parser_free(t);
	g_slice_free(trial, t);
}


void trial_print(trial *t){
	fprintf(stderr,"Trial Alive: %d Last Timestep: %d Socket: %d\n",t->alive,t->last_ts,t->socket);
	/*perhaps add map_print and vehicle_print*/
}

void telemetry_matlab(gpointer tele, gpointer file){
	FILE* outfile;
	telemetry* t;
	vehicle * v;
	
	outfile = (FILE*) file;
	t = (telemetry*) tele;
	v = &t->vehicle;

	fprintf(outfile,"v(end+1,:) = [%d %f, %f, %f, %f];\n",t->ts,v->x,v->y,v->dir,v->speed);
}

/*prints coordinate history to matlab compatible file*/
void trial_matlab(trial *t){
	FILE* outfile;
	char name[80];

	sprintf(name,"velocity%d.m",t->runcnt);
	
	outfile = fopen(name,"w");
	fprintf(outfile,"v = [];\n");
	g_queue_foreach(&t->telemetry,telemetry_matlab,outfile);
	fclose(outfile);
}
	

/* debugging stuff */
void object_print(object *o){
	fprintf(stderr,"Object: Type: %d x: %f, y: %f, rad: %f dir: %f speed: %f\n",o->type,o->x,o->y,o->rad,o->dir,o->speed);
}

void vehicle_print(vehicle *v){
	fprintf(stderr,"Vehicle: Accel:  %d Turn: %d x: %f y: %f dir: %f speed: %f\n",v->accel,v->turn,v->x,v->y,v->dir,v->speed);
}

// void map_print(map *m){}