#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netdb.h> 
#include <stdarg.h>
#include <assert.h>

#define UBYTE	0x07
#define BYTE	0x08
#define INTEGER	0x09
#define FLOAT	0x0a
#define DOUBLE	0x0b
#define STRING	0x0c
#define LIST	0x0e

typedef struct {
	int32_t size;
	int32_t pos;
	uint8_t *data;
} message;

int sockfd;
message msg;

void error(char *msg) {
    perror(msg);
    exit(0);
}

void msg_new() {
	msg.size = 0;
	msg.pos = 0;
	msg.data = calloc(255, sizeof(char));
}

void msg_free() {
	free(msg.data);
}

void msg_write(uint8_t cmd, char *format, ...) {
	va_list ap;
	char *str;
	int len;
	int old = msg.size;
	int i;

	msg.size++;
	msg.data[msg.size++] = cmd;
	va_start(ap, format);
	for (i = 0; i < strlen(format); i++) {
		switch (format[i]) {
			case 'b' :
				msg.data[msg.size++] = (uint8_t) va_arg(ap, int);
				break;
			case 'i' :
				*(uint32_t*) &msg.data[msg.size] = htonl(va_arg(ap, uint32_t));
				msg.size += 4;
				break;
			case 'd' :
				*(double*) &msg.data[msg.size] = va_arg(ap, double);
				msg.size += 8;
				break;
			case 's' :
				str = va_arg(ap, char*);
				len = strlen(str);
				*(uint32_t*) &msg.data[msg.size] = htonl(len);
				msg.size += 4;
				memcpy(&msg.data[msg.size], str, len);
				msg.size += len;
				break;
			default :
				break;
		}
	}
	va_end(ap);
	msg.data[old] = (uint8_t) (msg.size - old);
}

void msg_send() {
	int32_t size;
	int res;

	size = htonl(msg.size + 4);
	res = write(sockfd, (char*) &size, 4);
	if (res < 0) error("ERROR sending message size");
	res = write(sockfd, msg.data, msg.size);
	if (res < 0) error("ERROR sending message data");
	free(msg.data);
}

void msg_recv() {
	int32_t size;
	int res;
//	int i;

	res = read(sockfd, &size, 4);
	if (res < 0) error("ERROR receiving message size");
	msg.size = ntohl(size) - 4;
	msg.data = calloc(msg.size, sizeof(char));
	res = read(sockfd, msg.data, msg.size);
	if (res < 0) error("ERROR receiving message data");
//	printf("### ");
//	for (i = 0; i < res; i++) {
//		printf("%02x-", msg.data[i]);
//	}
//	printf("\n");
}

void msg_read(uint8_t cmd, char *format, ...) {
	va_list ap;
	char *s;
	uint8_t** str;
	uint8_t*** lst;
	uint32_t host;
	int cnt;
	int len;
	int old = msg.pos;
	uint32_t size;
	int i;
	int j;

	size = (uint8_t) msg.data[msg.pos++];
	if (!size) {
		size = ntohl(*(uint32_t*) &msg.data[msg.pos]);
		msg.pos += 4;
	}
//	printf("CMD: %02X DATA: %02X\n", cmd, msg.data[msg.pos]);
	assert(cmd == msg.data[msg.pos++]);
	va_start(ap, format);
	for (i = 0; i < strlen(format); i++) {
		switch (format[i]) {
			case 'B' :
				assert(va_arg(ap, int) == msg.data[msg.pos++]);
				break;
			case 'b' :
				*va_arg(ap, uint8_t*) = msg.data[msg.pos++];
				break;
			case 'f' :
				host = ntohl(*(uint32_t*) &msg.data[msg.pos]);
				msg.pos += 4;
				*va_arg(ap, float*) = *(float*) &host;
				break;
			case 'S' :
				len = ntohl(*(int32_t*) &msg.data[msg.pos]);
				msg.pos += 4;
				s = va_arg(ap, char*);
				assert(len == strlen(s) && !strncmp((char*) &msg.data[msg.pos], s, len));
				msg.pos += len;
				break;
			case 's' :
				len = ntohl(*(int32_t*) &msg.data[msg.pos]);
				msg.pos += 4;
				str = va_arg(ap, uint8_t**);
				*str = calloc(1 + len, sizeof(uint8_t));
				memcpy(*str, &msg.data[msg.pos], len);
				msg.pos += len;
				break;
			case 'l' :
				cnt = ntohl(*(uint32_t*) &msg.data[msg.pos]);
				msg.pos += 4;
				lst = va_arg(ap, uint8_t***);
				*lst = calloc(1 + cnt, sizeof(uint8_t*));
				for (j = 0; j < cnt; j++) {
					len = ntohl(*(uint32_t*) &msg.data[msg.pos]);
					msg.pos += 4;
					(*lst)[j] = calloc(1 + len, sizeof(uint8_t));
					memcpy((*lst)[j], &msg.data[msg.pos], len);
					msg.pos += len;
				}
			case '#' :
				assert(msg.pos - old == size);
				break;
			default :
				break;
		}
	}
	va_end(ap);
}


void do_close() {
	char *desc;

	msg_new();
	msg_write(0x7f, ""); 
	msg_send();
	msg_recv();

	msg_read(0x7f, "Bs#", 0x00, &desc);
//	printf("CLOSE: 0x%02x-%s\n", res, desc);
	msg_free();
}

void do_step() {
	char *desc;

	msg_new();
	msg_write(0x01, "db", 100000.0, 0); 
	msg_send();
	msg_recv();
	msg_read(0x01, "Bs#", 0x00, &desc);
//	printf("STEP: #%s#\n", desc);
	msg_free();
}

