//
// This file is part of MorphoGraphX - http://www.MorphoGraphX.org
// Copyright (C) 2012-2015 Richard S. Smith and collaborators.
//
// If you use MorphoGraphX in your work, please cite:
//   http://dx.doi.org/10.7554/eLife.05864
//
// MorphoGraphX is free software, and is licensed under under the terms of the 
// GNU General (GPL) Public License version 2.0, http://www.gnu.org/licenses.
// 
#ifndef MATRIX_HPP
#define MATRIX_HPP
/**
 * \file Matrix.hpp
 *
 * Defines the Matrix class template
 * This file is shared by cuda, do not include headers that nvcc can't handle (i.e. Qt)
 */

#include <Config.hpp>

#include <cuda/CudaGlobal.hpp>
#include <StaticAssert.hpp>
#include <Util.hpp>
#include <Vector.hpp>

#include <cmath>
#ifndef __CUDACC__
#  include <QTextStream>
#endif

namespace mgx 
{
  /**
   * \class Matrix Matrix.hpp <Matrix.hpp>
   *
   * Class representing a fixed-size dense matrix.
   */
  template <size_t nRows, size_t nCols, typename T = double> class Matrix {
  private:
    Vector<nCols, T> rows[nRows];
  
  public:
    typedef T value_type;
    typedef T& reference_type;
    typedef const T& const_reference_type;
    typedef T* pointer_type;
    typedef const T* const_pointer_type;
    typedef T* iterator;
    typedef const T* const_iterator;
  
    static const size_t numcols = nCols;
    static const size_t numrows = nRows;
  
    /**
     * Create a matrix filled with 0s.
     */
    CU_HOST_DEVICE
    Matrix(void)
    {
      for(size_t i = 0; i < nRows; i++)
        for(size_t j = 0; j < nCols; j++)
          rows[i][j] = 0;
    }
  
    /**
     * Copy a matrix.
     *
     * Can be used to cast a matrix onto another type.
     */
    template <typename T1> CU_HOST_DEVICE Matrix(const Matrix<nRows, nCols, T1>& mat)
    {
      for(size_t i = 0; i < nRows; i++)
        for(size_t j = 0; j < nCols; j++)
          rows[i][j] = T(mat(i, j));
    }
  
    /**
     * Fill the matrix with the array of vectors.
     *
     * \param vecs Array of \c nRows vectors.
     */
    template <typename T1> CU_HOST_DEVICE Matrix(const Vector<nCols, T1>* vecs)
    {
      for(size_t i = 0; i < nRows; i++)
        rows[i] = vecs[i];
    }

    /**
     * Initialize a 3 row matrix
     */
    template <typename T1> CU_HOST_DEVICE
    Matrix(const Vector<nCols,T1> &r0, const Vector<nCols,T1> &r1, const Vector<nCols,T1> &r2)
    {
      STATIC_ASSERT(nRows == 3);
      rows[0] = r0;
      rows[1] = r1;
      rows[2] = r2;
    } 

    /**
     * Fill in the matrix with the \c values.
     *
     * \param values nRows*nCols array. If \c c_style is true, \c values is
     * rows first (i.e. the first values correspond to the first row).
     * Otherwise, values are columns first.
     *
     * \param c_style Determine the ordering of values.
     */
    template <typename T1> CU_HOST_DEVICE 
    Matrix(const T1* values)
    {
      for(size_t i = 0; i < nRows; i++)
        rows[i] = Vector<nCols, T>(values + (i * nCols));
    }
  
    /**
     * Fill in the matrix with the \c values.
     *
     * \param values nRows*nCols array. If \c c_style is true, \c values is
     * rows first (i.e. the first values correspond to the first row).
     * Otherwise, values are columns first.
     *
     * \param c_style Determine the ordering of values.
     */
    CU_HOST_DEVICE
    Matrix(const T* values)
    {
      for(size_t i = 0; i < nRows; i++)
        rows[i] = Vector<nCols, T>(values + (i * nCols));
    }
  
    /**
     * Create a diagonal matrix.
     *
     * \param value Value placed on the diagonal.
     */
    CU_HOST_DEVICE
    Matrix(const T& value)
    {
      for(size_t i = 0; i < nRows; i++)
        for(size_t j = 0; j < nCols; j++)
          rows[i][j] = (i == j) ? value : 0;
    }
  
    /**
     * Returns the size of the matrix.
     *
     * First element is number of rows, second is number of columns
     */
    CU_HOST_DEVICE
    static Vector<2, size_t> size() {
      return Vector<2, size_t>(nRows, nCols);
    }
  
    /**
     * Returns the number of rows of the matrix
     */
    CU_HOST_DEVICE
    static size_t nbRows() {
      return nRows;
    }
  
    /**
     * Returns the number of columns of the matrix
     */
    CU_HOST_DEVICE
    static size_t nbColumns() {
      return nCols;
    }
  
    /**
     * Returns a constant raw pointer on the data
     *
     * The data are organised on by rows. (i.e. in the opposite order as OpenGL)
     */
    CU_HOST_DEVICE
    const T* c_data() const {
      return rows[0].c_data();
    }
  
    /**
     * Returns a raw pointer on the data.
     *
     * The data are organised on by rows. (i.e. in the opposite order as OpenGL)
     */
    CU_HOST_DEVICE
    T* data() {
      return rows[0].data();
    }
  
    /**
     * Matrix subtraction
     */
    CU_HOST_DEVICE
    Matrix operator-(void) const
    {
      Matrix ans;
  
      for(size_t i = 0; i < nRows; i++)
        ans.rows[i] = -rows[i];
  
      return ans;
    }
  
    /**
     * Matrix addition
     */
    CU_HOST_DEVICE
    Matrix operator+(const Matrix& mat) const
    {
      Matrix ans;
  
      for(size_t i = 0; i < nRows; i++)
        ans.rows[i] = rows[i] + mat.rows[i];
  
      /**
       * Matrix subtraction
       */
      return ans;
    }
  
    CU_HOST_DEVICE
    Matrix operator-(const Matrix& mat) const
    {
      Matrix ans;
  
      for(size_t i = 0; i < nRows; i++)
        ans.rows[i] = rows[i] - mat.rows[i];
  
      return ans;
    }
  
    /**
     * Matrix-scalar multiplication
     */
    CU_HOST_DEVICE
    Matrix operator*(const T& scalar) const
    {
      Matrix ans;
  
      for(size_t i = 0; i < nRows; i++)
        ans[i] = rows[i] * scalar;
  
      return ans;
    }
  
    /**
     * Matrix-scalar division
     */
    CU_HOST_DEVICE
    Matrix operator/(const T& scalar) const
    {
      Matrix ans;
  
      for(size_t i = 0; i < nRows; i++)
        ans[i] = rows[i] / scalar;
  
      return ans;
    }
  
    /**
     * Matrix-scalar multiplication
     */
    CU_HOST_DEVICE
    friend Matrix operator*(const T& scalar, const Matrix& mat)
    {
      Matrix ans;
  
      for(size_t i = 0; i < nRows; i++)
        ans[i] = scalar * mat.rows[i];
  
      return ans;
    }
  
    /**
     * Matrix*Column Vector
     */
    CU_HOST_DEVICE
    Vector<nRows, T> operator*(const Vector<nCols, T>& vec) const
    {
      Vector<nRows, T> ans;
      for(size_t i = 0; i < nRows; ++i) {
        T value = 0;
        for(size_t j = 0; j < nCols; ++j)
          value += rows[i][j] * vec[j];
        ans[i] = value;
      }
      return ans;
    }
  
    CU_HOST_DEVICE
    Matrix& operator=(const Matrix& mat)
    {
      for(size_t i = 0; i < nRows; i++)
        rows[i] = mat.rows[i];
  
      return (*this);
    }
  
    CU_HOST_DEVICE
    Matrix& operator+=(const Matrix& mat) {
      return ((*this) = (*this) + mat);
    }
  
    CU_HOST_DEVICE
    Matrix& operator-=(const Matrix& mat) {
      return ((*this) = (*this) - mat);
    }
  
    CU_HOST_DEVICE
    Matrix& operator*=(const T& scalar) {
      return ((*this) = (*this) * scalar);
    }
  
    CU_HOST_DEVICE
    Matrix& operator/=(const T& scalar) {
      return ((*this) = (*this) / scalar);
    }
  
    CU_HOST_DEVICE
    Matrix& operator*=(const Matrix& mat) {
      return ((*this) = (*this) * mat);
    }

    /**
     * Element-wise equality
     */  
    CU_HOST_DEVICE
    bool operator==(const Matrix& mat) const
    {
      for(size_t i = 0; i < nRows; i++)
        if(rows[i] != mat.rows[i])
          return false;
  
      return true;
    }

    /**
     * Element-wise inequality
     */ 
    CU_HOST_DEVICE
    bool operator!=(const Matrix& mat) const {
      return (!((*this) == mat));
    }

    /**
     * Comparison operator.
     *
     * Compare the axis in row order.
     */
    CU_HOST_DEVICE
    bool operator<(const Matrix& other) const
    {
      for(size_t i = 0; i < nRows; ++i) {
        if(rows[i] < other.rows[i])
          return true;
        if(rows[i] > other.rows[i])
          return false;
      }
      return false;
    }  

  #ifndef __CUDACC__
    friend QTextStream& operator<<(QTextStream& out, const Matrix& mat)
    {
      for(size_t i = 0; i < nRows; i++) {
        out << mat.rows[i];
        if(i != (nRows - 1))
          out << " ";
      }
  
      return out;
    }
  
    friend QTextStream& operator>>(QTextStream& in, Matrix& mat)
    {
      for(size_t i = 0; i < nRows && !in.atEnd(); i++)
        in >> mat.rows[i];
      return in;
    }
  #endif
  
    CU_HOST_DEVICE
    friend std::ostream& operator<<(std::ostream& out, const Matrix& mat)
    {
      for(size_t i = 0; i < nRows; i++) {
        out << mat.rows[i];
        if(i != (nRows - 1))
          out << " ";
      }
  
      return out;
    }
  
    CU_HOST_DEVICE
    friend std::istream& operator>>(std::istream& in, Matrix& mat)
    {
      for(size_t i = 0; i < nRows && in; i++)
        in >> mat.rows[i];
      return in;
    }
  
    /**
     * Returns the nth row
     *
     * \param idx Index of the returned row
     */
    CU_HOST_DEVICE
    Vector<nCols, T>& operator[](size_t idx) {
      return rows[idx];
    }
  
    /**
     * Returns the nth row
     *
     * \param idx Index of the returned row
     */
    CU_HOST_DEVICE
    Vector<nCols, T> operator[](size_t idx) const {
      return rows[idx];
    }
  
    /**
     * Return the value at row \c i, column \c j.
     */
    CU_HOST_DEVICE
    T& operator()(size_t i, size_t j) {
      return rows[i][j];
    }
  
    /**
     * Return the value at row \c i, column \c j.
     */
    CU_HOST_DEVICE
    T operator()(size_t i, size_t j) const {
      return rows[i][j];
    }
  
    /**
     * Returns an identity matrix.
     */
    CU_HOST_DEVICE
    static Matrix identity()
    {
      Matrix mat(1);
      return mat;
    }
  
    /**
     * Set the matrix to all zero.
     */
    CU_HOST_DEVICE
    Matrix& zero(void)
    {
      for(size_t i = 0; i < nRows; i++)
        for(size_t j = 0; j < nCols; j++)
          rows[i][j] = 0.0;
      return (*this);
    }
  
    /**
     * Set the matrix to a diagonal matrix.
     *
     * \param value Value to put on the diagonal
     */
    CU_HOST_DEVICE
    Matrix& operator=(const T& value)
    {
      for(size_t i = 0; i < nRows; ++i) {
        for(size_t j = 0; j < nCols; ++j) {
          if(i == j)
            rows[i][j] = value;
          else
            rows[i][j] = 0;
        }
      }
      return *this;
    }
  
    /**
     * Transpose the matrix
     */
    CU_HOST_DEVICE
    Matrix<nCols, nRows, T> operator~()
    {
      Matrix<nCols, nRows, T> t;
      for(size_t i = 0; i < nRows; ++i)
        for(size_t j = 0; j < nCols; ++j)
          t[i][j] = rows[j][i];
      return t;
    }
  
    /**
     * Creates the 3x3 matrix corresponding to a rotation.
     *
     * \param direction Axes of the rotation
     * \param angle Angle of the rotation
     */
    CU_HOST_DEVICE
    static Matrix<3, 3, T> rotation(const Vector<3, T>& direction, T angle)
    {
      T ca = std::cos(angle);
      T sa = std::sin(angle);
      Matrix<3, 3, T> r;
      T x = direction.x();
      T y = direction.y();
      T z = direction.z();
      r[0].set(ca + (1 - ca) * x * x, (1 - ca) * x * y - sa * z, (1 - ca) * z * x + sa * y);
      r[1].set((1 - ca) * y * x + sa * z, ca + (1 - ca) * y * y, (1 - ca) * z * y - sa * x);
      r[2].set((1 - ca) * x * z - sa * y, (1 - ca) * y * z + sa * x, ca + (1 - ca) * z * z);
      return r;
    }
  
    /**
     * Creates the 4x4 matrix corresponding to a rotation.
     *
     * \param direction Axes of the rotation
     * \param angle Angle of the rotation
     */
    CU_HOST_DEVICE
    static Matrix<4, 4, T> rotation(const Vector<4, T>& direction, T angle)
    {
      T ca = std::cos(angle);
      T sa = std::sin(angle);
      Matrix<4, 4, T> r;
      T x = direction.x() / direction.t();
      T y = direction.y() / direction.t();
      T z = direction.z() / direction.t();
      r[0].set(ca + (1 - ca) * x * x, (1 - ca) * x * y - sa * z, (1 - ca) * z * x + sa * y, 0);
      r[1].set((1 - ca) * y * x + sa * z, ca + (1 - ca) * y * y, (1 - ca) * z * y - sa * x, 0);
      r[2].set((1 - ca) * x * z - sa * y, (1 - ca) * y * z + sa * x, ca + (1 - ca) * z * z, 0);
      r[3].set(0, 0, 0, 1);
      return r;
    }
  
    /**
     * Trace of the matrix
     */
    CU_HOST_DEVICE
    T trace() const
    {
      T acc = 0;
      for(size_t i = 0; i < min(nRows, nCols); ++i) {
        acc += rows[i][i];
      }
      return acc;
    }
  
    CU_HOST_DEVICE
    void fillArray(T* array)
    {
      memcpy(array, &rows[0][0], sizeof(T) * nRows * nCols);
    }
  
    /**
     * Return the diagonal vector, if the matrix is square
     */
    CU_HOST_DEVICE
    Vector<nRows, T> diag() const
    {
      STATIC_ASSERT(nRows == nCols);
      Vector<nRows, T> res;
      for(size_t i = 0; i < nRows; ++i)
        res[i] = (*this)(i, i);
      return res;
    }
  };
  
