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();

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

    uint32_t            seq()       const;
    const Time          &stamp()     const;
    const std::string   &frameId()   const;

    Time            &stamp();
    std::string     &frameId();

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

template <class HeaderType>
bool toJson(const HeaderType &header,
            rapidjson::Value &value,
            rapidjson::Document::AllocatorType &allocator)
{
    using namespace messages::std_msgs;

    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);

    return true;
}

template <class HeaderType>
bool fromJson(const rapidjson::Value &value,
              HeaderType &header)
{
    using namespace messages::std_msgs;

    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