#include "SimCore/Identifier.hpp"
#include "SimCore/IdentifierMaker.hpp"
#include "SimCore/Messages/RadarTrack.hpp"
#include <SimCore/Messages/Track.hpp>
#include <SimCore/SimCore.hpp>
#include <WHISPER/Messages/Message.hpp>
#include <memory>
#define CATCH_CONFIG_MAIN
#include <SimCore/Position.hpp>
#include <catch2/catch.hpp>

SCENARIO("Testing the SimCore Track") {
  GIVEN("different Attributes for a Track in different forms") {
    SimCore::Identifier ParentID(0,1,false);
    Eigen::Vector3d GeocentPos1;
    GeocentPos1(SimCore::GeocentricPosition::X) = 3784014.333;
    GeocentPos1(SimCore::GeocentricPosition::Y) = 899869.779;
    GeocentPos1(SimCore::GeocentricPosition::Z) = 5037960.502;

    SimCore::Position pos(3784014.333, 899869.779, 5037960.502);

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

        SimCore::IdentifierMaker IDMaker;
        std::shared_ptr<SimCore::Identifier> ID =
            IDMaker.getNewIdentifier(ParentID.getNumber(),SimCore::ObjectSource::INTERNAL);
        LOG_S(INFO)<<"ID: " << ID->getNumber();

        std::shared_ptr<SimCore::RadarTrack> RadartrackPtr =
            std::make_shared<SimCore::RadarTrack>( WHISPER::SourceType::SENSOR,  *ID.get());

        RadartrackPtr->setPosition(pos);
        RadartrackPtr->setBearing(90);
        RadartrackPtr->setCourse(360);
        RadartrackPtr->setRange(1000);
        RadartrackPtr->setEnvironment(SimCore::EntityKind::SURFACE);

        bool isEqual = false; 
        WHEN("constructing Track Object with data") {

          std::string serializedMSG = RadartrackPtr->buildMessage(ParentID).serialize();

          // SimCore::RadarTrack radarTrack2(serializedMSG);

          //   SimCore::Identifier ID1(radarTrack2.getID());
            
          //   auto tr0 =   *RadartrackPtr.get();
          //   SimCore::Identifier ID2(tr0.getID());

          //   LOG_S(INFO)<<"ID21 source: " << ID1.isExternal();

          //   LOG_S(INFO)<<"ID2 source: " << ID2.isExternal();

          // if (ID1 == ID2) {
          //       isEqual = true;
          // }


          THEN("check if Track attributes are correct") {
            // REQUIRE(RadartrackPtr->getID().getNumber() == 1);
            // REQUIRE(isEqual == true);
            // REQUIRE(radarTrack2.getID() == RadartrackPtr->getID());
            // REQUIRE(radarTrack2.getID().getNumber() == RadartrackPtr->getID().getNumber());
            // REQUIRE(radarTrack2.getPostion().getGeocentricPos() ==  RadartrackPtr->getPostion().getGeocentricPos());

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