JsonMethodes.h

#ifndef MESSAGES_H
#define MESSAGES_H

#include <iostream>
#include <vector>
#include <boost/type_traits/remove_cv.hpp>
#include <boost/type_traits/remove_reference.hpp>

#include "ros_bridge/rapidjson/include/rapidjson/rapidjson.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"

#include "utilities.h"
#include "MessageTraits.h"
#include <ostream>

namespace ROSBridge {

//! @brief Namespace containing methodes for Json generation.
namespace JsonMethodes {

//! @brief Namespace containing methodes for std_msgs generation.
namespace StdMsgs {
//! @brief Namespace containing methodes for std_msgs/Time message generation.
namespace Time {    
    template<class TimeType>
    bool toJson(const TimeType &time,
                rapidjson::Value &value,
                rapidjson::Document::AllocatorType &allocator)
    {
        value.AddMember("secs", rapidjson::Value().SetUint(uint32_t(time.secs())), allocator);
        value.AddMember("nsecs", rapidjson::Value().SetUint(uint32_t(time.nSecs())), allocator);

        return true;
    }

    template<class TimeType>
    bool fromJson(const rapidjson::Value &value,
                  TimeType &time)
    {
        if (!value.HasMember("secs") || !value["secs"].IsUint()){
            assert(false);
            return false;
        }
        if (!value.HasMember("nsecs")|| !value["nsecs"].IsUint()){
            assert(false);
            return false;
        }
        time.setSecs(value["secs"].GetUint());
        time.setNSecs(value["nsecs"].GetUint());

        return true;
    }

} // Time

//! @brief Namespace containing methodes for std_msgs/Header message generation.
namespace Header {

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

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

} // Header
} // StdMsgs


//! @brief Namespace containing methodes for geometry_msgs generation.
namespace GeometryMsgs {
//! @brief Namespace containing methodes for geometry_msgs/Point32 message generation.
namespace Point32 {
using namespace ROSBridge::traits;

        namespace det { //detail

            template <class T>
            auto getZ(const T &p, Type2Type<Has3Components>)
            {
                return p.z();
            }

            template <class T>
            auto getZ(const T &p, Type2Type<Has2Components>)
            {
                (void)p;
                return 0.0; // p has no member z() -> add 0.
            }

            template <class T, typename V>
            bool setZ(const rapidjson::Value &doc, const T &p, Type2Type<V>)
            {
                p.setZ(doc["z"].GetFloat());
                return true;
            }

            template <class T>
            bool setZ(const rapidjson::Value &doc, const T &p, Type2Type<Has2Components>)
            {
                (void)doc;
                (void)p;
                return true;
            }
        }

