#include "control.h"
#include "path.h"

#include <stdio.h>

#define LATENCY 20

void trial_loop(trial *t) {
	do {
		if (-1 == trial_wait_for_start(t)) return;
		if (t->finished) break;
		if (t->runcnt == 0) {
			goradar(t);
			vehicle_accel(t);
			switch (t->v.turn) {
				case TURN_LEFT:
				case TURN_STRAIGHT:
					vehicle_hard_left(t); break;
				case TURN_RIGHT:
					vehicle_hard_right(t); break;
				default: break;
			}
			while (t->telemetry->length < 4) {
				if (-1 == trial_wait_for_input(t)) return;
			}
			/* TODO: call constant calculatoin */
		}
		while (t->alive) {
			simulate(t, getcurts(t) + LATENCY);
			goradar(t);
			if (-1 == trial_check_input(t)) return;
// 			if (-1 == trial_wait_for_input(t)) return;
		}
		trial_reset_run(t);
	} while (!t->finished);
}

int main(int argc, char **argv) {
	trial *t;
	
	if (argc <= 2) {
		fprintf(stderr, "Syntax: %s hostname port\n", argv[0]);
		return 1;
	}
	
	if (NULL == (t = trial_new(argv[1], argv[2]))) {
		return 2;
	}

	trial_loop(t);
	
	trial_free(t);
	
	return 0;
}