#define CATCH_CONFIG_MAIN  // This tells Catch to provide a main() - only do this in one cpp file
#include <catch2/catch.hpp>

#include <Units/PhysicalUnits.h>
#include <Geometry/CoordinateSystem.h>
#include <Geometry/Point.h>
#include <Geometry/Sphere.h>
#include <cmath>

using namespace phys::units;
using namespace phys::units::literals;

TEST_CASE( "transformations between CoordinateSystems")
{  
    CoordinateSystem rootCS;
    
    REQUIRE ( CoordinateSystem::GetTransformation(rootCS, rootCS).isApprox(EigenTransform::Identity()) );
    
    QuantityVector<length_d> const coordinates{0_m, 0_m, 0_m};
    Point p1(rootCS, coordinates);
    
    QuantityVector<magnetic_flux_density_d> components{1.*tesla, 0.*tesla, 0.*tesla};
    Vector<magnetic_flux_density_d> v1(rootCS, components);
    
    REQUIRE ( (p1.GetCoordinates() - coordinates).norm().magnitude() == Approx(0) );
    REQUIRE ( (p1.GetCoordinates(rootCS) - coordinates).norm().magnitude() == Approx(0) );
    
    SECTION ( "unconnected CoordinateSystems" ) {
        CoordinateSystem rootCS2;
        REQUIRE_THROWS(CoordinateSystem::GetTransformation(rootCS, rootCS2));
    }
    
    SECTION( "translations" ) {        
        QuantityVector<length_d> const translationVector{0_m, 4_m, 0_m};
        
        CoordinateSystem translatedCS = rootCS.translate(translationVector);
        
        REQUIRE (translatedCS.GetReference() == &rootCS);
        
        REQUIRE ( (p1.GetCoordinates(translatedCS) + translationVector).norm().magnitude() == Approx(0) );
        
        // Vectors are not subject to translations
        REQUIRE ((v1.GetComponents(rootCS) - v1.GetComponents(translatedCS)).norm().magnitude() == Approx( 0 ));
        
        Point p2(translatedCS, {0_m, 0_m, 0_m});
        REQUIRE ( ((p2 - p1).GetComponents() - translationVector).norm().magnitude() == Approx( 0 ));
    }
    
    SECTION( "multiple translations" ) {
        QuantityVector<length_d> const tv1{0_m, 5_m, 0_m};
        CoordinateSystem cs2 = rootCS.translate(tv1);
        
        QuantityVector<length_d> const tv2{3_m, 0_m, 0_m};
        CoordinateSystem cs3 = rootCS.translate(tv2);
        
        QuantityVector<length_d> const tv3{0_m, 0_m, 2_m};
        CoordinateSystem cs4 = cs3.translate(tv3);
        
        REQUIRE( cs4.GetReference()->GetReference() == &rootCS );
        
        REQUIRE( CoordinateSystem::GetTransformation(cs3, cs2).isApprox(rootCS.translate({3_m, -5_m, 0_m}).GetTransform()) );
        REQUIRE( CoordinateSystem::GetTransformation(cs2, cs3).isApprox(rootCS.translate({-3_m, +5_m, 0_m}).GetTransform()) );
    }
    
    SECTION( "rotations" ) {
        QuantityVector<length_d> const axis{0_m, 0_m, 1_km};
        double const angle = 90. / 180. * M_PI;
        
        CoordinateSystem rotatedCS = rootCS.rotate(axis, angle);
        REQUIRE (rotatedCS.GetReference() == &rootCS);
        
        REQUIRE( v1.GetComponents(rotatedCS)[1].magnitude() == Approx((-1. * tesla).magnitude()) );
        
        // vector norm invariant under rotation
        REQUIRE( v1.GetComponents(rotatedCS).norm().magnitude() == Approx( v1.GetComponents(rootCS).norm().magnitude() ) );
    }
    
    SECTION("multiple rotations") {
        QuantityVector<length_d> const zAxis{0_m, 0_m, 1_km};
        QuantityVector<length_d> const yAxis{0_m, 7_nm, 0_m};
        QuantityVector<length_d> const xAxis{2_m, 0_nm, 0_m};
        
        double const angle = 90. / 180. * M_PI;
        
        CoordinateSystem rotated1 = rootCS.rotate(zAxis, angle);
        CoordinateSystem rotated2 = rotated1.rotate(yAxis, angle);
        CoordinateSystem rotated3 = rotated2.rotate(zAxis, -angle);
        
        CoordinateSystem combined = rootCS.rotate(xAxis, -angle);
        
        auto comp1 = v1.GetComponents(rootCS);
        auto comp3 = v1.GetComponents(combined);
        REQUIRE ((comp1 - comp3).norm().magnitude() == Approx(0));
    }
}

TEST_CASE("Sphere") {
    CoordinateSystem rootCS;
    Point center(rootCS, {0_m, 3_m, 4_m});
    Sphere sphere(center, 5_m);
    
    SECTION ("isInside") {
        REQUIRE_FALSE(sphere.isInside(Point(rootCS, {100_m, 0_m, 0_m})));
        
        REQUIRE(sphere.isInside(Point(rootCS, {2_m, 3_m, 4_m})));
    }
}