#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);
}

gdouble init_rad, last_x, last_y;
gboolean running = FALSE, started = FALSE;
gdouble dv, dv_tic;

static void run(task_t *task, gpointer userdata) {
	gdouble cur_rad;
	UNUSED(userdata);

	if (0 == task->timestamp) return;

	cur_rad = sqrt(S_X*S_X+S_Y*S_Y);
	DV_X = DV_Y = 0.0;

	if (!running) {
		running = TRUE;
		started = TRUE;
		init_rad = cur_rad;
		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 (started) {
		gdouble vx = S_X - last_x, vy = S_Y - last_y, v = sqrt(vx*vx+vy*vy);
		started = FALSE;
		DV_X = -vx * (dv / v);
		DV_Y = -vy * (dv / v);
// 		printf("v: %f / %f\n", vx, vy);
		printf("Dv: %f / %f\n", DV_X, DV_Y);
	} else if (fabs(cur_rad - RADIUS) < 1000.0) {
		gdouble vx = S_X - last_x, vy = S_Y - last_y, v = sqrt(vx*vx+vy*vy);
		running = FALSE;
		printf("Reached target radius\n");
		DV_X = -vx * (dv_tic / v);
		DV_Y = -vy * (dv_tic / v);
		printf("Dv: %f / %f\n", DV_X, DV_Y);
	}

	last_x = S_X;
	last_y = S_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;
}