#pragma once

#include "ros_bridge/include/message_traits.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"

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/Point32 message generation.
namespace point32 {


std::string messageType();

namespace detail {
using namespace ros_bridge::traits;

template <class T>
auto getZ(const T &p, Type2Type<Has3Components>)
{
    return p.z();
}

template <class T>
auto getZ(const T &p, Type2Type<Has2Components>)
{
    (void)p;
    return 0.0; // p has no member z() -> add 0.
}

template <class T, typename V>
bool setZ(const rapidjson::Value &doc, const T &p, Type2Type<V>)
{
    p.setZ(doc["z"].GetFloat());
    return true;
}

template <class T>
bool setZ(const rapidjson::Value &doc, const T &p, Type2Type<Has2Components>)
{
    (void)doc;
    (void)p;
    return true;
}

} // namespace detail

//! @brief C++ representation of a geometry_msgs/Point/Point32
template<typename FloatType = _Float64, class OStream = std::ostream>
class GenericPoint {
public:
    GenericPoint(FloatType x, FloatType y, FloatType z) : _x(x), _y(y), _z(z){}

    FloatType x() const {return _x;}
    FloatType y() const {return _y;}
    FloatType z() const {return _z;}

    void setX(FloatType x) {_x = x;}
    void setY(FloatType y) {_y = y;}
    void setZ(FloatType z) {_z = z;}



    bool operator==(const GenericPoint &p1) {
        return (p1._x == this->_x
                && p1._y == this->_y
                && p1._z == this->_z);
    }

    bool operator!=(const GenericPoint &p1) {
        return !this->operator==(p1);
    }

    friend OStream& operator<<(OStream& os, const GenericPoint& p)
    {
        os << "x: " << p._x << " y: " << p._y<< " z: " << p._z;
        return os;
    }

private:
    FloatType _x;
    FloatType _y;
    FloatType _z;
};

typedef GenericPoint<> Point;
typedef GenericPoint<_Float32> Point32;



template <class T>
bool toJson(const T&p, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
{
    using namespace ros_bridge::traits;

    value.AddMember("x", rapidjson::Value().SetFloat(p.x()), allocator);
    value.AddMember("y", rapidjson::Value().SetFloat(p.y()), allocator);

    typedef typename Select<HasMemberZ<T>::value, Has3Components, Has2Components>::Result
            Components; // Check if PointType has 2 or 3 dimensions.
    auto z = detail::getZ(p, Type2Type<Components>()); // If T has no member z() replace it by 0.

    value.AddMember("z", rapidjson::Value().SetFloat(z), allocator);
    return true;
}

template <class PointType>
bool fromJson(const rapidjson::Value &value,
              PointType &p)     {
    using namespace ros_bridge::traits;

    if (!value.HasMember("x") || !value["x"].IsFloat()){
        assert(false);
        return false;
    }
    if (!value.HasMember("y") || !value["y"].IsFloat()){
        assert(false);
        return false;
    }
    if (!value.HasMember("z") || !value["z"].IsFloat()){
        assert(false);
        return false;
    }

    p.setX(value["x"].GetFloat());
    p.setY(value["y"].GetFloat());
    typedef typename Select<HasMemberSetZ<PointType>::value, Has3Components, Has2Components>::Result
            Components; // Check if PointType has 2 or 3 dimensions.
    (void)detail::setZ(value["z"], p,  Type2Type<Components>()); // If PointType has no member z() discard doc["z"].

    return true;
}

} // namespace point32
} // namespace geometry_msgs
} // namespace messages
} // namespace ros_bridge