//
//  vector.h
//  
//
//  Created by Sam Jaffe on 8/21/15.
//
//

#pragma once

namespace math { namespace vector {
  template <size_t, typename> class raw_vector;
} }

template <size_t D, typename T>
math::vector::raw_vector<D, T> operator +(T const&, math::vector::raw_vector<D, T> const&);
template <size_t D, typename T>
math::vector::raw_vector<D, T> operator -(T const&, math::vector::raw_vector<D, T> const&);
template <size_t D, typename T>
math::vector::raw_vector<D, T> operator *(T const&, math::vector::raw_vector<D, T> const&);
template <size_t D, typename T>
math::vector::raw_vector<D, T> operator /(T const&, math::vector::raw_vector<D, T> const&);

namespace math { namespace vector {
  struct fill_t {} fill;

  template <size_t D, typename T>
  class raw_vector {
  public:
    typedef T value_type;
    typedef T& reference;
    typedef const T& const_reference;
    typedef raw_vector<D, T> this_type;
    
    typedef decltype(std::sqrt(std::declval<T>())) sqrt_type; // double or float
    
    T& operator [](size_t index) { return data[index]; }
    
    inline const T& operator [](size_t index) const { return data[index]; }
    
    T& at(size_t index) {
      if (index >= D) throw std::out_of_range(std::to_string(index) + " is out of range [0, " + std::to_string(D) + ")");
      return operator [](index);
    }
    
    const T& at(size_t index) const {
      if (index >= D) throw std::out_of_range(std::to_string(index) + " is out of range [0, " + std::to_string(D) + ")");
      return operator [](index);
    }
    
    template <std::size_t Idx, typename = typename std::enable_if<Idx < D>::type>
    inline const_reference get() const { return data[Idx]; }
    
    template <std::size_t Idx, typename = typename std::enable_if<Idx < D>::type>
    inline reference get() { return data[Idx]; }
    
    reference x() { return get<0>(); }
    const_reference x() const { return get<0>(); }
    reference y() { return get<1>(); }
    const_reference y() const { return get<1>(); }
    reference z() { return get<2>(); }
    const_reference z() const { return get<2>(); }
    reference w() { return get<3>(); }
    const_reference w() const { return get<3>(); }
    reference r() { return get<0>(); }
    const_reference r() const { return get<0>(); }
    reference g() { return get<1>(); }
    const_reference g() const { return get<1>(); }
    reference b() { return get<2>(); }
    const_reference b() const { return get<2>(); }
    reference a() { return get<3>(); }
    const_reference a() const { return get<3>(); }
    
    this_type& operator+=(const this_type& other) {
      for (size_t i = 0; i < D; ++i) at(i) += other.at(i);
      return *this;
    }
    
    this_type& operator-=(const this_type& other) {
      for (size_t i = 0; i < D; ++i) at(i) -= other.at(i);
      return *this;
    }
    
    this_type& operator*=(const this_type& other) {
      for (size_t i = 0; i < D; ++i) at(i) *= other.at(i);
      return *this;
    }
    
    this_type& operator/=(const this_type& other) {
      for (size_t i = 0; i < D; ++i) at(i) /= other.at(i);
      return *this;
    }
    
    this_type& operator+=(const T& c) {
      return operator +=(this_type{c, fill});
    }
    
    this_type& operator -=(const T& c) {
      return operator -=(this_type{c, fill});
    }
    
    this_type& operator *=(const T& c)  {
      return operator *=(this_type{c, fill});
    }
    
    this_type& operator /=(const T& c) {
      return operator /=(this_type{c, fill});
    }
    
    this_type operator +(const this_type& other) const {
      return this_type(*this) += other;
    }
    
    this_type operator -(const this_type& other) const {
      return this_type(*this) -= other;
    }
    
    this_type operator *(const this_type& other) const {
      return this_type(*this) *= other;
    }
    
    this_type operator /(const this_type& other) const {
      return this_type(*this) /= other;
    }
    
    this_type operator +(const T& c) const {
      return operator +(this_type{c, fill});
    }
    
    this_type operator -(const T& c) const {
      return operator -(this_type{c, fill});
    }
    
    this_type operator *(const T& c) const {
      return operator *(this_type{c, fill});
    }
    
    this_type operator /(const T& c) const {
      return operator /(this_type{c, fill});
    }
    
    this_type operator -() const { return -1 * (*this); }
    
    bool operator ==(const this_type& other) const {
      return !memcmp(data, other.data, D * sizeof(T));
    } // technically wrong on float/double
    
    bool operator !=(const this_type& other) const {
      return !operator==(other);
    }
    
    bool operator <=(const this_type& other) const {
      for (size_t i = 0; i < D; ++i) { if (at(i) > other.at(i)) return false; }
      return true;
    }
    
    bool operator <(const this_type& other) const {
      for (size_t i = 0; i < D; ++i) { if (at(i) >= other.at(i)) return false; }
      return true;
    }
    
