#include "control.h"

#include <math.h>
#include <stdlib.h>

static double angle_for_rot(double from, double to) {
	double a = to - from;
	if (a > 180) a = -360 + a;
	else if (a < -180) a = 360 + a;
	return a;
}

void godown(trial *t) {
	double C1 = 1 / sqrt(2*M_PI);

	vehicle v;
	telemetry *tm;
	
	double xpot, ypot, r, angle;
	
	/* todo: simulate movement */
	v = t->vehicle;
	
	tm = (telemetry*) g_queue_peek_tail(&t->telemetry);
	
	r = sqrt(v.x*v.x + v.y*v.y);
	if (r < 4) return; /* done */
	xpot = v.x/r;
	ypot = v.y/r;
	
	for (guint i = 0; i < tm->objects->len; i++) {
		object *o = &g_array_index(tm->objects, object, i);
		
		double sigma = 0, h = 0;
		
		switch (o->type) {
			case BOLDER:
				sigma = 2 * o->rad;
				h = 1;
				break;
			case CRATER:
				sigma = 2 * o->rad;
				h = 3;
				break;
			case MARTIAN:
				sigma = 4 * o->rad;
				h = 1;
				break;
		}
		
		r = sqrt( (o->x - v.x) * (o->x - v.x) + (o->y - v.y) * (o->y - v.y) );
		r = C1 * h / (sigma*sigma) / exp (r / (2*sigma) );
		xpot += (o->x - v.x) * r;
		ypot += (o->y - v.y) * r;
	}
	
	angle = atan2(-ypot, -xpot)*180/M_PI;
	angle = angle_for_rot(v.dir, angle);
	if (angle < 1) {
		if (angle < -20) {
			vehicle_hard_right(t);
		} else {
			vehicle_right(t);
		}
	} else if (angle > 1) {
		if (angle > 20) {
			vehicle_hard_left(t);
		} else {
			vehicle_left(t);
		}
	}
	angle = abs(angle);
	if (angle > 60) {
		vehicle_break(t);
	} else if (angle > 30) {
		vehicle_roll(t);
	} else {
		vehicle_accel(t);
	}
}