#pragma once

#include <string>

#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 geographic_msgs {
//! @brief Namespace containing methodes for geometry_msgs/GeoPoint message generation.
namespace geo_point {

std::string messageType(){
    return "geometry_msgs/GeoPoint";
}

//! @brief C++ representation of geographic_msgs/GeoPoint.
template<class FloatType = _Float64, class OStream = std::ostream>
class GenericGeoPoint{
public:
    GenericGeoPoint()
        : _latitude(0)
        , _longitude(0)
        , _altitude(0)
    {}
    GenericGeoPoint(const GenericGeoPoint &other)
        : _latitude(other.latitude())
        , _longitude(other.longitude())
        , _altitude(other.altitude())
    {}
    GenericGeoPoint(FloatType latitude, FloatType longitude, FloatType altitude)
        : _latitude(latitude)
        , _longitude(longitude)
        , _altitude(altitude)
    {}

    FloatType latitude()     const {return _latitude;}
    FloatType longitude()    const {return _longitude;}
    FloatType altitude()     const {return _altitude;}

    void setLatitude    (FloatType latitude)    {_latitude  = latitude;}
    void setLongitude   (FloatType longitude)   {_longitude = longitude;}
    void setAltitude    (FloatType altitude)    {_altitude  = altitude;}

    bool operator==(const GenericGeoPoint &p1) {
        return (   p1._latitude == this->_latitude
                && p1._longitude == this->_longitude
                && p1._altitude == this->_altitude);
    }

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

    friend OStream& operator<<(OStream& os, const GenericGeoPoint& p)
    {
        os << "lat: " << p._latitude << " lon: " << p._longitude<< " alt: " << p._altitude;
        return os;
    }

private:
    FloatType _latitude;
    FloatType _longitude;
    FloatType _altitude;
};
typedef GenericGeoPoint<> GeoPoint;

namespace detail {

    template <class T>
    auto getAltitude(const T &p, traits::Type2Type<traits::Has3Components>)
    {
        return p.altitude();
    }

    template <class T>
    auto getAltitude(const T &p, traits::Type2Type<traits::Has2Components>)
    {
        (void)p;
        return 0.0;
    }

    template <class T>
    void setAltitude(const rapidjson::Value &doc, T &p, traits::Type2Type<traits::Has3Components>)
    {
        p.setAltitude(doc.GetFloat());
    }

    template <class T>
    void setAltitude(const rapidjson::Value &doc, T &p, traits::Type2Type<traits::Has2Components>)
    {
        (void)doc;
        (void)p;
    }
} // namespace detail

template <class T>
bool toJson(const T&p, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
{
    value.AddMember("latitude", rapidjson::Value().SetFloat((_Float64)p.latitude()), allocator);
    value.AddMember("longitude", rapidjson::Value().SetFloat((_Float64)p.longitude()), allocator);

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

    value.AddMember("altitude", rapidjson::Value().SetFloat((_Float64)altitude), allocator);
    value.AddMember("type", rapidjson::Value().SetString(messageType().c_str()), allocator);
    return true;
}



template <class PointType>
bool fromJson(const rapidjson::Value &value,
              PointType &p)     {
    if (!value.HasMember("latitude") || !value["latitude"].IsFloat()){
        assert(false);
        return false;
    }
    if (!value.HasMember("longitude") || !value["longitude"].IsFloat()){
        assert(false);
        return false;
    }
    if (!value.HasMember("altitude") || !value["altitude"].IsFloat()){
        assert(false);
        return false;
    }

    p.setLatitude(value["latitude"].GetFloat());
    p.setLongitude(value["longitude"].GetFloat());
    typedef typename traits::Select<
            traits::HasMemberSetAltitude<PointType>::value,
            traits::Has3Components,
            traits::Has2Components>::Result
            Components; // Check if PointType has 2 or 3 dimensions.
    detail::setAltitude(value["altitude"], p,  traits::Type2Type<Components>()); // If T has no member altitude() discard doc["altitude"];

    return true;
} // namespace detail


} // namespace geopoint
} // namespace geometry_msgs
} // namespace messages
} // namespace ros_bridge