#pragma once

#include <string>

#include "ros_bridge/include/message_traits.h"

#include <QJsonObject>
#include <QJsonValue>

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();

namespace {
const char *lonKey = "longitude";
const char *latKey = "latitude";
const char *altKey = "altitude";

} // namespace

//! @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 QJsonValue &o, T &p,
                 traits::Type2Type<traits::Has3Components>) {
  p.setAltitude(o.toDouble());
}

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

template <class T> bool toJson(const T &p, QJsonObject &value) {
  value[latKey] = p.latitude();
  value[lonKey] = p.longitude();

  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[altKey] = altitude;
  return true;
}

template <class PointType>
bool fromJson(const QJsonObject &value, PointType &p) {
  if (!value.contains("latitude") || !value["latitude"].isDouble()) {
    return false;
  }
  if (!value.contains("longitude") || !value["longitude"].isDouble()) {
    return false;
  }
  if (!value.contains("altitude") || !value["altitude"].isDouble()) {
    return false;
  }

  p.setLatitude(value["latitude"].toDouble());
  p.setLongitude(value["longitude"].toDouble());
  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 geo_point
} // namespace geographic_msgs
} // namespace messages
} // namespace ros_bridge