#include "WHISPER/Messages/Message.hpp"
#include <SimCore/Messages/SimTrack.hpp>
#include <iterator>
#include <memory>
#include <utility>

/// @brief 
namespace SimCore
{
    SimTrack::SimTrack()
    :ID_(SimCore::Identifier()),SourceType_(WHISPER::SourceType::SIMCOMTROLER)
    {
      EntityKind.setValue(SimCore::EntityKind::UNKNOWN);
      
      position_ = SimCore::Position();
      Speed.setValue(0);
      Course.setValue(0);
      Pitch.setValue(0);
      RCS.setValue(0);
    }

    SimTrack::SimTrack(WHISPER::SourceType sourcetype,SimCore::EntityKind entityKind)
    :ID_(SimCore::Identifier()),SourceType_(sourcetype)
    {
      EntityKind.setValue(entityKind);   
      
      position_ = SimCore::Position();
      Speed.setValue(0);
      Course.setValue(0);
      Pitch.setValue(0);
      RCS.setValue(0);
    }
    
    SimTrack::SimTrack(SimCore::Identifier id ,WHISPER::SourceType sourcetype,SimCore::EntityKind entityKind )
    :ID_(id),SourceType_(sourcetype)
    {
      EntityKind.setValue(entityKind);

      position_ = SimCore::Position();
      Speed.setValue(0);
      Course.setValue(0);
      Pitch.setValue(0);
      RCS.setValue(0);
    }


    SimTrack::~SimTrack()
    {
    }

    const SimCore::Identifier SimTrack::getIdentifier()
    {
        return ID_;
    }



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

     void SimTrack::setPosition(double x, double y, double z) {
        Position pos(x, y, z);
        position_ = pos;
    }
    
    SimCore::Position SimTrack::getPosition()
    {
        return position_;
    }


   WHISPER::Message SimTrack::buildMessage()
   {
    WHISPER::Message msg(ID_.getUUID(),WHISPER::MsgTopics::TRACK , WHISPER::MsgType::SIM_TRACK, this->SourceType_);


    messages::SimTrack::SimTrack SimTrack;

   if (position_.isValid())
   {
    SimTrack.mutable_geocentricposition()->set_x(position_.getGeocentricPos()(SimCore::X));
    SimTrack.mutable_geocentricposition()->set_y(position_.getGeocentricPos()(SimCore::Y));
    SimTrack.mutable_geocentricposition()->set_z(position_.getGeocentricPos()(SimCore::Z));
   }

    if(Speed.isValid()) SimTrack.set_contactspeed(this->Speed.getValue());
    if(Course.isValid()) SimTrack.set_contactcourse(this->Course.getValue());
    if(Pitch.isValid()) SimTrack.set_conactpitch(this->Pitch.getValue());
    if(RCS.isValid()) SimTrack.set_contactrcs(this->RCS.getValue());
    

    SimTrack.mutable_entityidentifier()->set_external((uint32_t)this->ID_.isExternal());
    SimTrack.mutable_entityidentifier()->set_uuid(this->ID_.getUUID());

      std::vector<std::shared_ptr<Emission>>::iterator it;
     for( it = EmissionList.begin(); it != EmissionList.end(); it++)
      {
         
        auto emissionMsg = it->get()->packEmission();
        SimTrack.add_contactemissions()->CopyFrom(emissionMsg);
         
      }
    // SimTrack.mutable_timestamp()->set_seconds(time(NULL));

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

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

        return msg;
   }
   



   SimTrack SimTrack::unpack(WHISPER::Message msg)
   {

    auto m =   msg.getProtoMessage();
    
    auto trackMsg = messages::SimTrack::SimTrack();
     if(m.payload().Is<messages::SimTrack::SimTrack>())
     {
        m.payload().UnpackTo(&trackMsg);
     }  
    
    SimCore::Identifier id(trackMsg.mutable_entityidentifier());
    

    WHISPER::SourceType sourcetype = (WHISPER::SourceType)trackMsg.contactsourcetype();
    SimCore::EntityKind entityKind = (SimCore::EntityKind)trackMsg.contactenvironemnt();
    
    SimCore::SimTrack track(id,sourcetype,entityKind);


    if (trackMsg.has_geocentricposition()) 
     {
        track.setPosition(trackMsg.mutable_geocentricposition()->x(), trackMsg.mutable_geocentricposition()->y(), trackMsg.mutable_geocentricposition()->z());
     }

     if (trackMsg.has_contactcourse()) 
     {
        track.Course.setValue(trackMsg.contactcourse());
     }

     if (trackMsg.has_contactspeed()) 
     {
        track.Speed.setValue(trackMsg.contactspeed());
     }

     if (trackMsg.has_conactpitch()) 
     {
        track.Pitch.setValue(trackMsg.contactspeed());
     }

      if (trackMsg.has_contactrcs()) 
     {
        track.RCS.setValue(trackMsg.contactrcs());
     }else
     {
      switch (entityKind)
      {
      case SimCore::EntityKind::AIR: 
      {
          track.RCS.setValue(RCS_AIR);

         break;
      }
         case SimCore::EntityKind::LAND: 
      {
         track.RCS.setValue(RCS_LAND);
         break;
      }
      case SimCore::EntityKind::SURFACE: 
      {
          track.RCS.setValue(RCS_SURFACE);
         break;
      }
      case SimCore::EntityKind::SPACE: 
      {
          track.RCS.setValue(RCS_SPACE);
         break;
      }
      
      default:
      {
         track.RCS.setValue(RCS_DEFAULT);
         break;
      }
      }

     }
   
      for (int i = 0; i < trackMsg.contactemissions_size(); i++)
      {
         const messages::SimTrack::Emission* emission = trackMsg.mutable_contactemissions(i);
         auto obj = SimCore::Emission::unpackEmission(*emission);
         track.addEmission(obj);
      }
      

    return track;
   }


 
   std::unique_ptr<SimCore::SimTrack> SimTrack::unpack(std::string msgString)
   {
      if (std::empty(msgString) == false &&  msgString != "NULL") 
      {
         WHISPER::Message whisperMessage(msgString);
         return std::make_unique<SimTrack>(std::move(unpack(whisperMessage)));
      }

      return nullptr;
   }




    void SimTrack::addEmission(Emission emission)
    {
      auto tmp = std::make_shared<Emission>(std::move(emission));
      this->EmissionList.push_back(std::move(tmp));
    }

   std::shared_ptr<Emission> SimTrack::getEmission(SimCore::Identifier id)
   {
      for(auto iterator = this->EmissionList.begin(); iterator != this->EmissionList.end(); iterator++)
      {
         
         if (iterator->get()->getID()== id)
         {
            return *iterator;
         }
         
      }
      return nullptr;
   
   }

   size_t SimTrack::coutEmissions()
   {
      return EmissionList.size();
   }


} // namespace SimCore
