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

#include <glib/gstdio.h>

/*
Output Ports:
Address  Sensor
0x0      Score
0x1      Fuel Remaining
0x2      s_x Relative to Earth
0x3      s_y Relative to Earth
0x4      s_x Relative to target satellite
0x5      s_y Relative to target satellite
*/

#define SCORE (task->out[0])
#define FUEL (task->out[1])
#define S_X (-task->out[2])
#define S_Y (-task->out[3])
#define T_X (S_X + task->out[4])
#define T_Y (S_Y + task->out[5])
#define DISTANCE (sqrt(task->out[4]*task->out[4]+task->out[5]*task->out[5]))

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

static const gdouble mu = G * Me;

static void debug(task_t * task) {
	printf(
		"Step: %9"G_GUINT64_FORMAT" Score: %8.5f   Fuel: %14.3f   x: %14.3f   y: %14.3f   tx: %14.3f   ty: %14.3f   dist: %14.3f\n",
		task->timestamp, SCORE, FUEL, S_X, S_Y, T_X, T_Y, DISTANCE);
}

static int plot_sat, plot_target_sat;
static GString *plot_str;

static void open_data_files(task_t *task) {
	plot_str = g_string_sized_new(0);
	g_string_printf(plot_str, "plot_task2_%u_sat.dat", (guint) task->scenario);
	plot_sat = g_open(plot_str->str, O_CREAT | O_TRUNC | O_WRONLY, 0644);
	g_string_printf(plot_str, "plot_task2_%u_target_sat.dat", (guint) task->scenario);
	plot_target_sat = g_open(plot_str->str, O_CREAT | O_TRUNC | O_WRONLY, 0644);
}

static void trace_data(task_t *task) {
	g_string_printf(plot_str, "%14.3f %14.3f\n", S_X, S_Y);
	write(plot_sat, GSTR_LEN(plot_str));
	g_string_printf(plot_str, "%14.3f %14.3f\n", T_X, T_Y);
	write(plot_target_sat, GSTR_LEN(plot_str));
}

static void data_close() {
	close(plot_sat);
	close(plot_target_sat);
	g_string_free(plot_str, TRUE);
}

