#pragma once

#include <QPolygonF>
#include <QPointF>

#include "ros_bridge/include/MessageGroups.h"
#include "ros_bridge/include/GenericMessages.h"
#include "ros_bridge/include/MessageBaseClass.h"
#include "ros_bridge/include/JsonMethodes.h"

namespace MsgGroupsNS = ROSBridge::MessageGroups;
namespace PolyStampedNS = ROSBridge::JsonMethodes::GeometryMsgs::PolygonStamped;

typedef ROSBridge::MessageBaseClass ROSMsg;

template <class PointType = QPointF, template <class, class...> class ContainerType = QVector>
class Polygon2DTemplate : public ROSMsg{ //
    typedef ROSBridge::GenericMessages::GeometryMsgs::GenericPolygon<PointType, ContainerType> Poly;
public:
    typedef MsgGroupsNS::PolygonStampedGroup Group; // has no header

    Polygon2DTemplate(){}
    Polygon2DTemplate(const Polygon2DTemplate &other) : ROSMsg(), _polygon(other._polygon){}

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

    const Poly &polygon() const {return _polygon;}
          Poly &polygon()       {return _polygon;}

  const ContainerType<PointType> &path() const {return _polygon.points();}
        ContainerType<PointType> &path()       {return _polygon.points();}

    virtual Polygon2DTemplate*Clone() const { // ROSMsg
        return new Polygon2DTemplate(*this);
    }
private:
    Poly _polygon;

};

typedef Polygon2DTemplate<> Polygon2D;