        template <class T>
        bool toJson(const T&p, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
        {
            value.AddMember("x", rapidjson::Value().SetFloat(p.x()), allocator);
            value.AddMember("y", rapidjson::Value().SetFloat(p.y()), allocator);

            typedef typename Select<HasMemberZ<T>::value, Has3Components, Has2Components>::Result
                    Components; // Check if PointType has 2 or 3 dimensions.
            auto z = det::getZ(p, Type2Type<Components>()); // If T has no member z() replace it by 0.

            value.AddMember("z", rapidjson::Value().SetFloat(z), allocator);

            return true;
        }

        template <class PointType>
        bool fromJson(const rapidjson::Value &value,
                      PointType &p)     {
            if (!value.HasMember("x") || !value["x"].IsFloat()){
                assert(false);
                return false;
            }
            if (!value.HasMember("y") || !value["y"].IsFloat()){
                assert(false);
                return false;
            }
            if (!value.HasMember("z") || !value["z"].IsFloat()){
                assert(false);
                return false;
            }

            p.setX(value["x"].GetFloat());
            p.setY(value["y"].GetFloat());
            typedef typename Select<HasMemberSetZ<PointType>::value, Has3Components, Has2Components>::Result
                    Components; // Check if PointType has 2 or 3 dimensions.
            (void)det::setZ(value["z"], p,  Type2Type<Components>()); // If PointType has no member z() discard doc["z"].

            return true;
        }

} // Point32

//! @brief Namespace containing methodes for geometry_msgs/Polygon message generation.
namespace Polygon {

template <class PolygonType>
bool toJson(const PolygonType &poly, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
{
    rapidjson::Value points(rapidjson::kArrayType);

    for(unsigned long i=0; i < std::uint64_t(poly.points().size()); ++i) {
        rapidjson::Document point(rapidjson::kObjectType);
        if ( !Point32::toJson(poly.points()[i], point, allocator) ){
            assert(false);
            return false;
        }
        points.PushBack(point, allocator);
    }

    value.AddMember("points", points, allocator);

    return true;
}

template <class PolygonType>
bool fromJson(const rapidjson::Value &value, PolygonType &poly)
{
    if (!value.HasMember("points") || !value["points"].IsArray()){
        assert(false);
        return false;
    }
    const auto &jsonArray = value["points"].GetArray();
    poly.points().clear();
    poly.points().reserve(jsonArray.Size());
    typedef decltype (poly.points()[0]) PointTypeCVR;
    typedef typename boost::remove_cv<typename boost::remove_reference<PointTypeCVR>::type>::type PointType;
    for (long i=0; i < jsonArray.Size(); ++i){
        PointType pt;
        if ( !Point32::fromJson(jsonArray[i], pt) ){
            assert(false);
            return false;
        }
        poly.points().push_back(std::move(pt));
    }
    return true;
}
} // namespace Polygon


//! @brief Namespace containing methodes for geometry_msgs/PolygonStamped message generation.
namespace PolygonStamped {
using namespace ROSBridge::JsonMethodes::StdMsgs;


template <class PolyStamped>
bool toJson(const PolyStamped &polyStamped, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
{
    return toJson(polyStamped.polygon(), polyStamped.header(), value, allocator);
}

template <class PolygonType, class HeaderType>
bool toJson(const PolygonType &poly, const HeaderType &h, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
{
    rapidjson::Document header(rapidjson::kObjectType);
    if (!Header::toJson(h, header, allocator)){
        assert(false);
        return false;
    }
    rapidjson::Document polygon(rapidjson::kObjectType);
    if (!Polygon::toJson(poly, polygon, allocator)){
        assert(false);
        return false;
    }
    value.AddMember("header", header, allocator);
    value.AddMember("polygon", polygon, allocator);

    return true;
}

namespace det {
template <class PolygonStampedType>
bool setHeader(const rapidjson::Value &doc, PolygonStampedType &polyStamped, Int2Type<1>) { // polyStamped.setHeader() exists
    typedef decltype (polyStamped.header()) HeaderTypeCVR;
    typedef typename boost::remove_cv<typename boost::remove_reference<HeaderTypeCVR>::type>::type HeaderType;
    HeaderType header;
    bool ret = Header::fromJson(doc, header);
    polyStamped.header() = header;
    return ret;
}
template <class PolygonStampedType>
bool setHeader(const rapidjson::Value &doc, PolygonStampedType &polyStamped, Int2Type<0>) { // polyStamped.setHeader() does not exists
    (void)doc;
    (void)polyStamped;
    return true;
}
} // namespace det

template <class PolygonType, class HeaderType>
bool fromJson(const rapidjson::Value &value, PolygonType &polyStamped)
{
    if ( !value.HasMember("header") ){
        assert(false);
        return false;
    }
    if ( !value.HasMember("polygon") ){
        assert(false);
        return false;
    }


    typedef traits::HasMemberSetHeader<PolygonType> HasHeader;
    if ( !det::setHeader(value["header"], polyStamped, Int2Type<HasHeader::value>())){
        assert(false);
        return false;
    }

    if ( !Polygon::fromJson(value["polygon"], polyStamped.polygon()) ){
        assert(false);
        return false;
    }

    return true;
}
} // namespace PolygonStamped

} // namespace GeometryMsgs


//! @brief Namespace containing methodes for geographic_msgs generation.
namespace GeographicMsgs {
//! @brief Namespace containing methodes for geographic_msgs/GeoPoint message generation.
namespace GeoPoint {
using namespace ROSBridge::traits;

        namespace det { //detail

            template <class T>
            auto getAltitude(const T &p, Type2Type<Has3Components>)
            {
                return p.altitude();
            }

            template <class T>
            auto getAltitude(const T &p, Type2Type<Has2Components>)
            {
                (void)p;
                return 0.0;
            }

            template <class T>
            void setAltitude(const rapidjson::Value &doc, T &p, Type2Type<Has3Components>)
            {
                p.setAltitude(doc.GetFloat());
            }

            template <class T>
            void setAltitude(const rapidjson::Value &doc, T &p, Type2Type<Has2Components>)
            {
                (void)doc;
                (void)p;
            }
        } // namespace det