static void run(task_t *task, gpointer userdata) {
	static satellite_t sat, target_sat;
	static gdouble phi, target_v = 0.0;
	static guint start_hohmann = 0;
	static gdouble dv, dv_tic;
	static gboolean reached = FALSE, outoffuel = FALSE;

	UNUSED(userdata);

	/* v^2/r == G*M_e/r^2 => v^2 = mu/r */
	/* T = s/v = 2*pi*r/sqrt(mu/r) = 2*pi*r3*sqrt(r3/mu) */
	/* T' = pi*sqrt((r1+r2)^3/(8*G*Me)) */

	if (task->finished) {
		debug(task);
		data_close();
	} else if (0 == task->timestamp) {
		open_data_files(task);
	}
	if (0 == task->timestamp || task->finished) return;

	DV_X = DV_Y = 0.0;

	if (10 == (task->timestamp % 32) && task->timestamp > start_hohmann && (task->timestamp - start_hohmann) < 7000) trace_data(task);

	satellite_update_pos(&sat, S_X, S_Y);
	satellite_update_pos(&target_sat, T_X, T_Y);

	if (!reached) {
		if (2 == task->timestamp) {
			gdouble target_sat_T_half = M_PI*target_sat.rad*sqrt(target_sat.rad/(G*Me));
			gdouble sat_T_half = M_PI*sat.rad*sqrt(sat.rad/(G*Me));
			gdouble hohmann_T = M_PI*(target_sat.rad+sat.rad)*sqrt((target_sat.rad+sat.rad)/(8*G*Me));
			gdouble timediff = target_sat_T_half + hohmann_T;
			gdouble starttime = timediff*sat_T_half/(target_sat_T_half - sat_T_half);
			satellite_update_move(&sat);
			target_v = sqrt(mu/target_sat.rad);
			dv = sqrt(mu/sat.rad)*(sqrt(2*target_sat.rad/(sat.rad + target_sat.rad))-1);
			dv_tic = sqrt(mu/target_sat.rad)*(1-sqrt(2*sat.rad/(sat.rad + target_sat.rad)));
			phi = M_PI * timediff / target_sat_T_half;
			start_hohmann = starttime ;
			if (sat.move.v.x * S_Y > 0.0 && sat.move.v.y * S_X < 0.0) target_v = -target_v;
			printf("Vorsprung für Target: %u (Winkel: %14.3f)\n", start_hohmann, (180.0*phi/M_PI));
// 			printf("T: %14.3f\n", 2*M_PI*target_sat.rad*sqrt(target_sat.rad/(G*Me)));
		} else if (start_hohmann == task->timestamp) {
			satellite_update_move(&sat);
			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);
			printf("-- Radius difference: %14.3f\n", fabs(atan2(S_Y, S_X) - atan2(T_Y, T_X))*180.0/M_PI);
			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 (start_hohmann < task->timestamp) {
			if (sat.rad +2.0 >= target_sat.rad) {
				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, distance: %14.3f\n", (guint) task->timestamp, DISTANCE);
				printf("v: %f / %f\n", sat.move.v.x, sat.move.v.y);
				printf("Dv: %f / %f\n", DV_X, DV_Y);
			}
		}
#if 0
	} else if (DISTANCE > 400) {
		gdouble dx = task->out[4], dy = task->out[5];
#endif
#if 0
	} else if (DISTANCE > 500.0) {
		gdouble phidiff = 180.0*(atan2(S_Y, S_X) - atan2(T_Y, T_X))/M_PI;
		gdouble target_rad = target_sat.rad - sat.rad;
/*		gdouble tar_dv_x = -target_v * S_Y / sat.rad;
		gdouble tar_dv_y = target_v * S_X / sat.rad;*/
		satellite_update_move(&sat);
		printf("Distance: %14.3f\n", phidiff);
		if (phidiff < 0 || phidiff > 180.0) { /* slow down */
			target_rad += 20.0;
		} else { /* speed up */
			target_rad -= 20.0;
		}
		target_rad = fmax(-5.0, fmin(5.0, target_rad));
		DV_X = /*tar_dv_x - sat.move.v.x*/ + target_rad * S_X / sat.rad;
		DV_Y = /*tar_dv_y - sat.move.v.y*/ + target_rad * S_Y / sat.rad;
			printf("v: %f / %f\n", sat.move.v.x, sat.move.v.y);
			printf("Dv: %f / %f\n", DV_X, DV_Y);
		debug(task);
#endif
	} else if (!outoffuel) {
		satellite_update_move(&sat);
		if (FALSE && fabs(sat.rad - target_sat.rad) > 3) {
			gdouble tar_dv_x = -target_v * S_Y / sat.rad;
			gdouble tar_dv_y = target_v * S_X / sat.rad;
			gdouble target_rad = target_sat.rad - sat.rad;
			target_rad = fmax(-10.0, fmin(10.0, target_rad));
			DV_X = tar_dv_x - sat.move.v.x + target_rad * S_X / sat.rad;
			DV_Y = tar_dv_y - sat.move.v.y + target_rad * S_Y / sat.rad;
			printf("-- orbit correction\n");
			printf("v: %f / %f\n", sat.move.v.x, sat.move.v.y);
			printf("Dv: %f / %f\n", DV_X, DV_Y);
		} else {
			/* target_v is clockwise */
			gdouble tar_dv_x = -target_v * S_Y / sat.rad;
			gdouble tar_dv_y = target_v * S_X / sat.rad;
			DV_X = tar_dv_x - sat.move.v.x;
			DV_Y = tar_dv_y - sat.move.v.y;
		}
		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 (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, 2001);
	return 0;
}