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

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_array_new(FALSE, FALSE, sizeof(object));
	control_parser_new(t);
	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;
	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) {
	ssize_t len = read(t->socket, buffer, sizeof(buffer));
	if (len < 0) {
		fprintf(stderr, "read error: %s\n", strerror(errno));
		return -1;
	} else 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_array_free(t->map.solid_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*/
}



/* 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){}