#ifndef TJG_VECTOR_H
#define TJG_VECTOR_H
#pragma once

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

#include <boost/units/quantity.hpp>

#include <cstddef>

namespace tjg {

template<class Unit, class Rep = double> class Vector;

template<class Unit, class Rep = double>
class Point {
  typedef Unit unit_type;
  typedef Rep value_type;
  typedef ::boost::units::quantity<unit_type, value_type> quantity_type;

  typedef ::boost::geometry::point<
      value_type, 3, ::boost::geometry::cs::cartesian> Impl;

  Impl _impl;

public:
  Point() : _impl(value_type()) { }

  template<class UX, class TX, class UY, class TY>
  Point(const ::boost::units::quantity<UX, TX>& x_,
        const ::boost::units::quantity<UY, TY>& y_)
    : _impl(static_cast<quantity_type>(x_).value(),
            static_cast<quantity_type>(y_).value())
    { }

  template<class UX, class TX, class UY, class TY, class UZ, class TZ>
  Point(const ::boost::units::quantity<UX, TX>& x_,
        const ::boost::units::quantity<UY, TY>& y_,
        const ::boost::units::quantity<UZ, TZ>& z_)
    : _impl(static_cast<quantity_type>(x_).value(),
            static_cast<quantity_type>(y_).value(),
            static_cast<quantity_type>(z_).value())
    { }

  // Conversion to/from geometry::point
  explicit Point(Impl pt) : _impl(pt) { }

  const Impl& as_point() const { return _impl; }
        Impl& as_point()       { return _impl; }

  operator Impl() const { return as_point(); }

  template<int N>
  quantity_type get() const
    { return quantity_type::from_value(_impl.get<N>()); }

  quantity_type x() const { return get<0>(); }
  quantity_type y() const { return get<1>(); }
  quantity_type z() const { return get<2>(); }

  template<int N, class U, class T>
  void set(const ::boost::units::quantity<U, T>& v)
    { _impl.set<N>(static_cast<quantity_type>(v).value()); }

  template<class U, class T>
  void x(const ::boost::units::quantity<U, T>& v) { set<0>(v); }

  template<class U, class T>
  void y(const ::boost::units::quantity<U, T>& v) { set<1>(v); }

  template<class U, class T>
  void z(const ::boost::units::quantity<U, T>& v) { set<2>(v); }

  friend bool operator==(const Point& lhs, const Point& rhs)
    { return ::boost::geometry::equals(lhs._impl, rhs._impl); }

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

}; // Point

// TODO Tell boost::geometry how to use Point.

template<class Unit, class Rep>
class Vector {

public:
  typedef Unit unit_type;
  typedef Rep value_type;
  typedef ::boost::units::quantity<unit_type, value_type> quantity_type;
  typedef Point<unit_type, value_type> point_type;

private:
  typedef ::boost::geometry::point<
      value_type, 3, ::boost::geometry::cs::cartesian> Impl;

  Impl _impl;

public:
  Vector() : _impl(value_type()) { }

  template<class UX, class TX, class UY, class TY>
  Vector(const ::boost::units::quantity<UX, TX>& dx,
         const ::boost::units::quantity<UY, TY>& dy)
    : _impl(static_cast<quantity_type>(dx).value(),
            static_cast<quantity_type>(dy).value())
    { }

  template<class UX, class TX, class UY, class TY, class UZ, class TZ>
  Vector(const ::boost::units::quantity<UX, TX>& dx,
         const ::boost::units::quantity<UY, TY>& dy,
         const ::boost::units::quantity<UZ, TZ>& dz)
    : _impl(static_cast<quantity_type>(dx).value(),
            static_cast<quantity_type>(dy).value(),
            static_cast<quantity_type>(dz).value())
    { }

  Vector(const point_type& from_, const point_type& to_)
    : _impl(to_.as_point())
    { ::boost::geometry::subtract_point(_impl, from_.as_point()); }

  template<int N>
  quantity_type get() const
    { return quantity_type::from_value(_impl.get<N>()); }

