#include <SimCore/Messages/SimTrack.hpp>
#include <WHISPER/Messages/Message.hpp>
#include <SimCore/SimCore.hpp>
#include <algorithm>
#include <memory>
#define CATCH_CONFIG_MAIN
#include <catch2/catch.hpp>
#include <SimCore/Position.hpp>




SCENARIO("Testing the SimCore Track")
{
  GIVEN("different Attributes for a Track 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;

  SimCore::Emission emi1("test Radar");

    SimCore::Position pos1(GeocentPos1(SimCore::X),GeocentPos1(SimCore::Y),GeocentPos1(SimCore::Z));

    double course = 90;
    double speed = 10;
    double knots = speed * SimCore::MsKt;
    bool testOperator = false;

    std::shared_ptr<WHISPER::Message> msg = NULL;

    std::shared_ptr<SimCore::SimTrack> trackPtr = NULL;
   
    std::string serializedMSG;

    WHEN("constructing Track Object with data")
    {
        SimCore::Position pos(  GeocentPos1(SimCore::GeocentricPosition::X),  GeocentPos1(SimCore::GeocentricPosition::Y),  GeocentPos1(SimCore::GeocentricPosition::Z));
        SimCore::SimTrack track(WHISPER::SourceType::SHIP,SimCore::EntityKind::LAND);
        track.Speed.setValue(speed);
        track.Course.setValue(course);
        track.setPosition(pos1);

        if (track.getPosition() == pos) {
          testOperator = true;
        }

        auto idEmi = emi1.getID();
        track.addEmission(emi1);
        // serializedMSG = track.buildMessage(parentID).serialize();
        // msg = std::make_shared<WHISPER::Message>(serializedMSG);

        // if (msg.get()->msgType_ == WHISPER::GROUND_TRUTH_TRACK) {
         
        //  trackPtr = std::make_shared<SimCore::SimTrack>(std::move(SimCore::SimTrack::unpack(*msg)));
        // }

        // std::string trackstring = track.buildMessage(parentID).serialize();

        // SimCore::GroundTruthTrack trackDeserialized = SimCore::GroundTruthTrack::unpack(*msg);
        std::shared_ptr<SimCore::Emission> esm=  track.getEmission(emi1.getID());
        esm->packEmission();

      THEN("check if Track attributes are correct")
      {
        REQUIRE(testOperator == true);
        REQUIRE(track.Course.getValue() == course);
        REQUIRE(track.Speed.getValue() == speed);
        REQUIRE(track.getPosition().getGeocentricPos() == pos.getGeocentricPos());
        REQUIRE(track.coutEmissions() > 0);
        REQUIRE(track.getEmission(emi1.getID()).get()->getID() == idEmi);

        // REQUIRE(msg.get()->msgType_ == WHISPER::GROUND_TRUTH_TRACK);
        // REQUIRE(trackPtr->Speed.getValue() == speed);
        // REQUIRE(trackPtr->getPosition().getGeocentricPos() == GeocentPos1);
        // REQUIRE(trackPtr->getIdentifier().getNumber() > 0);
        // REQUIRE(trackDeserialized.getIdentifier().getNumber() > 0);





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