        template <class T>
        bool toJson(const T&p, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
        {
            value.AddMember("latitude", rapidjson::Value().SetFloat((_Float64)p.latitude()), allocator);
            value.AddMember("longitude", rapidjson::Value().SetFloat((_Float64)p.longitude()), allocator);

            typedef typename Select<HasMemberAltitude<T>::value, Has3Components, Has2Components>::Result
                    Components; // Check if PointType has 2 or 3 dimensions.
            auto altitude = det::getAltitude(p, Type2Type<Components>()); // If T has no member altitude() replace it by 0.0;

            value.AddMember("altitude", rapidjson::Value().SetFloat((_Float64)altitude), allocator);
            return true;
        }



        template <class PointType>
        bool fromJson(const rapidjson::Value &value,
                      PointType &p)     {
            if (!value.HasMember("latitude") || !value["latitude"].IsFloat()){
                assert(false);
                return false;
            }
            if (!value.HasMember("longitude") || !value["longitude"].IsFloat()){
                assert(false);
                return false;
            }
            if (!value.HasMember("altitude") || !value["altitude"].IsFloat()){
                assert(false);
                return false;
            }

            p.setLatitude(value["latitude"].GetFloat());
            p.setLongitude(value["longitude"].GetFloat());
            typedef typename Select<HasMemberSetAltitude<PointType>::value, Has3Components, Has2Components>::Result
                    Components; // Check if PointType has 2 or 3 dimensions.
            det::setAltitude(value["altitude"], p,  Type2Type<Components>()); // If T has no member altitude() discard doc["altitude"];

            return true;
        }

} // GeoPoint
} // GeographicMsgs


//! @brief Namespace containing methodes for jsk_recognition_msgs generation.
namespace JSKRecognitionMsgs {
//! @brief Namespace containing methodes for jsk_recognition_msgs/PolygonArray message generation.
namespace PolygonArray {
using namespace ROSBridge::traits;
using namespace ROSBridge::JsonMethodes::StdMsgs;
using namespace ROSBridge::JsonMethodes::GeometryMsgs;

    namespace PAdetail {
        //! Helper functions to generate Json entries for labels and likelihood.

        //! \note \p p has member \fn labels().
        template <class PolygonArrayType, int  k>
        void labelsToJson(const PolygonArrayType &p, rapidjson::Value &labels, rapidjson::Document::AllocatorType &allocator, Int2Type<k>){
            for(unsigned long i=0; i < (unsigned long)p.labels().size(); ++i)
                labels.PushBack(rapidjson::Value().SetUint(p.labels()[i]), allocator);
        }

        //! \note \p p has no member \fn labels().
        template <class PolygonArrayType>
        void labelsToJson(const PolygonArrayType &p, rapidjson::Value &labels, rapidjson::Document::AllocatorType &allocator, Int2Type<0>){
            for(unsigned long i=0; i < (unsigned long)(p.polygons().size()); ++i)
                labels.PushBack(rapidjson::Value().SetUint(0), allocator); // use zero!
        }        

        //! \note \p p has member \fn likelihood().
        template <class PolygonArrayType, int  k>
        void likelihoodToJson(const PolygonArrayType &p,
                              rapidjson::Value &likelyhood,
                              rapidjson::Document::AllocatorType &allocator,
                              Int2Type<k>){
            p.likelyhood().clear();
            for(unsigned long i=0; i < (unsigned long)p.likelyhood().size(); ++i)
                likelyhood.PushBack(rapidjson::Value().SetFloat(p.likelyhood()[i]), allocator);
        }

        //! \note \p p has no member \fn likelihood().
        template <class PolygonArrayType>
        void likelihoodToJson(const PolygonArrayType &p,
                              rapidjson::Value &likelyhood,
                              rapidjson::Document::AllocatorType &allocator,
                              Int2Type<0>){
            for(unsigned long i=0; i < (unsigned long)p.polygons().size(); ++i)
                likelyhood.PushBack(rapidjson::Value().SetFloat(0), allocator); // use zero!
        }

        //! \note \p p has member \fn labels().
        template <class PolygonArrayType, int k>
        void setLabels(const rapidjson::Value &doc, PolygonArrayType &p, Int2Type<k>){
            p.labels().clear();
            for(unsigned long i=0; i < (unsigned long)doc.Size(); ++i)
                p.labels().push_back(doc[i]);
        }

        //! \note \p p has no member \fn labels().
        template <class PolygonArrayType>
        void setLabels(const rapidjson::Value &doc, PolygonArrayType &p, Int2Type<0>){
            (void)doc;
            (void)p;
        }

