#include <boost/array.hpp>
#include <boost/throw_exception.hpp>
#include <boost/units/operators.hpp>
#include <boost/units/quantity.hpp>
#include <boost/typeof/typeof.hpp>
#include <stdexcept>
#include <cmath>
#include <algorithm>

namespace rci { namespace cartesian {

struct uninitialized_t { };
static const uninitialized_t uninitialized = { };

template<int Dim=3, class Rep = double>
class vector {
public:
  static const int dim = Dim;
  typedef Rep rep;

  boost::array<rep, dim> coord;

  vector(uninitialized_t) { }

  vector() { coord.fill(rep()); }

  vector(const vector& v) : coord(v.coord) { }
  vector& operator=(const vector& v) { coord = v.coord; return *this; }

  template<class U>
  vector(const vector<dim, U>& v) { coord = v.coord; }

  template<class U>
  vector& operator=(const vector<dim, U>& v) {
    for (int i = 0; i != dim; ++i)
      coord[i] = v.coord[i];
    return *this;
  }

  template<class InIter>
  vector(InIter first_, InIter last_) {
    using namespace std;
    if (distance(first_, last_) != dim) {
      std::out_of_range e("cartesian::vector<> invalid input iterators");
      boost::throw_exception(e);
    }
    copy(first_, last_, coord.begin());
  } // iterator initialization

  template<int N> const rep& get() const { return coord[N]; }
  template<int N>       rep& get()       { return coord[N]; }

  template<int N> void set(rep v) { coord[N] = v; }
  
  rep& operator[](int i) { return coord[i]; }
  const rep& operator[](int i) const { return coord[i]; }

  vector operator+() const { return vector(*this); }

  vector operator-() const {
    vector rval(uninitialized);
    for (int i=0; i!=dim; ++i)
      rval.coord[i] = -coord[i];
    return rval;
  }

  template<class U>
  vector& operator+=(const vector<dim, U>& rhs) {
    for (int i=0; i!=dim; ++i)
      coord[i] += rhs[i];
    return *this;
  }

  template<class U>
  vector& operator-=(const vector<dim, U>& rhs) {
    for (int i=0; i!=dim; ++i)
      coord[i] -= rhs[i];
    return *this;
  }

  template<class U>
  vector& operator*=(const U& rhs) {
    for (int i=0; i!=dim; ++i)
      coord[i] *= rhs;
    return *this;
  }

  template<class U>
  vector& operator/=(const U& rhs) {
    for (int i=0; i!=dim; ++i)
      coord[i] /= rhs;
    return *this;
  }

}; // vector

template<int N, int Dim, class Rep>
Rep& get(vector<Dim, Rep>& v) { return v.get<N>(); }

template<int N, int Dim, class Rep>
const Rep& get(const vector<Dim, Rep>& v) { return v.get<N>(); }

template<int N, int Dim, class Rep>
void set(vector<Dim, Rep>& v, Rep x) { return v.set<N>(x); }

} } // rci::cartesian

#include BOOST_TYPEOF_INCREMENT_REGISTRATION_GROUP()

BOOST_TYPEOF_REGISTER_TEMPLATE(rci::cartesian::vector, (int)(class))

