#ifndef ROSJSONFACTORY_H
#define ROSJSONFACTORY_H

#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/include/messages.h"
#include "ROSMessageType.h"
#include <QString>
#include "WimaAreaData.h"
#include "utilities.h"
#include "ros_bridge/include/ROSMessageTraits.h"

#include "ROSMessageGroups.h"

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

class StdHeaderPolicy;

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

    ROSJsonFactory(){}

    //!
    //! \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 ROSMessageType.
    //! \param msg Instance of a \class ROSMessageType 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 ROSMessageType.");
        static_assert(!::boost::is_same<typename T::Group, EmptyGroup>::value,
                "msg belongs to group EmptyGroup (not allowed). Please specify Group (typedef MessageGroup Group)");
        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!
    }

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

        return doc;
    }

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

        return doc;
    }

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

        return doc;
    }

    // ===========================================================================
    // PolygonStampedGroup
    // ===========================================================================
    template<class U>
    rapidjson::Document *_create(const U &msg, Type2Type<PolygonStampedGroup>){
        using namespace ros_bridge;
        using namespace ros_bridge::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 ros_bridge;
        rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
        bool ret = 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 ros_bridge;
        Header::Header header(HeaderPolicy::header(msg));
        rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
        bool ret = PolygonStamped::toJson(msg, header, *doc, doc->GetAllocator());
        assert(ret);
        (void)ret;

        return doc;
    }

    // ===========================================================================
    // PolygonArrayGroup
    // ===========================================================================
    template<class U>
    rapidjson::Document *_create(const U &msg, Type2Type<PolygonArrayGroup>){
        using namespace ros_bridge;
        using namespace ros_bridge::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 ros_bridge;
        rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
        bool ret = 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 ros_bridge;
        Header::Header header(HeaderPolicy::header(msg));
        rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
        bool ret = PolygonArray::toJson(msg, header, *doc, doc->GetAllocator());
        assert(ret);
        (void)ret;

        return doc;
    }


};

class StdHeaderPolicy{
public:
    StdHeaderPolicy():_seq(-1){};

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


    //!
    //! \brief time Returns the current time.
    //! \return Returns the current time.
    template<typename T>
    ros_bridge::Time::Time time(const T&msg) {
        (void)msg;
        return ros_bridge::Time::Time(0,0);
    }
private:
    long _seq;
};

#endif // ROSJSONFACTORY_H
