#pragma once
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/include/messages/geometry_msgs/point32.h"

#include <type_traits>
#include <vector>

namespace ros_bridge {
//! @brief Namespace containing classes and methodes ros message generation.
namespace messages {
//! @brief Namespace containing classes and methodes for geometry_msgs generation.
namespace geometry_msgs {
//! @brief Namespace containing methodes for geometry_msgs/Polygon message generation.
namespace polygon {


std::string messageType();

//! @brief C++ representation of geometry_msgs/Polygon
template <class PointTypeCVR,
          template <class, class...> class ContainerType = std::vector>
class GenericPolygon {
    typedef typename std::remove_cv_t<typename std::remove_reference_t<PointTypeCVR>> PointType;
public:
    GenericPolygon() {}
    GenericPolygon(const GenericPolygon &poly) : _points(poly.points()){}


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

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

private:
    ContainerType<PointType> _points;
};
typedef GenericPolygon<geometry_msgs::point32::Point> Polygon;


template <class PolygonType>
bool toJson(const PolygonType &poly, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
{
    using namespace geometry_msgs::point32;

    rapidjson::Value points(rapidjson::kArrayType);

    for(unsigned long i=0; i < std::uint64_t(poly.points().size()); ++i) {
        rapidjson::Document point(rapidjson::kObjectType);
        if ( !point32::toJson(poly.points()[i], point, allocator) ){
            assert(false);
            return false;
        }
        points.PushBack(point, allocator);
    }

    value.AddMember("points", points, allocator);
    return true;
}

template <class PolygonType>
bool fromJson(const rapidjson::Value &value, PolygonType &poly)
{
    using namespace geometry_msgs::point32;

    if (!value.HasMember("points") || !value["points"].IsArray()){
        assert(false);
        return false;
    }
    const auto &jsonArray = value["points"].GetArray();
    poly.points().clear();
    poly.points().reserve(jsonArray.Size());
    typedef decltype (poly.points()[0]) PointTypeCVR;
    typedef typename std::remove_cv_t<typename std::remove_reference_t<PointTypeCVR>> PointType;
    for (long i=0; i < jsonArray.Size(); ++i){
        PointType pt;
        if ( !point32::fromJson(jsonArray[i], pt) ){
            assert(false);
            return false;
        }
        poly.points().push_back(std::move(pt));
    }
    return true;
}

} // namespace polygon
} // namespace geometry_msgs
} // namespace messages
} // namespace ros_bridge