#include "SimCore/Messages/GroundThruthTrack.hpp"
#include "SimCore/Messages/Track.hpp"
#include "SimCore/Position.hpp"
#include "SimCore/SimCore.hpp"
#include "SimCore/UtilFunctions.hpp"
#include "WHISPER/Messages/Message.hpp"
#include <SimCore/Templates/Sensor.hpp>
#include <memory>

namespace SimCore {
      
   Sensor::Sensor(SimCore::Identifier OwnID, SimCore::Identifier ParentID, SimCore::SensorKinds SensorKind,std::uint32_t GroundTruthPort, std::uint32_t ParentPort,std::string ParentIPAddress):
    OwnID_(OwnID),ParentID_(ParentID),SensorKind_(SensorKind),GroundTruthPort_(GroundTruthPort),ParentPort_(ParentPort),ParentIPAddress_(ParentIPAddress)
    {

        std::string ownIP = SimCore::UtilFunctions::getOwnIP();
        auto ip = SimCore::UtilFunctions::explode(ownIP, '.');
        ip[3] = "255";
        LOG_S(INFO)<<SimCore::UtilFunctions::implode(ip,'.');
        incommingGroundThruthMessages = std::make_shared<WHISPER::threadSafeQueue<WHISPER::Message>>();
        outgoingGroundThruthMessages = std::make_shared<WHISPER::threadSafeQueue<WHISPER::Message>>();

        incommingParentMessages = std::make_shared<WHISPER::threadSafeQueue<WHISPER::Message>>();
        outgoingParentMessages = std::make_shared<WHISPER::threadSafeQueue<WHISPER::Message>>();

        incommingTrackMessages =  std::make_shared<WHISPER::threadSafeQueue<SimCore::GroundTruthTrack>>();
    

        GroundTruthUDPService_ =  std::make_shared<WHISPER::InternalUDPService>(OwnID.getParentNumber(),OwnID.getNumber(),WHISPER::SENSOR,GroundTruthPort_,SimCore::UtilFunctions::implode(ip,'.'),ownIP);    
        ParentUDPService_ =  std::make_shared<WHISPER::InternalUDPService>(OwnID.getParentNumber(),OwnID.getNumber(),WHISPER::SENSOR,ParentPort,ParentIPAddress_,ownIP);    


    };