namespace rci { namespace cartesian {

template<int Dim, class Rep> inline
typename boost::units::multiply_typeof_helper<Rep, Rep>::type
abs_squared(const vector<Dim, Rep>& v) {
  typedef typename boost::units::multiply_typeof_helper<Rep, Rep>::type
      result_type;
  result_type result = result_type();
  for (int i=0; i!=Dim; ++i) {
    const Rep& x = v[i];
    result += x * x;
  }
  return result;
} // abs_squared

template<int Dim, class Rep> inline
Rep abs(const vector<Dim, Rep>& v) {
  using namespace std;
  return sqrt(abs_squared(v));
} // abs

// vector + vector
template<int Dim, class T1, class T2> inline
typename boost::units::add_typeof_helper<
    vector<Dim, T1>,
    vector<Dim, T2> >::type
operator+(const vector<Dim, T1>&lhs, const vector<Dim, T2>& rhs) {
  typedef typename boost::units::add_typeof_helper<
      vector<Dim, T1>,
      vector<Dim, T2> >::type result_type;
  result_type result(uninitialized);
  for (int i=0; i!=Dim; ++i)
    result[i] = lhs[i] + rhs[i];
  return result;
}

// vector - vector
template<int Dim, class T1, class T2> inline
typename boost::units::subtract_typeof_helper<
    vector<Dim, T1>,
    vector<Dim, T2>
>::type operator-(const vector<Dim, T1>&lhs, const vector<Dim, T2>& rhs) {
  typedef typename boost::units::subtract_typeof_helper<
      vector<Dim, T1>,
      vector<Dim, T2> >::type result_type;
  result_type result(uninitialized);
  for (int i=0; i!=Dim; ++i)
    result[i] = lhs[i] - rhs[i];
  return result;
}

// T * vector
template<int Dim, class T1, class T2> inline
typename boost::units::multiply_typeof_helper<T1, vector<Dim, T2> >::type
operator*(const T1& lhs, const vector<Dim, T2>&rhs) {
  typedef typename boost::units::multiply_typeof_helper<
      T1, vector<Dim, T2>
    >::type result_type;
  result_type result(uninitialized);
  for (int i=0; i!=Dim; ++i)
    result[i] = lhs * rhs[i];
  return result;
}

// vector * T
template<int Dim, class T1, class T2> inline
typename boost::units::multiply_typeof_helper<T2, vector<Dim, T1> >::type
operator*(const vector<Dim, T1>&lhs, const T2& rhs)
  { return rhs * lhs; }

// vector / T
template<int Dim, class T1, class T2> inline
typename boost::units::divide_typeof_helper<vector<Dim, T1>, T2>::type
operator/(const vector<Dim, T1>&lhs, const T2& rhs) {
  typedef typename boost::units::divide_typeof_helper<vector<Dim, T1>, T2>::type
      result_type;
  result_type result(uninitialized);
  for (int i=0; i!=Dim; ++i)
    result[i] = lhs[i] / rhs;
  return result;
}

// vector * vector
template<int Dim, class T1, class T2> inline
typename boost::units::multiply_typeof_helper<
    vector<Dim, T1>,
    vector<Dim, T2> >::type
operator*(const vector<Dim, T1>& lhs, const vector<Dim, T2>& rhs) {
  typename boost::units::multiply_typeof_helper<
      vector<Dim, T1>,
      vector<Dim, T2> >::type result_type;
  result_type result = result_type();
  for (int i=0; i!=Dim; ++i)
    result += lhs[i] * rhs[i];
  return result;
} // dot product

// vector == vector
template<int Dim, class T1, class T2> inline
bool operator==(const vector<Dim, T1>& lhs, const vector<Dim, T2>& rhs) {
  for (int i=0; i!=Dim; ++i) {
    if (lhs[i] != rhs[i])
      return false;
  }
  return true;
}

// vector != vector
template<int Dim, class T1, class T2> inline
bool operator!=(const vector<Dim, T1>& lhs, const vector<Dim, T2>& rhs)
  { return !(lhs == rhs); }

template<class Ch, class Tr, int Dim, class Rep> inline
std::basic_ostream<Ch, Tr>&
operator<<(std::basic_ostream<Ch, Tr>& os, const vector<Dim, Rep>& v) {
  os << '<' << v[0];
  for (int i=1; i!=Dim; ++i)
    os << ' ' << v[i];
  return os << '>';
} // << vector<Dim, Rep>

} } // rci::cartesian

#include <boost/units/systems/si/length.hpp>

#include <iostream>
#include <iomanip>

using namespace std;
using namespace boost;
using namespace boost::units;
using namespace rci;
using namespace rci::cartesian;

int main() {
  cartesian::vector<3, double> v;
  v[0] = 1.0;
  v[1] = 2.0;
  v[2] = 3.0;
  cout << v << endl;

  cartesian::vector<3, quantity<si::length> > w;
  quantity<si::length> factor = 1.0 * si::meter;
  w = v * factor;
  cout << w << endl;

} // main