#include "control.h"

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

struct angle {
	double a;
	int c;
};
typedef struct angle angle;

gint cmp_angle(gconstpointer _a, gconstpointer _b, gpointer p) {
	const angle *a = (const angle*) _a, *b = (const angle*) _b;
	UNUSED(p);
	return (a->a == b->a) ? 0 : (a->a < b->a) ? -1 : 1;
}

void angle_free(angle* a) {
	g_slice_free(angle, a);
}

angle* angle_new(double a, int c) {
	angle *x = g_slice_new(angle);
	x->a = a; x->c = c;
	return x;
}

static double fixangle(double a) {
	while (a < -180) a += 360;
	while (a > 180) a -= 360;
	return a;
}

int distance_weight(double distance) {
	if (distance > 50) {
		if (distance > 150) return 0;
		else return 1;
	} else {
		if (distance > 20) return 10;
		else return 200;
	}
}

static inline gboolean insert_object(GSequence *angles, object *o, vehicle *v, double ve_ho_angle) {
	double phi, phi2, avoid = 0, oangle;
	double distance;
	int distweight;
	
	distance = sqrt((o->y - v->y) * (o->y - v->y) + (o->x - v->x) * (o->x - v->x));
	distweight = distance_weight(distance - o->rad);
	if (distweight == 0) return FALSE;
	
	oangle = fixangle(atan2(-o->y, -o->x)*180/M_PI - ve_ho_angle);
	if (fabs(oangle) > 90) return FALSE;
	
	phi = fixangle(atan2(o->y - v->y, o->x - v->x)*180/M_PI - ve_ho_angle);
	
	if (fabs(phi) > 120) return FALSE;
	switch (o->type) {
		case BOLDER:
			avoid = 1 * o->rad + VEHICLE_RADIUS + 1; break;
		case CRATER:
			avoid = 1 * o->rad + VEHICLE_RADIUS + 2; break;
		case MARTIAN:
			/* todo: guess martian movement */
			avoid = o->rad + VEHICLE_RADIUS + 5; break;
	}
	phi2 = atan(avoid / distance )*180/M_PI;
	g_sequence_insert_sorted(angles, angle_new(phi - phi2, 1), cmp_angle, NULL);
	g_sequence_insert_sorted(angles, angle_new(phi + phi2, -1), cmp_angle, NULL);
	return TRUE;
}

void goradar(trial *t) {
	vehicle v;
	telemetry *tm;
	GSequence *angles = g_sequence_new((GDestroyNotify) angle_free);
	GSequenceIter *prev, *cur, *end;
	angle *acur, *aprev;
	double ve_ho_angle, ang = 1000, weight = 0;
	int c;
	GHashTableIter iter;
	guint i, objects = 0;
	gpointer key;
	object *value;

	v = t->sim.tm.vehicle;
	ve_ho_angle = atan2(-v.y, -v.x)*180/M_PI;
	
	tm = &t->sim.tm;

	g_sequence_insert_sorted(angles, angle_new(-120, 0), cmp_angle, NULL);
	g_sequence_insert_sorted(angles, angle_new(120, 0), cmp_angle, NULL);
	for (i = 0; i < tm->objects->len; i++) {
		object *o = &g_array_index(tm->objects, object, i);
		if (insert_object(angles, o, &v, ve_ho_angle)) objects++;
	}
	g_hash_table_iter_init(&iter, t->map.solid_objects);
	while (g_hash_table_iter_next(&iter, &key, (gpointer*) &value)) {
		if (insert_object(angles, value, &v, ve_ho_angle)) objects++;
	}

	prev = g_sequence_get_begin_iter(angles), end = g_sequence_get_end_iter(angles);
	if (prev != end) {
		aprev = (angle*) g_sequence_get(prev);
		c = aprev->c;
		while ((cur = g_sequence_iter_next(prev)) != end) {
			acur = (angle*) g_sequence_get(cur);
			assert(aprev->a <= acur->a);
			if (acur->a - aprev->a > 1) {
				double ax, w;
				if (aprev->a <= 0) {
					if (acur->a <= 0) {
						ax = acur->a;
					} else {
						ax = 0;
					}
				} else {
					ax = aprev->a;
				}
				w = 30 - fabs(ax)/30 - c;
// 				fprintf(stderr, "Angle %f, weight %f\n", ax, w);
				if (w > weight) {
// 				fprintf(stderr, "Selected %f\n", ax);
					ang = ax;
					weight = w;
				}
			}
			
			c += acur->c;
			prev = cur;
			aprev = acur;
		}
	}

	ang = fixangle(ang - v.dir + ve_ho_angle);
// 	fprintf(stderr, "Angle: %f, objects: %u\n", ang, objects);
	if (ang < 1) {
		if (ang < -15) {
			vehicle_hard_right(t);
		} else {
			vehicle_right(t);
		}
	} else if (ang > 1) {
		if (ang > 15) {
			vehicle_hard_left(t);
		} else {
			vehicle_left(t);
		}
	}
	ang = fabs(ang);
	if (ang > 90) {
		vehicle_break(t);
	} else if (ang > 60) {
		vehicle_roll(t);
	} else {
		vehicle_accel(t);
	}
}