#include "control.h"
#include "lookahead.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->vehicle.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 < 5) {
				if (-1 == trial_wait_for_input(t)) return;
			}
			fit_parameter(	(telemetry*) g_queue_peek_nth(&t->telemetry,2),
					(telemetry*) g_queue_peek_nth(&t->telemetry,3),
					(telemetry*) g_queue_peek_nth(&t->telemetry,4),
					&t->map);
			fprintf(stderr,"Fitparameter: a: %f aw: %f k: %f\n",t->map.a,t->map.aw,t->map.k);

		}
		while (t->alive) {
			timestamp step=getcurts(t) + LATENCY - t->sim.tm.ts;
			if (step > 0) {
				t->sim.tm.vehicle.w = turn2w(t, t->sim.tm.vehicle.turn);
				dgl(t,&t->sim.tm.vehicle,&t->sim.tm.vehicle,step,step);
				t->sim.tm.ts += step;
				t->sim.steps++;
			}
//			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;
}