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

#include <stdio.h>

#define LATENCY 20

void trial_loop(trial *t) {
// 	path *p;
// 	int offset;
	do {
		if (-1 == trial_wait_for_start(t)) return;
		if (t->finished) break;
// 		p = path_new();
		/*create trivial path towards the origin*/
// 		offset = path_app_fitseq(p,t,0,400);
// 		offset = path_app_target(p,t,offset,0,0);
		while (t->alive) {
// 			path_execute(t,p);
			simulate(t, getcurts(t) + LATENCY);
			godown(t);
			if (-1 == trial_check_input(t)) return;
		}
		trial_matlab(t);
		trial_fit(t,0,400);
		trial_reset_run(t);
// 		path_free(p);
	} 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;
}