#include <SimCore/Identifier.hpp>
#include <SimCore/Messages/Protos/GroundTruthTrack.pb.h>
#include <SimCore/Messages/Track.hpp>
#include <SimCore/SimCore.hpp>
#include <WHISPER/Messages/Message.hpp>
#include <google/protobuf/message.h>
#include <SimCore/Messages/GroundThruthTrack.hpp>




namespace SimCore 
{
    GroundTruthTrack::GroundTruthTrack(WHISPER::SourceType src,SimCore::Identifier id,SimCore::TrackKind kind):Track(src, id, kind)
    {

    }

    WHISPER::Message GroundTruthTrack::buildMessage(SimCore::Identifier parentID)
    {
    WHISPER::Message msg(parentID.getParentNumber(), parentID.getNumber(),WHISPER::MsgTopics::TRACK , WHISPER::MsgType::GROUND_TRUTH_TRACK, WHISPER::SourceType::GATEWAY);


    messages::track::GroundTruthTrack groundTruthTrack;

    groundTruthTrack.mutable_geocentricposition()->set_x(position_.getGeocentricPos()(SimCore::X));
    groundTruthTrack.mutable_geocentricposition()->set_y(position_.getGeocentricPos()(SimCore::Y));
    groundTruthTrack.mutable_geocentricposition()->set_z(position_.getGeocentricPos()(SimCore::Z));

    groundTruthTrack.set_contactspeed(speed_);
    groundTruthTrack.set_contactcourse(course_);
    
    

    groundTruthTrack.mutable_entityidentifier()->set_number(this->getIdentifier().getNumber());
    groundTruthTrack.mutable_entityidentifier()->set_external((uint32_t)this->getIdentifier().isExternal());
    groundTruthTrack.mutable_entityidentifier()->set_parent(this->getIdentifier().getParentNumber());

    
    groundTruthTrack.mutable_timestamp()->set_seconds(time(NULL));

    auto any = std::make_shared<google::protobuf::Any>();

    any->PackFrom(groundTruthTrack);
    msg.addPayLoad(any);

        return msg;
    }

    void GroundTruthTrack::setPosition(Position pos) {
        position_ = pos;
    }

    void GroundTruthTrack::setPosition(double x, double y, double z) {
        Position pos(x, y, z);
        position_ = pos;
    }

    Position GroundTruthTrack::getPostion() { return position_; }

    void GroundTruthTrack::setSpeed(double speed) {
        speed_ = speed;
    }
    void GroundTruthTrack::setCourse(double course) {
        course_ = course;
    }

    void GroundTruthTrack::setSpeedinKnots(double knots) {
        speed_ = knots / SimCore::MsKt;
    }

    void GroundTruthTrack::setEnvironment(SimCore::EntityKind env ) 
    {
        environemnt_ = env;
    }

    double GroundTruthTrack::getSpeed() { return speed_; }
    double GroundTruthTrack::getSpeedinKnots() { return speed_ * SimCore::MsKt; }
    double GroundTruthTrack::getCourse() { return course_; }
    SimCore::EntityKind GroundTruthTrack::getEnvironment(){ return environemnt_; }




    GroundTruthTrack GroundTruthTrack::unpack(WHISPER::Message msg)
    {   
     auto m =   msg.getProtoMessage();
     
     auto trackMsg = messages::track::GroundTruthTrack();
     if(m.payload().Is<messages::track::GroundTruthTrack>())
     {
        m.payload().UnpackTo(&trackMsg);
     }       
     SimCore::Identifier id(trackMsg.mutable_entityidentifier()->parent(),trackMsg.mutable_entityidentifier()->number(),trackMsg.mutable_entityidentifier()->external());
     GroundTruthTrack track((WHISPER::SourceType)m.sourcetype(), id,(SimCore::TrackKind)trackMsg.trackkind());
     if (trackMsg.has_geocentricposition()) 
     {
        track.setPosition(trackMsg.mutable_geocentricposition()->x(), trackMsg.mutable_geocentricposition()->y(), trackMsg.mutable_geocentricposition()->z());
     }
     if (trackMsg.has_contactcourse()) 
     {
        track.setCourse(trackMsg.contactcourse());
     }
     if (trackMsg.has_contactspeed()) 
     {
        track.setSpeed(trackMsg.contactspeed());
     }
     

     return track;
    }

}