#include "control.h"
#include "lookahead.h"

#include <stdio.h>

#define LATENCY 2

void trial_loop(trial *t) {
	FILE* sim;
	char name[80];
	do {
		sprintf(name,"simulation%d.m",t->runcnt);
		sim = fopen(name,"w");
		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 < 6) {
				if (-1 == trial_wait_for_input(t)) return;
			}
			fit_parameter(	(telemetry*) g_queue_peek_nth(&t->telemetry,3),
					(telemetry*) g_queue_peek_nth(&t->telemetry,4),
					(telemetry*) g_queue_peek_nth(&t->telemetry,5),
					&t->map);
			fprintf(stderr,"Fitparameter: a: %f aw: %f k: %f\n",t->map.a,t->map.aw,t->map.k);
		}
		while (t->alive) {
			radar_dgl(t, getcurts(t) + LATENCY);
			vehicle_matlab(sim,t->sim.tm.ts,&t->sim.tm.vehicle);
			goradar(t);
// 			vehicle_break(t);
// 			vehicle_hard_right(t);
			if (-1 == trial_check_input(t)) return;
// 			if (-1 == trial_wait_for_input(t)) return;
		}
		trial_matlab(t);
		trial_reset_run(t);
		fclose(sim);
	} 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;
}