#include <catch2/catch.hpp>

#include <glm/glm.hpp>

#include "../src/orbits.cpp"

// NOTE: using WithinAbs instead of Approx because:
// 	https://github.com/catchorg/Catch2/issues/1962
// 	https://github.com/catchorg/Catch2/issues/1507
using Catch::Matchers::WithinAbs;


TEST_CASE("orbit construction", "[orbits]")
{

}

// NOTE: example 4.6 in "Space Flight Dynamics" by Craig A. Kluever
TEST_CASE("orbit propagation", "[orbits]")
{
	double a = 26564.5;		// semi-major axis in km
	double e = 0.7411;		// eccentricity
	double mu = 398601.68;	// gravitational parameter
	double r = 6378;		// body radius in km
	double initial_anom = 260 * M_PI / 180; // NOTE: radians
	double time_step = 60 * 50; // NOTE: seconds
	system_2body sys = systemInit(gravBodyInit(mu, r), orbitInit(a, e));

	double E1 = getEccAnomFromTrueAnom(sys.ep.e, initial_anom);
	REQUIRE_THAT(E1, WithinAbs(-0.8615, 1e-4));

	double M1 = getMeanAnomFromEccAnom(E1, sys.ep.e);
	REQUIRE_THAT(M1, WithinAbs(-0.2992, 1e-4));

	double n = getMeanMotion(sys.body.mu, sys.ep.a);
	REQUIRE_THAT(n, WithinAbs(0.00014582, 1e-8));

	double M2 = getPropagatedMeanAnom(M1, n, time_step);
	REQUIRE_THAT(M2, WithinAbs(0.1383, 1e-5));

	// TODO: could also test for the other trial values listed in table 4.1
	double E2_1 = getInitialTrialValue(M2, sys.ep.e);
	REQUIRE_THAT(E2_1, WithinAbs(0.315452, 1e-5));

	double ecc_anom = getPropagatedEccAnomaly(sys, initial_anom, time_step);
	REQUIRE_THAT(ecc_anom, WithinAbs(0.481518, 1e-5));

	double true_anom = getPropagatedTrueAnomaly(sys, initial_anom, time_step);
	REQUIRE_THAT(true_anom, WithinAbs(1.1339, 1e-4));

	double r2 = orbitGetRadialDistance(sys.ep.e, sys.ep.p, true_anom);
	REQUIRE_THAT(r2, WithinAbs(9116.1, 0.1));

	glm::vec2 pos = polarToRect(true_anom, r2);
	REQUIRE_THAT(pos.x, WithinAbs(3856.9, 0.1));
	REQUIRE_THAT(pos.y, WithinAbs(8259.9, 0.1));
}

// NOTE: example 2.5 - c
TEST_CASE("orbital period", "[orbits]")
{
	double a = 24371;		// semi-major axis in km
	double mu = 398601.68;	// gravitational parameter

	double T = orbitGetPeriod(a, mu);
	REQUIRE_THAT(T, WithinAbs(37863.5, 50e-3));
}

// NOTE: example 4.1 in "Space Flight Dynamics" by Craig A. Kluever
TEST_CASE("time of flight example 4.1 a", "[orbits]")
{
	double a = 26564.5;		// semi-major axis in km
	double e = 0.7411;		// eccentricity
	double mu = 398601.68;	// gravitational parameter
	double r = 6378;		// body radius in km
	system_2body sys = systemInit(gravBodyInit(mu, r), orbitInit(a, e));

	// NOTE: get ToF from periapsis to true anomaly at 154.85 degrees
	double theta_0 = 0.0;
	double theta_1 = 154.85 * M_PI / 180;

	double n = getMeanMotion(sys.body.mu, sys.ep.a);
	REQUIRE_THAT(n, WithinAbs(0.00014582, 1e-8));

	double ecc_1 = getEccAnomFromTrueAnom(sys.ep.e, theta_1);
	REQUIRE_THAT(ecc_1, WithinAbs(2.0927, 1e-4));

	// NOTE: we really want < 33ms accuracy here, but either there's floating
	// 	point error, or the examples in the book are incorrect or just not
	// 	precise enough
	double tof = orbitGetTimeOfFlight(sys, theta_0, theta_1);
	REQUIRE_THAT(tof, WithinAbs(9945.2, 0.5));
}

TEST_CASE("time of flight example 4.2", "[orbits]")
{
	double a = 26564.5;		// semi-major axis in km
	double e = 0.7411;		// eccentricity
	double mu = 398601.68;	// gravitational parameter
	double r = 6378;		// body radius in km
	system_2body sys = systemInit(gravBodyInit(mu, r), orbitInit(a, e));

	// NOTE: get ToF from periapsis to true anomaly at 154.85 degrees
	double theta_1 = 230 * M_PI / 180;
	double theta_2 = 120 * M_PI / 180;

	double n = getMeanMotion(mu, a);
	REQUIRE_THAT(n, WithinAbs(0.00014582, 1e-8));

	double ecc_1 = getEccAnomFromTrueAnom(e, theta_1);
	REQUIRE_THAT(orbitClampAngle(ecc_1), WithinAbs(4.9012, 1e-4));

	double M1 = getMeanAnomFromEccAnom(ecc_1, e);
	REQUIRE_THAT(orbitClampAngle(M1), WithinAbs(5.6291, 1e-4));

	double ecc_2 = getEccAnomFromTrueAnom(e, theta_2);
	REQUIRE_THAT(orbitClampAngle(ecc_2), WithinAbs(1.1778, 1e-4));

	// NOTE: see note in example 4.1 above about accuracy
	double tof = orbitGetTimeOfFlight(sys, theta_1, theta_2);
	REQUIRE_THAT(tof, WithinAbs(7867.5, 2 * 0.5));
}