  /**
   * Row-Vector - Matrix multipliation
   * \relates Matrix
   */
  template <size_t nRows, size_t nCols, typename T>
  CU_HOST_DEVICE Vector<nCols, T> operator*(const Vector<nCols, T>& vec, const Matrix<nRows, nCols, T>& mat);
  
  /**
   * Matrix multiplication
   * \relates Matrix
   */
  template <size_t nRows, size_t nSize, size_t nCols, typename T>
  CU_HOST_DEVICE Matrix<nRows, nCols, T> operator*(const Matrix<nRows, nSize, T>& mat1,
                                                   const Matrix<nSize, nCols, T>& mat2);
  
  /**
   * Determinant of the matrix
   * \relates Matrix
   */
  template <typename T> T CU_HOST_DEVICE det(const Matrix<1, 1, T>& mat);
  /**
   * Determinant of the matrix
   * \relates Matrix
   */
  template <typename T> T CU_HOST_DEVICE det(const Matrix<2, 2, T>& mat);
  /**
   * Determinant of the matrix
   * \relates Matrix
   */
  template <typename T> T CU_HOST_DEVICE det(const Matrix<3, 3, T>& mat);
  /**
   * Determinant of the matrix
   *
   * \warning the method used is \f$O(n^3)\f$ complexity !
   * \relates Matrix
   */
  template <size_t nRows, typename T> T CU_HOST_DEVICE det(const Matrix<nRows, nRows, T>& mat);
  
