#include "game.h"


// forware declarations

void changeOrbitColor(GameOrbit* orbit, vec3* color_data);


// interface

GameOrbit*
getFreeOrbit(GameState* gs)
{
	GameOrbit* orbit = nullptr;

	// NOTE: first check if we have a freed orbit to use
	for (u32 i = 0; i < gs->num_orbits; i++) {
		if (!gs->orbits[i].in_use)
			orbit = &gs->orbits[i];
	}

	// NOTE: if we can't re-use a freed orbit, initialize a new one
	if (!orbit) {
		assert(gs->num_orbits < gs->max_orbits);
		orbit = &gs->orbits[gs->num_orbits++];
	}

	orbit->in_use = true;
	return orbit;
}

GameOrbit*
getSelectedOrbit(GameState* gs)
{
	GameOrbit* orbit = nullptr;

	for (u32 i = 0; i < gs->num_orbits; i++) {
		if (gs->orbits[i].selected)
			orbit = &gs->orbits[i];
	}

	return orbit;
}

void
disableGameOrbit(GameState* gs, GameOrbit* orbit)
{
	if (gs->last_selected_orbit == orbit)
		gs->last_selected_orbit = nullptr;

	*orbit = {0};
}

double
getTimeStep(GameState* gs)
{
	u64 last_sdl_tick = gs->game_time_ms;
	gs->game_time_ms = SDL_GetTicks64();
	u64 current_tick = gs->game_time_ms - last_sdl_tick;
	double time_step = 0;

	// NOTE: update sim time base on time since last frame * sim_speed
	if (gs->running) {
		u64 last_game_tick = gs->sim_time_ms;
		gs->sim_time_ms = gs->sim_time_ms + current_tick * gs->sim_speed;
		u32 diff_ms = gs->sim_time_ms - last_game_tick;
		time_step = double(diff_ms) / 1000;
	}

	return time_step;
}

Ellipse3D
ellipse3DInit(dmat3 rotation, EllipseParameters ep, uint vert_count)
{
	assert(ep.a > 0 && ep.b > 0 &&
		ep.a >= ep.b &&
		vert_count > 0);

	Ellipse3D e3d = { nullptr, vert_count};
	// FIXME: should be allocated from GameState->arena
	e3d.vertices = UTIL_ALLOC(vert_count, vec3);
	ellipse3DUpdate(rotation, ep, e3d);
	return e3d;
}

void
ellipse3DUpdate(dmat3 rotation, EllipseParameters ep, Ellipse3D& e3d)
{
	double angle = 2 * M_PI / e3d.vert_count;

	for (uint i = 0; i < e3d.vert_count; i++) {
		double a = angle * i;
		// FIXME: we should have a function for this...
		// NOTE: solving for distance in polar coordinates relative to focus
		double r = ep.a * (1 - pow(ep.e, 2)) / (1 + ep.e * cos(a));
		e3d.vertices[i] = rotation * vec3(polarToRect(a, r), 0);
	}
}

void
selectOrbit(GameState* gs, GameOrbit* orbit)
{
	assert(orbit && orbit->ellipse_entity);

	// FIXME: need to allocate these somewhere other than stack
	static vec3 selected_colors[DEFAULT_ORBIT_VERTICES];

	for (u32 i = 0; i < DEFAULT_ORBIT_VERTICES; i++)
		selected_colors[i] = SELECTED_ELLIPSE_COLOR;

	static vec3 default_colors[DEFAULT_ORBIT_VERTICES];

	for (u32 i = 0; i < DEFAULT_ORBIT_VERTICES; i++)
		default_colors[i] = DEFAULT_ELLIPSE_COLOR;

	if (gs->last_selected_orbit != nullptr) {
		changeOrbitColor(gs->last_selected_orbit, default_colors);
		gs->last_selected_orbit->selected = false;
	}

	changeOrbitColor(orbit, selected_colors);
	orbit->selected = true;
	gs->last_selected_orbit = orbit;
}

bool
addManeuver(GameOrbit* orbit,
			ImpulseType impulse_type,
			double true_anomaly,
			double impulse_delta_v)
{
	ManeuverNode& node = orbit->maneuver;

	if (true_anomaly < -M_PI || true_anomaly > M_PI) {
		LOGF(Error, "invalid true true_anomaly: %f\n", true_anomaly);
		return false;
	}

	if (node.active) {
		LOGF(Warning, "cannot add multiple maneuvers (yet)\n");
		return false;
	}

	if (impulse_delta_v == 0.f) {
		LOGF(Warning, "refusing to add maneuver with 0 dv\n");
		return false;
	}

	node.impulse_type = impulse_type;
	// TODO: thrust calculations
	//double flight_path_angle = orbit->system.sat.gamma;
	//node.impulse_vector.x = ...


	if (node.impulse_type == ImpulseType::PROGRADE) {
		double clamped_theta = orbitClampAngle(true_anomaly) + 2 * M_PI;
		LOGF(Debug, "adding prograde maneuver\n,"
			"theta: %f,\n"
			"clamped theta: %f,\n"
			"dv: %f\n",
			true_anomaly,
			clamped_theta,
			impulse_delta_v);

		node.true_anomaly = true_anomaly;
		node.impulse_delta_v = impulse_delta_v;
		node.active = true;

		return true;
	}

	LOGF(Warning, "failed to add maneuver\n");
	assert(0);
	return false;
}

