#pragma once

#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/include/JsonMethodes.h"
#include "MessageBaseClass.h"
#include "utilities.h"

#include "ros_bridge/include/MessageTraits.h"
#include "ros_bridge/include/MessageGroups.h"
#include "ros_bridge/include/GenericMessages.h"

#include "boost/type_traits.hpp"
#include "boost/type_traits/is_base_of.hpp"

namespace ROSBridge {

class StdHeaderPolicy;

//!
//! \brief The JsonFactory class is used to create ROS messages.
//!
//! The JsonFactory class is used to create \class rapidjson::Document documents containing ROS messages
//! from classes derived from \class MessageBaseClass. Each class has a group mark (typedef ... Group) which allows the
//! JsonFactory to determine the ROS message type it will create.
template <class HeaderPolicy = StdHeaderPolicy>
class GenericJsonFactory : public HeaderPolicy
{
    typedef MessageBaseClass ROSMsg;
public:

    GenericJsonFactory() : HeaderPolicy() {}

    //!
    //! \brief Creates a \class rapidjson::Document document containing a ROS mesage from \p msg.
    //!
    //! Creates a \class rapidjson::Document document containing a ROS message from \p msg.
    //! A compile-time error will occur, if \p msg belongs to the \struct EmptyGroup or is
    //! not derived from \class MessageBaseClass.
    //! \param msg Instance of a \class MessageBaseClass subclass belonging to a ROSMessageGroup.
    //! \return rapidjson::Document document containing a ROS message.
    template <class T>
    rapidjson::Document *create(const T &msg){
        static_assert(boost::is_base_of<ROSMsg, T>::value, "Type of msg must be derived from MessageBaseClass.");
        static_assert(!::boost::is_same<typename T::Group, MessageGroups::EmptyGroup>::value,
                "msg belongs to group EmptyGroup (not allowed). Please specify Group (typedef MessageGroup Group)");
        //cout << T::Group::label() << endl;
        return _create(msg, Type2Type<typename T::Group>());
    }

private:

    // ===========================================================================
    // EmptyGroup and not implemented Groups
    // ===========================================================================
    template<class U, class V>
    rapidjson::Document *_create(const U &msg, Type2Type<V>){
        (void)msg;
        assert(false); // Implementation missing for group U::Group!
        return nullptr;
    }

    // ===========================================================================
    // Point32Group
    // ===========================================================================
    template<class U>
    rapidjson::Document *_create(const U &msg, Type2Type<MessageGroups::Point32Group>){
        using namespace ROSBridge;
        rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
        bool ret = JsonMethodes::GeometryMsgs::Point32::toJson<_Float32>(msg, *doc, doc->GetAllocator());
        assert(ret);
        (void)ret;

        return doc;
    }

    // ===========================================================================
    // GeoPointGroup
    // ===========================================================================
    template<class U>
    rapidjson::Document *_create(const U &msg, Type2Type<MessageGroups::GeoPointGroup>){
        using namespace ROSBridge;
        rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
        bool ret = JsonMethodes::GeographicMsgs::GeoPoint::toJson(msg, *doc, doc->GetAllocator());
        assert(ret);
        (void)ret;

        return doc;
    }

    // ===========================================================================
    // PolygonGroup
    // ===========================================================================
    template<class U>
    rapidjson::Document *_create(const U &msg, Type2Type<MessageGroups::PolygonGroup>){
        using namespace ROSBridge;
        rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
        bool ret = JsonMethodes::GeometryMsgs::Polygon::toJson(msg, *doc, doc->GetAllocator());
        assert(ret);
        (void)ret;

        return doc;
    }

    // ===========================================================================
    // PolygonStampedGroup
    // ===========================================================================
    template<class U>
    rapidjson::Document *_create(const U &msg, Type2Type<MessageGroups::PolygonStampedGroup>){
        using namespace ROSBridge;
        using namespace ROSBridge::traits;
        typedef HasMemberHeader<U> HasHeader;

        return _createPolygonStamped(msg, Int2Type<HasHeader::value>());
    }

    template<class U, int k>
    rapidjson::Document *_createPolygonStamped(const U &msg, Int2Type<k>){ // U has member header(), use integraded header.
        using namespace ROSBridge;
        rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
        bool ret = JsonMethodes::GeometryMsgs::PolygonStamped::toJson(msg, *doc, doc->GetAllocator());
        assert(ret);
        (void)ret;

        return doc;

    }

    template<class U>
    rapidjson::Document *_createPolygonStamped(const U &msg, Int2Type<0>){// U has no member header(), generate one on the fly.
        using namespace ROSBridge;
        GenericMessages::StdMsgs::Header header(HeaderPolicy::header(msg));
        rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
        bool ret = JsonMethodes::GeometryMsgs::PolygonStamped::toJson(msg.polygon(), header, *doc, doc->GetAllocator());
        assert(ret);
        (void)ret;

        return doc;
    }

    // ===========================================================================
    // PolygonArrayGroup
    // ===========================================================================
    template<class U>
    rapidjson::Document *_create(const U &msg, Type2Type<MessageGroups::PolygonArrayGroup>){
        using namespace ROSBridge;
        using namespace ROSBridge::traits;
        typedef HasMemberHeader<U> HasHeader;

        return _createPolygonArray(msg, Int2Type<HasHeader::value>());
    }

    template<class U, int k>
    rapidjson::Document *_createPolygonArray(const U &msg, Int2Type<k>){ // U has member header(), use integraded header.
        using namespace ROSBridge;
        rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
        bool ret = JsonMethodes::JSKRecognitionMsgs::PolygonArray::toJson(msg, *doc, doc->GetAllocator());
        assert(ret);
        (void)ret;

        return doc;

    }

    template<class U>
    rapidjson::Document *_createPolygonArray(const U &msg, Int2Type<0>){// U has no member header(), generate one on the fly.
        using namespace ROSBridge;
        GenericMessages::StdMsgs::Header header(HeaderPolicy::header(msg));
        rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
        bool ret = JsonMethodes::JSKRecognitionMsgs::PolygonArray::toJson(msg, header, *doc, doc->GetAllocator());
        assert(ret);
        (void)ret;

        return doc;
    }

    // ===========================================================================
    // ProgressGroup
    // ===========================================================================
    template<class U>
    rapidjson::Document *_create(const U &msg, Type2Type<MessageGroups::ProgressGroup>){
        using namespace ROSBridge;
        rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
        bool ret = JsonMethodes::NemoMsgs::Progress::toJson(msg, *doc, doc->GetAllocator());
        assert(ret);
        (void)ret;

        return doc;
    }
};

class StdHeaderPolicy
{
    typedef ROSBridge::GenericMessages::StdMsgs::Header Header;
    typedef ROSBridge::GenericMessages::StdMsgs::Time Time;
public:
    StdHeaderPolicy():_seq(-1){}

    //!
    //! \brief header Returns the header belonging to msg.
    //! \return Returns the header belonging to msg.
    template<class T>
    Header header(const T &msg) {
        return Header(std::uint32_t(++_seq), time(msg), "/map");
    }


    //!
    //! \brief time Returns the current time.
    //! \return Returns the current time.    
    template<class T>
    Time time(const T &msg) {
        (void)msg;
        return Time(0,0);
    }

private:
    long _seq;
};

typedef GenericJsonFactory<> JsonFactory;

} // end namespace ros_bridge