  template <size_t nRows, typename T>
  CU_HOST_DEVICE T matrix_minor(const Matrix<nRows, nRows, T>& mat, size_t i, size_t j);
  
  /**
   * Returns the cofactor of the matrix for position (i,j)
   * \relates Matrix
   */
  template <size_t nRows, typename T> CU_HOST_DEVICE T cofactor(const Matrix<nRows, nRows, T>& mat, size_t i, size_t j);
  
  /**
   * Inverse the matrix
   *
   * \relates Matrix
   */
  template <typename T> CU_HOST_DEVICE Matrix<1, 1, T> inverse(const Matrix<1, 1, T>& mat);
  /**
   * Inverse the matrix
   *
   * \relates Matrix
   */
  template <typename T> CU_HOST_DEVICE Matrix<2, 2, T> inverse(const Matrix<2, 2, T>& mat);
  /**
   * Inverse the matrix
   *
   * \relates Matrix
   */
  template <typename T> CU_HOST_DEVICE Matrix<3, 3, T> inverse(const Matrix<3, 3, T>& mat);
  /**
   * Inverse the matrix
   *
   * \relates Matrix
   */
  template <typename T> CU_HOST_DEVICE Matrix<4, 4, T> inverse(const Matrix<4, 4, T>& mat);
  
