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

ManeuverNode*
getFreeManeuver(GameState* gs)
{
	for (u32 i = 0; i < gs->max_maneuvers; i++) {
		if (gs->maneuver_nodes[i].active == false) {
			gs->maneuver_nodes[i].active = true;
			return &gs->maneuver_nodes[i];
		}
	}

	LOGF(Error, "No free maneuver nodes\n");
	return nullptr;
}

// TODO: appendManeuver(orbit, node) would be better
bool
addManeuver(GameState* gs,
			GameOrbit* orbit,
			ImpulseType impulse_type,
			double true_anomaly,
			double impulse_delta_v)
{
	if (true_anomaly < -M_PI || true_anomaly > M_PI) {
		LOGF(Error, "invalid true true_anomaly: %f\n", true_anomaly);
		return false;
	}

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

	ManeuverNode* node = getFreeManeuver(gs);
	assert(node);

	if (orbit->last_maneuver) {
		node->previous_maneuver = orbit->last_maneuver;
		orbit->last_maneuver->next_maneuver = node;
		orbit->last_maneuver = node;
	} else {
		orbit->first_maneuver = node;
		orbit->last_maneuver = node;
	}

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

// NOTE: test if the maneuver node would have occured between 2 orbit positions
// 	eg) between 2 time steps
bool
testManeuverStep(ManeuverNode* maneuver,
		double previous_theta,
		double next_theta)
{
	assert(maneuver);

	// 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, ManeuverNode* maneuver)
{
	TwoBodySystem& sys = orbit->system;

	assert(maneuver);
	// FIXME: assuming prograde impulse vector
	assert(maneuver->impulse_type == ImpulseType::PROGRADE);

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

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

	// apply impulse along velocity vector
	dvec3 impulse = glm::normalize(vel) * maneuver->impulse_delta_v;
	vel = vel + impulse;
	sys.elements = orbitGetElementsFromStateVectors(pos, vel, mu);
	systemInit(orbit->system, sys.body, sys.elements);

	// update satellite true anamoly
	sys.sat.theta = sys.elements.nu;

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

void
removeManeuver(GameOrbit* orbit, ManeuverNode* maneuver)
{
	assert(maneuver);
	// FIXME: we're only removing the first maneuver for now
	assert(maneuver == orbit->first_maneuver);

	if (orbit->last_maneuver == maneuver) {
		orbit->first_maneuver = nullptr;
		orbit->last_maneuver = nullptr;
	} else {
		orbit->first_maneuver = maneuver->next_maneuver;
		maneuver->previous_maneuver = nullptr;
	}

	maneuver->impulse_type = ImpulseType::NONE;
	maneuver->true_anomaly = 0.f;
	maneuver->impulse_delta_v = 0.f;
	maneuver->active = false;
	maneuver->previous_maneuver = nullptr;
	maneuver->next_maneuver = nullptr;
}


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