#pragma once
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/include/message_traits.h"
#include "ros_bridge/include/messages/geometry_msgs/polygon_stamped.h"
#include "ros_bridge/include/messages/std_msgs/header.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 jsk_recognition_msgs {
//! @brief Namespace containing methodes for jsk_recognition_msgs/PolygonArray message generation.
namespace polygon_array {


std::string messageType();


//! @brief C++ representation of jsk_recognition_msgs/PolygonArray
template <class PolygonType = geometry_msgs::polygon_stamped::PolygonStamped,
          template <class, class...> class ContainerType = std::vector,
          class HeaderType =  std_msgs::header::Header,
          class IntType = long,
          class FloatType = double>
class GenericPolygonArray{
public:
    GenericPolygonArray() {}
    GenericPolygonArray(const GenericPolygonArray &other)
      : _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)
      : _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;}


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


namespace detail {
    //! Helper functions to generate Json entries for labels and likelihood.

    //! \note \p p has member \fn labels().
    template <class PolygonArrayType, int  k>
    void labelsToJson(const PolygonArrayType &p,
                      rapidjson::Value &labels,
                      rapidjson::Document::AllocatorType &allocator,
                      traits::Int2Type<k>){
        for(unsigned long i=0; i < (unsigned long)p.labels().size(); ++i)
            labels.PushBack(rapidjson::Value().SetUint(p.labels()[i]), allocator);
    }

    //! \note \p p has no member \fn labels().
    template <class PolygonArrayType>
    void labelsToJson(const PolygonArrayType &p,
                      rapidjson::Value &labels,
                      rapidjson::Document::AllocatorType &allocator,
                      traits::Int2Type<0>){
        for(unsigned long i=0; i < (unsigned long)(p.polygons().size()); ++i)
            labels.PushBack(rapidjson::Value().SetUint(0), allocator); // use zero!
    }

    //! \note \p p has member \fn likelihood().
    template <class PolygonArrayType, int  k>
    void likelihoodToJson(const PolygonArrayType &p,
                          rapidjson::Value &likelyhood,
                          rapidjson::Document::AllocatorType &allocator,
                          traits::Int2Type<k>){
        p.likelyhood().clear();
        for(unsigned long i=0; i < (unsigned long)p.likelyhood().size(); ++i)
            likelyhood.PushBack(rapidjson::Value().SetFloat(p.likelyhood()[i]), allocator);
    }

    //! \note \p p has no member \fn likelihood().
    template <class PolygonArrayType>
    void likelihoodToJson(const PolygonArrayType &p,
                          rapidjson::Value &likelyhood,
                          rapidjson::Document::AllocatorType &allocator,
                          traits::Int2Type<0>){
        for(unsigned long i=0; i < (unsigned long)p.polygons().size(); ++i)
            likelyhood.PushBack(rapidjson::Value().SetFloat(0), allocator); // use zero!
    }

    //! \note \p p has member \fn labels().
    template <class PolygonArrayType, int k>
    void setLabels(const rapidjson::Value &doc,
                   PolygonArrayType &p,
                   traits::Int2Type<k>){
        p.labels().clear();
        for(unsigned long i=0; i < (unsigned long)doc.Size(); ++i)
            p.labels().push_back(doc[i]);
    }

    //! \note \p p has no member \fn labels().
    template <class PolygonArrayType>
    void setLabels(const rapidjson::Value &doc,
                   PolygonArrayType &p,
                   traits::Int2Type<0>){
        (void)doc;
        (void)p;
    }

    //! \note \p p has member \fn likelihood().
    template <class PolygonArrayType, int k>
    void setLikelihood(const rapidjson::Value &doc,
                       PolygonArrayType &p,
                       traits::Int2Type<k>){
        for(unsigned long i=0; i < (unsigned long)doc.Size(); ++i)
            p.likelihood().push_back(doc[i]);
    }

    //! \note \p p has no member \fn likelihood().
    template <class PolygonArrayType>
    void setLikelihood(const rapidjson::Value &doc,
                       PolygonArrayType &p,
                       traits::Int2Type<0>){
        (void)doc;
        (void)p;
    }

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

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

        rapidjson::Value jPolygons(rapidjson::kArrayType);
        for(unsigned long i=0; i < (unsigned long)(p.polygons().size()); ++i){
            rapidjson::Document jPolygon(rapidjson::kObjectType);
            if (!polygon_stamped::toJson(p.polygons()[i].polygon(), h, jPolygon, allocator)){
                assert(false);
                return false;
            }
            jPolygons.PushBack(jPolygon, allocator);
        }
        value.AddMember("polygons", jPolygons, allocator);


