#pragma once

#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/include/messages/geometry_msgs/polygon.h"
#include "ros_bridge/include/messages/std_msgs/header.h"
#include "ros_bridge/include/message_traits.h"

#include <type_traits>

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/PolygonStamped message generation.
namespace polygon_stamped {


std::string messageType();

//! @brief C++ representation of geometry_msgs/PolygonStamped
template <class PolygonType  = geometry_msgs::polygon::Polygon,
          class HeaderType = std_msgs::header::Header>
class GenericPolygonStamped {
    typedef PolygonType Polygon;
public:
    GenericPolygonStamped(){}
    GenericPolygonStamped(const GenericPolygonStamped &other) :
        _header(other.header())
      , _polygon(other.polygon())
    {}
    GenericPolygonStamped(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;}

private:
    HeaderType   _header;
    Polygon      _polygon;
};

typedef GenericPolygonStamped<> PolygonStamped;

// ===================================================================================
namespace detail {

template <class PolygonStampedType>
bool setHeader(const rapidjson::Value &doc,
               PolygonStampedType &polyStamped,
               traits::Int2Type<1>) { // polyStamped.header() exists
    using namespace std_msgs;

    typedef decltype (polyStamped.header()) HeaderTypeCVR;
    typedef typename std::remove_cv_t<typename std::remove_reference_t<HeaderTypeCVR>> HeaderType;
    HeaderType header;
    bool ret = header::fromJson(doc, header);
    polyStamped.header() = header;
    return ret;
}
template <class PolygonStampedType>
bool setHeader(const rapidjson::Value &doc,
               PolygonStampedType &polyStamped,
               traits::Int2Type<0>) { // polyStamped.header() does not exists
    (void)doc;
    (void)polyStamped;
    return true;
}


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

    rapidjson::Document header(rapidjson::kObjectType);
    if (!header::toJson(h, header, allocator)){
        assert(false);
        return false;
    }
    rapidjson::Document polygon(rapidjson::kObjectType);
    if (!polygon::toJson(poly, polygon, allocator)){
        assert(false);
        return false;
    }
    value.AddMember("header", header, allocator);
    value.AddMember("polygon", polygon, allocator);

    return true;
}

template<class PolyStamped, int k>
bool _toJson(const PolyStamped &polyStamped,
             rapidjson::Value &value,
             rapidjson::Document::AllocatorType &allocator,
             traits::Int2Type<k>){ // PolyStamped has member header(), use integraded header.
return _toJson(polyStamped, polyStamped.header(), value, allocator);
}

template<class PolyStamped>
bool _toJson(const PolyStamped &polyStamped,
             rapidjson::Value &value,
             rapidjson::Document::AllocatorType &allocator,
             traits::Int2Type<0>){ // PolyStamped has no member header(), generate one on the fly.
using namespace std_msgs::header;
return _toJson(polyStamped, Header(), value, allocator);
}

} // namespace detail
// ===================================================================================


template <class PolygonType, class HeaderType>
bool toJson(const PolygonType &poly,
            const HeaderType &h,
            rapidjson::Value &value,
            rapidjson::Document::AllocatorType &allocator)
{
    return detail::_toJson(poly, h, value, allocator);
}

template <class PolyStamped>
bool toJson(const PolyStamped &polyStamped,
            rapidjson::Value &value,
            rapidjson::Document::AllocatorType &allocator)
{
    typedef traits::HasMemberHeader<PolyStamped> HasHeader;
    return detail::_toJson(polyStamped, value, allocator, traits::Int2Type<HasHeader::value>());
}

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

    if ( !value.HasMember("header") ){
        assert(false);
        return false;
    }
    if ( !value.HasMember("polygon") ){
        assert(false);
        return false;
    }

    typedef traits::HasMemberSetHeader<PolygonType> HasHeader;
    if ( !detail::setHeader(value["header"], polyStamped, traits::Int2Type<HasHeader::value>())){
        assert(false);
        return false;
    }

    if ( !polygon::fromJson(value["polygon"], polyStamped.polygon()) ){
        assert(false);
        return false;
    }

    return true;
}

} // namespace polygon_stamped
} // namespace geometry_msgs
} // namespace messages
} // namespace ros_bridge