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

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

void check_coll(gpointer key, gpointer value, gpointer ud){
	double dx,dy;
	dx = value.x - ud.x;
	dy = value.y - ud.y;
	 *ud.collision |= (sqrt(dx*dx + dy*dy) - value.rad) < 1;
}

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

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

	a1 = (t2->speed - t1->speed)/dt1;
	a2 = (t3->speed - t2->speed)/dt2;

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

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

	w1 = (t2->dir - t1->dir)/dt1;
	w2 = (t3->dir - t2->dir)/dt2;

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

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

	if((deltat/dt)*dt != deltat)
		fprintf(stderr,"deltat not a multiple of %d, possibly inaccurate results\n",dt);
	
	/*TODO: find out a,k,aw*/
	a = t->map.a;
	k = t->map.k;
	aw = t->map.aw;
	case(before.turn){
		case(TURN_HARD_LEFT): wsoll = t->map.max_hard_turn; break;
		case(TURN_LEFT): wsoll = t->map.max_turn;  break;
		case(TURN_STRAIGHT): wsoll = 0; break;
		case(TURN_RIGHT): wsoll = -t->map.max_turn; break;
		case(TURN_HARD_RIGHT): wsoll = -t->map.max_hard_turn; break;
	}
	aw *= (w > wsoll) ? -1 : 1;

	for(i = 0;i<end;i++){
		
		/*calculate derivative*/
		diff.x = cos(tmp.dir)*tmp.speed;
		diff.y = sin(tmp.dir)*tmp.speed;
		diff.speed = fmax(-tmp.speed,a - k*tmp.speed*tmp.speed);
		/*maybe its logistic: k*w*(wsoll - w)*/
		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_foreach(t->map.solid->objects,check_coll,&collision);
		if(collision){
			return -1;
		}
		
	}

	after.x = tmp.x;
	after.y = tmp.y;
	after.speed = tmp.speed;
	after.w = tmp.w;
	after.dir = tmp.dir;
	
}


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) == -1) continue;
			/*TODO: Bewertung*/
		}
	}
}