#pragma once
#include <iostream>
#include <vector>

#include "boost/type_traits/remove_cv_ref.hpp"
#include "ros_bridge/include/MessageBaseClass.h"

namespace ROSBridge {
//!@brief Namespace containing ROS message generics.
namespace GenericMessages {
//!@brief Namespace containing ROS std_msgs generics.

typedef std::ostream OStream;
namespace StdMsgs {

//! @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) {}
    Time(const Time &time): _secs(time.secs()), _nsecs(time.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;
};


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

    Header(const Header &header) :
        _seq(header.seq())
      , _stamp(header.stamp())
      , _frameId(header.frameId()) {}

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

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

    void setSeq     (uint32_t seq)                  {_seq = seq;}
    void setStamp   (const Time &stamp)             {_stamp = stamp;}
    void setFrameId (const std::string  &frameId)   {_frameId = frameId;}
private:
    uint32_t    _seq;
    Time        _stamp;
    std::string _frameId;
};
} //StdMsgs
//!@brief Namespace containing ROS geometry_msgs generics.
namespace GeometryMsgs {

// ==============================================================================
// class GenericPoint
// ==============================================================================

//! @brief C++ representation of a geometry_msgs/Point32
template<typename FloatType = _Float64>
class GenericPoint : public ROSBridge::MessageBaseClass {
public:
    typedef ROSBridge::MessageGroups::Point32Group Group;
    GenericPoint() : ROSBridge::MessageBaseClass(){}
    GenericPoint(FloatType x, FloatType y, FloatType z) : ROSBridge::MessageBaseClass(), _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;}



    bool operator==(const GenericPoint &p1) {
        return (p1._x == this->_x
                && p1._y == this->_y
                && p1._z == this->_z);
    }

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

    friend OStream& operator<<(OStream& os, const GenericPoint& p)
    {
        os << "x: " << p._x << " y: " << p._y<< " z: " << p._z;
        return os;
    }

    GenericPoint *Clone() const override {return new GenericPoint(*this);}

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

typedef GenericPoint<> Point;
typedef GenericPoint<_Float32> Point32;


// ==============================================================================
// class GenericPolygon
// ==============================================================================

//! @brief C++ representation of geometry_msgs/Polygon
template <class PointTypeCVR,
          template <class, class...> class ContainerType = std::vector>
class GenericPolygon : public ROSBridge::MessageBaseClass {
    typedef typename boost::remove_cv_ref_t<PointTypeCVR> PointType;
    typedef ROSBridge::MessageBaseClass Base;
public:
    typedef ROSBridge::MessageGroups::PolygonGroup Group;
    GenericPolygon() : Base() {}
    GenericPolygon(const GenericPolygon &poly) : Base(), _points(poly.points()){}


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

          GenericPolygon *Clone() const override { return new GenericPolygon(*this);}

          GenericPolygon &operator=(const GenericPolygon &other) {
              this->_points = other._points;
              return *this;
          }

private:
    ContainerType<PointType> _points;
};

typedef GenericPolygon<Point> Polygon;


// ==============================================================================
// class GenericPolygonStamped
// ==============================================================================
using namespace ROSBridge::GenericMessages::StdMsgs;

//! @brief C++ representation of geometry_msgs/PolygonStamped
template <class PolygonType  = Polygon,
          class HeaderType = Header>
class GenericPolygonStamped : public ROSBridge::MessageBaseClass{
    typedef PolygonType Polygon;
    typedef ROSBridge::MessageBaseClass Base;
public:
    typedef ROSBridge::MessageGroups::PolygonStampedGroup Group;
    GenericPolygonStamped() : Base() {}
    GenericPolygonStamped(const GenericPolygonStamped &other) :
         Base()
      , _header(other.header())
      , _polygon(other.polygon())
    {}
    GenericPolygonStamped(const HeaderType &header, Polygon &polygon) :
         Base()
      , _header(header)
      , _polygon(polygon)
    {}


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

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

