#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") {
    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(SimCore::ObjectSource::INTERNAL);
        LOG_S(INFO)<<"ID: " << ID->getNumber();

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

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

        bool isEqual = false; 
        WHEN("constructing Track Object with data") {
          std::string serializedMSG = RadartrackPtr->serialize();

          SimCore::RadarTrack radarTrack2(serializedMSG);

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

            LOG_S(INFO)<<"ID2 source: " << ID2.getObjectSource();

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


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

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