  /**
   * Inverse the matrix
   *
   * \relates Matrix
   *
   * \warning This algorithm is sub-optimal
   */
  template <size_t nRows, typename T> CU_HOST_DEVICE Matrix<nRows, nRows, T> inverse(const Matrix<nRows, nRows, T>& mat);
  
  /**
   * Transpose a matrix.
   *
   * \relates Matrix
   */
  template <size_t nRows, size_t nCols, typename T>
  CU_HOST_DEVICE Matrix<nCols, nRows, T> transpose(const Matrix<nRows, nCols, T>& mat);
  
  /**
   * Return the norm of the matrix.
   *
   * The norm is defined as the square-root of the sum of the square of the
   * values.
   *
   * \relates Matrix
   */
  template <size_t nRows, size_t nCols, typename T> CU_HOST_DEVICE T norm(const Matrix<nRows, nCols, T>& mat);
  
  /**
   * Return the square norm of the matrix.
   *
   * \see norm(const Matrix&)
   *
   * \relates Matrix
   */
  template <size_t nRows, size_t nCols, typename T> CU_HOST_DEVICE T normsq(const Matrix<nRows, nCols, T>& mat);
  
  /**
   * Apply a unary function to each element of the matrix
   *
   * \relates Matrix
   */
  template <size_t nRows, size_t nCols, typename T>
  CU_HOST_DEVICE Matrix<nRows, nCols, T> map(const T& (*fct)(const T &), const Matrix<nRows, nCols, T>& m)
  {
    Matrix<nRows, nCols, T> result;
    for(size_t i = 0; i < nRows; ++i) {
      const Vector<nCols, T>& mrow = m[i];
      Vector<nCols, T>& rrow = result[i];
      for(size_t j = 0; j < nCols; ++j) {
        rrow[j] = (*fct)(mrow[j]);
      }
    }
    return result;
  }
  
  /**
   * Apply a unary function to each element of the matrix
   *
   * \relates Matrix
   */
  template <size_t nRows, size_t nCols, typename T>
  CU_HOST_DEVICE Matrix<nRows, nCols, T> map(T (*fct)(T), const Matrix<nRows, nCols, T>& m)
  {
    Matrix<nRows, nCols, T> result;
    for(size_t i = 0; i < nRows; ++i) {
      const Vector<nCols, T>& mrow = m[i];
      Vector<nCols, T>& rrow = result[i];
      for(size_t j = 0; j < nCols; ++j) {
        rrow[j] = (*fct)(mrow[j]);
      }
    }
    return result;
  }
  
  /**
   * Apply a unary function to each element of the matrix
   *
   * \relates Matrix
   */
  template <size_t nRows, size_t nCols, typename T>
  CU_HOST_DEVICE Matrix<nRows, nCols, T> map(T (*fct)(const T&), const Matrix<nRows, nCols, T>& m)
  {
    Matrix<nRows, nCols, T> result;
    for(size_t i = 0; i < nRows; ++i) {
      const Vector<nCols, T>& mrow = m[i];
      Vector<nCols, T>& rrow = result[i];
      for(size_t j = 0; j < nCols; ++j) {
        rrow[j] = (*fct)(mrow[j]);
      }
    }
    return result;
  }
  
