#include "control.h"

#include <math.h>


static inline double fixdir(double a) {
	if (a < 0) a += 360;
	else if (a > 360) a -= 360;
	return a;
}

void simulate(trial *t, timestamp ts) {
	double move, angle;
	
	timestamp diff = ts - t->sim.tm.ts;
	if (diff == 0) return;
	t->sim.tm.ts = ts;
	t->sim.steps++;
	
	move = t->sim.tm.vehicle.speed / 1000 * diff;
	angle = t->sim.tm.vehicle.dir*M_PI/180;
	t->sim.tm.vehicle.x += cos(angle)*move;
	t->sim.tm.vehicle.y += sin(angle)*move;
	
/*	switch (t->sim.vehicle.turn) {
		case TURN_HARD_LEFT : t->sim.vehicle.dir = fixdir(t->sim.vehicle.dir + t->map.max_hard_turn/1000*diff); break;
		case TURN_LEFT      : t->sim.vehicle.dir = fixdir(t->sim.vehicle.dir + t->map.max_turn/1000*diff); break;
		case TURN_STRAIGHT  : break;
		case TURN_RIGHT     : t->sim.vehicle.dir = fixdir(t->sim.vehicle.dir - t->map.max_turn/1000*diff); break;
		case TURN_HARD_RIGHT: t->sim.vehicle.dir = fixdir(t->sim.vehicle.dir - t->map.max_hard_turn/1000*diff); break;
	}
	
	t->sim.vehicle.accel = t->vehicle.accel;
	t->sim.vehicle.turn = t->vehicle.turn;*/
}