#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 {
namespace JsonMethodes {

typedef std::ostream OStream;

namespace Time {
    //! @brief C++ representation of std_msgs/Time
    class Time{
    public:
        Time(): _secs(0), _nsecs(0) {}
        Time(uint32_t secs, uint32_t nsecs): _secs(secs), _nsecs(nsecs) {}

        uint32_t secs()  const {return _secs;}
        uint32_t nSecs() const {return _nsecs;}

        void setSecs(uint32_t secs)     {_secs = secs;}
        void setNSecs(uint32_t nsecs)   {_nsecs = nsecs;}

    private:
        uint32_t _secs;
        uint32_t _nsecs;
    };

    template<class TimeType>
    bool toJson(const TimeType &time,
                rapidjson::Document &doc,
                rapidjson::Document::AllocatorType &allocator)
    {
        doc.AddMember("secs", rapidjson::Value().SetUint((uint32_t)time.secs()), allocator);
        doc.AddMember("nsecs", rapidjson::Value().SetUint((uint32_t)time.nSecs()), allocator);

        return true;
    }

    template<class TimeType, class JsonType>
    bool _fromJson(const JsonType &doc,
                  TimeType &time)
    {
        if (!doc.HasMember("secs") || !doc["secs"].IsUint())
            return false;
        if (!doc.HasMember("nsecs")|| !doc["nsecs"].IsUint())
            return false;
        time.setSecs(doc["secs"].GetUint());
        time.setNSecs(doc["nsecs"].GetUint());

        return true;
    }

    template<class TimeType>
    bool fromJson(const rapidjson::Document &doc,
                  TimeType &time)
    {
        return _fromJson(doc, time);
    }

    template<class TimeType>
    bool fromJson(const rapidjson::Value &doc,
                  TimeType &time)
    {
        return _fromJson(doc, time);
    }

}

namespace Header {



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

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

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

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


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

        rapidjson::Document stamp(rapidjson::kObjectType);
        if (!Time::toJson(header.stamp(), doc, allocator))
                return false;
        doc.AddMember("stamp", stamp, allocator);

        doc.AddMember("frame_id",
                      rapidjson::Value().SetString(header.frameId().data(),
                                                   header.frameId().length(),
                                                   allocator),
                      allocator);

        return true;
    }

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

        return true;
    }


    template<class HeaderType>
    bool fromJson(const rapidjson::Value &doc,
                  HeaderType &header)
    {
        return _fromJson(doc, header);
    }

    template<class HeaderType>
    bool fromJson(const rapidjson::Document &doc,
                  HeaderType &header)
    {
        return _fromJson(doc, header);
    }

}

namespace Point32 {
using namespace ROSBridge::traits;

        //! @brief C++ representation of geometry_msgs/Point32.
        template<typename FloatType = _Float64>
        class Point{
        public:
            Point(FloatType x, FloatType y, FloatType z) : _x(x), _y(y), _z(z){}

            FloatType x() const {return _x;}
            FloatType y() const {return _y;}
            FloatType z() const {return _z;}

            void setX(FloatType x) {_x = x;}
            void setY(FloatType y) {_y = y;}
            void setZ(FloatType z) {_z = z;}


        private:
            FloatType _x;
            FloatType _y;
            FloatType _z;
        };

        typedef Point<> Point64;
        typedef Point<_Float32> Point32;

        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::Document &doc, rapidjson::Document::AllocatorType &allocator)
        {
            doc.AddMember("x", rapidjson::Value().SetFloat(p.x()), allocator);
            doc.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.

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

            return true;
        }

        template <class PointType, class JsonType>
        bool _fromJson(const JsonType &doc,
                      PointType &p)     {
            if (!doc.HasMember("x") || !doc["x"].IsFloat())
                return false;
            if (!doc.HasMember("y") || !doc["y"].IsFloat())
                return false;
            if (!doc.HasMember("z") || !doc["z"].IsFloat())
                return false;

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

            return true;
        }


        template<class PointType>
        bool fromJson(const rapidjson::Value &doc,
                      PointType &point)
        {
            return _fromJson(doc, point);
        }

        template<class PointType>
        bool fromJson(const rapidjson::Document &doc,
                      PointType &point)
        {
            return _fromJson(doc, point);
        }

}

namespace GeoPoint {
using namespace ROSBridge::traits;

        //! @brief C++ representation of geographic_msgs/GeoPoint.
        class GeoPoint{
        public:
            GeoPoint()
                : _latitude(0)
                , _longitude(0)
                , _altitude(0)
            {}
            GeoPoint(_Float64 latitude, _Float64 longitude, _Float64 altitude)
                : _latitude(latitude)
                , _longitude(longitude)
                , _altitude(altitude)
            {}

            _Float64 latitude()     const {return _latitude;}
            _Float64 longitude()    const {return _longitude;}
            _Float64 altitude()     const {return _altitude;}