  /**
   * Apply a unary function to each element of the matrix
   *
   * \relates Matrix
   */
  template <size_t nRows, size_t nCols, typename T, typename T1>
  CU_HOST_DEVICE Matrix<nRows, nCols, T> map(const T& (*fct)(const T1 &), const Matrix<nRows, nCols, T1>& m)
  {
    Matrix<nRows, nCols, T> result;
    for(size_t i = 0; i < nRows; ++i) {
      const Vector<nCols, T1>& mrow = m[i];
      Vector<nCols, T>& rrow = result[i];
      for(size_t j = 0; j < nCols; ++j) {
        rrow[j] = (*fct)(mrow[j]);
      }
    }
    return result;
  }
  
  /**
   * Apply a unary function to each element of the matrix
   *
   * \relates Matrix
   */
  template <size_t nRows, size_t nCols, typename T, typename T1>
  CU_HOST_DEVICE Matrix<nRows, nCols, T> map(T (*fct)(T1), const Matrix<nRows, nCols, T1>& m)
  {
    Matrix<nRows, nCols, T> result;
    for(size_t i = 0; i < nRows; ++i) {
      const Vector<nCols, T1>& mrow = m[i];
      Vector<nCols, T>& rrow = result[i];
      for(size_t j = 0; j < nCols; ++j) {
        rrow[j] = (*fct)(mrow[j]);
      }
    }
    return result;
  }
  
  /**
   * Apply a unary function to each element of the matrix
   *
   * \relates Matrix
   */
  template <size_t nRows, size_t nCols, typename T, typename T1>
  CU_HOST_DEVICE Matrix<nRows, nCols, T> map(T (*fct)(const T1&), const Matrix<nRows, nCols, T1>& m)
  {
    Matrix<nRows, nCols, T> result;
    for(size_t i = 0; i < nRows; ++i) {
      const Vector<nCols, T1>& mrow = m[i];
      Vector<nCols, T>& rrow = result[i];
      for(size_t j = 0; j < nCols; ++j) {
        rrow[j] = (*fct)(mrow[j]);
      }
    }
    return result;
  }
  
  /**
   * Apply a binary function to each element of the matrix
   *
   * \relates Matrix
   */
  template <size_t nRows, size_t nCols, typename T>
  CU_HOST_DEVICE Matrix<nRows, nCols, T> map(T (*fct)(T, T), const Matrix<nRows, nCols, T>& m1,
                                             const Matrix<nRows, nCols, T>& m2)
  {
    Matrix<nRows, nCols, T> result;
    for(size_t i = 0; i < nRows; ++i) {
      const Vector<nCols, T>& mrow1 = m1[i];
      const Vector<nCols, T>& mrow2 = m2[i];
      Vector<nCols, T>& rrow = result[i];
      for(size_t j = 0; j < nCols; ++j) {
        rrow[j] = (*fct)(mrow1[j], mrow2[j]);
      }
    }
    return result;
  }
  
  /**
   * Apply a binary function to each element of the matrix
   *
   * \relates Matrix
   */
  template <size_t nRows, size_t nCols, typename T>
  CU_HOST_DEVICE Matrix<nRows, nCols, T> map(T (*fct)(const T&, const T&), const Matrix<nRows, nCols, T>& m1,
                                             const Matrix<nRows, nCols, T>& m2)
  {
    Matrix<nRows, nCols, T> result;
    for(size_t i = 0; i < nRows; ++i) {
      const Vector<nCols, T>& mrow1 = m1[i];
      const Vector<nCols, T>& mrow2 = m2[i];
      Vector<nCols, T>& rrow = result[i];
      for(size_t j = 0; j < nCols; ++j) {
        rrow[j] = (*fct)(mrow1[j], mrow2[j]);
      }
    }
    return result;
  }
  
  /**
   * Apply a binary function to each element of the matrix
   *
   * \relates Matrix
   */
  template <size_t nRows, size_t nCols, typename T>
  CU_HOST_DEVICE Matrix<nRows, nCols, T> map(const T& (*fct)(const T &, const T &), const Matrix<nRows, nCols, T>& m1,
                                             const Matrix<nRows, nCols, T>& m2)
  {
    Matrix<nRows, nCols, T> result;
    for(size_t i = 0; i < nRows; ++i) {
      const Vector<nCols, T>& mrow1 = m1[i];
      const Vector<nCols, T>& mrow2 = m2[i];
      Vector<nCols, T>& rrow = result[i];
      for(size_t j = 0; j < nCols; ++j) {
        rrow[j] = (*fct)(mrow1[j], mrow2[j]);
      }
    }
    return result;
  }
  
  /**
   * Apply a binary function to each element of the matrix
   *
   * \relates Matrix
   */
  template <size_t nRows, size_t nCols, typename T, typename T1, typename T2>
  CU_HOST_DEVICE Matrix<nRows, nCols, T> map(T (*fct)(T1, T2), const Matrix<nRows, nCols, T1>& m1,
                                             const Matrix<nRows, nCols, T2>& m2)
  {
    Matrix<nRows, nCols, T> result;
    for(size_t i = 0; i < nRows; ++i) {
      const Vector<nCols, T1>& mrow1 = m1[i];
      const Vector<nCols, T2>& mrow2 = m2[i];
      Vector<nCols, T>& rrow = result[i];
      for(size_t j = 0; j < nCols; ++j) {
        rrow[j] = (*fct)(mrow1[j], mrow2[j]);
      }
    }
    return result;
  }
  
