#include "orbits.h"


ellipse_parameters
constructEllipseAB(double a, double b)
{
	assert(a > 0 && b > 0 && a >= b);
	ellipse_parameters 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 = -1 * ep.c;
	ep.f2.x = ep.c;
	return ep;
}

ellipse_parameters
constructEllipseAE(double a, double e)
{
	assert(e >= 0 && e < 1);
	double b = a * sqrt(1 - pow(e, 2.0));
	return constructEllipseAB(a, b);
}

bool
validateEllipse(const ellipse_parameters& ep)
{
	// TODO: find out why satellite position gets wonky with orbit
	// 	eccentricity > 0.995 while passing through true anom = 0.
	// 	maybe divide by 0, or some floating point error?
	return (ep.a > 0 &&
		ep.b > 0 &&
		ep.a >= ep.b &&
		ep.e >= 0 &&
		ep.e < 0.995);
}

bool
ellipsesEqual(ellipse_parameters& e1, ellipse_parameters& e2)
{
	return (e1.a == e2.a &&
		e1.b == e2.b &&
		e1.e == e2.e);
}

void
ellipseCopy(const ellipse_parameters& e1, ellipse_parameters& e2)
{
	e2.a = e1.a;
	e2.b = e1.b;
	e2.e = e1.e;
	e2.c = e1.c;
	e2.p = e1.p;
	e2.f1 = e1.f1;
	e2.f2 = e1.f2;
}

void
orbitCopy(const orbital_elements& o1, orbital_elements& o2)
{
	ellipseCopy(o1.ep, o2.ep);
	o2.iota = o1.iota;
	o2.omega = o1.omega;
	o2.mu = o1.mu;
	o2.nu = o1.nu;
	o2.pos = o1.pos;
}

ellipse_3d
constructEllipse3D(ellipse_parameters ep, uint vert_count)
{
	assert(ep.a > 0 && ep.b > 0 &&
		ep.a >= ep.b &&
		vert_count > 0);

	ellipse_3d e3d = { ep, nullptr, vert_count};
	// TODO: need to free this allocation at some point
	e3d.vertices = UTIL_ALLOC(vert_count, glm::vec3);
	ellipse3DUpdate(ep, e3d);
	return e3d;
}

void
ellipse3DUpdate(ellipse_parameters ep, ellipse_3d& e3d)
{
	double angle = 2 * M_PI / e3d.vert_count;

	for (uint i = 0; i < e3d.vert_count; i++) {
		double a = angle * i;
		// 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] = glm::vec3(polarToRect(a, r), 0);
	}
}

double
getEccAnomFromTrueAnom(double ecc, double true_anom)
{
	return 2 * atan(sqrt((1 - ecc) / (1 + ecc)) * tan(true_anom / 2));
}

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

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

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

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

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

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

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(orbital_elements orbit,
	double initial_anom,
	unsigned int time_step)
{
	double e = orbit.ep.e;
	double E1 = getEccAnomFromTrueAnom(e, initial_anom);
	double M1 = getMeanAnomFromEccAnom(E1, e);
	double n = getMeanMotion(orbit.mu, orbit.ep.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
getPropagatedTrueAnomaly(orbital_elements orbit,
	double initial_anom,
	unsigned int time_step)
{
	double ecc_anom = getPropagatedEccAnomaly(orbit, initial_anom, time_step);
	return getTrueAnomFromEccAnom(orbit.ep.e, ecc_anom);
}

double
getRadialDistance(double e, double p, double true_anom)
{
	return p / (1 + e * cos(true_anom));
}

glm::vec2
polarToRect(double angle, double r)
{
	return glm::vec2(r * cos(angle), r * sin(angle));
}