#pragma once



#include "Entities/Movement.hpp"
#include "SimCore/Messages/GroundThruthTrack.hpp"
#include "SimCore/Messages/Track.hpp"
#include "SimCore/Orientation.hpp"
#include "SimCore/SafeMap.hpp"
#include "SimCore/SimCore.hpp"

#include <WHISPER/InternalUDPService.hpp>
#include <WHISPER/InternalUDPSender.hpp>
#include <WHISPER/InternalUDPListener.hpp>

#include <WHISPER/Messages/Message.hpp>
#include <WHISPER/threadSafeQueue.hpp>
#include <SimCore/Identifier.hpp>
#include <SimCore/Position.hpp>
#include <atomic>
#include <chrono>
#include <memory>
#include <string>
#include <thread>
#include <utility>


namespace Entities {

    struct SensorClientData
    { 
        std::string SensorName;
        bool isActive;
        SimCore::Identifier SensorID;
        std::shared_ptr<WHISPER::InternalUDPSender> SensorSender;
    };

    struct EffectorClientData
    { 
        std::string EffectorName;
        bool isActive;
        SimCore::Identifier EffectorID;
        std::shared_ptr<WHISPER::InternalUDPSender> EffectorSender;
    };



    class Entity {
        public: 
             Entity(const SimCore::Identifier OwnID,
                    std::string EnttityName,
                    WHISPER::SourceType OwnType,
                    SimCore::Identifier ParentID, 
                    SimCore::EntityKind EntityKind,
                    std::uint32_t GroundTruthPort, 
                    std::uint32_t CommandPort,
                    std::string CommandIPAddress);
             ~Entity();

            void start();
            void stop();

        protected:

            std::shared_ptr<WHISPER::threadSafeQueue<WHISPER::Message>> incommingCommandMessages = nullptr;
            std::shared_ptr<WHISPER::threadSafeQueue<WHISPER::Message>> outgoingCommandMessages = nullptr;

            virtual void specificPhysicsCalculations(std::chrono::milliseconds::rep duration) = 0;
            virtual void specificReloadCharacteristicts() = 0;

            std::unique_ptr<Entities::Movement> Movement_ = nullptr;

        protected:

            std::string EntityName_;
            SimCore::GroundTruthTrack ownTrack_;
            SimCore::Identifier ParentID_;
            SimCore::EntityKind EntityKind_;
            std::uint32_t GroundTruthPort_;
            std::uint32_t CommandPort_;
            std::string CommandIPAddress_;

        private:





            std::vector<std::thread> threads;

            
            std::atomic<bool> stopCommandWorker = false;
            std::atomic<bool> stopSensorWorker = false;
            std::atomic<bool> stopTrackWorker = false;
            std::atomic<bool> stopPhysicsWorker = false;


            std::atomic<bool> physicsIsRunning = false;


            void CommandWorker();
            void SensorWorker();
            void TrackWorker();
            void physicsWorker();


            void startSensor();


              
            std::shared_ptr<WHISPER::InternalUDPSender> GroundTruthUDPSender_ = nullptr;   

            std::shared_ptr<std::list<Entities::SensorClientData>> SensorStore_;

            std::shared_ptr<SimCore::SafeMap<SimCore::Identifier, std::shared_ptr<SimCore::Track>>> Trackstore_;


    };
}