#include "SimCore/Identifier.hpp"
#include "SimCore/IdentifierMaker.hpp"
#include <SimCore/Messages/GroundThruthTrack.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;
    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::GroundTruthTrack> trackPtr = NULL;
    SimCore::Identifier parentID(0,1,false);
    SimCore::IdentifierMaker IDMaker;
    auto ID = IDMaker.getNewIdentifier(1,SimCore::ObjectSource::EXTERNAL);


    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::GroundTruthTrack track(WHISPER::SourceType::SHIP,*ID.get(),SimCore::TrackKind::GROUND_TRUTH_TRACK);
        track.setCourse(course);
        track.setSpeed(speed);
        track.setPosition(GeocentPos1(SimCore::X),GeocentPos1(SimCore::Y),GeocentPos1(SimCore::Z));

        if (track.getPostion() == pos) {
          testOperator = true;
        }
        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::GroundTruthTrack>(std::move(SimCore::GroundTruthTrack::unpack(*msg)));
        }

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

        SimCore::GroundTruthTrack trackDeserialized = SimCore::GroundTruthTrack::unpack(*msg);


      THEN("check if Track attributes are correct")
      {
        REQUIRE(testOperator == true);
        REQUIRE(track.getCourse() == course);
        REQUIRE(track.getSpeed() == speed);
        REQUIRE(track.getSpeedinKnots() == knots);
        REQUIRE(track.getPostion().getGeocentricPos() == pos.getGeocentricPos());

        REQUIRE(msg.get()->msgType_ == WHISPER::GROUND_TRUTH_TRACK);
        REQUIRE(trackPtr->getSpeed() == speed);
        REQUIRE(trackPtr->getPostion().getGeocentricPos() == GeocentPos1);
        REQUIRE(trackPtr->getIdentifier().getNumber() > 0);
        REQUIRE(trackDeserialized.getIdentifier().getNumber() > 0);





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