#include "task_main.h"
#include "physics.h"

/*
Output Ports:
Address  Sensor
0x0      Score
0x1      Fuel Remaining
0x2      s_x Relative to Earth
0x3      s_y Relative to Earth
0x4      Target Orbit Radius
*/

#define SCORE (task->out[0])
#define FUEL (task->out[1])
#define S_X (-task->out[2])
#define S_Y (-task->out[3])
#define RADIUS (task->out[4])

#define DV_X (task->in[2])
#define DV_Y (task->in[3])

static const gdouble mu = G * Me;

static void debug(task_t * task) {
	gdouble rad = sqrt(S_X*S_X+S_Y*S_Y);
	printf(
		"Step: %9"G_GUINT64_FORMAT" Score: %5f   Fuel: %5f   x: %5f   y: %5f   r: %5f (cur: %5f)\n",
		task->timestamp, SCORE, FUEL, S_X, S_Y, RADIUS, rad);
}

static void run(task_t *task, gpointer userdata) {
	static satellite_t sat;
	static gdouble init_rad, target_v;
	static gdouble dv, dv_tic;
	static gboolean reached = FALSE, outoffuel = TRUE;
	static guint32 reached_ts = 0;
	UNUSED(userdata);

	if (task->finished) debug(task);
	if (0 == task->timestamp || task->finished) return;

	satellite_update_pos(&sat, S_X, S_Y);

	DV_X = DV_Y = 0.0;

	if (!reached) {
		if (1 == task->timestamp) {
			debug(task);
			init_rad = sat.rad;

			/* v^2/r == G*M_e/r^2 => v^2 = mu/r */
			target_v = sqrt(mu/RADIUS);

			dv = sqrt(mu/init_rad)*(sqrt(2*RADIUS/(init_rad + RADIUS))-1);
			dv_tic = sqrt(mu/RADIUS)*(1-sqrt(2*init_rad/(init_rad + RADIUS)));
			printf("-- Orbit: %f -> %f; dv = %f, dv' = %f\n", init_rad, RADIUS, dv, dv_tic);
		} else if (2 == task->timestamp) {
			debug(task);
			satellite_update_move(&sat);
			if (sat.move.v.x * S_Y > 0.0 && sat.move.v.y * S_X < 0.0) target_v = -target_v;
			DV_X = sat.move.v.x * (dv / sat.move.v_abs);
			DV_Y = sat.move.v.y * (dv / sat.move.v_abs);
			printf("-- Leaving initial orbit at %u\n", (guint) task->timestamp);
			debug(task);
			printf("v: %f / %f\n", sat.move.v.x, sat.move.v.y);
			printf("Dv: %f / %f\n", DV_X, DV_Y);
		} else if (fabs(sat.rad - RADIUS) < 100.0) {
			satellite_update_move(&sat);
			debug(task);
			reached = TRUE;
			DV_X = sat.move.v.x * (dv_tic / sat.move.v_abs);
			DV_Y = sat.move.v.y * (dv_tic / sat.move.v_abs);
			printf("-- Reached target orbit at %u\n", (guint) task->timestamp);
			printf("v: %f / %f\n", sat.move.v.x, sat.move.v.y);
			printf("Dv: %f / %f\n", DV_X, DV_Y);
		}
	} else if (!outoffuel) {
		/* target_v is clockwise (TODO) */
		gdouble tar_dv_x = -target_v * S_Y / sat.rad;
		gdouble tar_dv_y = target_v * S_X / sat.rad;
		satellite_update_move(&sat);
		DV_X = tar_dv_x - sat.move.v.x;
		if (fabs(DV_X) < 0.5) DV_X = 0.0;
		DV_Y = tar_dv_y - sat.move.v.y;
		if (fabs(DV_Y) < 0.5) DV_Y = 0.0;
		if (DV_X*DV_X + DV_Y*DV_Y > FUEL*FUEL) {
			printf("-- Out of fuel\n");
			printf("v: %f / %f\n", sat.move.v.x, sat.move.v.y);
			printf("Dv: %f / %f\n", DV_X, DV_Y);
			DV_X = 0.0; DV_Y = 0.0;
			outoffuel = TRUE;
		}
	}

	if (reached_ts > 0 && fabs(sat.rad - RADIUS) >= 1000.0) {
// 		debug(task);
		printf("-- Left target radius at %u (duration: %u)\n", (guint) task->timestamp, (guint) task->timestamp - reached_ts);
		reached_ts = 0;
	} else if (reached_ts == 0 && fabs(sat.rad - RADIUS) < 1000.0) {
// 		debug(task);
		reached_ts = (guint) task->timestamp;
		printf("-- Reached target radius at %u\n", (guint) task->timestamp);
	}

	if (fabs(DV_X) < 0.1) DV_X = 0.0;
	if (fabs(DV_Y) < 0.1) DV_Y = 0.0;

	sat.dv.x = DV_X; sat.dv.y = DV_Y;
}

static void run_debug(task_t *task, gpointer userdata) {
	debug(task);
	run(task, userdata);
}

int main(int argc, char **argv) {
	task_main(argc, argv, run, run_debug, NULL, 1001);
	return 0;
}