#include <SimCore/SimCore.hpp>
#define CATCH_CONFIG_MAIN
#include <catch2/catch.hpp>
#include <SimCore/Position.hpp>




SCENARIO("Testing the SimCorePositionClass")
{
  GIVEN("different position in different forms")
  {
    Eigen::Vector3d GeocentPos1;
    GeocentPos1(SimCore::GeocentricPosition::X) = 3784014.333;
    GeocentPos1(SimCore::GeocentricPosition::Y) = 899869.779;
    GeocentPos1(SimCore::GeocentricPosition::Z) = 5037960.502;
    Eigen::Vector3d GeodesPos1;
    GeodesPos1(SimCore::GeodesicPosition::LATITUDE) = 52.516181;
    GeodesPos1(SimCore::GeodesicPosition::LONGITUDE) = 13.376935;
    GeodesPos1(SimCore::GeodesicPosition::HEIGHT) = 0;



    WHEN("constructing Position Object with data")
    {
    SimCore::Position pos1(  GeocentPos1(SimCore::GeocentricPosition::X),  GeocentPos1(SimCore::GeocentricPosition::Y),  GeocentPos1(SimCore::GeocentricPosition::Z));
    SimCore::Position pos1b;
    pos1b.setGeodesicPos(GeodesPos1(SimCore::GeodesicPosition::LATITUDE), GeodesPos1(SimCore::GeodesicPosition::LONGITUDE), GeodesPos1(SimCore::GeodesicPosition::HEIGHT)); 

      THEN("positions attributes are correct")
      {
        REQUIRE(pos1.getGeocentricPos()(SimCore::GeocentricPosition::X) == GeocentPos1(SimCore::GeocentricPosition::X));
        REQUIRE(std::abs(pos1.getGeodesicPos()(SimCore::GeodesicPosition::LATITUDE)  - GeodesPos1(SimCore::GeodesicPosition::LATITUDE)) <= 0.0001);
        REQUIRE(std::abs(pos1.getGeodesicPos()(SimCore::GeodesicPosition::LONGITUDE)  - GeodesPos1(SimCore::GeodesicPosition::LONGITUDE)) <= 0.0001);
        REQUIRE(std::abs(pos1.getGeodesicPos()(SimCore::GeodesicPosition::HEIGHT) - GeodesPos1(SimCore::GeodesicPosition::HEIGHT)) <= 0.001 );

        REQUIRE(std::abs(pos1b.getGeocentricPos()(SimCore::X) - GeocentPos1(SimCore::X)) <= 0.001);
        REQUIRE(std::abs(pos1b.getGeocentricPos()(SimCore::Y) - GeocentPos1(SimCore::Y)) <= 0.001 );
        REQUIRE(std::abs(pos1b.getGeocentricPos()(SimCore::Z) - GeocentPos1(SimCore::Z)) <= 0.001);

      } //THEN
    } // WHEN
  } // GIVEN
} //SCENARIO