#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]")
{
	orbital_elements orbit = {};
	// TODO: might want another contructor for ellipse_parameters
	double a = 26564.5; // kilometers
	double e = 0.7411;
	double b = a * sqrt(1 - pow(e, 2.0));
	ellipse_parameters ep = constructEllipse(a, b);
	orbit.ep = ep;
	double initial_anom = 260 * M_PI / 180; // radians
	unsigned int time_step = 60 * 50; // seconds

	// NOTE: solve for gravitational parameter from mean motion
	double mean_motion = 0.00014582; // rad/s
	orbit.mu = pow(mean_motion, 2) * pow(ep.a, 3);


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

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

	double n = getMeanMotion(orbit.mu, orbit.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, ep.e);
	REQUIRE_THAT(E2_1, WithinAbs(0.315452, 1e-5));

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

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

	double r2 = getRadialPosition(ep, 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));
}