#ifndef MESSAGES_H
#define MESSAGES_H

#include <iostream>
#include <vector>

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

namespace ros_bridge {

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

        uint32_t getSecs()  const {return _secs;}
        uint32_t getNSecs() const {return _nsecs;}

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

        bool toJson(rapidjson::Document &doc,
                    rapidjson::Document::AllocatorType &allocator) const
        {
            doc.AddMember("secs", rapidjson::Value().SetUint(this->_secs), allocator);
            doc.AddMember("nsecs", rapidjson::Value().SetUint(this->_nsecs), allocator);

            return true;
        }

    private:
        uint32_t _secs;
        uint32_t _nsecs;
    };

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

        uint32_t            getSeq()       const {return _seq;};
        const Time         *getStamp()     const {return _stamp;};
        const std::string  &getFrameId()   const {return _frameId;};

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

        bool toJson(rapidjson::Document &doc,
                    rapidjson::Document::AllocatorType &allocator) const
        {
            doc.AddMember("seq", rapidjson::Value().SetUint(this->_seq), allocator);
            rapidjson::Document stamp(rapidjson::kObjectType);
            if (!this->_stamp->toJson(stamp, allocator))
                    return false;
            doc.AddMember("stamp", stamp, allocator);
            doc.AddMember("frame_id",
                          rapidjson::Value().SetString(this->_frameId.data(),
                                                       this->_frameId.length(),
                                                       allocator),
                          allocator);

            return true;
        }

    private:
        uint32_t    _seq;
        const Time *_stamp;
        std::string _frameId;
    };

    //! @brief C++ representation of geometry_msgs/Point32
    template <class PointType>
    class Point32{
    public:        
        Point32() {}
        Point32(const PointType* p): _point(p) {}

        const PointType    *getPoint()                          const   {return _point;}
        void                setPoint(const PointType *point)            {point;}

        bool toJson(rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator) const
        {

            doc.AddMember("x", rapidjson::Value().SetFloat(_point->x()), allocator);
            doc.AddMember("y", rapidjson::Value().SetFloat(_point->y()), allocator);
            doc.AddMember("z", rapidjson::Value().SetFloat(_point->z()), allocator);

            return true;
        }

    private:
        const PointType *_point;
    };

    //! @brief C++ representation of geometry_msgs/Polygon
    template <class PointType,
              template <class, class...> class ContainerType =  std::vector,
              class PointWrapper  = class Point32<PointType> >
    class Polygon{
    public:
        Polygon(){}
        Polygon(const ContainerType<PointType> *points) : _points(points) {}


        const ContainerType<PointType> *getPoints() const {return _points;}

        void setPoints(const ContainerType<PointType> *points) {_points = points;}

        bool toJson(rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator)
        {
            rapidjson::Value points(rapidjson::kArrayType);

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

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

            return true;
        }

    private:
        const ContainerType<PointType> *_points;
    };

    //! @brief C++ representation of geometry_msgs/PolygonStamped
    template <class PointType, class PolygonType = Polygon<PointType>, class HeaderType = Header>
    class PolygonStamped : public PolygonType{
    public:
        PolygonStamped();
        PolygonStamped(const Header *header, const PolygonType *polygon) :
            _header(header)
          , _polygon(polygon) {}


        const HeaderType  *getHeader()  const   {return _header;}
        const PolygonType *getPolygon() const   {return _polygon;}

        void setHeader(const HeaderType *header)          {_header = header;}
        void setPolygon(const PolygonType *polygon)   {_polygon = polygon;}

        bool toJson(rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator)
        {
            rapidjson::Document header(rapidjson::kObjectType);
            if (!this->_header->toJson(header, allocator))
                return false;
            rapidjson::Document polygon(rapidjson::kObjectType);
            if (!this->_polygon->toJson(polygon, allocator))
                return false;
            doc.AddMember("header", header, allocator);
            doc.AddMember("polygon", polygon, allocator);

            return true;
        }

    private:
        HeaderType *_header;
        PolygonType *_polygon;
    };

    //! @brief C++ representation of jsk_recognition_msgs/PolygonArray
    template <class PointType,
              class PolygonType = Polygon<PointType>,
              template <class, class...> class ContainerType = std::vector,
              class HeaderType = Header >
    class PolygonArray{
    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                 *getHeader()     const {return _header;}
        const ContainerType<PolygonType> *getPolygons()   const {return _polygons;}
        const ContainerType<uint32_t>    *getLabels()     const {return _labels;}
        const ContainerType<_Float32>    *getLikelihood() const {return _likelihood;}

        void *setHeader     (const HeaderType                 *header)      {_header     = header;}
        void *setPolygons   (const 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;
    };
}

#endif // MESSAGES_H