#include "orbits.h"


const static uint ELLIPSE_VERT_COUNT = 256;


void
systemInit(TwoBodySystem& system, GravBody gb, OrbitalElements el)
{
	system.body = gb;
	system.elements = el;
	system.ep = ellipseInitAE(el.a, el.e);
	system.epsilon = orbitGetSpecificEnergy(system.ep.a, gb.mu);
	system.h = orbitGetAngularMomentum(system.ep.p, gb.mu);
	system.orbital_period = orbitGetPeriod(system.ep.a, gb.mu);
	system.r_periapsis = system.ep.a - system.ep.c;
	system.r_apoapsis = 2 * system.ep.a - system.r_periapsis;
	system.rotation = orbitGetXForm(el);
}

GravBody
gravBodyInit(double mu, double r)
{
	GravBody gb = {0};
	gb.mu = mu;
	gb.radius = r;
	return gb;
}

EllipseParameters
ellipseInitAB(double a, double b)
{
	assert(a > 0 && b > 0 && a >= b);
	EllipseParameters ep = { a, b };
	ep.c = sqrt(a * a - b * b);
	ep.e = ep.c / ep.a;
	ep.p = ep.a * (1 - pow(ep.e, 2));
	ep.f1.x = ep.c;
	ep.f2.x = -1 * ep.c;
	return ep;
}

// FIXME: we should avoid calling ellipseInitAB, and recalculate the properties
//	to avoid floating point errors in the known quantity 'e'
EllipseParameters
ellipseInitAE(double a, double e)
{
	assert(e >= 0 && e < 1);
	double b = a * sqrt(1 - pow(e, 2.0));
	return ellipseInitAB(a, b);
}

OrbitalElements
orbitInit(double a, double e, double iota, double ohm, double omega, double nu)
{
	// TODO: remaining elements: iota, ohm, omega, nu
	OrbitalElements o = {0};
	o.a = a;
	o.e = e;
	o.iota = iota;
	o.ohm = ohm;
	o.omega = omega;
	o.nu = nu;
	return o;
}

glm::dvec3
orbitGetEccentricityVector(glm::dvec3 r, glm::dvec3 v, double mu)
{
	double v_mag = orbitGetVectorMagnitude(v);
	double r_mag = orbitGetVectorMagnitude(r);
	return 1 / mu * ((pow(v_mag, 2) - mu / r_mag) * r - (glm::dot(r, v) * v));
}

glm::dvec3
orbitGetPositionVector(double r, double theta)
{
	return glm::dvec3(r * cos(theta), r * sin(theta), 0);
}

glm::dvec3
orbitGetVelocityVector(double mu, double h, double e, double theta)
{
	return glm::dvec3(-1 * (mu / h) * sin(theta), mu / h * (e + cos(theta)), 0);
}

glm::dmat3
orbitGetXForm(OrbitalElements elements)
{
	const OrbitalElements& el = elements;
	glm::mat3 M(1.0);
	M[0][0] = cos(el.ohm) * cos(el.omega) - sin(el.ohm) * sin(el.omega) * cos(el.iota);
	M[1][0] = -cos(el.ohm) * sin(el.omega) - sin(el.ohm) * cos(el.omega) * cos(el.iota);
	M[2][0] = sin(el.ohm) * sin(el.iota);
	M[0][1] = sin(el.ohm) * cos(el.omega) + cos(el.ohm) * sin(el.omega) * cos(el.iota);
	M[1][1] = -sin(el.ohm) * sin(el.omega) + cos(el.ohm) * cos(el.omega) * cos(el.iota);
	M[2][1] = -cos(el.ohm) * sin(el.iota);
	M[0][2] = sin(el.omega) * sin(el.iota);
	M[1][2] = cos(el.omega) * sin(el.iota);
	M[2][2] = cos(el.iota);
	return M;
}

double
orbitGetVectorMagnitude(glm::dvec3 v)
{
	return(sqrt(pow(v.x, 2) + pow(v.y, 2) + pow(v.z, 2)));
}


