#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 "ROSMessageTraits.h"

namespace ros_bridge {

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

}

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

}

namespace Point32 {
using namespace ros_bridge::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 <typename FloatType, class T, typename V>
            bool toJson(const T &p, rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator, Type2Type<V>)
            {
                doc.AddMember("x", rapidjson::Value().SetFloat((FloatType)p.x()), allocator);
                doc.AddMember("y", rapidjson::Value().SetFloat((FloatType)p.y()), allocator);
                doc.AddMember("z", rapidjson::Value().SetFloat((FloatType)p.z()), allocator);

                return true;
            }

            template <typename FloatType, class T>
            bool toJson(const T &p, rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator, Type2Type<Has2Components>)
            {
                doc.AddMember("x", rapidjson::Value().SetFloat((FloatType)p.x()), allocator);
                doc.AddMember("y", rapidjson::Value().SetFloat((FloatType)p.y()), allocator);
                doc.AddMember("z", rapidjson::Value().SetFloat((FloatType)0.0), allocator); // _point has no member z() -> add 0.

                return true;
            }
        }

        template <typename FloatType = _Float32, class T>
        bool toJson(const T&p, rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator)
        {
            typedef typename Select<HasMemberZ<T>::value, Has3Components, Has2Components>::Result
                    Components; // Check if PointType has 2 or 3 dimensions.
            return det::toJson<FloatType>(p, doc, allocator,  Type2Type<Components>());
        }

}

namespace GeoPoint {
using namespace ros_bridge::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;}


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

        namespace det { //detail

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

                return true;
            }

            template <class T>
            bool toJson(const T &p, rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator, Type2Type<Has2Components>)
            {
                doc.AddMember("latitudex", rapidjson::Value().SetFloat((_Float64)p.latitude()), allocator);
                doc.AddMember("longitude", rapidjson::Value().SetFloat((_Float64)p.longitude()), allocator);
                doc.AddMember("altitude", rapidjson::Value().SetFloat((_Float64)0.0), allocator); // _point has no member altitude() -> add 0.

                return true;
            }
        }

        template <class T>
        bool toJson(const T&p, rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator)
        {
            typedef typename Select<HasMemberAltitude<T>::value, Has3Components, Has2Components>::Result
                    Components; // Check if PointType has 2 or 3 dimensions.
            return det::toJson(p, doc, allocator,  Type2Type<Components>());
        }

}

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<_Float32>(poly.points()[i], point, allocator) )
                return false;
            points.PushBack(point, allocator);
        }

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

        return true;
    }


}

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 PolygonArray {
using namespace ros_bridge::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(){}

        //!
        //! \brief PolygonArray
        //! \param header Header
        //! \param polygons Polygons
        //! \param labels Labels
        //! \param likelihood Header
        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!
        }
    }


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

}
}

#endif // MESSAGES_H