#include "lookahead.h"
#include <stdio.h>
#include <math.h>


double turn2w(trial* t, turn_t turn) {
	switch(turn){
		case(TURN_HARD_LEFT): return t->map.max_hard_turn/1000.0;
		case(TURN_LEFT): return t->map.max_turn/1000.0;
		case(TURN_STRAIGHT): return 0;
		case(TURN_RIGHT): return -t->map.max_turn/1000.0;
		case(TURN_HARD_RIGHT): return -t->map.max_hard_turn/1000.0;
		default:
			fprintf(stderr,"Unknown turn state in turn2w\n");
			return 0;
	}
}

struct collcheck {
	int collision;
	double x,y;
};

void check_coll(gpointer key, gpointer value, gpointer ud){
	struct collcheck* coll;
	object* ob;

	UNUSED(key);

	coll = (struct collcheck*) ud;
	ob = (object*) value;

	double dx,dy;
	dx = ob->x - coll->x;
	dy = ob->y - coll->y;
	coll->collision |= (sqrt(dx*dx + dy*dy) - ob->rad) < 1;
}

void fit_parameter(telemetry* t1, telemetry* t2, telemetry* t3, map* m){
	double a1,a2;
	double w1,w2;
	double dt1,dt2;
	vehicle *v1, *v2, *v3;

	v1 = &t1->vehicle;
	v2 = &t2->vehicle;
	v3 = &t3->vehicle;

	dt1 = (t2->ts - t1->ts);
	dt2 = (t3->ts - t2->ts);

	a1 = (v2->speed - v1->speed)/dt1;
	a2 = (v3->speed - v2->speed)/dt2;

	if(a1/a2 > 1.1 || a1/a2 < 0.9){
		fprintf(stderr,"Inconsistent accelerations: a1: %f a2: %f. inaccurate results possible\n",a1,a2);
	}

	m->a = (a1 + a2)/2;
	m->k = m->a/m->max_speed/m->max_speed;

	w1 = (v2->dir - v1->dir)/dt1;
	w2 = (v3->dir - v2->dir)/dt2;

	m->aw = 2*(w2 - w1)/(dt1 + dt2);
}


void radar_dgl(trial* t, timestamp ts){
	timestamp step = ts- t->sim.tm.ts;
	if (step > 0){
		dgl(t,&t->sim.tm.vehicle,&t->sim.tm.vehicle,step,1);
		t->sim.tm.ts += step;
		t->sim.steps++;
	}
}

/*updates the simulation state when new telemetry arrives*/
void sim_update(trial* t){

	telemetry *tm,*tmp;
	tm = (telemetry*) g_queue_peek_tail(&t->telemetry);
	fprintf(stderr, "Time diff: %i\n", tm->ts - t->sim.tm.ts);
	/*run simulation up to new telemetry timestamp*/
	radar_dgl(t,tm->ts);
	
	/*calculate misspredict*/
	fprintf(stderr, "Miss predict: dx=%f, dy=%f, ddir=%f, steps=%u\n",
		t->sim.tm.vehicle.x - t->vehicle.x,
		t->sim.tm.vehicle.y - t->vehicle.y,
		t->sim.tm.vehicle.dir - t->vehicle.dir,
		t->sim.steps);

	/*update simulation vehicle from telemetry: everything and calculate w*/
	t->sim.tm.vehicle = tm->vehicle;
	g_array_set_size(t->sim.tm.objects, 0);
	g_array_append_vals(t->sim.tm.objects, tm->objects->data, tm->objects->len);
	t->sim.steps = 0;
	if(t->telemetry.length > 1){
		tmp = (telemetry*) g_queue_peek_tail_link(&t->telemetry)->prev->data;
		t->sim.tm.vehicle.w = (tm->vehicle.dir - tmp->vehicle.dir)/(tm->ts - tmp->ts);
	}
}

int dgl(trial* t, vehicle* after, vehicle* before, timestamp h, timestamp deltat){
	double a,k,aw;
	double wsoll;
	vehicle tmp = *before;
	vehicle diff;
	timestamp dt = h;
	int i,end=deltat/dt;
	struct collcheck collision;

	if(deltat < dt)
		return 0;

	a = t->map.a;
	k = t->map.k;
	aw = t->map.aw;

	wsoll = turn2w(t,before->turn);
	aw *= (before->w > wsoll) ? -1 : 1;

	for(i = 0;i<=end;i++){
		if(i == end) dt = deltat - (end-1)*dt;
		
		/*calculate derivative*/
		diff.x = cos(tmp.dir)*tmp.speed/1000;
		diff.y = sin(tmp.dir)*tmp.speed/1000;
		diff.speed = fmax(-tmp.speed,a - k*tmp.speed*tmp.speed*1e-6);
		diff.w = (tmp.w == wsoll) ? 0 : aw;
		diff.dir = tmp.w;
		/*Euler Step*/
		tmp.x += dt*diff.x;
		tmp.y += dt*diff.y;
		tmp.speed += (tmp.speed > 0) ? dt*diff.speed : 0;
		tmp.w += dt*diff.w;
		tmp.dir += dt*diff.dir;

		/*check for collisions TODO: optimize*/
		collision.x = tmp.x;
		collision.y = tmp.y;
		collision.collision = 0;
		g_hash_table_foreach(t->map.solid_objects,check_coll,&collision);
		if(collision.collision){
			return -1;	
		}
		
	}


	after->x = tmp.x;
	after->y = tmp.y;
	after->speed = tmp.speed;
	after->w = tmp.w;
	after->dir = tmp.dir;
	return 0;	
}


void lookahead(trial* t){
	int accel, turn; 
	vehicle current, future;

	current = t->vehicle;

	for(accel=0; accel < 3;accel++){
		current.accel = accel;
		for(turn=0; turn < 5; turn++){
			current.turn = turn;
			if(dgl(t,&future,&current,20,10) == -1) continue;
			/*TODO: Bewertung*/
		}
	}
}