  /**
   * Apply a binary function to each element of the matrix
   *
   * \relates Matrix
   */
  template <size_t nRows, size_t nCols, typename T, typename T1, typename T2>
  CU_HOST_DEVICE Matrix<nRows, nCols, T> map(T (*fct)(const T1&, const T2&), const Matrix<nRows, nCols, T1>& m1,
                                             const Matrix<nRows, nCols, T2>& m2)
  {
    Matrix<nRows, nCols, T> result;
    for(size_t i = 0; i < nRows; ++i) {
      const Vector<nCols, T1>& mrow1 = m1[i];
      const Vector<nCols, T2>& mrow2 = m2[i];
      Vector<nCols, T>& rrow = result[i];
      for(size_t j = 0; j < nCols; ++j) {
        rrow[j] = (*fct)(mrow1[j], mrow2[j]);
      }
    }
    return result;
  }
  
  /**
   * Apply a binary function to each element of the matrix
   *
   * \relates Matrix
   */
  template <size_t nRows, size_t nCols, typename T, typename T1, typename T2>
  CU_HOST_DEVICE Matrix<nRows, nCols, T> map(const T& (*fct)(const T1 &, const T2 &), const Matrix<nRows, nCols, T1>& m1,
                                             const Matrix<nRows, nCols, T2>& m2)
  {
    Matrix<nRows, nCols, T> result;
    for(size_t i = 0; i < nRows; ++i) {
      const Vector<nCols, T1>& mrow1 = m1[i];
      const Vector<nCols, T2>& mrow2 = m2[i];
      Vector<nCols, T>& rrow = result[i];
      for(size_t j = 0; j < nCols; ++j) {
        rrow[j] = (*fct)(mrow1[j], mrow2[j]);
      }
    }
    return result;
  }
  
  template <size_t nRows, size_t nCols, typename T>
  CU_HOST_DEVICE Vector<nCols, T> operator*(const Vector<nCols, T>& vec, const Matrix<nRows, nCols, T>& mat)
  {
    Vector<nCols, T> ans;
    for(size_t i = 0; i < nCols; ++i) {
      T value = 0;
      for(size_t j = 0; j < nRows; ++j)
        value += mat(j, i) * vec[j];
      ans[i] = value;
    }
    return ans;
  }
  
  template <size_t nRows, size_t intSize, size_t nCols, typename T>
  CU_HOST_DEVICE Matrix<nRows, nCols, T> operator*(const Matrix<nRows, intSize, T>& mat1,
                                                   const Matrix<intSize, nCols, T>& mat2)
  {
    Matrix<nRows, nCols, T> ans;
    for(size_t i = 0; i < nRows; i++)
      for(size_t j = 0; j < nCols; j++) {
        T acc = 0;
        for(size_t k = 0; k < intSize; k++)
          acc += mat1(i, k) * mat2(k, j);
        ans(i, j) = acc;
      }
    return ans;
  }
  
  template <typename T> CU_HOST_DEVICE T det(const Matrix<1, 1, T>& mat) {
    return mat(0, 0);
  }
  
  template <typename T> CU_HOST_DEVICE T det(const Matrix<2, 2, T>& mat)
  {
    return mat(0, 0) * mat(1, 1) - mat(0, 1) * mat(1, 0);
  }
  
  template <typename T> CU_HOST_DEVICE T det(const Matrix<3, 3, T>& mat)
  {
    return mat(0, 0) * mat(1, 1) * mat(2, 2) + mat(0, 1) * mat(1, 2) * mat(2, 0) + mat(0, 2) * mat(1, 0) * mat(2, 1)
           - mat(0, 0) * mat(1, 2) * mat(2, 1) - mat(0, 1) * mat(1, 0) * mat(2, 2) - mat(0, 2) * mat(1, 1) * mat(2, 0);
  }
  
  template <size_t nRows, typename T> CU_HOST_DEVICE T det(const Matrix<nRows, nRows, T>& mat)
  {
    STATIC_ASSERT(nRows > 3);
    T acc = 0;
    for(size_t i = 0; i < nRows; i++) {
      acc += mat(i, 0) * cofactor(mat, i, 0);
    }
    return acc;
  }
  
  template <size_t nRows, typename T>
  CU_HOST_DEVICE T matrix_minor(const Matrix<nRows, nRows, T>& mat, size_t i, size_t j)
  {
    STATIC_ASSERT(nRows > 0);
    Matrix<nRows - 1, nRows - 1, T> ans;
    for(size_t i1 = 0, i2 = 0; i1 < nRows; i1++, i2++) {
      if(i1 == i) {
        i2--;
      } else {
        for(size_t j1 = 0, j2 = 0; j1 < nRows; j1++, j2++) {
          if(j1 == j) {
            j2--;
          } else {
            ans(i2, j2) = mat(i1, j1);
          }
        }
      }
    }
    return det(ans);
  }
  
  template <size_t nRows, typename T> CU_HOST_DEVICE T cofactor(const Matrix<nRows, nRows, T>& mat, size_t i, size_t j)
  {
    T inv = 1;
    if((i + j) % 2) {
      inv = -1;
    }
    return inv * matrix_minor(mat, i, j);
  }
  
