#include <SimCore/Identifier.hpp>
#include <SimCore/Messages/Protos/RadarTrack.pb.h>
#include <SimCore/Messages/Protos/SensorTrack.pb.h>

#include <SimCore/Messages/SensorTrack.hpp>
#include <SimCore/Position.hpp>
#include <SimCore/SimCore.hpp>
#include <WHISPER/Messages/Message.hpp>
#include <google/protobuf/any.pb.h>
#include <google/protobuf/message.h>
#include <google/protobuf/timestamp.pb.h>
#include <SimCore/Messages/Protos/Track.pb.h>
#include <SimCore/Messages/RadarTrack.hpp>
#include <cstdint>
#include <memory>
#include <sys/time.h>
#include <utility>

namespace SimCore {



RadarTrack::RadarTrack( WHISPER::SourceType src,SimCore::Identifier id)
    : Track(src,id,SimCore::TrackKind::RADAR_TRACK) 
    {

  
  }

WHISPER::Message RadarTrack::buildMessage(SimCore::Identifier parentID)
{
  WHISPER::Message msg(parentID.getParentNumber(), parentID.getNumber(),WHISPER::MsgTopics::TRACK , WHISPER::MsgType::RADAR_TRACK, WHISPER::SourceType::SENSOR);

  messages::track::RadarTrack radarTrack;

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

  radarTrack.set_contactspeed(speed_);
  radarTrack.set_contactcourse(course_);
  radarTrack.set_contactbearing(bearing_);
  radarTrack.set_contactrange(range_);
  radarTrack.set_contactenvironemnt((uint32_t)environemnt_);
  
  messages::track::SensorTrack SensorTrack;

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

  SensorTrack.mutable_radartrack()->CopyFrom(radarTrack);
  SensorTrack.mutable_timestamp()->set_seconds(time(NULL));

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

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

return msg;
}

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

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

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

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

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

void RadarTrack::setBearing(double bearing) {
  bearing_ = bearing; 
}

void RadarTrack::setRange(double range) {
  range_ = range;
}

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



double RadarTrack::getSpeed() { return speed_; }
double RadarTrack::getSpeedinKnots() { return speed_ * SimCore::MsKt; }
double RadarTrack::getCourse() { return course_; }
double RadarTrack::getBearing() { return bearing_; }
double RadarTrack::getRange() { return range_; }
SimCore::EntityKind RadarTrack::getEnvironment(){ return environemnt_; }

RadarTrack RadarTrack::unpack(WHISPER::Message msg)
{
  auto m =   msg.getProtoMessage();
     
     auto SensortrackMsg = messages::track::SensorTrack();
     if(m.payload().Is<messages::track::SensorTrack>())
     {
        m.payload().UnpackTo(&SensortrackMsg);
     }       

     SimCore::Identifier id(SensortrackMsg.mutable_entityidentifier()->parent(),SensortrackMsg.mutable_entityidentifier()->number(),SensortrackMsg.mutable_entityidentifier()->external());
     RadarTrack track((WHISPER::SourceType)m.sourcetype(), id);
     
     if (SensortrackMsg.has_radartrack()) 
     {
        auto radarTrack = SensortrackMsg.radartrack();

        track.setPosition(radarTrack.mutable_geocentricposition()->x(), radarTrack.mutable_geocentricposition()->y(), radarTrack.mutable_geocentricposition()->z());
        track.setCourse(radarTrack.contactcourse());
        track.setSpeed(radarTrack.contactspeed());
        track.setBearing(radarTrack.contactbearing());
        track.setEnvironment((SimCore::EntityKind)radarTrack.contactenvironemnt());
     
     
     }
     

     return track;
}

} // namespace SimCore