#ifndef MY_QUANTITY_CARTESIAN_DISTANCE_H
#define MY_QUANTITY_CARTESIAN_DISTANCE_H
#pragma once

#include <boost/geometry/geometry.hpp>
#include <boost/geometry/strategies/distance.hpp>

#include <boost/units/quantity.hpp>
#include <boost/units/cmath.hpp>
#include <boost/units/io.hpp>

#include <boost/type_traits.hpp>
#include <boost/mpl/if.hpp>

#include <ostream>
#include <cstddef>

namespace boost { namespace geometry {

template<class Unit, class T>
struct cartesian_distance< ::boost::units::quantity<Unit, T> > {
private:
  typedef ::boost::units::quantity<Unit, T> coord_type;
  typedef typename ::boost::units::multiply_typeof_helper<
          coord_type, coord_type
      >::type squared_type;

  cartesian_distance<T> m_distance;

  typedef typename ::boost::mpl::if_< ::boost::is_integral<T>, double, T>::type
      value_cast_type;

  typedef ::boost::units::quantity<Unit, value_cast_type> cast_type;

public:
  explicit cartesian_distance(const squared_type& v)
    : m_distance(v.value()) { }

  operator cast_type() const
    { return cast_type::from_value(static_cast<value_cast_type>(m_distance)); }

  bool very_small() const { return m_distance.very_small(); }

  squared_type squared_value() const
    { return squared_type::from_value(m_distance.squared_value()); }

  friend
  bool operator==(const cartesian_distance& lhs, const cartesian_distance& rhs)
    { return (lhs.m_distance == rhs.m_distance); }

  friend
  bool operator!=(const cartesian_distance& lhs, const cartesian_distance& rhs)
    { return !(lhs == rhs); }

  friend
  bool operator<(const cartesian_distance& lhs, const cartesian_distance& rhs)
    { return (lhs.m_distance < rhs.m_distance); }

  friend
  bool operator>(const cartesian_distance& lhs, const cartesian_distance& rhs)
    { return (rhs < lhs); }

  friend
  bool operator>=(const cartesian_distance& lhs, const cartesian_distance& rhs)
    { return !(lhs < rhs); }

  friend
  bool operator<=(const cartesian_distance& lhs, const cartesian_distance& rhs)
    { return !(rhs < lhs); }

  template<typename Ch, class Tr>
  friend std::basic_ostream<Ch, Tr>&
  operator<<(std::basic_ostream<Ch, Tr>& os, const cartesian_distance& d)
    { return os << static_cast<cast_type>(d); }
                            
}; // cartesian_distance for ::boost::units::quantity

namespace strategy { namespace distance {

namespace detail {

template<class Point1, class Point2, std::size_t I, class Unit, typename T>
struct compute_pythagoras<Point1, Point2, I,
                          ::boost::units::quantity<Unit, T> >
{
private:
  typedef ::boost::units::quantity<Unit, T> coord_type;
  typedef typename ::boost::units::multiply_typeof_helper<
          coord_type, coord_type
      >::type result_type;

public:
  static result_type apply(const Point1& p1, const Point2& p2) {
    coord_type c1 = ::boost::numeric_cast<coord_type>(get<I-1>(p1));
    coord_type c2 = ::boost::numeric_cast<coord_type>(get<I-1>(p2));
    coord_type d = c2 - c1;
    return d * d
        + compute_pythagoras<Point1, Point2, I-1, coord_type>::apply(p1, p2);
  } // apply
}; // compute_pythagoras specialization for units

template<class Point1, class Point2, class Unit, typename T>
struct compute_pythagoras<Point1, Point2, 1,
                          ::boost::units::quantity<Unit, T> >
{
private:
  typedef ::boost::units::quantity<Unit, T> coord_type;
  typedef typename ::boost::units::multiply_typeof_helper<
          coord_type, coord_type
      >::type result_type;

public:
  static result_type apply(const Point1& p1, const Point2& p2) {
    coord_type c1 = ::boost::numeric_cast<coord_type>(get<0>(p1));
    coord_type c2 = ::boost::numeric_cast<coord_type>(get<0>(p2));
    coord_type d = c2 - c1;
    return d * d;
  } // apply
}; // compute_pythagoras specialization for units

template<class Point1, class Point2, class Unit, typename T>
struct compute_pythagoras<Point1, Point2, 0,
                          ::boost::units::quantity<Unit, T> >
{
private:
  typedef ::boost::units::quantity<Unit, T> coord_type;
  typedef typename ::boost::units::multiply_typeof_helper<
          coord_type, coord_type
      >::type result_type;

public:
  static result_type apply(const Point1&, const Point2&)
    { return result_type::from_value(::boost::numeric_cast<T>(0)); }
}; // compute_pythagoras specialization for units

} // detail

template<std::size_t Dim, class System,
         class Unit1, class T1, class Unit2, class T2,
         typename CalculationType>
struct pythagoras<
  point<::boost::units::quantity<Unit1, T1>, Dim, System>,
  point<::boost::units::quantity<Unit2, T2>, Dim, System>,
  CalculationType>
{
  typedef point<::boost::units::quantity<Unit1, T1>, Dim, System>
      first_point_type;
  typedef point<::boost::units::quantity<Unit2, T2>, Dim, System>
      second_point_type;
  typedef typename select_calculation_type<
          first_point_type, second_point_type, CalculationType
      >::type calculation_type;

  typedef cartesian_distance<calculation_type> return_type;

  return_type apply(const first_point_type& p1, const second_point_type& p2)
      const
  {
    BOOST_CONCEPT_ASSERT( (concept::ConstPoint<first_point_type>) );
    BOOST_CONCEPT_ASSERT( (concept::ConstPoint<second_point_type>) );
    return return_type(
          detail::compute_pythagoras<
              first_point_type, second_point_type, Dim, calculation_type
          >::apply(p1, p2)
        );
  } // apply
}; // pythagoras specialied for cartesian

} } } } // boost::geometry::strategy::distance

#endif