  template <typename T> CU_HOST_DEVICE Matrix<1, 1, T> inverse(const Matrix<1, 1, T>& mat)
  {
    Matrix<1, 1, T> ans;
    ans[0][0] = 1 / mat(0, 0);
    return ans;
  }
  
  template <typename T> CU_HOST_DEVICE Matrix<2, 2, T> inverse(const Matrix<2, 2, T>& mat)
  {
    Matrix<2, 2, T> ans;
    T d;
    d = det(mat);
    if(d == 0)
      return ans;
    ans(0, 0) = mat(1, 1) / d;
    ans(0, 1) = mat(0, 1) / -d;
    ans(1, 0) = mat(1, 0) / -d;
    ans(1, 1) = mat(0, 0) / d;
    return ans;
  }
  
  template <typename T> CU_HOST_DEVICE Matrix<3, 3, T> inverse(const Matrix<3, 3, T>& mat)
  {
    Matrix<3, 3, T> ans;
    T d;
    d = det(mat);
    if(d == 0)
      return ans;
    ans(0, 0) = (mat(1, 1) * mat(2, 2) - mat(1, 2) * mat(2, 1)) / d;
    ans(1, 1) = (mat(0, 0) * mat(2, 2) - mat(0, 2) * mat(2, 0)) / d;
    ans(2, 2) = (mat(1, 1) * mat(0, 0) - mat(1, 0) * mat(0, 1)) / d;
    ans(1, 0) = (mat(1, 2) * mat(2, 0) - mat(1, 0) * mat(2, 2)) / d;
    ans(0, 1) = (mat(2, 1) * mat(0, 2) - mat(0, 1) * mat(2, 2)) / d;
    ans(2, 0) = (mat(1, 0) * mat(2, 1) - mat(1, 1) * mat(2, 0)) / d;
    ans(0, 2) = (mat(0, 1) * mat(1, 2) - mat(1, 1) * mat(0, 2)) / d;
    ans(1, 2) = (mat(0, 2) * mat(1, 0) - mat(0, 0) * mat(1, 2)) / d;
    ans(2, 1) = (mat(2, 0) * mat(0, 1) - mat(0, 0) * mat(2, 1)) / d;
    return ans;
  }
  
