#include "SimCore/Identifier.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;

  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("TEST",SimCore::Kind::EntityKind::LAND, SimCore::Side::FRIEND);
        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().serialize();

         msg = std::make_shared<WHISPER::Message>(serializedMSG);

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

        auto tmp = SimCore::SimTrack::unpack(serializedMSG);

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

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


        std::shared_ptr<SimCore::Emission> esm=  track.getEmission(emi1.getID());
        esm->packEmission();

      THEN("check if Track attributes are correct")
      {
        REQUIRE(tmp->getIdentifier().getUUID() == track.getIdentifier().getUUID());

        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::SIM_TRACK);
        REQUIRE(trackPtr->Speed.getValue() == speed);
        REQUIRE(trackPtr->getPosition().getGeocentricPos() == GeocentPos1);

        REQUIRE(trackDeserialized.getEmission(emi1.getID()).get()->getID() == idEmi);
        REQUIRE(trackDeserialized.Name.getValue() == "TEST");




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