        rapidjson::Value jLabels(rapidjson::kArrayType);
        typedef traits::HasMemberLabels<PolygonArrayType> HasLabels;
        detail::labelsToJson(p, jLabels, allocator, traits::Int2Type<HasLabels::value>());
        value.AddMember("labels", jLabels, allocator);


        rapidjson::Value jLikelihood(rapidjson::kArrayType);
        typedef traits::HasMemberLikelihood<PolygonArrayType> HasLikelihood;
        detail::likelihoodToJson(p, jLikelihood, allocator, traits::Int2Type<HasLikelihood::value>());
        value.AddMember("likelihood", jLikelihood, allocator);

        return true;
    }

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

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


//!
//! Create PolygonArray message from \p p and \p h. \p p doesn't have it's own header.
//! \param p Class implementing the PolygonArray interface.
//! \param h Class implementing the Header interface.
//! \param doc Rapidjson document used to store the PolygonArray message.
//! \param allocator Allocator used by doc. Can be obtained e.g. by calling doc.getAllocator().
//!
//! \note The \fn labels() and \fn likelihood() members are optinal. If any of them is missing they
//! will be replaced by arrays filled with zero and size p.polygons.size().
//!
//! \note If this function is called, the headers in p.polygons[i] (entries implement the the PolygonStamped interface)
//! are ignored.
template <class PolygonArrayType, class HeaderType>
bool toJson(const PolygonArrayType &p,
            const HeaderType &h,
            rapidjson::Value &value,
            rapidjson::Document::AllocatorType &allocator)
{
    return detail::_toJson(p, h, value, allocator);
}



//!
//! Create PolygonArray message from \p p. \p p contains a header.
//! \param p Class implementing the PolygonArrayType interface.
//! \param doc Rapidjson document used to store the PolygonArray message.
//! \param allocator Allocator used by doc. Can be obtained e.g. by calling doc.getAllocator().
//!
//! \note The \fn labels() and \fn likelihood() members are optinal. If any of them is missing they
//! will be replaced by arrays filled with zero and size p.polygons.size().
//!
//! \note If the header() function is missing, a default constructed header is used.
template <class PolygonArrayType>
bool toJson(const PolygonArrayType &p,
            rapidjson::Value &value,
            rapidjson::Document::AllocatorType &allocator)
{
    typedef traits::HasMemberHeader<PolygonArrayType> HasHeader;
    return detail::_toJson(p, value, allocator, traits::Int2Type<HasHeader::value>());
}



template <class PolygonArrayType>
bool fromJson(const rapidjson::Value &value,
              PolygonArrayType &p)
{
    using namespace geometry_msgs;
    if ( !value.HasMember("header")){
        assert(false);
        return false;
    }
    if ( !value.HasMember("polygons") || !value["polygons"].IsArray() ){
        assert(false);
        return false;
    }
    if ( !value.HasMember("labels") || !value["labels"].IsArray() ){
        assert(false);
        return false;
    }
    if ( !value.HasMember("likelihood") || !value["likelihood"].IsArray() ){
        assert(false);
        return false;
    }


    typedef traits::HasMemberHeader<PolygonArrayType> HasHeader;
    if ( !polygon_stamped::detail::setHeader(value["header"],
                                             p,
                                             traits::Int2Type<HasHeader::value>())){
        assert(false);
        return false;
    }

    const auto &jPolygonStamped = value["polygons"];
    p.polygons().clear();
    p.polygons().reserve(jPolygonStamped.Size());
    typedef decltype (p.polygons()[0]) PolyStampedCVR;
    typedef typename std::remove_cv_t<
            typename std::remove_reference_t<PolyStampedCVR>>
            PolyStamped;
    for (unsigned int i=0; i < jPolygonStamped.Size(); ++i) {
        if ( !jPolygonStamped[i].HasMember("header") ){
            assert(false);
            return false;
        }
        PolyStamped polygonStamped;

        if ( !polygon_stamped::detail::setHeader(jPolygonStamped[i]["header"],
                                                 polygonStamped,
                                                 traits::Int2Type<HasHeader::value>())){
            assert(false);
            return false;
        }

        if ( !polygon::fromJson(jPolygonStamped[i]["polygon"], polygonStamped.polygon())){
            assert(false);
            return false;
        }
        p.polygons().push_back(std::move(polygonStamped));
    }

    typedef traits::HasMemberLabels<PolygonArrayType> HasLabels;
    detail::setLabels(value["labels"], p, traits::Int2Type<HasLabels::value>());

    typedef traits::HasMemberLikelihood<PolygonArrayType> HasLikelihood;
    detail::setLikelihood(value["likelihood"], p, traits::Int2Type<HasLikelihood::value>());

    return true;
}


} // namespace polygon_array
} // namespace geometry_msgs
} // namespace messages
} // namespace ros_bridge