    bool operator >=(const this_type& other) const {
      for (size_t i = 0; i < D; ++i) { if (at(i) < other.at(i)) return false; }
      return true;
    }
    
    bool operator >(const this_type& other) const {
      for (size_t i = 0; i < D; ++i) { if (at(i) <= other.at(i)) return false; }
      return true;
    }
    
    template <size_t D2>
    typename std::enable_if<(D == 2 || D == 3) && D == D2, raw_vector<3, T> >::type cross(const raw_vector<D2, T>& v) const {
      return D == 2
      ? raw_vector<3, T>{0, 0, at(0)*v.at(1) - at(1)*v.at(0)}
      : raw_vector<3, T>{
        at(1)*v.at(2) - at(2)*v.at(1),
        at(2)*v.at(0) - at(0)*v.at(2),
        at(0)*v.at(1) - at(1)*v.at(0)
      };
    }
    
    T dot(const this_type& v) const {
      T accum{};
      for (size_t i = 0; i < D; ++i) accum += data[i] * v[i];
      return accum;
    }
    
    T lengthSquared() const {
      return dot(*this);
    }
    
    sqrt_type length() const {
      return std::sqrt(lengthSquared());
    }
    
    T distanceSquared(const this_type& other) const {
      return operator -(other).lengthSquared();
    }
    
    sqrt_type distance(const this_type& other) const {
      return std::sqrt(distanceSquared(other));
    }
    
    raw_vector<D, sqrt_type> unit() const { return raw_vector<D, sqrt_type>(*this)/length(); }
    
    void swap(raw_vector& that) {
      using std::swap;
      for (size_t i = 0; i < D; ++i) swap(data[i], that.data[i]);
    }
    
    constexpr raw_vector() = default;
    
    raw_vector(const this_type& that) {
      for (size_t i = 0; i < D; ++i) data[i] = that.data[i];
    }
    
    raw_vector(this_type&& that) {
      for (size_t i = 0; i < D; ++i) data[i] = that.data[i];
    }
    
    raw_vector& operator=(const this_type& other) {
      for (size_t i = 0; i < D; ++i) data[i] = other.data[i];
      return *this;
    }
    
    raw_vector& operator=(this_type&& other) {
      for (size_t i = 0; i < D; ++i) data[i] = other.data[i];
      return *this;
    }
    
    raw_vector(std::initializer_list<T> vals) {
      const size_t D2 = vals.size();
      const T* ptr = vals.begin();
      for (size_t i = 0; i < std::min(D, D2); ++i) data[i] = *ptr++;
      for (size_t j = std::min(D, D2); j < std::max(D, D2); ++j) data[j] = T();
    }
    
    template <std::size_t D2, typename T2,
    typename = typename std::enable_if<D2 != D || !std::is_same<T, T2>::value>::type>
    explicit raw_vector(const raw_vector<D2, T2>& that) {
      for (size_t i = 0; i < std::min(D, D2); ++i) data[i] = static_cast<T>(that[i]);
      for (size_t j = std::min(D, D2); j < std::max(D, D2); ++j) data[j] = T();
    }
    
    raw_vector(T const& t, fill_t) {
      for (size_t i = 0; i < D; ++i) data[i] = t;
    }
    
  private:
    value_type data[D] = {0};
  };
} }

template <size_t D, typename T>
math::vector::raw_vector<D, T> operator +(T const& lhs, math::vector::raw_vector<D, T> const& rhs) {
  return rhs + lhs;
}

template <size_t D, typename T>
math::vector::raw_vector<D, T> operator -(T const& lhs, math::vector::raw_vector<D, T> const& rhs) {
  return math::vector::raw_vector<D, T>(lhs, math::vector::fill) - rhs;
}

template <size_t D, typename T>
math::vector::raw_vector<D, T> operator *(T const& lhs, math::vector::raw_vector<D, T> const& rhs) {
  return rhs * lhs;
}

template <size_t D, typename T>
math::vector::raw_vector<D, T> operator /(T const& lhs, math::vector::raw_vector<D, T> const& rhs) {
  return math::vector::raw_vector<D, T>(lhs, math::vector::fill) / rhs;
}

template <typename CharT, typename Traits, size_t N, typename T>
std::basic_ostream<CharT, Traits>& operator<<(std::basic_ostream<CharT, Traits>& out, const math::vector::raw_vector<N, T>& v) {
  out << '(' << v[0];
  for (size_t i = 1; i < N; ++i) { out << ',' << v[i]; }
  out << ')';
  return out;
}

namespace std {
  template <size_t D, typename T>
  math::vector::raw_vector<D, T> abs(math::vector::raw_vector<D, T> const& data) {
    math::vector::raw_vector<D, T> rval;
    for (size_t i = 0; i < D; ++i) rval[i] = std::abs(data[i]);
    return rval;
  }
}