#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>


struct KinVec{
  double x,y,z;
}
struct MovementVec{
  double dSpd,dClb,dCrs;
}
KinVec Generate_Kin_Vect(MovementVec Mov_Dat, LLA_Rad LLA_rCoord)
{
    KinVec Result;

    //Transform Mov_Dat to topocentric vector
    UVW UVW_Coord;

    UVW_Coord.dU = Mov_Dat.dSpd * cos(Mov_Dat.dClb) * sin(Mov_Dat.dCrs);
    UVW_Coord.dV = Mov_Dat.dSpd * cos(Mov_Dat.dClb) * cos(Mov_Dat.dCrs);
    UVW_Coord.dW = Mov_Dat.dSpd * sin(Mov_Dat.dClb);

    //Transform topocentric vector to geocentric vector

    Result.x = -UVW_Coord.dU * sin(LLA_rCoord.dLon) -
                 UVW_Coord.dV * sin(LLA_rCoord.dLat) * cos(LLA_rCoord.dLon) +
                 UVW_Coord.dW * cos(LLA_rCoord.dLat) * cos(LLA_rCoord.dLon);

    Result.y = UVW_Coord.dU * cos(LLA_rCoord.dLon) -
                 UVW_Coord.dV * sin(LLA_rCoord.dLat) * sin(LLA_rCoord.dLon) +
                 UVW_Coord.dW * cos(LLA_rCoord.dLat) * sin(LLA_rCoord.dLon);

    Result.z = UVW_Coord.dV * cos(LLA_rCoord.dLat) +
                 UVW_Coord.dW * sin(LLA_rCoord.dLat);


    return Result;
};


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;
      }

      MovementVec movc;
      movc.dSpd = 10;
      movc.dClb = 0;
      movc.dCrs = 0;
      // 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