//
// NOTE: propagate anomaly functions:
//

// FIXME: variable names 'ecc' should be just 'e'
inline double
getEccAnomFromTrueAnom(double ecc, double true_anom)
{
	return 2 * atan(sqrt((1 - ecc) / (1 + ecc)) * tan(true_anom / 2));
}

inline double
getTrueAnomFromEccAnom(double ecc, double ecc_anom)
{
	return 2 * atan(sqrt((1 + ecc) / (1 - ecc)) * tan(ecc_anom / 2));
}

inline double
getMeanAnomFromEccAnom(double ecc_anom, double ecc)
{
	return ecc_anom - ecc * sin(ecc_anom);
}

inline double
getMeanMotion(double mu, double a)
{
	return sqrt(mu / pow(a, 3));
}

inline double
getPropagatedMeanAnom(double mean_anom, double mean_motion, double time_step)
{
	return mean_anom + mean_motion * (time_step);
}

inline double
getInitialTrialValue(double mean_anom, double ecc)
{
	return mean_anom + ecc * sin(mean_anom)
		+ ((pow(ecc, 2) / 2) * sin(2 * mean_anom));
}

inline double
getTrialError(double ecc, double test_anom, double mean_anom)
{
	return test_anom - ecc * sin(test_anom) - mean_anom;
}

inline double
getNextTrialValue(double err, double ecc, double test_anom, double mean_anom)
{
	// compute derivative of the error function
	double derr = 1 - ecc * cos(test_anom);

	// use Newton's method to compute next trial value of E2
	return test_anom - (err / derr);
}

double
getPropagatedEccAnomaly(TwoBodySystem sys,
	double initial_anom,
	double time_step)
{
	double e = sys.ep.e;
	double E1 = getEccAnomFromTrueAnom(e, initial_anom);
	double M1 = getMeanAnomFromEccAnom(E1, e);
	double n = getMeanMotion(sys.body.mu, sys.elements.a);
	double M2 = getPropagatedMeanAnom(M1, n, time_step);
	double E2_1 = getInitialTrialValue(M2, e);

	// test if guess is a solution to kepler's equation
	const double ACCEPTABLE_ERROR = 0.00000001;
	double E2_test = E2_1;

	for (uint i = 0; i < 10; i++) {
		double err = getTrialError(e, E2_test, M2);

		if (fabs(err) < ACCEPTABLE_ERROR)
			break;

		E2_test = getNextTrialValue(err, e, E2_test, M2);
	}

	return E2_test;
}

double
orbitGetPropagatedTrueAnomaly(TwoBodySystem sys,
	double initial_anom,
	double time_step)
{
	// FIXME: I don't think we need this now that we have gs->running?
	// NOTE: 'pause' simulation when time_step is set close to 0
	if (time_step < 1e-8 && time_step > -1e-8)
		return initial_anom;

	double ecc_anom = getPropagatedEccAnomaly(sys, initial_anom, time_step);
	return getTrueAnomFromEccAnom(sys.ep.e, ecc_anom);
}


// FIXME: organize into interface/internal functions

double
orbitTimeSincePeriapsis(TwoBodySystem sys, double theta);

double
orbitGetTimeOfFlight(TwoBodySystem sys, double theta_begin, double theta_end)
{
	double e = sys.ep.e;

	double n = getMeanMotion(sys.body.mu, sys.ep.a);

	double ecc_begin = getEccAnomFromTrueAnom(sys.ep.e, theta_begin);
	double ecc_end = getEccAnomFromTrueAnom(sys.ep.e, theta_end);

	// NOTE: test if flight passes through perisapsis
	if (ecc_begin > ecc_end)
		ecc_end += 2 * M_PI;

	double M1 = getMeanAnomFromEccAnom(ecc_begin, e);
	double M2 = getMeanAnomFromEccAnom(ecc_end, e);

	// NOTE: Kepler's equation for time of flight
	double tof_begin = 1 / n * M1;
	double tof_end = 1 / n * M2;

	return tof_end - tof_begin;
}