            void setLatitude    (_Float64 latitude)    {_latitude  = latitude;}
            void setLongitude   (_Float64 longitude)   {_longitude = longitude;}
            void setAltitude    (_Float64 altitude)    {_altitude  = altitude;}



            bool operator==(const GeoPoint &p1) {
                return (p1._latitude == this->_latitude
                        && p1._longitude == this->_longitude
                        && p1._altitude == this->_altitude);
            }

            bool operator!=(const GeoPoint &p1) {
                return !this->operator==(p1);
            }

            friend std::ostream& operator<<(std::ostream& os, const GeoPoint& p)
            {
                os << "lat: " << p._latitude << " lon: " << p._longitude<< " alt: " << p._altitude << "\n";
                return os;
            }

        private:
            _Float64 _latitude;
            _Float64 _longitude;
            _Float64 _altitude;
        };

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

        template <class T>
        bool toJson(const T&p, rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator)
        {
            doc.AddMember("latitude", rapidjson::Value().SetFloat((_Float64)p.latitude()), allocator);
            doc.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;

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



        template <class PointType, class JsonType>
        bool _fromJson(const JsonType &doc,
                      PointType &p)     {
            if (!doc.HasMember("latitude") || !doc["latitude"].IsFloat())
                return false;
            if (!doc.HasMember("longitude") || !doc["longitude"].IsFloat())
                return false;
            if (!doc.HasMember("altitude") || !doc["altitude"].IsFloat())
                return false;

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

            return true;
        }

        template<class PointType>
        bool fromJson(const rapidjson::Value &doc,
                      PointType &point)
        {
            return _fromJson(doc, point);
        }

        template<class PointType>
        bool fromJson(const rapidjson::Document &doc,
                      PointType &point)
        {
            return _fromJson(doc, point);
        }

}

namespace Polygon {

    //! @brief C++ representation of geometry_msgs/Polygon
    template <class PointTypeCVR,
              template <class, class...> class ContainerType =  std::vector>
    class Polygon{
        typedef typename boost::remove_cv<typename boost::remove_reference<PointTypeCVR>::type>::type PointType;
    public:
        Polygon(){}
        Polygon(const Polygon &poly) : _points(poly.points()){}


        const ContainerType<PointType> &points() const {return _points;}
              ContainerType<PointType> &points()       {return _points;}

    private:
        ContainerType<PointType> _points;
    };

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

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

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

        return true;
    }

    template <class PolygonType, class JsonType>
    bool _fromJson(const JsonType &doc, PolygonType &poly)
    {
        if (!doc.HasMember("points") || !doc["points"].IsArray())
            return false;
        const auto &array = doc["points"].GetArray();
        for (long i=0; i < array.Size(); ++i){
            if ( !Point32::fromJson(array[i], poly.points()[i]) )
                return false;
        }
        return true;
    }

    template<class PolygonType>
    bool fromJson(const rapidjson::Value &doc,
                  PolygonType &poly)
    {
        return _fromJson(doc, poly);
    }

    template<class PolygonType>
    bool fromJson(const rapidjson::Document &doc,
                  PolygonType &poly)
    {
        return _fromJson(doc, poly);
    }


}

namespace PolygonStamped {


    //! @brief C++ representation of geometry_msgs/PolygonStamped
    template <class PointTypeCVR,
              template<class,class...> class PolygonType = Polygon::Polygon, class HeaderType = Header::Header>
    class PolygonStamped {
    typedef typename boost::remove_cv<typename boost::remove_reference<PointTypeCVR>::type>::type PointType;
        typedef PolygonType<PointType> Polygon;
    public:
        PolygonStamped(){};
        PolygonStamped(const HeaderType &header, Polygon &polygon) :
            _header(header)
          , _polygon(polygon)
        {}


        const HeaderType  &header()  const   {return _header;}
        const Polygon     &polygon() const   {return _polygon;}

        HeaderType  &header()  {return _header;}
        Polygon     &polygon() {return _polygon;}

        void setHeader(const HeaderType &header)          {_header = header;}
        void setPolygon(const Polygon &polygon)
        {
            _polygon = polygon;
        }

    private:
        HeaderType   _header;
        Polygon      _polygon;
    };


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

    template <class PolygonType, class HeaderType>
    bool toJson(const PolygonType &poly, const HeaderType &h, rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator)
    {
        rapidjson::Document header(rapidjson::kObjectType);
        if (!Header::toJson(h, header, allocator))
            return false;
        rapidjson::Document polygon(rapidjson::kObjectType);
        if (!Polygon::toJson(poly, polygon, allocator))
            return false;
        doc.AddMember("header", header, allocator);
        doc.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()) HeaderType;
        HeaderType header;
        bool ret = Header::fromJson(doc, header);
        polyStamped.setHeader(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;
    }
    }

    template <class PolygonType, class HeaderType, class JsonType>
    bool _fromJson(const JsonType &doc, PolygonType &polyStamped)
    {
        if ( !doc.HasMember("header") )
            return false;
        if ( !doc.HasMember("polygon") )
            return false;


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

        if ( !Polygon::fromJson(doc["polygon"], polyStamped.polygon()) )
            return false;

        return true;
    }

    template<class PolygonType>
    bool fromJson(const rapidjson::Value &doc,
                  PolygonType &poly)
    {
        return _fromJson(doc, poly);
    }

    template<class PolygonType>
    bool fromJson(const rapidjson::Document &doc,
                  PolygonType &poly)
    {
        return _fromJson(doc, poly);
    }

}