        //! \note \p p has member \fn likelihood().
        template <class PolygonArrayType, int k>
        void setLikelihood(const rapidjson::Value &doc, PolygonArrayType &p, Int2Type<k>){
            for(unsigned long i=0; i < (unsigned long)doc.Size(); ++i)
                p.likelihood().push_back(doc[i]);
        }

        //! \note \p p has no member \fn likelihood().
        template <class PolygonArrayType>
        void setLikelihood(const rapidjson::Value &doc, PolygonArrayType &p, Int2Type<0>){
            (void)doc;
            (void)p;
        }
    }


    //!
    //! Create PolygonArray message from \p p. \p p contains a header.
    //! \param p Class implementing the PolygonArrayType interface.
    //! \param doc Rapidjson document used to store the PolygonArray message.
    //! \param allocator Allocator used by doc. Can be obtained e.g. by calling doc.getAllocator().
    //!
    //! \note The \fn labels() and \fn likelihood() members are optinal. If any of them is missing they
    //! will be replaced by arrays filled with zero and size p.polygons.size().
    //!
    //! \note If this function is called p.polygons[i] (entries implement the the PolygonStampedGroup interface)
    //! must contain a header.
    template <class PolygonArrayType>
    bool toJson(const PolygonArrayType &pArray, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
    {
        rapidjson::Document header(rapidjson::kObjectType);
        if (Header::toJson(pArray.header(), header, allocator)){
            assert(false);
            return false;
        }
        value.AddMember("header", header, allocator);

        rapidjson::Value polygons(rapidjson::kArrayType);
        for(unsigned long i=0; i < pArray.polygons().size(); ++i){
            rapidjson::Document polygon(rapidjson::kObjectType);
            if (!PolygonStamped::toJson(pArray.polygons()[i], polygon, allocator)){
                assert(false);
                return false;
            }
            polygons.PushBack(polygon, allocator);
        }
        value.AddMember("polygons", polygons, allocator);


        rapidjson::Value labels(rapidjson::kArrayType);
        typedef HasMemberLabels<PolygonArrayType> HasLabels;
        PAdetail::labelsToJson(pArray, labels, allocator, Int2Type<HasLabels::value>());
        value.AddMember("labels", labels, allocator);


        rapidjson::Value likelihood(rapidjson::kArrayType);
        typedef HasMemberLikelihood<PolygonArrayType> HasLikelihood;
        PAdetail::likelihoodToJson(pArray, likelihood, allocator, Int2Type<HasLikelihood::value>());
        value.AddMember("likelihood", likelihood, allocator);

        return true;
    }


    //!
    //! Create PolygonArray message from \p p and \p h. \p p doesn't have it's own header.
    //! \param p Class implementing the PolygonArrayType interface.
    //! \param h Class implementing the HeaderType interface.
    //! \param doc Rapidjson document used to store the PolygonArray message.
    //! \param allocator Allocator used by doc. Can be obtained e.g. by calling doc.getAllocator().
    //!
    //! \note The \fn labels() and \fn likelihood() members are optinal. If any of them is missing they
    //! will be replaced by arrays filled with zero and size p.polygons.size().
    //!
    //! \note If this function is called the headers in p.polygons[i] (entries implement the the PolygonStampedGroup interface)
    //! are ignored.
    template <class PolygonArrayType, class HeaderType>
    bool toJson(const PolygonArrayType &p, const HeaderType &h, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
    {
        rapidjson::Document header(rapidjson::kObjectType);
        if (!Header::toJson(h, header, allocator)){
            assert(false);
            return false;
        }
        value.AddMember("header", header, allocator);

        rapidjson::Value polygons(rapidjson::kArrayType);
        for(unsigned long i=0; i < (unsigned long)(p.polygons().size()); ++i){
            rapidjson::Document polygon(rapidjson::kObjectType);
            if (!PolygonStamped::toJson(p.polygons()[i].polygon(), h, polygon, allocator)){
                assert(false);
                return false;
            }
            polygons.PushBack(polygon, allocator);
        }
        value.AddMember("polygons", polygons, allocator);


        rapidjson::Value labels(rapidjson::kArrayType);
        typedef HasMemberLabels<PolygonArrayType> HasLabels;
        PAdetail::labelsToJson(p, labels, allocator, Int2Type<HasLabels::value>());
        value.AddMember("labels", labels, allocator);


        rapidjson::Value likelihood(rapidjson::kArrayType);
        typedef HasMemberLikelihood<PolygonArrayType> HasLikelihood;
        PAdetail::likelihoodToJson(p, likelihood, allocator, Int2Type<HasLikelihood::value>());
        value.AddMember("likelihood", likelihood, allocator);

        return true;
    }

    template <class PolygonArrayType>
    bool fromJson(const rapidjson::Value &value, PolygonArrayType &p)
    {
        if ( !value.HasMember("header")){
            assert(false);
            return false;
        }
        if ( !value.HasMember("polygons") || !value["polygons"].IsArray() ){
            assert(false);
            return false;
        }
        if ( !value.HasMember("labels") || !value["labels"].IsArray() ){
            assert(false);
            return false;
        }
        if ( !value.HasMember("likelihood") || !value["likelihood"].IsArray() ){
            assert(false);
            return false;
        }


        typedef traits::HasMemberHeader<PolygonArrayType> HasHeader;
        if ( !PolygonStamped::det::setHeader(value["header"], p, Int2Type<HasHeader::value>())){
            assert(false);
            return false;
        }


        const auto &polyStampedJson = value["polygons"];
        p.polygons().clear();
        p.polygons().reserve(polyStampedJson.Size());
        typedef decltype (p.polygons()[0]) PolyStampedCVR;
        typedef typename boost::remove_cv<typename boost::remove_reference<PolyStampedCVR>::type>::type
                PolyStamped;
        for (unsigned int i=0; i < polyStampedJson.Size(); ++i) {
            if ( !polyStampedJson[i].HasMember("header") ){
                assert(false);
                return false;
            }
            PolyStamped polyStamped;

            if ( !PolygonStamped::det::setHeader(polyStampedJson[i]["header"], polyStamped, Int2Type<HasHeader::value>())){
                assert(false);
                return false;
            }

            if ( !Polygon::fromJson(polyStampedJson[i]["polygon"], polyStamped.polygon())){
                assert(false);
                return false;
            }

            p.polygons().push_back(std::move(polyStamped));
        }

        typedef traits::HasMemberLabels<PolygonArrayType> HasLabels;
        PAdetail::setLabels(value["labels"], p, Int2Type<HasLabels::value>());

        typedef traits::HasMemberLikelihood<PolygonArrayType> HasLikelihood;
        PAdetail::setLikelihood(value["likelihood"], p, Int2Type<HasLikelihood::value>());

        return true;
    }

} // end namespace PolygonArray
} // namespace JSKRekognitionMsgs


//! @brief Namespace containing methodes for nemo_msgs generation.
namespace NemoMsgs {
//! @brief Namespace containing methodes for nemo_msgs/Progress generation.
namespace Progress {
    template <class ProgressType>
    bool toJson(const ProgressType &p, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
    {
        rapidjson::Value progressJson(rapidjson::kArrayType);
        for(unsigned long i=0; i < std::uint64_t(p.progress().size()); ++i){
            progressJson.PushBack(rapidjson::Value().SetInt(std::int32_t(p.progress()[i])), allocator);
        }
        value.AddMember("progress", progressJson, allocator);
        return true;
    }

    template <class ProgressType>
    bool fromJson(const rapidjson::Value &value, ProgressType &p)
    {
        if (!value.HasMember("progress") || !value["progress"].IsArray()){
            assert(false);
            return false;
        }

        const auto& jsonProgress = value["progress"];
        unsigned long sz = jsonProgress.Size();
        p.progress().clear();
        p.progress().reserve(sz);
        for (unsigned long i=0; i < sz; ++i)
            p.progress().push_back(std::int32_t(jsonProgress[i].GetInt()));
        return true;
    }
} // namespace Progress

//! @brief Namespace containing methodes for nemo_msgs/Heartbeat generation.
namespace Heartbeat {
    template <class HeartbeatType>
    bool toJson(const HeartbeatType &heartbeat, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
    {
        value.AddMember("status", std::int32_t(heartbeat.status()), allocator);
        return true;
    }

    template <class HeartbeatType>
    bool fromJson(const rapidjson::Value &value, HeartbeatType &heartbeat)
    {
        if (!value.HasMember("status") || !value["status"].IsInt()){
            assert(false);
            return false;
        }

        heartbeat.setStatus(value["status"].GetInt());
        return true;
    }
} // namespace Heartbeat
} // namespace NemoMsgs
} // namespace JsonMethodes
} // namespace ROSBridge

#endif // MESSAGES_H