#include "SimCore/Identifier.hpp"
#include "SimCore/IdentifierMaker.hpp"
#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;
    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;
    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::SimTrack track("TEST",SimCore::Kind::SURFACE,SimCore::Side::FRIEND);
        track.Course.setValue(course);
        track.Speed.setValue(speed);
        track.setPosition(GeocentPos1(SimCore::X),GeocentPos1(SimCore::Y),GeocentPos1(SimCore::Z));

        if (track.getPosition() == pos) {
          testOperator = true;
        }
        serializedMSG = track.buildMessage().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().serialize();

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


      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(msg.get()->msgType_ == WHISPER::GROUND_TRUTH_TRACK);
        REQUIRE(trackPtr->Speed.getValue() == speed);
        REQUIRE(trackPtr->getPosition().getGeocentricPos() == GeocentPos1);





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