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



SCENARIO("Testing the SimCore Sensor")
{
  GIVEN("different Attributes for a Movement ")
  {
  std::shared_ptr<SimCore::Position> pos1 = std::make_shared<SimCore::Position>();
    
    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;
        

      double pitch = 0;
      double course = 360;
      double speed = 1;


      Entities::Movement mov2(pos1,course,speed,pitch);
     

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

   

      std::shared_ptr<SimCore::Position> pos3 = std::make_shared<SimCore::Position>();
      pos3->setGeodesicPos(55, 6, 0);
      Entities::Movement mov3(pos3,0,1,90);
      mov3.updatePosition(1000);

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

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


        // REQUIRE((pos2.getGeocentricPos() - mov.getPosition()->getGeocentricPos()).norm() <= 0.1);
        REQUIRE((pos2.getGeodesicPos() - pos1->getGeodesicPos()).norm() <= 0.1);

        REQUIRE(round(mov3.getPosition()->getGeodesicPos()(SimCore::HEIGHT)) == 1);

        REQUIRE(mov2.getCourse() == SimCore::UtilFunctions::DegToRad(course));
        REQUIRE(mov2.getPitch() == SimCore::UtilFunctions::DegToRad(pitch));
        REQUIRE(mov2.getSpeed() == speed);



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