EntityLibrary/src/Entities/Entity.cpp

#include "DirectCommunicationServer.hpp"
#include "Entities/Movement.hpp"
#include "Orders/MoveOrder.hpp"
#include "SimCore/Messages/SimTrack.hpp"
#include "SimCore/UtilFunctions.hpp"
#include "WHISPER/InternalUDPListener.hpp"
#include "WHISPER/InternalUDPSender.hpp"
#include "WHISPER/Messages/Message.hpp"
#include "WHISPER/Messages/stringData.hpp"
#include "WHISPER/threadSafeQueue.hpp"
#include "crossguid/guid.hpp"
#include "kubecontrol/KubePod.hpp"
#include "kubecontrol/PodController.hpp"
#include "yaml-cpp/node/parse.h"
#include <SimCore/SimCore.hpp>

#include <Entities/Entity.hpp>

#include <chrono>
#include <functional>
#include <memory>
#include <string>
#include <thread>



#define __MOVEMENT_SERVER_PORT__ 5556
#define __SENSOR_SERVER_PORT__ 5557



namespace Entities
{

    Entity::Entity(const SimCore::Identifier OwnID, 
                   std::string EnttityName,
                   WHISPER::SourceType OwnType,
                   SimCore::EntityKind EntityKind,
                   std::uint32_t GroundTruthPort, 
                   ushort CommandPort,
                   bool online):
                   EntityName_(EnttityName),
                   EntityKind_(EntityKind),
                   online_(online)
                 
    {
        PodController_ = std::make_unique<kubecontrol::PodController>("docs/config");
        OwnShipTrack = std::make_shared<SimCore::SimTrack>(OwnID, OwnType, EntityKind);
        OwnShipTrack->setPosition(SimCore::Position());

        MovemtServer_ =  std::make_shared<DirectCommunication::DirectCommunicationServer>(__MOVEMENT_SERVER_PORT__);

        CommandCommsServer_ = std::make_shared<DirectCommunication::DirectCommunicationServer>(CommandPort);

        CommandCommsServer_->registerMessageCallback(std::bind(&Entity::handleExternalComms,this,std::placeholders::_1));
    }


    Entity::~Entity()
    {
        PodController_->stopAllPods();
        stop();
        MovemtServer_.reset();
    }



    void Entity::setPosition(SimCore::Position pos)
    {
        OwnShipTrack->setPosition(pos);
        LOG_S(INFO)<< "POS: LAT: "<< OwnShipTrack->getPosition().getGeodesicPos()(SimCore::LATITUDE) << " LON: " << OwnShipTrack->getPosition().getGeodesicPos()(SimCore::LONGITUDE);

        Orders::MoveOrder moveorder(OwnShipTrack->getIdentifier(),WHISPER::SourceType::ENTITY);
        moveorder.setPosition(pos);
        if(MovementWorkerStarted == true)
        {
            MovemtServer_->sendMessage(moveorder.buildMessage().serialize());
            LOG_S(INFO)<<"Move Order send";
        }
    }
    void Entity::setSpeed(double val)
    {
        OwnShipTrack->Speed.setValue(val);
        Orders::MoveOrder moveorder(OwnShipTrack->getIdentifier(),WHISPER::SourceType::ENTITY);
        moveorder.Speed.setValue(val);
        if(MovementWorkerStarted == true)
        {
            MovemtServer_->sendMessage(moveorder.buildMessage().serialize());
            LOG_S(INFO)<<"Move Order send with Speed";
        }
    }
    void Entity::setCourse(double val)
    {
        OwnShipTrack->Course.setValue(val);
        Orders::MoveOrder moveorder(OwnShipTrack->getIdentifier(),WHISPER::SourceType::ENTITY);
        moveorder.Course.setValue(val);
        if(MovementWorkerStarted == true)
        {
            MovemtServer_->sendMessage(moveorder.buildMessage().serialize());
            LOG_S(INFO)<<"Move Order send with Course";
        }
    }
    void Entity::setPitch( double val)
    {
        OwnShipTrack->Pitch.setValue(val);
        

    }

    void Entity::start()
    {   

        stopMainLoop = false;
   

        threads.emplace_back(std::thread(&Entity::MainLoop,this));
       


    }

    void Entity::stop()
    {
        stopMainLoop = true;
       


        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::startMovementWorker()
    {   
        if (online_ == true) {
            LOG_S(INFO)<<"__ONLINE__";
            auto MovementPod = kubecontrol::KubePod("movmentpod-"+EntityName_,xg::newGuid().str(),"movementimage:latest");
            MovementPod.setEnvironmentVar("SERVER_IP", SimCore::UtilFunctions::getOwnIP());
            LOG_S(INFO)<< MovementPod.createYAML();
            PodController_->startPod(MovementPod);
            LOG_S(INFO)<<PodController_->getServerAddress();
            std::this_thread::sleep_for(std::chrono::milliseconds(2000));
        }else {
        
            LOG_S(INFO)<< "Wait for MomentApp";
        }


        if (MovemtServer_->countClients() > 0 ) 
        {
           if(OwnShipTrack->getPosition().isValid())
           {
                LOG_S(INFO)<< "POS: LAT: "<< OwnShipTrack->getPosition().getGeodesicPos()(SimCore::LATITUDE) << " LON: " << OwnShipTrack->getPosition().getGeodesicPos()(SimCore::LONGITUDE);

                MovementWorkerStarted = true;
                MovemtServer_->sendMessage(OwnShipTrack->buildMessage().serialize());
                LOG_S(INFO)<< "Initial Message send to MovementWorker";
           }

        }
    }

    void Entity::handleMovement()
    {
        if (!MovementWorkerStarted) 
            {
                startMovementWorker();
            }else 
            {
                std::string msg = MovemtServer_->getLatestMessage();
                auto newTrack = SimCore::SimTrack::unpack(msg);
                if (newTrack != nullptr) 
                {
                    OwnShipTrack->setPosition(newTrack->getPosition());
                    // LOG_S(INFO)<< "new POS: LAT: "<< OwnShipTrack->getPosition().getGeodesicPos()(SimCore::LATITUDE) << " LON: " << OwnShipTrack->getPosition().getGeodesicPos()(SimCore::LONGITUDE);
                }
            }

            if (MovemtServer_->countClients() == 0) 
            {
            MovementWorkerStarted = false;
            }
    }

    void Entity::handleExternalComms(std::string msg)
    {
    //    LOG_S(INFO)<<msg;
       auto MoveOrder = Orders::MoveOrder::unpack(msg);

       if (MoveOrder != nullptr) 
       {
            if(MoveOrder->Speed.isValid()) this->setSpeed(MoveOrder->Speed.getValue());
            if(MoveOrder->Course.isValid()) this->setCourse(MoveOrder->Course.getValue());
            if(MoveOrder->getPosition().isValid())
            {
                LOG_S(INFO)<<"New Position received";

                this->setPosition(MoveOrder->getPosition());
            }
            // CommandCommsServer_->sendMessage("Hello Client");
       } 

      
       CommandCommsServer_->sendMessage(this->OwnShipTrack->buildMessage().serialize());

    }




    void Entity::MainLoop()
    {
        LOG_S(INFO)<< "main loop started";
        while (!stopMainLoop) 
        {   
           handleMovement();
           childWorker();

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

        }


    }



   



}