    Sensor::~Sensor(){
        // this->stop();
        if (ReceivingGroundThruthIsRunnung && sendCalculatedDataIsRunnung && CalculationIsRunnung) {
        this->stop();
        }

        
        this->incommingGroundThruthMessages.reset();

       
        this->outgoingGroundThruthMessages.reset();


        // GroundTruthUDPService_->disconnect();
        while (!this->GroundTruthUDPService_.unique()) {
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
        this->GroundTruthUDPService_.reset();

        LOG_S(INFO)<<"groundThruth is closed";
        ParentUDPService_->disconnect();
        while (!this->ParentUDPService_.unique()) {
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
        this->ParentUDPService_.reset();


        LOG_S(INFO)<<"all destructed";

    }    

    void Sensor::start(){
        ReloadCharacteristicts();


    stopReceivingGroundThruth = false;
    receiveGroundTruthThread  = std::thread(&Sensor::groundThruthData,this);

    stopsendCalculatedData = false;
    sendCalculatedDataThread = std::thread(&Sensor::parentData,this);

    stopCalculationData = false;
    sensorCalculationThread = std::thread(&Sensor::SensorCalculations,this);
    }

    void Sensor::stop() {


        while (ReceivingGroundThruthIsRunnung == true ) {
            stopReceivingGroundThruth = true;
            LOG_S(INFO)<<"waiting for groundthruth thread thread";
            if (receiveGroundTruthThread.joinable() == true ) {
                    receiveGroundTruthThread.join();
            }
        }

        while (sendCalculatedDataIsRunnung == true ) {
            stopsendCalculatedData = true;
            LOG_S(INFO)<<"waiting for parent sending thread thread";
            if (sendCalculatedDataThread.joinable() == true ) {
                    sendCalculatedDataThread.join();
            }
        }

        while (CalculationIsRunnung == true ) {
            stopCalculationData = true;
            LOG_S(INFO)<<"waiting for calculation thread thread";
              if (sensorCalculationThread.joinable() == true ) {

                    sensorCalculationThread.join();
              }
        }
        LOG_S(INFO)<<"all stopped";

    }


    void Sensor::groundThruthData()
    {   
        this->ReceivingGroundThruthIsRunnung = true;
        GroundTruthUDPService_->connect(incommingGroundThruthMessages);
        GroundTruthUDPService_->subscribe(WHISPER::MsgTopicsMap[WHISPER::MsgTopics::TRACK]);

        while (stopReceivingGroundThruth == false) {
            if (incommingGroundThruthMessages->size() > 0) {
                WHISPER::Message msg; 
                incommingGroundThruthMessages->get(msg);
                if (msg.msgType_ == WHISPER::MsgType::GROUND_TRUTH_TRACK) {
                    auto elem = GroundTruthTrack::unpack(msg);
                //   auto Track = SimCore::Track(msg.serialize());
                  incommingTrackMessages->addElement(elem);
                }
            }   

            if (outgoingGroundThruthMessages->size() > 0) {
                 WHISPER::Message msg;
                 outgoingGroundThruthMessages->get(msg);
                GroundTruthUDPService_->publish(msg.serialize(), WHISPER::MsgTopicsMap[(WHISPER::MsgTopics)msg.topic_]);
            }     

        }
        GroundTruthUDPService_->disconnect();
        this->ReceivingGroundThruthIsRunnung = false;
    }

    void Sensor::parentData()
    {
        this->sendCalculatedDataIsRunnung = true;

        ParentUDPService_->connect(incommingParentMessages);
        ParentUDPService_->subscribe(WHISPER::MsgTopicsMap[WHISPER::MsgTopics::COMMANDS]);

        while (stopsendCalculatedData == false) {
            
            if (incommingParentMessages->size() > 0) {
                 WHISPER::Message msg; 
                 incommingParentMessages->get(msg);
                LOG_S(INFO)<< "Message received from Parent is" << msg.msgType_;
                std::uint32_t type = 0;
                    
                    if (msg.msgType_ == WHISPER::MsgType::OWN_TRACK) {
                        auto OwnTrack = SimCore::GroundTruthTrack::unpack(msg);
                        // SimCore::Track OwnTrack(msg.serialize());
                        auto tmpPos = OwnTrack.getPostion().getGeocentricPos();
                        if (this->ownShipPosition_ == nullptr) {
                            this->ownShipPosition_ = std::make_shared<SimCore::Position>(
                            tmpPos[SimCore::GeocentricPosition::X], 
                            tmpPos[SimCore::GeocentricPosition::Y], 
                            tmpPos[SimCore::GeocentricPosition::Z]);

                        }else {
                        this->ownShipPosition_->setGeocentricPos(
                            tmpPos[SimCore::GeocentricPosition::X], 
                            tmpPos[SimCore::GeocentricPosition::Y], 
                            tmpPos[SimCore::GeocentricPosition::Z]);
                        }
                    }
                    
                }   
                
            


            if (outgoingParentMessages->size() > 0) {
                WHISPER::Message msg;
                outgoingParentMessages->get(msg);
                ParentUDPService_->publish(msg.serialize(), WHISPER::MsgTopicsMap[(WHISPER::MsgTopics)msg.topic_]);
            }

        }

        this->sendCalculatedDataIsRunnung = false;
    }


    void Sensor::SensorCalculations()
    {   
        CalculationIsRunnung = true;
        specificSensorCalculations();

        while (!stopCalculationData) {
            specificSensorCalculations();
            std::this_thread::sleep_for(std::chrono::milliseconds(500));

        }
        LOG_S(INFO)<<"calculation is stopt";

        CalculationIsRunnung = false;
    }

    void Sensor::ReloadCharacteristicts()
    {

        specificReloadCharacteristicts();
    }



}