header.h

#pragma once

#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/include/messages/std_msgs/time.h"

#include <string>

namespace ros_bridge {
//! @brief Namespace containing classes and methodes ros message generation.
namespace messages {
//! @brief Namespace containing classes and methodes for std_msgs generation.
namespace std_msgs {
//! @brief Namespace containing classes and methodes for std_msgs/Header message generation.
namespace header {


std::string messageType(){
    return "std_msgs/Header";
}

//! @brief C++ representation of std_msgs/Header
class Header{
public:
    using Time = std_msgs::time::Time;
    Header() : _seq(_defaultSeq++), _stamp(Time()), _frameId("") {}
    Header(uint32_t seq, const Time &stamp, const std::string &frame_id) :
        _seq(seq)
      , _stamp(stamp)
      , _frameId(frame_id) {}

    Header(const Header &header) :
        _seq(header.seq())
      , _stamp(header.stamp())
      , _frameId(header.frameId()) {}

    uint32_t            seq()       const {return _seq;}
    const Time          &stamp()     const {return _stamp;}
    const std::string   &frameId()   const {return _frameId;}

    Time            &stamp()         {return _stamp;}
    std::string     &frameId()       {return _frameId;}

    void setSeq     (uint32_t seq)                  {_seq = seq;}
    void setStamp   (const Time &stamp)             {_stamp = stamp;}
    void setFrameId (const std::string  &frameId)   {_frameId = frameId;}
private:
    static uint32_t _defaultSeq = 0;
    uint32_t    _seq;
    Time        _stamp;
    std::string _frameId;
};

template <class HeaderType>
bool toJson(const HeaderType &header,
            rapidjson::Value &value,
            rapidjson::Document::AllocatorType &allocator)     {
    value.AddMember("seq", rapidjson::Value().SetUint(uint32_t(header.seq())), allocator);

    rapidjson::Value stamp(rapidjson::kObjectType);
    if (!Time::toJson(header.stamp(), stamp, allocator)){
        assert(false);
        return false;
    }
    value.AddMember("stamp", stamp, allocator);

    value.AddMember("frame_id",
                  rapidjson::Value().SetString(header.frameId().data(),
                                               header.frameId().length(),
                                               allocator),
                  allocator);
    value.AddMember("type", rapidjson::Value().SetString(messageType().c_str()), allocator);

    return true;
}

template <class HeaderType>
bool fromJson(const rapidjson::Value &value,
              HeaderType &header)     {
    if (!value.HasMember("seq")|| !value["seq"].IsUint()){
        assert(false);
        return false;
    }
    if (!value.HasMember("stamp")){
        assert(false);
        return false;
    }
    if (!value.HasMember("frame_id")|| !value["frame_id"].IsString()){
        assert(false);
        return false;
    }
    header.setSeq(value["seq"].GetUint());
    decltype(header.stamp()) time;
    if (!Time::fromJson(value["stamp"], time)){
        assert(false);
        return false;
    }
    header.setStamp(time);
    header.setFrameId(value["frame_id"].GetString());

    return true;
}

} // namespace time
} // namespace std_msgs
} // namespace messages
} // namespace ros_bridge