namespace PolygonArray {
using namespace ROSBridge::traits;

    //! @brief C++ representation of jsk_recognition_msgs/PolygonArray
    template <class PointTypeCVR,
              template <class, class...> class Polygon = Polygon::Polygon,
              template <class, class...> class ContainerType = std::vector,
              class HeaderType =  Header::Header>
    class PolygonArray{
        typedef typename boost::remove_cv<typename boost::remove_reference<PointTypeCVR>::type>::type PointType;
        typedef Polygon<PointType> PolygonType;
    public:
        PolygonArray(){}
        PolygonArray(const HeaderType                   &header,
                     const ContainerType<PolygonType>   &polygons,
                     const ContainerType<uint32_t>      &labels,
                     const ContainerType<_Float32>      &likelihood)
          : _header(header)
          , _polygons(polygons)
          , _labels(labels)
          , _likelihood(likelihood)
        {}

        const HeaderType                 &header()     const {return _header;}
              HeaderType                 &header()           {return _header;}
        const ContainerType<PolygonType> &polygons()   const {return _polygons;}
              ContainerType<PolygonType> &polygons()         {return _polygons;}
        const ContainerType<uint32_t>    &labels()     const {return _labels;}
              ContainerType<uint32_t>    &labels()           {return _labels;}
        const ContainerType<_Float32>    &likelyhood() const {return _likelihood;}
              ContainerType<_Float32>    &likelyhood()       {return _likelihood;}

        void setHeader     (const HeaderType                &header)        {_header     = &header;}
        void setPolygons   (ContainerType<PolygonType>      &polygon)       { _polygons   = polygon; }
        void setLabels     (const ContainerType<uint32_t>    &labels)       {_labels     = labels;}
        void setLikelihood (const ContainerType<_Float32>    &likelihood)   {_likelihood = likelihood;}



    private:
        HeaderType                  _header;
        ContainerType<PolygonType>  _polygons;
        ContainerType<uint32_t>     _labels;
        ContainerType<_Float32>     _likelihood;
    };


    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>){
            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>){
            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 &p, rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator)
    {
        rapidjson::Document header(rapidjson::kObjectType);
        if (Header::toJson(p.header(), header, allocator))
            return false;
        doc.AddMember("header", header, allocator);

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


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


        rapidjson::Value likelihood(rapidjson::kArrayType);
        typedef HasMemberLikelihood<PolygonArrayType> HasLikelihood;
        PAdetail::likelihoodToJson(p, likelihood, allocator, Int2Type<HasLikelihood::value>());
        doc.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::Document &doc, rapidjson::Document::AllocatorType &allocator)
    {
        rapidjson::Document header(rapidjson::kObjectType);
        if (!Header::toJson(h, header, allocator))
            return false;
        doc.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], h, polygon, allocator))
                return false;
            polygons.PushBack(polygon, allocator);
        }
        doc.AddMember("polygons", polygons, allocator);


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


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

        return true;
    }

    template <class PolygonArrayType, class JsonType>
    bool _fromJson(const JsonType &doc, PolygonArrayType &p)
    {
        if ( !doc.HasMember("header"))
            return false;
        if ( !doc.HasMember("polygons") || !doc["polygons"].IsArray() )
            return false;
        if ( !doc.HasMember("labels") || !doc["labels"].IsArray() )
            return false;
        if ( !doc.HasMember("likelihood") || !doc["likelihood"].IsArray() )
            return false;


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


        const auto &polygons = doc["polygons"];
        p.polygons().reserve(polygons.Size());
        for (long i=0; i < polygons.Size(); ++i) {
            if ( !polygons[i].HasMember("header") )
                return false;
            typedef traits::HasMemberSetHeader<PolygonArrayType> HasHeader;
            if ( !PolygonStamped::det::setHeader(polygons[i]["header"], p.polygons[i]().header(), Int2Type<HasHeader::value>()))
                return false;

            if ( !Polygon::fromJson(polygons[i], p.polygons[i].points()))
                return false;
        }

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

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


        return true;
    }

    template<class PolygonArrayType>
    bool fromJson(const rapidjson::Value &doc,
                  PolygonArrayType &polyArray)
    {
        return _fromJson(doc, polyArray);
    }

    template<class PolygonArrayType>
    bool fromJson(const rapidjson::Document &doc,
                  PolygonArrayType &polyArray)
    {
        return _fromJson(doc, polyArray);
    }

}
}// end namespace JsonMethodes
} // end namespace ROSBridge

#endif // MESSAGES_H