void
removeManeuver(GameOrbit* orbit)
{
	// TODO: multiple maneuver nodes
	ManeuverNode& node = orbit->maneuver;
	node.impulse_type = ImpulseType::NONE;
	node.true_anomaly = 0.f;
	node.impulse_delta_v = 0.f;
	node.active = false;
}

// NOTE: test if the maneuver node would have occured between 2 orbit positions
// 	eg) between 2 time steps
bool
testManeuverStep(const ManeuverNode& maneuver,
		double previous_theta,
		double next_theta)
{
	if (!maneuver.active)
		return false;

	// NOTE: clamp angles between 0 and 2 pi to simplify inequalities
	double r = orbitClampAngle(maneuver.true_anomaly);
	double clamped_previous_theta = orbitClampAngle(previous_theta);
	double clamped_next_theta = orbitClampAngle(next_theta);

	// FIXME: unhandled cases:
	// 	2) direction is ccw

	// NOTE: sat passess through 0
	if (clamped_next_theta - clamped_previous_theta < 0) {
		if (r >= clamped_previous_theta || r <= clamped_next_theta)
			return true;
	}

	else if (r >= clamped_previous_theta && r <= clamped_next_theta)
		return true;

	return false;
}

void
applyManeuver(GameOrbit* orbit, double previous_true_anomaly)
{
	const ManeuverNode& node = orbit->maneuver;
	TwoBodySystem& sys = orbit->system;

	LOGF(Debug, "applying maneuver,\n"
			"theta: %f\n,"
			"dv: %f,\n"
			"sat.theta: %f,\n"
			"previous theta: %f\n",
			node.true_anomaly,
			node.impulse_delta_v,
			sys.sat.theta,
			previous_true_anomaly);

	// FIXME: assuming prograde impulse vector
	assert(node.impulse_type == ImpulseType::PROGRADE);

	// re-calculate state vectors at the maneuver node
	double theta = node.true_anomaly;
	double mu = sys.body.mu;
	double r = orbitGetRadialDistance(sys.ep.e, sys.ep.p, theta);
	double v = orbitGetVelocity(sys.epsilon, mu, r);
	double gamma = orbitGetFlightPathAngle(sys.ep.e, theta);

	// calculate new satellite state vectors
	v += node.impulse_delta_v; // prograde only

	// get new angular momentum and specific orbital energy
	double h = orbitGetAngularMomentumFromStateVectors(r, v, gamma);
	double epsilon = orbitGetSpecificEnergyFromStateVectors(r, v, mu);

	// update orbit pararmeters
	sys.elements.a = orbitGetSemiMajorAxis(epsilon, mu);
	double p = orbitGetSemiLatusRectum(h, mu);
	sys.elements.e = ellipseGetEccentricity(sys.elements.a, p);

	dvec3 pos = orbitGetPositionVector(r, theta);
	dvec3 vel = orbitGetVelocityVector(mu, h, sys.elements.e, theta);

	sys.rotation = orbitGetXForm(sys.elements);
	sys.sat.position = sys.rotation * pos;
	sys.sat.velocity = sys.rotation * vel;
	systemInit(orbit->system, sys.body, sys.elements);

	// update ellipse3D & GLBuffer vertices
	ellipse3DUpdate(sys.rotation, sys.ep, orbit->e3d);
	GLBuffer* buf = &orbit->ellipse_entity->meshes[0].vertex_attrib_buffers[0];
	assert(utilCStrMatch(buf->name, "position"));
	assert(buf->data_size == orbit->e3d.vert_count * sizeof(vec3));
	updateGLBuffer(buf, orbit->e3d.vertices);
}

// internal

void
changeOrbitColor(GameOrbit* orbit, vec3* color_data)
{
	GLBuffer* color_buf = nullptr;
	GLMesh* gl_mesh = &orbit->ellipse_entity->meshes[0];

	for (u32 i = 0; i < gl_mesh->num_vertex_attrib_buffers; i ++) {
		if (utilCStrMatch(gl_mesh->vertex_attrib_buffers[i].name, "color"))
			color_buf = &gl_mesh->vertex_attrib_buffers[i];
	}

	assert(color_buf && color_buf->data_size == orbit->e3d.vert_count * 3 * 4);
	updateGLBuffer(color_buf, color_data);
}