#ifndef TJG_BASIC_VECTOR_H
#define TJG_BASIC_VECTOR_H
#pragma once

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

#include <ostream>

namespace tjg_basic {

template<class Rep> class Vector;

template<class Rep>
struct Point {
  typedef Rep value_type;
  value_type x, y, z;
  typedef Vector<value_type> vector_type;

  Point() : x(), y(), z() { }

  Point(value_type x_, value_type y_, value_type z_ = value_type())
    : x(x_), y(y_), z(z_) { }

  template<int N> value_type get() const;
  template<> value_type get<0>() const { return x; }
  template<> value_type get<1>() const { return y; }
  template<> value_type get<2>() const { return z; }

  template<int N> void set(value_type v);
  template<> void set<0>(value_type v) { x = v; }
  template<> void set<1>(value_type v) { y = v; }
  template<> void set<2>(value_type v) { z = v; }

  Point& operator+=(const vector_type& v);
  Point& operator-=(const vector_type& v);

  friend bool operator==(const Point& lhs, const Point& rhs)
    { return lhs.x == rhs.x && lhs.y == rhs.y && lhs.z == rhs.z; }

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

}; // Point

template<class Rep>
struct Vector {
  typedef Rep value_type;
  typedef Point<Rep> point_type;

  value_type x, y, z;

  Vector() : x(), y(), z() { }

  Vector(value_type dx, value_type dy, value_type dz = value_type())
    : x(dx), y(dy), z(dz) { }

  Vector(const point_type& from_, const point_type& to_)
    : x(to_.x - from_.x), y(to_.y - from_.y), z(to_.z - from_.z) { }

  template<int N> value_type get() const;
  template<> value_type get<0>() const { return x; }
  template<> value_type get<1>() const { return y; }
  template<> value_type get<2>() const { return z; }

  template<int N> void set(value_type v);
  template<> void set<0>(value_type v) { x = v; }
  template<> void set<1>(value_type v) { y = v; }
  template<> void set<2>(value_type v) { z = v; }

  Vector& operator+=(const Vector& rhs)
    { x += rhs.x; y += rhs.y; z += rhs.z; return *this; }

  Vector& operator-=(const Vector& rhs)
    { x -= rhs.x; y -= rhs.y; z -= rhs.z; return *this; }

  Vector& operator*=(const value_type& rhs)
    { x *= rhs; y *= rhs; z *= rhs; return *this; }

  Vector& operator/=(const value_type& rhs)
    { x /= rhs; y /= rhs; z /= rhs; 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 Vector operator*(Vector lhs, const value_type& rhs)
    { return lhs *= rhs; }

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

  // dot product
  friend value_type operator*(const Vector& lhs, const Vector& rhs)
    { return lhs.x * rhs.x + lhs.y * rhs.y + lhs.z + rhs.z; }

  friend Vector cross_product(const Vector& lhs, const Vector& rhs) {
    return Vector(lhs.y * rhs.z - lhs.z * rhs.y,
                  lhs.z * rhs.x - lhs.x * rhs.z,
                  lhs.x * rhs.y - lhs.y * rhs.x);
  } // cross_product

  friend point_type operator+(point_type lhs, const Vector& rhs)
    { lhs.x += rhs.x; lhs.y += rhs.y; lhs.z += rhs.z; 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 lhs.x == rhs.x && lhs.y == rhs.y && lhs.z == rhs.z; }

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

  friend value_type abs_squared(const Vector& v)
    { return v.x * v.x + v.y * v.y + v.z * v.z; }

  friend value_type abs(const Vector& v) {
    using namespace std;
    return sqrt(abs_squared(v));
  }

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

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

  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 T> inline
Point<T>& Point<T>::operator+=(const Vector<T>& rhs)
  { x += rhs.x; y += rhs.y; z += rhs.z; return *this; }

template<class T> inline
Point<T>& Point<T>::operator-=(const Vector<T>& rhs)
  { x -= rhs.x; y -= rhs.y; z -= rhs.z; return *this; }

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

// boost::units support

template<class Units, class Rep> inline
boost::units::quantity<Units, Rep> 
operator*(const boost::units::quantity<Units, Vector<Rep> >& x,
          const boost::units::quantity<Units, Vector<Rep> >& y)
{
  typedef boost::units::quantity<Units, Rep> return_type;
  return return_type::from_value(x.value() * y.value());
}

template<class Units, class Rep> inline
boost::units::quantity<Units, Vector<Rep> > 
cross_product(const boost::units::quantity<Units, Vector<Rep> >& x,
              const boost::units::quantity<Units, Vector<Rep> >& y)
{
  typedef boost::units::quantity<Units, Vector<Rep> > return_type;
  return return_type::from_value(cross_product(x.value(), y.value()));
}

template<class Units, class Rep> inline
boost::units::quantity<Units, Rep>
abs(const boost::units::quantity<Units, Vector<Rep> >& v)
  { return boost::units::quantity<Units, Rep>::from_value(abs(v.value())); }

template<class Units, class Rep> inline
typename boost::units::power_typeof_helper<
    boost::units::quantity<Units, Rep>, boost::units::static_rational<2> >
::type abs_squared(const boost::units::quantity<Units, Vector<Rep> >& v) {
  typedef typename boost::units::power_typeof_helper<
      quantity<Units, Rep>, static_rational<2>
    >::type return_type;
  return return_type::from_value(abs_squared(v.value()));
} // abs_squared

// ostream support

template<class Ch, class  Tr, class T> inline
std::basic_ostream<Ch, Tr>&
operator<<(std::basic_ostream<Ch, Tr>& os, const Point<T>& p)
  { return os << '(' << p.x << ' ' << p.y << ' ' << p.z << ')'; }

template<class Ch, class Tr, class T> inline
std::basic_ostream<Ch, Tr>&
operator<<(std::basic_ostream<Ch, Tr>& os, const Vector<T>& v)
  { return os << '<' << v.x << ' ' << v.y << ' ' << v.z << '>'; }

} // tjg_basic

#endif