#include "Entities/Movement.hpp"
#include "WHISPER/InternalUDPListener.hpp"
#include "WHISPER/InternalUDPSender.hpp"
#include "WHISPER/Messages/Message.hpp"
#include "WHISPER/Messages/stringData.hpp"
#include "WHISPER/threadSafeQueue.hpp"
#include <SimCore/SimCore.hpp>

#include <Entities/Entity.hpp>

#include <chrono>
#include <memory>
#include <thread>



#define calculationPeriode 200

namespace Entities
{

    Entity::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):
                   EntityName_(EnttityName),
                   ownTrack_(OwnType, OwnID, SimCore::TrackKind::GROUND_TRUTH_TRACK),
                   ParentID_(ParentID),
                   EntityKind_(EntityKind),
                   GroundTruthPort_(GroundTruthPort),
                   CommandPort_(CommandPort),
                   CommandIPAddress_(CommandIPAddress)
    {
    
    }


    Entity::~Entity()
    {
        stop();
    }

    void Entity::start()
    {   

        stopCommandWorker = false;
        stopSensorWorker = false;
        stopTrackWorker = false;
        stopPhysicsWorker = false;

        threads.emplace_back(std::thread(&Entity::CommandWorker,this));
        threads.emplace_back(std::thread(&Entity::SensorWorker,this));
        threads.emplace_back(std::thread(&Entity::TrackWorker,this));
        threads.emplace_back(std::thread(&Entity::physicsWorker,this));


    }

    void Entity::stop()
    {
        stopCommandWorker = true;
        stopSensorWorker = true;
        stopTrackWorker = true;
        stopPhysicsWorker = true;       
        // for (auto &th :threads)
        //     {
        //         if (th.joinable()) {
        //          th.join();
        //         }
                
        //     }


        for (std::vector<std::thread>::iterator it = threads.begin(); it != threads.end();)
        {
            if (it->joinable()) 
            {
                it->join();
                it = threads.erase(it);
            }
        
        }

        LOG_S(ERROR)<< threads.size();

    
    }

    void Entity::physicsWorker()
    {
    physicsIsRunning = true;

    while (!stopPhysicsWorker) {
    auto start =  std::chrono::steady_clock::now();
    

    std::this_thread::sleep_for(std::chrono::milliseconds(calculationPeriode));   


    auto end = std::chrono::steady_clock::now();
    std::chrono::milliseconds::rep duration =  std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();


    if(Movement_ != nullptr)
    {
    // Movement_->updatePosition(duration);
    Movement_->updatePositionSimple(duration);

    }


    specificPhysicsCalculations(duration);
    }    

    physicsIsRunning = false;
    }



    void Entity::CommandWorker()
    {
        auto CommandUDPListener = std::make_shared<WHISPER::InternalUDPListener>(CommandPort_) ;    
        auto receiverQueue = std::make_shared<WHISPER::threadSafeQueue<WHISPER::Message>>();
        CommandUDPListener->connect(receiverQueue);

        
        auto CommandUDPSender = std::make_shared<WHISPER::InternalUDPSender>(CommandIPAddress_,CommandPort_);    


        while (!stopCommandWorker) 
        {
            if (receiverQueue->size() > 0) {
                
                auto msg =  WHISPER::Message();
                receiverQueue->get(msg);

                switch (msg.msgType_) {
                
                    case WHISPER::MsgType::STRINGDATA :{
                        WHISPER::StringData stringMsg =  WHISPER::StringData(msg.serialize());
                        std::string str = stringMsg.data_;
                        break;
                    }

                    case WHISPER::MsgType::COMMAND: {
                        WHISPER::StringData string = WHISPER::StringData(msg.serialize());
                        break;
                    }
                }

            }


            LOG_S(INFO)<<"hello from command worker";
            std::this_thread::sleep_for(std::chrono::milliseconds(900));   

        }


    }

    void Entity::SensorWorker()
    {


        while (!stopSensorWorker) 
        {
            LOG_S(INFO)<<"hello from sensor worker";
            std::this_thread::sleep_for(std::chrono::milliseconds(900));   

        }

    }

    void Entity::TrackWorker()
    {
        while (!stopTrackWorker) 
        {
            LOG_S(INFO)<<"hello from track worker";
            std::this_thread::sleep_for(std::chrono::milliseconds(900));   

        }

    }

    void Entity::startSensor()
    {
        
        LOG_S(ERROR)<< "starting new pods is not implemented yet";
    }



}