#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, vr, angle, vdirx, vdiry;
	
	/* todo: simulate movement */
	v = t->sim.tm.vehicle;
	
	tm = &t->sim.tm;
	
	vr = sqrt(v.x*v.x + v.y*v.y);
	if (vr < 4) return; /* done */
	xpot = 0.1 * v.x/vr;
	ypot = 0.1 * v.y/vr;
	vr = vr*vr;
	vdirx = cos(v.dir);
	vdiry = sin(v.dir);
	
	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:
				r = (o->x * o->x + o->y + o->y);
				if ((vdirx*(o->x-v.x) + vdiry*(o->y-v.y)) < 0) continue;
				sigma = 2 * o->rad + 1;
				h = 1;
				break;
			case CRATER:
				r = (o->x * o->x + o->y + o->y);
				if ((vdirx*(o->x-v.x) + vdiry*(o->y-v.y)) < 0) continue;
				sigma = 2 * o->rad + 3;
				h = 1;
				break;
			case MARTIAN:
				sigma = 6;
				h = 1;
				break;
		}
		
		r = sqrt((o->x - v.x) * (o->x - v.x) + (o->y - v.y) * (o->y - v.y)) - o->rad;
		r = r*r;
// 		r = r*r*r * 10;
		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);
		}
	}
// 	fprintf(stderr, "Angle: %f, objects: %u\n", angle, tm->objects->len);
	angle = fabs(angle);
	if (angle > 60) {
		vehicle_break(t);
	} else if (angle > 50) {
		vehicle_roll(t);
	} else {
		vehicle_accel(t);
	}
}