#pragma once

#include <cmath>

#define GLM_FORCE_XYZW_ONLY
#include <glm/glm.hpp>

#include "util.h"


#define DEG2RAD(x) x * M_PI / 180


struct EllipseParameters
{
	double a;		// semi-major axis
	double b;		// semi-minor axis
	double e;		// eccentricity
	double c;		// linear eccentricity
	double p;		// semilatus rectum
	glm::vec2 f1;	// 'primary' focus
	glm::vec2 f2;	// 'vacant' focus
};

struct OrbitalElements
{
	// NOTE: classical orbital elements
	double a;		// semimajor axis
	double e;		// eccentricity
	double iota;	// (ι) inclination
	double ohm;		// (Ω) longitude of the ascending node
	double omega;	// (ω) argument of periapsis
	double nu;		// (ν) true anomaly at T0
};

struct GravBody
{
	double mu;			// (μ) gravitational parameter
	double radius;		// radius of ideal sphere representing the body
	// double r_atmos;	// TODO: bodies w/ atmosphere
};

struct Satellite
{
	glm::dvec3 position;
	glm::dvec3 velocity;
	double theta;		// true anomaly
	double r;			// radius magnitude at theta
	double gamma;		// (γ) flight path angle
	double v;			// velocity magnitute
};

struct StateVectors
{
	glm::dvec3 position;
	glm::dvec3 velocity;
};

// NOTE: top level composite for 2 body system
struct TwoBodySystem
{
	GravBody body;
	Satellite sat;
	EllipseParameters ep;
	OrbitalElements elements;
	glm::dmat3 rotation;

	double epsilon;			// (ε) specific orbital energy, MJ/kg
	double h;				// angular momentum
	double r_apoapsis;		// apoapse distance from body center
	double r_periapsis;		// periapsis distance from body center
	double orbital_period;	// in seconds
};

void
systemInit(TwoBodySystem& system, GravBody gb, OrbitalElements el);

GravBody
gravBodyInit(double mu, double r);

EllipseParameters
ellipseInitAB(double a, double b);

EllipseParameters
ellipseInitAE(double a, double e);

inline double
ellipseGetEccentricity(double a, double p)
{
	return sqrt(1 - p / a);
}

inline bool
ellipseValidate(const EllipseParameters& 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);
}

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

OrbitalElements
orbitInit(double a, double e, double iota, double ohm, double omega, double nu);

OrbitalElements
orbitGetElementsFromStateVectors(glm::dvec3 r, glm::dvec3 v, double mu);

StateVectors
orbitGetStateVectorsFromElements(const OrbitalElements& el, double mu);

glm::dvec3 orbitGetEccentricityVector(glm::dvec3 r, glm::dvec3 v, double mu);

// NOTE: returns position vector in perifocal plane
glm::dvec3 orbitGetPositionVector(double r, double theta);

// NOTE: returns velocity vector in perifocal plane
glm::dvec3 orbitGetVelocityVector(double mu, double h, double e, double theta);

// NOTE: return transform from perifocal to grav body (IJK) coordinate system
//	in column major format
glm::dmat3 orbitGetXForm(OrbitalElements elements);

double orbitGetVectorMagnitude(glm::dvec3 v);

inline double
orbitGetAngularMomentum(double p, double mu)
{
	return sqrt(mu * p);
}

inline double
orbitGetSemiMajorAxis(double epsilon, double mu)
{
	return -1 * mu / (2 * epsilon);
}

inline double
orbitGetSemiLatusRectum(double h, double mu)
{
	return pow(h, 2) / mu;
}

inline double
orbitGetAngularMomentumFromStateVectors(double r, double v, double gamma)
{
	return r * v * cos(gamma);
}

inline double
orbitGetSpecificEnergy(double a, double mu)
{
	return -1 * mu / (2 * a);
}

inline double
orbitGetSpecificEnergyFromStateVectors(double r, double v, double mu)
{
	return pow(v, 2) / 2 - mu / r;
}

inline double
orbitGetVelocity(double epsilon, double mu, double r)
{
	return sqrt(2 * (epsilon + mu / r));
}

inline double
orbitGetFlightPathAngle(double e, double true_anom)
{
	return atan(e * sin(true_anom) / (1 + e * cos(true_anom)));
}

inline double
orbitGetPeriod(double a, double mu)
{
	return 2 * M_PI / sqrt(mu) * sqrt(pow(a, 3));
}

// NOTE: aka) the trajectory equation (eq. 2.45)
inline double
orbitGetRadialDistance(double e, double p, double true_anom)
{
	return p / (1 + e * cos(true_anom));
}

// NOTE: return an equivalent angle between 0 and 2 pi radians
inline double
orbitClampAngle(double theta)
{
	double revs = floor(theta / (2 * M_PI));
	return theta - revs * 2 * M_PI;
}

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

void
orbitUpdate(OrbitalElements& o, double a, double e);

/* NOTE: how-to propagate orbit position given initial true anomaly, semimajor
 *	axis, mean motion, and eccentricity: ref) section 4.4, Kepler's Problem,
 * 	"Space Flight Dynamics" by Craig A. Kluever
 *
 * obtain initial eccentric anomaly (E1) from true anomaly (ϴ1) and e:
 *	tan(E1/2) = sqrt((1-e)/(1+e)) * tan(ϴ1/2)
 *
 * obtain inital mean anomaly:
 * 	M1 = E1 - e*sin(E1)
 *
 * obtain propagated mean anomaly, mean motion (n) is sqrt(μ/a^3):
 *	M2 = M1 + n(t2 - t1)
 *
 * express Kepler's equation in terms of propagated mean anomly:
 * 	M2 = E2 - e*sin(E2)
 *
 * Use Newton's method to search for an E2 that satisfies Kepler's equation:
 *		guess a starting value for E2_k (k indicates iteration index):
 *			E2_1 = M2 + e*sin(M2) + ((e^2 / 2) * sin(2 * M2))
 *
 *		test if guess is a solution:
 *			F(E2_1) = E2_1 - e*sin(E2) - M2
 *
 *			if the result of the error function is not below some small value
 *				(1 * 10^-8), compute the derivative, and and iterate again:
 *				f'(E2_1) = 1 - e*cos(E2_1)
 *
 *			use Newton's method to compute the next trial value of E2:
 *				E2_2 = E2_1 - (f(E2_1) / f'(E2_1))
 *
 *		after we converge on a solution, convert eccentric anomaly back to true
 *			anomoly:
 *			tan(ϴ2/2) = sqrt((1+e) / (1-e)) * tan(E2/2)
 */
double
orbitGetPropagatedTrueAnomaly(TwoBodySystem sys,
	double initial_anom,
	double time_step);	// in seconds

double
orbitGetTimeOfFlight(TwoBodySystem sys, double theta_begin, double theta_end);

// NOTE return impulse magnitute for Hohmann transfer orbit
double orbitGetTransferVelocity(const TwoBodySystem& sys,
	const OrbitalElements& target);

// NOTE: return impulse magnitude for orbit circularization
double
orbitGetCircVelocity(const TwoBodySystem& sys, bool raise_apoapse = true);