#ifndef MESSAGES_H
#define MESSAGES_H

#include <iostream>
#include <vector>
#include <array>

namespace ros_bridge {

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

        uint32_t getSecs()  const;
        uint32_t getNSecs() const;

        void setSecs    (uint32_t secs);
        void setNSecs   (uint32_t nsecs);

    private:
        uint32_t _secs;
        uint32_t _nsecs;
    };

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

        uint32_t            getSeq()       const;
        const Time         &getStamp()     const;
        const std::string  &getFrameId()   const;

        void setSeq     (uint32_t seq);
        void setStamp   (const Time & stamp);
        void setFrameId (const std::string  &frame_id);

    private:
        uint32_t    _seq;
        Time        _stamp;
        std::string _frame_id;
    };

    //! @brief C++ representation of geometry_msgs/Point32
    class Point32{
    public:
        Point32();
        Point32(_Float32 x, _Float32 y, _Float32 z);

        template<unsigned int i>
        void set(_Float32 component)
        {
            static_assert (i < 3, "Index out of bonds.");
            _components[i] = component;
        }

        template<unsigned int i>
        _Float32 get(void) const
        {
            static_assert (i < 3, "Index out of bonds.");
            return _components[i];
        }

    private:
        std::array<_Float32, 3> _components;
    };

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

        const std::vector<Point32> &get() const
        {
            return _points;
        }

        void set(std::vector<Point32> &points)
        {
            _points = points;
        }

        void clear()
        {
            _points.clear();
        }

        PointType &at(unsigned int i)
        {
            return _points.at(i);
        }

        void push_back(const Point32 &p)
        {
            _points.push_back(p);
        }

        void pop_back()
        {
            _points.pop_back();
        }

    private:
        std::vector<PointType> _points;
    };

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

    private:
        Header _header;
    };

    //! @brief C++ representation of jsk_recognition_msgs/PolygonArray
    template <class PolygonType = Polygon<>, class HeaderType = Header, template<class> class ContainerType = std::vector >
    class PolygonArray{
    public:
        PolygonArray()
         : PolygonArray(HeaderType()
                        , ContainerType<PolygonType>()
                        , ContainerType<uint32_t>()
                        , ContainerType<_Float32>())
        {}

        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)
        {}

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

#endif // MESSAGES_H