#ifndef TJG_BASIC_VECTOR_H
#define TJG_BASIC_VECTOR_H
#pragma once

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

#include <cstddef>

namespace tjg_basic {

struct Point {
  typedef double value_type;
  value_type x, y, z;

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

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

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

  explicit Point(const boost_point& pt)
    : x(pt.get<0>()), y(pt.get<1>()), z(pt.get<2>()) { }

  operator boost_point() const { return boost_point(x, y, 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; }

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

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

}; // Point

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

struct Vector {
  typedef double value_type;
  typedef ::boost::geometry::point<value_type, 3,
                                   ::boost::geometry::cs::cartesian>
      boost_point;

  double x, y, z;

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

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

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

  explicit Vector(const boost_point& pt)
    : x(pt.get<0>()), y(pt.get<1>()), z(pt.get<2>()) { }

  operator boost_point() const { return boost_point(x, y, 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 value_type operator*(const Vector& lhs, const Vector& rhs) {
    return ::boost::geometry::dot_product(boost_point(lhs), boost_point(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; }

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

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

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

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

  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)
    { return std::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

inline Vector operator-(const Point& lhs, const Point& rhs)
  { return Vector(rhs, lhs); }

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

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

} // tjg_basic

#endif