//
//  matrix_helpers.hpp
//  math
//
//  Created by Sam Jaffe on 8/20/16.
//

#pragma once

#include "matrix.hpp"

namespace math { namespace matrix {
  
  template <typename T, std::size_t N>
  matrix<T, N, N> identity() {
    matrix<T, N, N> rval;
    VECTOR_FOR_EACH(i) { rval.at(i,i) = 1; }
    return rval;
  }
  
  template <typename T, std::size_t N>
  matrix<T, N, N> diagonal(vector::vector<T, N> const & vec) {
    matrix<T, N, N> rval = identity<T, N>();
    VECTOR_FOR_EACH(i) { rval.at(i,i) = vec[i]; }
    return rval;
  }
  
  template <typename T, std::size_t N>
  matrix<T, N+1, N+1> translation(vector::vector<T, N> const & vec) {
    matrix<T, N+1, N+1> rval = identity<T, N+1>();
    VECTOR_FOR_EACH(i) { rval.at(i,N) = vec[i]; }
    return rval;
  }
  
  template <typename T, std::size_t N>
  matrix<T, N+1, N+1> scalar(vector::vector<T, N> const & vec) {
    matrix<T, N+1, N+1> rval = identity<T, N+1>();
    VECTOR_FOR_EACH(i) { rval.at(i,i) = vec[i]; }
    return rval;
  }
  
  template <size_t N>
  struct rotation_t {
    constexpr rotation_t(size_t f, size_t s)
    : first(f), second(s) {}
    size_t first, second;
  };
  
  namespace rotate {
    constexpr rotation_t<3> const X_AXIS{1,2};
    constexpr rotation_t<3> const Y_AXIS{2,0};
    constexpr rotation_t<3> const Z_AXIS{0,1};
    
    constexpr rotation_t<3> const ROLL{1,2};
    constexpr rotation_t<3> const PITCH{2,0};
    constexpr rotation_t<3> const YAW{0,1};
    
    constexpr rotation_t<2> const ROT_2D{0,1};
  }
  
  template <typename T, size_t D>
  auto rotation(T theta, rotation_t<D> r) -> matrix<decltype(sin(theta)), D, D> {
    static_assert(D >= 2, "cannot rotate with 1D matrix");
    using G = decltype(sin(theta));
    using std::sin;
    using std::cos;
    matrix<G, D, D> rval = identity<G, D>();
    G const vsin = sin(theta);
    G const vcos = cos(theta);
    rval.at(r.first, r.first) = vcos;
    rval.at(r.second, r.second) = vcos;
    rval.at(r.first, r.second) = -vsin;
    rval.at(r.second, r.first) = vsin;
    return rval;
  }
  
  template <size_t D, typename T, size_t RD>
  auto rotation(T theta, rotation_t<RD> r) -> matrix<decltype(sin(theta)), D, D> {
    static_assert(D > RD, "rotation<D> has to increase the number of dimensions");
    using G = decltype(sin(theta));
    return rotation(theta, r).concat(identity<G, D - RD>(), concat_strategy::diagonal);
  }
  
  template <typename T, size_t N>
  vector::vector<T, N> operator*(matrix<T, N+1, N+1> const & lhs, vector::vector<T, N> const & rhs) {
    vector::vector<T, N+1> tmp(rhs);
    tmp[N] = 1;
    return vector::vector<T, N>(lhs * tmp);
  }
  
} }