#include "Entities/Movement.hpp"
#include "SimCore/Position.hpp"
#include "SimCore/SimCore.hpp"
#include <memory>
#include <string>
#include <thread>
#define CATCH_CONFIG_MAIN
#include <catch2/catch.hpp>
#include <loguru.hpp>






SCENARIO("Testing the SimCore Sensor")
{
  GIVEN("different Attributes for a Movement ")
  {
  SimCore::Position pos1;
    
    WHEN("constructing Track Object with data")
    {
      pos1.setGeodesicPos(55, 6, 0);
      LOG_S(INFO)<<"LAT: "<<pos1.getGeodesicPos()(SimCore::LATITUDE)<<" LON: "<< pos1.getGeodesicPos()(SimCore::LONGITUDE)<< " H: "<< pos1.getGeodesicPos()(SimCore::HEIGHT);

      Entities::Movement mov(pos1);
      mov.setCourse(90);
      mov.setSpeed(100);//m/s
      mov.setPitch(0);

      mov.updatePosition(10000);// 10 seconds
      
      LOG_S(INFO)<<"LAT: "<<mov.getPosition().getGeodesicPos()(SimCore::LATITUDE)<<" LON: "<< mov.getPosition().getGeodesicPos()(SimCore::LONGITUDE)<< " H: "<< mov.getPosition().getGeodesicPos()(SimCore::HEIGHT);

      GeographicLib::Geodesic geod(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
      double h2, lat2,lon2;
      geod.Direct(55,6,90,1000,lat2,lon2);
      SimCore::Position pos2;
      pos2.setGeodesicPos(lat2, lon2, 0);
      GeographicLib::GeodesicLine line = geod.InverseLine(55,6 , lat2, lon2);
      LOG_S(INFO)<<"distance: "<<line.Distance()<< " lat2: "<< lat2 << " lon2: " << lon2;
        
        
      Entities::Movement mov2(pos1);
      mov2.setCourse(0);//degree
      mov2.setSpeed(100);//m/s
      mov2.setPitch(45);//degree

      LOG_S(INFO)<<"LAT: "<<mov2.getPosition().getGeodesicPos()(SimCore::LATITUDE)<<" LON: "<< mov2.getPosition().getGeodesicPos()(SimCore::LONGITUDE)<< " H: "<< mov2.getPosition().getGeodesicPos()(SimCore::HEIGHT);

      double Vd = 100 * sin(45* M_PI / 180.0);
      double h = Vd*10;
      LOG_S(INFO)<<"höhe: "<< h;
      double i;

      for ( i = 0; i <= 10; ) {
        mov2.updatePosition(500);
        i += 0.5;
      }

      // LOG_S(INFO)<< mov.getEulerAngles();

      THEN("check if Track attributes are correct")
      {

        // LOG_S(INFO)<<mov.getPosition().getGeodesicPos()(SimCore::LATITUDE);
        REQUIRE((pos2.getGeocentricPos() - mov.getPosition().getGeocentricPos()).norm() <= 0.1);
        REQUIRE((h - mov2.getPosition().getGeodesicPos()(SimCore::HEIGHT)) <= 0.1);



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