  template <typename T> Matrix<4, 4, T> inverse(const Matrix<4, 4, T>& m)
  {
    Matrix<4, 4, T> inv;
  
    inv(0, 0) = (m(1, 1) * m(2, 2) * m(3, 3) - m(1, 1) * m(3, 2) * m(2, 3) - m(1, 2) * m(2, 1) * m(3, 3)
                 + m(1, 2) * m(3, 1) * m(2, 3) + m(1, 3) * m(2, 1) * m(3, 2) - m(1, 3) * m(3, 1) * m(2, 2));
  
    inv(0, 1) = (-m(0, 1) * m(2, 2) * m(3, 3) + m(0, 1) * m(3, 2) * m(2, 3) + m(0, 2) * m(2, 1) * m(3, 3)
                 - m(0, 2) * m(3, 1) * m(2, 3) - m(0, 3) * m(2, 1) * m(3, 2) + m(0, 3) * m(3, 1) * m(2, 2));
  
    inv(0, 2) = (m(0, 1) * m(1, 2) * m(3, 3) - m(0, 1) * m(3, 2) * m(1, 3) - m(0, 2) * m(1, 1) * m(3, 3)
                 + m(0, 2) * m(3, 1) * m(1, 3) + m(0, 3) * m(1, 1) * m(3, 2) - m(0, 3) * m(3, 1) * m(1, 2));
  
    inv(0, 3) = (-m(0, 1) * m(1, 2) * m(2, 3) + m(0, 1) * m(2, 2) * m(1, 3) + m(0, 2) * m(1, 1) * m(2, 3)
                 - m(0, 2) * m(2, 1) * m(1, 3) - m(0, 3) * m(1, 1) * m(2, 2) + m(0, 3) * m(2, 1) * m(1, 2));
  
    inv(1, 0) = (-m(1, 0) * m(2, 2) * m(3, 3) + m(1, 0) * m(3, 2) * m(2, 3) + m(1, 2) * m(2, 0) * m(3, 3)
                 - m(1, 2) * m(3, 0) * m(2, 3) - m(1, 3) * m(2, 0) * m(3, 2) + m(1, 3) * m(3, 0) * m(2, 2));
  
    inv(1, 1) = (m(0, 0) * m(2, 2) * m(3, 3) - m(0, 0) * m(3, 2) * m(2, 3) - m(0, 2) * m(2, 0) * m(3, 3)
                 + m(0, 2) * m(3, 0) * m(2, 3) + m(0, 3) * m(2, 0) * m(3, 2) - m(0, 3) * m(3, 0) * m(2, 2));
  
    inv(1, 2) = (-m(0, 0) * m(1, 2) * m(3, 3) + m(0, 0) * m(3, 2) * m(1, 3) + m(0, 2) * m(1, 0) * m(3, 3)
                 - m(0, 2) * m(3, 0) * m(1, 3) - m(0, 3) * m(1, 0) * m(3, 2) + m(0, 3) * m(3, 0) * m(1, 2));
  
    inv(1, 3) = (m(0, 0) * m(1, 2) * m(2, 3) - m(0, 0) * m(2, 2) * m(1, 3) - m(0, 2) * m(1, 0) * m(2, 3)
                 + m(0, 2) * m(2, 0) * m(1, 3) + m(0, 3) * m(1, 0) * m(2, 2) - m(0, 3) * m(2, 0) * m(1, 2));
  
    inv(2, 0) = (m(1, 0) * m(2, 1) * m(3, 3) - m(1, 0) * m(3, 1) * m(2, 3) - m(1, 1) * m(2, 0) * m(3, 3)
                 + m(1, 1) * m(3, 0) * m(2, 3) + m(1, 3) * m(2, 0) * m(3, 1) - m(1, 3) * m(3, 0) * m(2, 1));
  
    inv(2, 1) = (-m(0, 0) * m(2, 1) * m(3, 3) + m(0, 0) * m(3, 1) * m(2, 3) + m(0, 1) * m(2, 0) * m(3, 3)
                 - m(0, 1) * m(3, 0) * m(2, 3) - m(0, 3) * m(2, 0) * m(3, 1) + m(0, 3) * m(3, 0) * m(2, 1));
  
    inv(2, 2) = (m(0, 0) * m(1, 1) * m(3, 3) - m(0, 0) * m(3, 1) * m(1, 3) - m(0, 1) * m(1, 0) * m(3, 3)
                 + m(0, 1) * m(3, 0) * m(1, 3) + m(0, 3) * m(1, 0) * m(3, 1) - m(0, 3) * m(3, 0) * m(1, 1));
  
    inv(2, 3) = (-m(0, 0) * m(1, 1) * m(2, 3) + m(0, 0) * m(2, 1) * m(1, 3) + m(0, 1) * m(1, 0) * m(2, 3)
                 - m(0, 1) * m(2, 0) * m(1, 3) - m(0, 3) * m(1, 0) * m(2, 1) + m(0, 3) * m(2, 0) * m(1, 1));
  
    inv(3, 0) = (-m(1, 0) * m(2, 1) * m(3, 2) + m(1, 0) * m(3, 1) * m(2, 2) + m(1, 1) * m(2, 0) * m(3, 2)
                 - m(1, 1) * m(3, 0) * m(2, 2) - m(1, 2) * m(2, 0) * m(3, 1) + m(1, 2) * m(3, 0) * m(2, 1));
  
    inv(3, 1) = (m(0, 0) * m(2, 1) * m(3, 2) - m(0, 0) * m(3, 1) * m(2, 2) - m(0, 1) * m(2, 0) * m(3, 2)
                 + m(0, 1) * m(3, 0) * m(2, 2) + m(0, 2) * m(2, 0) * m(3, 1) - m(0, 2) * m(3, 0) * m(2, 1));
  
    inv(3, 2) = (-m(0, 0) * m(1, 1) * m(3, 2) + m(0, 0) * m(3, 1) * m(1, 2) + m(0, 1) * m(1, 0) * m(3, 2)
                 - m(0, 1) * m(3, 0) * m(1, 2) - m(0, 2) * m(1, 0) * m(3, 1) + m(0, 2) * m(3, 0) * m(1, 1));
  
    inv(3, 3) = (m(0, 0) * m(1, 1) * m(2, 2) - m(0, 0) * m(2, 1) * m(1, 2) - m(0, 1) * m(1, 0) * m(2, 2)
                 + m(0, 1) * m(2, 0) * m(1, 2) + m(0, 2) * m(1, 0) * m(2, 1) - m(0, 2) * m(2, 0) * m(1, 1));
  
    T det = m(0, 0) * inv(0, 0) + m(1, 0) * inv(0, 1) + m(2, 0) * inv(0, 2) + m(3, 0) * inv(0, 3);
  
    T inv_det = 1.0 / det;
  
    return inv_det * inv;
  }
  
  template <size_t nRows, typename T> CU_HOST_DEVICE Matrix<nRows, nRows, T> inverse(const Matrix<nRows, nRows, T>& mat)
  {
    Matrix<nRows, nRows, T> ans;
    T d = det(mat);
    if(d == 0)
      return ans;
    for(size_t i = 0; i < nRows; ++i)
      for(size_t j = 0; j < nRows; ++j) {
        ans(i, j) = cofactor(mat, j, i) / d;
      }
    return ans;
  }
  
  template <size_t nRows, size_t nCols, typename T>
  CU_HOST_DEVICE Matrix<nCols, nRows, T> transpose(const Matrix<nRows, nCols, T>& mat)
  {
    Matrix<nCols, nRows, T> ans;
    for(size_t i = 0; i < nRows; i++)
      for(size_t j = 0; j < nCols; j++)
        ans(j, i) = mat(i, j);
    return ans;
  }
  
  template <size_t nRows, size_t nCols, typename T> CU_HOST_DEVICE T normsq(const Matrix<nRows, nCols, T>& mat)
  {
    T acc = 0;
    for(size_t i = 0; i < nRows; i++)
      for(size_t j = 0; j < nCols; j++) {
        const T& v = mat(i, j);
        acc += v * v;
      }
    return acc;
  }
  
  template <size_t nRows, size_t nCols, typename T> CU_HOST_DEVICE T norm(const Matrix<nRows, nCols, T>& mat)
  {
    return std::sqrt(normsq(mat));
  }
}
#endif