    GenericPolygonStamped *Clone() const override {return new GenericPolygonStamped(*this);}

private:
    HeaderType   _header;
    Polygon      _polygon;
};

typedef GenericPolygonStamped<> PolygonStamped;

} // namespace GeometryMsgs
//!@brief Namespace containing ROS geographic_msgs generics.
namespace GeographicMsgs {


// ==============================================================================
// class GenericGeoPoint
// ==============================================================================

//! @brief C++ representation of geographic_msgs/GeoPoint.
class GeoPoint : public ROSBridge::MessageBaseClass{
    typedef ROSBridge::MessageBaseClass Base;
public:
    typedef ROSBridge::MessageGroups::GeoPointGroup Group;
    GeoPoint()
        : Base()
        , _latitude(0)
        , _longitude(0)
        , _altitude(0)
    {}
    GeoPoint(const GeoPoint &other)
        : Base()
        ,  _latitude(other.latitude())
        , _longitude(other.longitude())
        , _altitude(other.altitude())
    {}
    GeoPoint(_Float64 latitude, _Float64 longitude, _Float64 altitude)
        : Base()
        , _latitude(latitude)
        , _longitude(longitude)
        , _altitude(altitude)
    {}
    ~GeoPoint() override {}

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

    GeoPoint *Clone() const override {return new GeoPoint(*this);}

    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 OStream& operator<<(OStream& os, const GeoPoint& p)
    {
        os << "lat: " << p._latitude << " lon: " << p._longitude<< " alt: " << p._altitude;
        return os;
    }

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

} // namespace GeographicMsgs
//!@brief Namespace containing ROS jsk_recognition_msgs generics.
namespace JSKRecognitionMsgs {
using namespace ROSBridge::GenericMessages::StdMsgs;
using namespace ROSBridge::GenericMessages::GeometryMsgs;

// ==============================================================================
// class GenericPolygonArray
// ==============================================================================

//! @brief C++ representation of jsk_recognition_msgs/PolygonArray
template <class PolygonType = Polygon,
          template <class, class...> class ContainerType = std::vector,
          class HeaderType =  Header,
          class IntType = long,
          class FloatType = double>
class GenericPolygonArray : ROSBridge::MessageBaseClass{
    typedef ROSBridge::MessageBaseClass Base;
public:
    typedef ROSBridge::MessageGroups::PolygonArrayGroup Group;
    GenericPolygonArray() : Base() {}
    GenericPolygonArray(const GenericPolygonArray &other)
      : Base()
      , _header(other.header())
      , _polygons(other.polygons())
      , _labels(other.labels())
      , _likelihood(other.likelihood())
    {}
    GenericPolygonArray(const HeaderType                   &header,
                 const ContainerType<PolygonType>   &polygons,
                 const ContainerType<IntType>      &labels,
                 const ContainerType<FloatType>      &likelihood)
      : Base()
      , _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<IntType>     &labels()     const {return _labels;}
          ContainerType<IntType>     &labels()           {return _labels;}
    const ContainerType<FloatType>   &likelyhood() const {return _likelihood;}
          ContainerType<FloatType>   &likelyhood()       {return _likelihood;}

          GenericPolygonArray *Clone() const override {return new GenericPolygonArray(*this);}


private:
    HeaderType                  _header;
    ContainerType<PolygonType>  _polygons;
    ContainerType<IntType>     _labels;
    ContainerType<FloatType>     _likelihood;
};

typedef GenericPolygonArray<> PolygonArray;

} // namespace JSKRecognitionMsgs
//!@brief Namespace containing ROS nemo_msgs generics.
namespace NemoMsgs {

// ==============================================================================
// class GenericProgress
// ==============================================================================

//! @brief C++ representation of nemo_msgs/Progress message
template <class IntType = long, template <class, class...> class ContainterType = std::vector>
class GenericProgress : public MessageBaseClass{
public:
    typedef MessageGroups::ProgressGroup Group;
    GenericProgress(){}
    GenericProgress(const ContainterType<IntType> &progress) :_progress(progress){}
    GenericProgress(const GenericProgress &p) :_progress(p.progress()){}
    ~GenericProgress() {}

    virtual GenericProgress *Clone() const override { return new GenericProgress(*this); }

    virtual const ContainterType<IntType> &progress(void) const {return _progress;}
    virtual       ContainterType<IntType> &progress(void)       {return _progress;}
protected:
    ContainterType<IntType> _progress;
};

typedef GenericProgress<> Progress;

} // namespace NemoMsgs
} // namespace GenericMessages
} // namespace ROSBridge