#include "Entities/Tracklist/Tracklist.hpp"
#include "Entities/Tracklist/TracklistItem.hpp"
#include "SimCore/IdentifierMaker.hpp"
#include "SimCore/Messages/RadarTrack.hpp"
#include "SimCore/Position.hpp"
#include "WHISPER/Messages/Message.hpp"
#include <SimCore/Identifier.hpp>
#include <SimCore/SimCore.hpp>
#include <memory>
#include <thread>
#define CATCH_CONFIG_MAIN
#include <catch2/catch.hpp>
#include <loguru.hpp>






SCENARIO("Testing the SimCore Sensor")
{
  GIVEN("different Attributes for a Track in different forms")
  {
   


    SimCore::Identifier OwnID(0,1,false);
    SimCore::IdentifierMaker IDMaker;
    auto RadarID = IDMaker.getNewIdentifier(1,SimCore::INTERNAL);
    auto TrackID1 = IDMaker.getNewIdentifier(1,SimCore::INTERNAL);
    LOG_S(INFO)<<TrackID1->serialize();

    TrackList::TrackList InternalTracklist(OwnID);
    TrackList::SensorData sensor1;
    sensor1.sensorID = *RadarID.get();
    sensor1.Sensorname = "TRS-3D";

    LOG_S(INFO)<<"making radar track";
    auto Track1 = std::make_shared<SimCore::RadarTrack>(WHISPER::SourceType::SENSOR,*TrackID1.get());

    SimCore::Position pos1 ;
    pos1.setGeodesicPos(55, 6, 0);
    Track1->setPosition(pos1);
    
    LOG_S(INFO)<<"adding radar track";

    InternalTracklist.addTrack(Track1, sensor1);


    LOG_S(INFO)<<"radar track added";




  
    WHEN("constructing Track Object with data")
    {
       
      THEN("check if Track attributes are correct")
      { 

        REQUIRE(InternalTracklist.size() == 1);
          REQUIRE(InternalTracklist.getTrack(Track1->getIdentifier())->getSpeed() == 0);

        LOG_S(INFO)<<"add same track again";
        
        Track1->setSpeed(10);

        InternalTracklist.addTrack(Track1, sensor1);
        REQUIRE(InternalTracklist.getTrack(Track1->getIdentifier())->getSpeed() == 10);

        REQUIRE(InternalTracklist.size() == 1);

        // std::this_thread::sleep_for(std::chrono::milliseconds(2000));   
        // REQUIRE(InternalTracklist.size() == 1);
        // std::this_thread::sleep_for(std::chrono::milliseconds(1000));   
        // REQUIRE(InternalTracklist.size() == 1);


        InternalTracklist.stopSanitizer();


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