  quantity_type x() const { return get<0>(); }
  quantity_type y() const { return get<1>(); }
  quantity_type z() const { return get<2>(); }

  template<int N, class U, class T>
  void set(const ::boost::units::quantity<U, T>& v)
    { _impl.set<N>(static_cast<quantity_type>(v).value()); }

  template<class U, class T>
  void x(const ::boost::units::quantity<U,T>& v) { set<0>(v); }

  template<class U, class T>
  void y(const ::boost::units::quantity<U,T>& v) { set<1>(v); }

  template<class U, class T>
  void z(const ::boost::units::quantity<U,T>& v) { set<2>(v); }

  Vector& operator+=(const Vector& rhs) {
    ::boost::geometry::add_point(_impl, rhs._impl);
    return *this;
  }

  Vector& operator-=(const Vector& rhs) {
    ::boost::geometry::subtract_point(_impl, rhs._impl);
    return *this;
  }

  Vector& operator*=(const quantity_type& x) {
    ::boost::geometry::multiply_value(_impl, x.value());
    return *this;
  }

  Vector& operator/=(const quantity_type& x) {
    ::boost::geometry::divide_value(_impl, x.value());
    return *this;
  }

  friend Vector operator+(Vector lhs, const Vector& rhs)
    { return lhs += rhs; }

  friend Vector operator-(Vector lhs, const Vector& rhs)
    { return lhs -= rhs; }

  friend quantity_type operator*(const Vector& lhs, const Vector& rhs) {
    return quantity_type::from_value(
        ::boost::geometry::dot_product(lhs._impl, rhs._impl));
  }

  friend Vector operator*(Vector lhs, const quantity_type& rhs)
    { return lhs *= rhs.value(); }

  friend Vector operator/(Vector lhs, const quantity_type& rhs)
    { return lhs /= rhs.value(); }

  friend Vector cross_product(const Vector& lhs, const Vector& rhs)
    { return Vector(::boost::geometry::cross_product(lhs._impl, rhs._impl)); }

  friend point_type operator+(point_type lhs, const Vector& rhs) {
    ::boost::geometry::add_point(lhs.as_point(), rhs._impl);
    return lhs;
  }

  friend point_type operator+(const Vector& lhs, point_type rhs)
    { return rhs + lhs; }

  friend bool operator==(const Vector& lhs, const Vector& rhs)
    { return ::boost::geometry::equals(lhs._impl, rhs._impl); }

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

  friend quantity_type abs(const Vector& v) {
    return quantity_type::from_value(
        static_cast<value_type>(
          ::boost::geometry::distance(v._impl, Impl(value_type()))
    ));
  } // abs

  friend quantity_type abs_squared(const Vector& v) {
    return ::boost::geometry::distance(v._impl, Impl(value_type())
        ).squared_distance();
  } // abs_squared

  // Is it a good idea to order Vectors based on their magnitude?

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

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

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

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

}; // Vector

template<class Unit, class Rep> inline
Vector<Unit, Rep> operator-(const Point<Unit, Rep>& lhs,
                            const Point<Unit, Rep>& rhs)
  { return Vector<Unit, Rep>(rhs, lhs); }

template<class Ch, class  Tr, class Unit, class Rep>
std::basic_ostream<Ch, Tr>&
operator<<(std::basic_ostream<Ch, Tr>& os, const Point<Unit, Rep>& p) {
  os << '(' << p.x().value()
     << ' ' << p.y().value()
     << ' ' << p.z().value()
     << ") " << Unit();
  return os;
} // << Point

template<class Ch, class Tr, class Unit, class Rep>
std::basic_ostream<Ch, Tr>&
operator<<(std::basic_ostream<Ch, Tr>& os, const Vector<Unit, Rep>& v) {
  os << '<' << v.x().value()
     << ' ' << v.y().value()
     << ' ' << v.z().value()
     << "> " << Unit();
  return os;
} // << Vector

} // tjg

#endif