void do_steps(int n) {
	int i;
	for (i = 0; i < n; i++) {
		do_step();
	}
}

char **get_edges() {
	char *desc;
	char **edges;
//	int i = 0;

	msg_new();
	msg_write(0xaa, "bs", 0x00, "");
	msg_send();
	msg_recv();
	msg_read(0xaa, "Bs", 0x00, &desc);
	msg_read(0xba, "BSBl#", 0x00, "", LIST, &edges);
//	printf("EDGES: ");
//	while (edges[i]) {
//		printf("[%s] ", edges[i]);
//		i++;
//	}
//	printf("\n");
	msg_free();

	return edges;
}

char **get_lanes() {
	char *desc;
	char **lanes;
//	int i = 0;

	msg_new();
	msg_write(0xa3, "bs", 0x00, "");
	msg_send();
	msg_recv();
	msg_read(0xa3, "Bs", 0x00, &desc);
	msg_read(0xb3, "BSBl#", 0x00, "", LIST, &lanes);
//	printf("LANES: ");
//	while (lanes[i]) {
//		printf("{%s} ", lanes[i]);
//		i++;
//	}
//	printf("\n");
	msg_free();

	return lanes;
}

char *get_lane_edge(char *lane) {
	char *desc;
	char *edge;

	msg_new();
	msg_write(0xa3, "bs", 0x31, lane);
	msg_send();
	msg_recv();
	msg_read(0xa3, "Bs", 0x00, &desc);
	msg_read(0xb3, "BSBs#", 0x31, lane, STRING, &edge);
	msg_free();

	return edge;
}

float get_lane_length(char *lane) {
	char *desc;
	float length;

	msg_new();
	msg_write(0xa3, "bs", 0x44, lane);
	msg_send();
	msg_recv();
	msg_read(0xa3, "Bs", 0x00, &desc);
	msg_read(0xb3, "BSBf#", 0x44, lane, FLOAT, &length);
	msg_free();

	return length;
}

float *get_edge_lengths(char **edges) {
	int count;
	float *lengths;
	char **lanes;
	int i, j;

	printf("HERE\n");
	lanes = get_lanes();
	count = 0;
	while (edges[count++]);
	printf("HERE\n");
	lengths = (float*) calloc(count, sizeof(float));
	for (i = 0; lanes[i]; i++) {
		char *edge = get_lane_edge(lanes[i]);
		float length = get_lane_length(lanes[i]);
		for (j = 0; edges[j]; j++) {
			if (!strcmp(edges[j], edge)) {
				lengths[j] = length;
				break;
			}
		}
	}
	for (i = 0; edges[i]; i++) {
		printf("LENGTH OF [%s] is %f\n", edges[i], lengths[i]);
	}

	return lengths;
}

char **get_edge_vehicles(char *edge) {
	char *desc;
	char **vehicles;
	int i = 0;

	msg_new();
	msg_write(0xaa, "bs", 0x12, edge);
	msg_send();
	msg_recv();
	msg_read(0xaa, "Bs", 0x00, &desc);
	msg_read(0xba, "BSBl#", 0x12, edge, LIST, &vehicles);
//	printf("VEHICLES: 0x%02x-#%s#\n", res, desc);
//	printf("ON #%s#: ", edge);
//	while (vehicles[i]) {
//		printf("<%s> ", vehicles[i]);
//		i++;
//	}
//	printf("\n");
	msg_free();

	return vehicles;
}

float get_vehicle_speed(char *vehicle) {
	char *desc;
	float speed;

	msg_new();
	msg_write(0xa4, "bs", 0x40, vehicle);
	msg_send();
	msg_recv();
	msg_read(0xa4, "Bs", 0x00, &desc);
	msg_read(0xb4, "BSBf#", 0x40, vehicle, FLOAT, &speed);
//	printf("SPEED: 0x%02x-#%s#\n", res, desc);
//	printf("speed is %f\n", speed);
	msg_free();

	return speed;
}

int main(int argc, char* argv[]) {
	FILE *fout;

	int portno;
	struct sockaddr_in serv_addr;
	struct hostent *server;

	int step;

	char **edges;
	char **vehicles;
	float speed;
	float *lengths;
	int i, j;

	if (argc < 3) {
		fprintf(stderr,"usage %s hostname port\n", argv[0]);
		exit(0);
	}
	portno = atoi(argv[2]);
	sockfd = socket(AF_INET, SOCK_STREAM, 0);
	if (sockfd < 0) 
		error("ERROR opening socket");
	server = gethostbyname(argv[1]);
	if (server == NULL) {
		fprintf(stderr,"ERROR, no such host\n");
		exit(0);
	}
	bzero((char *) &serv_addr, sizeof(serv_addr));
	serv_addr.sin_family = AF_INET;
	bcopy((char *)server->h_addr, (char *)&serv_addr.sin_addr.s_addr, server->h_length);
	serv_addr.sin_port = htons(portno);
	if (connect(sockfd,(struct sockaddr*) &serv_addr,sizeof(serv_addr)) < 0) 
		error("ERROR connecting");

	fout = fopen("results.txt", "w");

	edges = get_edges();
	lengths = get_edge_lengths(edges);
	for (step = 0; step < 1; step++) {
		printf("Step %03i ... ", step + 1);
		do_step();
		for (i = 0; edges[i] && i < 20; i++) {
			speed = 0;
			vehicles = get_edge_vehicles(edges[i]);
			for (j = 0; vehicles[j]; j++) {
				speed += get_vehicle_speed(vehicles[j]);
			}
			free(vehicles);
			speed /= j;
			fprintf(fout, "%f\t", speed);
		}
		free(edges);
		fprintf(fout, "\n");
		printf("OK\n");
	}
	do_close();

	fclose(fout);

	return 0;
}