/*
 * SimpleMath - A simple highly inefficient single header C++ math library
 * Copyright (c) 2019 Martin Felis <martin@fysx.org>
 *
 * This is a highly inefficient math library. It was conceived while he was
 * waiting for code to compile which used a highly efficient math library.
 *
 * It is intended to be used as a fast compiling substitute for the
 * blazingly fast Eigen3
 * http://eigen.tuxfamily.org/index.php?title=Main_Page library and tries
 * to mimic its API to a certain extent.
 *
 * Feel free to use it wherever you like (even claim it as yours!). However,
 * no guarantees are given that this code does what it says it would.
 *
 * Should you need a more formal license go with the following (zlib license):
 *
 * This software is provided 'as-is', without any express or implied
 * warranty. In no event will the authors be held liable for any damages
 * arising from the use of this software.
 *
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 *
 * 1. The origin of this software must not be misrepresented; you must not
 *    claim that you wrote the original software. If you use this software
 *    in a product, an acknowledgment in the product documentation would be
 *    appreciated but is not required.
 * 2. Altered source versions must be plainly marked as such, and must not be
 *    misrepresented as being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 *
*/

#pragma once

#include <sstream>
#include <cassert>
#include <iostream>
#include <cstring> // for memcpy
#include <cmath>
#include <limits>
#include <type_traits>

namespace SimpleMath {

//
// Forward Declarations
//
enum {
        Dynamic = -1
};

template <typename ScalarType, int NumRows = Dynamic, int NumCols = Dynamic>
struct Matrix;

template <typename Derived>
struct CommaInitializer;

template <typename Derived, typename ScalarType, int NumRows = -1, int NumCols = -1>
struct Block;

template <typename Derived, typename ScalarType, int NumRows, int NumCols>
struct Transpose;

typedef Matrix<float, 3, 3> Matrix33f;
typedef Matrix<float, 3, 1> Vector3f;

template <typename Derived>
class LLT;

template <typename Derived>
class PartialPivLU;

template <typename Derived>
class HouseholderQR;

template <typename Derived>
class ColPivHouseholderQR;


//
// Main MatrixBase class which defines all functions available on the
// derived matrix types.
//
template <typename Derived, typename ScalarType, int Rows, int Cols>
struct MatrixBase {
  typedef MatrixBase<Derived, ScalarType, Rows, Cols> MatrixType;
  typedef ScalarType value_type;

  enum {
    RowsAtCompileTime = Rows,
    ColsAtCompileTime = Cols
  };


  Derived& operator=(const Derived& other) {
    if (static_cast<const void*>(this) != static_cast<const void*>(&other)) {
      for (size_t i = 0; i < other.rows(); i++) {
        for (size_t j = 0; j < other.cols(); j++) {
          this->operator()(i,j) = other(i,j);
        }
      }
    }

    return *this;
  }


  template <typename OtherDerived, typename OtherScalarType, int OtherRows, int OtherCols>
  Derived& operator=(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols>& other) {
    if (static_cast<const void*>(this) != static_cast<const void*>(&other)) {
      for (size_t i = 0; i < other.rows(); i++) {
        for (size_t j = 0; j < other.cols(); j++) {
          this->operator()(i,j) = other(i,j);
        }
      }
    }
    return *this;
  }

  //
  // operators with scalars
  //
  Matrix<ScalarType, Rows, Cols> operator*(const double& scalar) const {
    Matrix<ScalarType, Rows, Cols> result (rows(), cols());
    for (size_t i = 0; i < rows(); i++) {
      for (size_t j = 0; j < cols(); j++) {
        result (i,j) = operator()(i,j) * static_cast<ScalarType>(scalar);
        }
    }
    return result;
  }

  Matrix<ScalarType, Rows, Cols> operator*(const float& scalar) const {
    Matrix<ScalarType, Rows, Cols> result (rows(), cols());
    for (size_t i = 0; i < rows(); i++) {
      for (size_t j = 0; j < cols(); j++) {
        result (i,j) = operator()(i,j) * static_cast<ScalarType>(scalar);
      }
    }
    return result;
  }

  //
  // operators with other matrices
  //
  template <typename OtherDerived, typename OtherScalarType, int OtherRows, int OtherCols>
  bool operator==(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols>& other) const {
    for (size_t i = 0; i < other.rows(); i++) {
      for (size_t j = 0; j < other.cols(); j++) {
        if (this->operator()(i,j) != other(i,j))
          return false;
      }
    }

    return true;
  }

  template <typename OtherDerived, typename OtherScalarType, int OtherRows, int OtherCols>
  bool operator!=(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols>& other) const {
    return !(operator==(other));
  }

  CommaInitializer<Derived> operator<< (const value_type& value) {
    return CommaInitializer<Derived> (*(static_cast<Derived*>(this)), value);
  }

  template <typename OtherDerived>
  Derived operator+(const OtherDerived& other) const {
    Derived result (*(static_cast<const Derived*>(this)));
    result += other;
    return result;
  }

  template <typename OtherDerived>
  Derived operator-(const OtherDerived& other) {
    Derived result (*(static_cast<Derived*>(this)));
    result -= other;
    return result;
  }

  template <typename OtherDerived>
  Derived operator-(const OtherDerived& other) const {
    Derived result (*(static_cast<const Derived*>(this)));
    result -= other;
    return result;
  }

  template<typename OtherDerived, typename OtherScalarType, int OtherRows, int OtherCols>
  Matrix<ScalarType, Rows, OtherCols>
  operator*(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols> &other) const {
    Matrix<ScalarType, Rows, OtherCols> result(Matrix<ScalarType, Rows, OtherCols>::Zero(rows(), other.cols()));
    for (size_t i = 0; i < rows(); i++) {
      for (size_t j = 0; j < other.cols(); j++) {
        for (size_t k = 0; k < other.rows(); k++) {
          result(i, j) += operator()(i, k) * other(k, j);
        }
      }
    }
    return result;
  }

  template <typename OtherDerived>
  Derived operator*=(const MatrixBase<OtherDerived, typename OtherDerived:: value_type, OtherDerived::RowsAtCompileTime, OtherDerived::ColsAtCompileTime> &other) {
    Derived copy (*static_cast<const Derived*>(this));
    this->setZero();
    for (size_t i = 0; i < rows(); i++) {
      for (size_t j = 0; j < other.cols(); j++) {
        for (size_t k = 0; k < other.rows(); k++) {
          this->operator()(i, j) += copy.operator()(i, k) * other(k, j);
        }
      }
    }
    return *this;
  }

  Matrix<ScalarType, Rows, Cols> operator-() const {
    Matrix<ScalarType, Rows, Cols> copy (*static_cast<const Derived*>(this));
    for (size_t i = 0; i < rows(); i++) {
      for (size_t j = 0; j < cols(); j++) {
        copy(i,j) *= static_cast<ScalarType>(-1.);
      }
    }
    return copy;
  }

  Derived operator*=(const ScalarType& s) {
    for (size_t i = 0; i < rows(); i++) {
      for (size_t j = 0; j < cols(); j++) {
        operator()(i,j) *= s;
      }
    }

    return *this;
  }

  template <typename OtherDerived>
  inline void evalTo (OtherDerived& dest) const {
     for (unsigned int i = 0; i < rows(); i++) {
      for (unsigned int j = 0; j < cols(); j++) {
        dest(i,j) = this->operator()(i, j);
      }
    }
  }

  void resize(unsigned int nrows, unsigned int ncols = 1) {
    static_assert(Rows == Dynamic, "Resize of fixed size matrices not allowed.");

    // Resize the this matrix (so far only possible for subclasses of the
    // Matrix class)
    Matrix<ScalarType, Rows, Cols>* this_matrix = static_cast<Matrix<ScalarType, Rows, Cols>*>(this);
    this_matrix->mStorage.resize(nrows, ncols);
  }

  void conservativeResize(unsigned int nrows, unsigned int ncols = 1) {
    static_assert(Rows == Dynamic, "Resize of fixed size matrices not allowed.");

    Derived copy(*this);

    unsigned int arows = std::min(nrows, (unsigned int) rows());
    unsigned int acols = std::min(ncols, (unsigned int) cols());

    resize(nrows, ncols);
    setZero();

    // TODO: set entries to zero within the loop
    for (unsigned int i = 0; i < arows; i++) {
      for (unsigned int j = 0; j < acols; j++) {
        this->operator()(i, j) = copy(i,j);
      }
    }
  }

  void setZero() {
    for (size_t i = 0; i < rows(); i++) {
      for (size_t j = 0; j < cols(); j++) {
        operator()(i,j) = static_cast<ScalarType>(0.0);
      }
    }
  }

  void set(const ScalarType& v0) {
    static_assert(cols() * rows() == 1, "Invalid matrix size");
    data()[0] = v0;
  }

  void set(
      const ScalarType& v0,
      const ScalarType& v1,
      const ScalarType& v2
      ) {
    assert(cols() * rows() == 3);

    data()[0] = v0;
    data()[1] = v1;
    data()[2] = v2;
  }

  void set(
      const ScalarType& v0,
      const ScalarType& v1,
      const ScalarType& v2,
      const ScalarType& v3
      ) {
    assert(cols() * rows() == 4);

    data()[0] = v0;
    data()[1] = v1;
    data()[2] = v2;
    data()[3] = v3;
  }

  void set(
      const ScalarType& v0,
      const ScalarType& v1,
      const ScalarType& v2,
      const ScalarType& v3,
      const ScalarType& v4,
      const ScalarType& v5
      ) {
    assert(cols() * rows() == 6);

    data()[0] = v0;
    data()[1] = v1;
    data()[2] = v2;
    data()[3] = v3;
    data()[4] = v4;
    data()[5] = v5;
  }

  void set(
      const ScalarType& v00,
      const ScalarType& v01,
      const ScalarType& v02,
      const ScalarType& v03,
      const ScalarType& v04,
      const ScalarType& v05,

      const ScalarType& v10,
      const ScalarType& v11,
      const ScalarType& v12,
      const ScalarType& v13,
      const ScalarType& v14,
      const ScalarType& v15,

      const ScalarType& v20,
      const ScalarType& v21,
      const ScalarType& v22,
      const ScalarType& v23,
      const ScalarType& v24,
      const ScalarType& v25,

      const ScalarType& v30,
      const ScalarType& v31,
      const ScalarType& v32,
      const ScalarType& v33,
      const ScalarType& v34,
      const ScalarType& v35,

      const ScalarType& v40,
      const ScalarType& v41,
      const ScalarType& v42,
      const ScalarType& v43,
      const ScalarType& v44,
      const ScalarType& v45,

      const ScalarType& v50,
      const ScalarType& v51,
      const ScalarType& v52,
      const ScalarType& v53,
      const ScalarType& v54,
      const ScalarType& v55
      ) {
    assert(cols() == 6 && rows() == 6);

    operator()(0,0) = v00;
    operator()(0,1) = v01;
    operator()(0,2) = v02;
    operator()(0,3) = v03;
    operator()(0,4) = v04;
    operator()(0,5) = v05;

    operator()(1,0) = v10;
    operator()(1,1) = v11;
    operator()(1,2) = v12;
    operator()(1,3) = v13;
    operator()(1,4) = v14;
    operator()(1,5) = v15;

    operator()(2,0) = v20;
    operator()(2,1) = v21;
    operator()(2,2) = v22;
    operator()(2,3) = v23;
    operator()(2,4) = v24;
    operator()(2,5) = v25;

    operator()(3,0) = v30;
    operator()(3,1) = v31;
    operator()(3,2) = v32;
    operator()(3,3) = v33;
    operator()(3,4) = v34;
    operator()(3,5) = v35;

    operator()(4,0) = v40;
    operator()(4,1) = v41;
    operator()(4,2) = v42;
    operator()(4,3) = v43;
    operator()(4,4) = v44;
    operator()(4,5) = v45;

    operator()(5,0) = v50;
    operator()(5,1) = v51;
    operator()(5,2) = v52;
    operator()(5,3) = v53;
    operator()(5,4) = v54;
    operator()(5,5) = v55;
   }

  size_t rows() const {
    return static_cast<const Derived*>(this)->rows();
  }

  size_t cols() const {
    return static_cast<const Derived*>(this)->cols();
  }

  size_t size() const {
    return static_cast<const Derived*>(this)->rows() * static_cast<const Derived*>(this)->cols();
  }

  const ScalarType& operator()(const size_t& i, const size_t& j) const {
    return static_cast<const Derived*>(this)->operator()(i,j);
  }
  ScalarType& operator()(const size_t& i, const size_t& j) {
    return static_cast<Derived*>(this)->operator()(i,j);
  }

  const ScalarType& operator[](const size_t& i) const {
    assert(cols() == 1);
    return static_cast<const Derived*>(this)->operator()(i,0);
  }
  ScalarType& operator[](const size_t& i) {
    assert(cols() == 1);
    return static_cast<Derived*>(this)->operator()(i,0);
  }

  operator ScalarType() const {
#ifndef NDEBUG
    if ( static_cast<const Derived*>(this)->cols() != 1
        || static_cast<const Derived*>(this)->rows() != 1) {
      std::cout << "Error trying to cast to scalar type. Dimensions are: "
        << static_cast<const Derived*>(this)->rows() << ", "
        << static_cast<const Derived*>(this)->cols() << "."
        << std::endl;
    }
#endif
    assert ( static_cast<const Derived*>(this)->cols() == 1
        && static_cast<const Derived*>(this)->rows() == 1);
    return static_cast<const Derived*>(this)->operator()(0,0);
  }

  //
  // Numerical functions
  //

  // TODO: separate functions for float or ScalarType matrices
  ScalarType dot(const Derived& other) const {
    assert ((rows() == 1 || cols() == 1) && (other.rows() == 1 || other.cols() == 1));

    ScalarType result = 0.0;

    size_t n = rows() * cols();
    for (size_t i = 0; i < n; ++i) {
      result += operator[](i) * other[i];
    }

    return result;
  }

  // TODO: separate functions for float or ScalarType matrices
  ScalarType squaredNorm() const {
    ScalarType result = static_cast<ScalarType>(0.0);

    size_t nr = rows();
    size_t nc = cols();
    for (size_t i = 0; i < nr; ++i) {
      for (size_t j = 0; j < nc; ++j) {
        result += operator()(i, j) * operator()(i, j);
      }
    }

    return result;
  }

  // TODO: separate functions for float or ScalarType matrices
  ScalarType norm() const {
    return static_cast<ScalarType>(std::sqrt(squaredNorm()));
  }

  // TODO: separate functions for float or ScalarType matrices
  Derived normalized() const {
    Derived result (*this);

    ScalarType length = this->norm();

    return result / length;
  }

  // TODO: separate functions for float or ScalarType matrices
  Derived normalize() {
    ScalarType length = norm();

    *this *= static_cast<ScalarType>(1.0) / length;

    return *this;
  }

  Derived cross(const Derived& other) const {
    assert(cols() * rows() == 3);

    Derived result(rows(), cols());
    result[0] = operator[](1) * other[2] - operator[](2) * other[1];
    result[1] = operator[](2) * other[0] - operator[](0) * other[2];
    result[2] = operator[](0) * other[1] - operator[](1) * other[0];

    return result;
  }

  Derived inverse() const {
    if (rows() == cols()) {
       if (rows() == 1) {
        Derived result(rows(), cols());
        result(0,0) = static_cast<ScalarType>(1.) / operator()(0,0);

        return result;
       } else if (rows() == 2) {
        const ScalarType& a = operator()(0,0);
        const ScalarType& b = operator()(0,1);
        const ScalarType& c = operator()(1,0);
        const ScalarType& d = operator()(1,1);

        Derived result(rows(), cols());

        ScalarType detinv = static_cast<ScalarType>(1.) / (a * d - b * c);

        result(0,0) = d * detinv;
        result(0,1) = -b * detinv;
        result(1,0) = -c * detinv;
        result(1,1) = d * detinv;

        return result;
      } else if (rows() == 3) {
        // source:
        // https://stackoverflow.com/questions/983999/simple-3x3-matrix-inverse-code-c

        // computes the inverse of a matrix m
        ScalarType det = operator()(0, 0) * (operator()(1, 1) * operator()(2, 2)
            - operator()(2, 1) * operator()(1, 2))
            - operator()(0, 1) * (operator()(1, 0) * operator()(2, 2)
            - operator()(1, 2) * operator()(2, 0))
            + operator()(0, 2) * (operator()(1, 0) * operator()(2, 1)
            - operator()(1, 1) * operator()(2, 0));

        ScalarType invdet = 1. / det;

        Derived result(rows(), cols());

        result(0,0) = (operator()(1, 1) * operator()(2, 2) - operator()(2, 1) * operator()(1, 2)) * invdet;
        result(0,1) = (operator()(0, 2) * operator()(2, 1) - operator()(0, 1) * operator()(2, 2)) * invdet;
        result(0,2) = (operator()(0, 1) * operator()(1, 2) - operator()(0, 2) * operator()(1, 1)) * invdet;
        result(1,0) = (operator()(1, 2) * operator()(2, 0) - operator()(1, 0) * operator()(2, 2)) * invdet;
        result(1,1) = (operator()(0, 0) * operator()(2, 2) - operator()(0, 2) * operator()(2, 0)) * invdet;
        result(1,2) = (operator()(1, 0) * operator()(0, 2) - operator()(0, 0) * operator()(1, 2)) * invdet;
        result(2,0) = (operator()(1, 0) * operator()(2, 1) - operator()(2, 0) * operator()(1, 1)) * invdet;
        result(2,1) = (operator()(2, 0) * operator()(0, 1) - operator()(0, 0) * operator()(2, 1)) * invdet;
        result(2,2) = (operator()(0, 0) * operator()(1, 1) - operator()(1, 0) * operator()(0, 1)) * invdet;

        return result;
      }
    }
    return colPivHouseholderQr().inverse();
  }

  ScalarType trace() const {
    assert(rows() == cols());

    ScalarType result = static_cast<ScalarType>(0.0);

    for (unsigned int i = 0; i < rows(); i++) {
      result += operator()(i,i);
    }

    return result;
  }

  ScalarType mean() const {
    assert(rows() == 1 || cols() == 1);
    ScalarType result = static_cast<ScalarType>(0.0);
    for (unsigned int i = 0; i < rows(); i++) {
      result += operator[](i);
    }

    return result / static_cast<ScalarType>(rows() * cols());
  }

  ScalarType sum() const {
    assert(rows() == 1 || cols() == 1);
    ScalarType result = static_cast<ScalarType>(0.0);
    for (unsigned int i = 0; i < rows(); i++) {
      result += operator[](i);
    }

    return result;
  }

  ScalarType minCoeff() const {
    assert(rows() > 0 && cols() > 0);
    ScalarType result = operator()(0, 0);
    const unsigned int ni = rows();
    const unsigned int nj = cols();
    for (unsigned int i = 0; i < ni; i++) {
      for (unsigned int j = 0; j < nj; j++) {
        if (operator()(i, j) < result) {
          result = operator()(i, j);
        }
      }
    }
    return result;
  }

  ScalarType maxCoeff() const {
    assert(rows() > 0 && cols() > 0);
    ScalarType result = operator()(0, 0);
    const unsigned int ni = rows();
    const unsigned int nj = cols();
    for (unsigned int i = 0; i < ni; i++) {
      for (unsigned int j = 0; j < nj; j++) {
        if (operator()(i, j) > result) {
          result = operator()(i, j);
        }
      }
    }
    return result;
  }

  const LLT<Derived> llt() const {
    return LLT<Derived>(*this);
  }

  const PartialPivLU<Derived> partialPivLu() const {
    return PartialPivLU<Derived>(*this);
  }

  const HouseholderQR<Derived> householderQr() const {
    return HouseholderQR<Derived>(*this);
  }

  const ColPivHouseholderQR<Derived> colPivHouseholderQr() const {
    return ColPivHouseholderQR<Derived>(*this);
  }

  ScalarType* data() {
    return static_cast<Derived*>(this)->data();
  }

  const ScalarType* data() const {
    return static_cast<const Derived*>(this)->data();
  }

  //
  // Special Constructors
  //
  static Derived Zero(size_t NumRows = (Rows == Dynamic) ? 1 : Rows,
    size_t NumCols = (Cols == Dynamic) ? 1 : Cols) {
    Derived result (NumRows, NumCols);

    for (size_t i = 0; i < NumRows; i++) {
      for (size_t j = 0; j < NumCols; j++) {
        result(i,j) = static_cast<ScalarType>(0.0);
      }
    }

    return result;
  }

  static Derived Identity(size_t NumRows = Rows, size_t NumCols = Cols) {
    Derived result (Derived::Zero(NumRows, NumCols));

    for (size_t i = 0; i < NumRows; i++) {
        result(i,i) = static_cast<ScalarType>(1.0);
    }

    return result;
  }

  static Derived Constant(size_t NumRows, const ScalarType &value) {
    Derived result (NumRows, 1);

    for (size_t i = 0; i < NumRows; i++) {
      result(i,0) = value;
    }

    return result;
  }

  static Derived Constant(size_t NumRows, size_t NumCols, const ScalarType &value) {
    Derived result (NumRows, NumCols);

    for (size_t i = 0; i < NumRows; i++) {
      for (size_t j = 0; j < NumCols; j++) {
        result(i,j) = value;
      }
    }

    return result;
  }

  static Derived Random(size_t NumRows = (Rows == Dynamic) ? 1 : Rows, size_t NumCols = (Cols == Dynamic) ? 1 : Cols) {
    Derived result (NumRows, NumCols);

    for (size_t i = 0; i < NumRows; i++) {
      for (size_t j = 0; j < NumCols; j++) {
        result(i,j) = (static_cast<value_type>(rand()) / static_cast<value_type>(RAND_MAX)) * 2.0 - 1.0;
      }
    }

    return result;
  }


  //
  // Block accessors
  //
  template <
    int block_rows,
    int block_cols
    >
  Block<
    Derived,
    ScalarType,
    block_rows,
    block_cols
    > block(int block_row_index, int block_col_index) {
      assert(block_row_index + block_rows <= rows());
      assert(block_col_index + block_cols <= cols());
    return Block<Derived, ScalarType, block_rows, block_cols>(static_cast<Derived*>(this), block_row_index, block_col_index);
  }

  template <
    int block_rows,
    int block_cols
    >
  const Block<
    Derived,
    ScalarType,
    block_rows,
    block_cols
    > block(int block_row_index, int block_col_index) const {
      assert(block_row_index + block_rows <= rows());
      assert(block_col_index + block_cols <= cols());
    return Block<Derived, ScalarType, block_rows, block_cols>(const_cast<Derived*>(static_cast<const Derived*>(this)), block_row_index, block_col_index);
  }

  Block<
    Derived,
    ScalarType
    > block(int block_row_index, int block_col_index,
        int block_num_rows, int block_num_cols) {
      assert(block_row_index + block_num_rows <= rows());
      assert(block_col_index + block_num_cols <= cols());
    return Block<Derived, ScalarType>(static_cast<Derived*>(this), block_row_index, block_col_index, block_num_rows, block_num_cols);
  }

  const Block<
    const Derived,
    ScalarType
    > block(int block_row_index, int block_col_index,
        int block_num_rows, int block_num_cols) const {
      assert(block_row_index + block_num_rows <= rows());
      assert(block_col_index + block_num_cols <= cols());
    return Block<const Derived, ScalarType>(static_cast<const Derived*>(this), block_row_index, block_col_index, block_num_rows, block_num_cols);
  }

  // TODO: head, tail

  //
  // Transpose
  //
  Transpose<Derived, ScalarType, Cols, Rows> transpose() {
    return Transpose<Derived, ScalarType, Cols, Rows>(static_cast<Derived*>(this));
  }

  const Transpose<const Derived, ScalarType, Cols, Rows> transpose() const {
    return Transpose<const Derived, ScalarType, Cols, Rows>(static_cast<const Derived*>(this));
  }
};


template <typename ScalarType, int SizeAtCompileTime, int NumRows, int NumCols>
struct Storage;

template <typename ScalarType, int SizeAtCompileTime, int NumCols>
struct Storage<ScalarType, SizeAtCompileTime, -1, NumCols> : public Storage<ScalarType, 0, -1, -1> {};

// fixed storage
template <typename ScalarType, int SizeAtCompileTime, int NumRows, int NumCols>
struct Storage {
  ScalarType mData[SizeAtCompileTime];

  Storage() {}

  Storage(int rows, int cols) {
    resize(rows, cols);
  }

  inline size_t rows() const { return NumRows; }

  inline size_t cols() const { return NumCols; }

#ifndef NDEBUG
  void resize(int num_rows, int num_cols) {
    if (num_rows != NumRows || num_cols != NumCols) {
      std::cout << "Error: trying to resize fixed matrix from "
        << NumRows << ", " << NumCols << " to "
        << num_rows << ", " << num_cols << "." << std::endl;
    }
    assert (num_rows == NumRows && num_cols == NumCols);
#else
    void resize(int UNUSED(num_rows), int UNUSED(num_cols)) {
#endif
    // Resizing of fixed size matrices not allowed

  }

  inline ScalarType& coeff(int row_index, int col_index) {
//    assert (row_index >= 0 && row_index <= NumRows);
//    assert (col_index >= 0 && col_index <= NumCols);
    return mData[row_index * NumCols + col_index];
  }

  inline const ScalarType& coeff(int row_index, int col_index) const {
//    assert (row_index >= 0 && row_index <= NumRows);
//    assert (col_index >= 0 && col_index <= NumCols);
    return mData[row_index * NumCols + col_index];
  }
};

template <typename ScalarType, int NumCols>
struct Storage<ScalarType, 0, Dynamic, NumCols> {
    ScalarType* mData = nullptr;
    int mRows = 0;
    int mCols = 0;

    Storage() {}

    ~Storage() {
      delete[] mData;
    }

    Storage(int rows, int cols) {
        resize(rows, cols);
    }

    inline size_t rows() const { return mRows; }
    inline size_t cols() const { return mCols; }

    void resize(int num_rows, int num_cols) {
        if (mRows != num_rows || mCols != num_cols) {
            if (mData != nullptr) {
                delete[] mData;
            }

            mData = new ScalarType[num_rows * num_cols];
            mRows = num_rows;
            mCols = num_cols;
        }
    }

    inline ScalarType& coeff(int row_index, int col_index) {
//        assert (row_index >= 0 && row_index <= mRows);
//        assert (col_index >= 0 && col_index <= mCols);
        return mData[row_index * mCols + col_index];
    }
    inline const ScalarType& coeff(int row_index, int col_index) const {
//        assert (row_index >= 0 && row_index <= mRows);
//        assert (col_index >= 0 && col_index <= mCols);
        return mData[row_index * mCols + col_index];
    }
};


template <typename ScalarType>
struct Storage<ScalarType, 0, Dynamic, Dynamic> {
    ScalarType* mData = nullptr;
    int mRows = 0;
    int mCols = 0;

    Storage() {}

    ~Storage() {
      delete[] mData;
    }

    Storage(int rows, int cols) {
        resize(rows, cols);
    }

    inline size_t rows() const { return mRows; }
    inline size_t cols() const { return mCols; }

    void resize(int num_rows, int num_cols) {
        if (mRows != num_rows || mCols != num_cols) {
            if (mData != nullptr) {
                delete[] mData;
            }

            mData = new ScalarType[num_rows * num_cols];
            mRows = num_rows;
            mCols = num_cols;
        }
    }

    inline ScalarType& coeff(int row_index, int col_index) {
//        assert (row_index >= 0 && row_index <= mRows);
//        assert (col_index >= 0 && col_index <= mCols);
        return mData[row_index * mCols + col_index];
    }
    inline const ScalarType& coeff(int row_index, int col_index) const {
//        assert (row_index >= 0 && row_index <= mRows);
//        assert (col_index >= 0 && col_index <= mCols);
        return mData[row_index * mCols + col_index];
    }
};


template <typename ScalarType, int NumRows, int NumCols>
struct Matrix : public MatrixBase<Matrix<ScalarType, NumRows, NumCols>, ScalarType, NumRows, NumCols> {
  typedef Matrix DerivedBase;

  enum {
    RowsAtCompileTime = (NumCols == Dynamic || NumRows == Dynamic) ? -1 : NumRows,
    ColsAtCompileTime = (NumCols == Dynamic || NumRows == Dynamic) ? -1 : NumCols,
    SizeAtCompileTime = (NumRows != Dynamic && NumCols != Dynamic) ? NumRows * NumCols : 0
  };

  Storage<ScalarType, SizeAtCompileTime, RowsAtCompileTime, ColsAtCompileTime> mStorage;

  Matrix() :
    mStorage (
        SizeAtCompileTime / ColsAtCompileTime,
        SizeAtCompileTime / RowsAtCompileTime
        ) {}

  explicit Matrix(int rows) : mStorage (rows, 1) {}
  explicit Matrix(unsigned int rows) : mStorage (rows, 1) {}
  explicit Matrix(size_t rows) : mStorage (rows, 1) {}

  explicit Matrix(int rows, int cols) :
    mStorage(rows, cols) {}

  explicit Matrix(int rows, unsigned int cols) :
    mStorage(rows, cols) {}

  explicit Matrix(int rows, size_t cols) :
    mStorage(rows, cols) {}

  explicit Matrix(unsigned int rows, int cols) :
    mStorage(rows, cols) {}

  explicit Matrix(unsigned int rows, unsigned int cols) :
    mStorage(rows, cols) {}

  explicit Matrix(unsigned int rows, size_t cols) :
    mStorage(rows, cols) {}

  explicit Matrix(size_t rows, int cols) :
    mStorage(rows, cols) {}

  explicit Matrix(size_t rows, unsigned int cols) :
    mStorage(rows, cols) {}

  explicit Matrix(size_t rows, size_t cols) :
    mStorage(rows, cols) {}

  template<typename OtherDerived, typename OtherScalarType, int OtherRows, int OtherCols>
  Matrix(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols> &other) {
    mStorage.resize(other.rows(), other.cols());
    for (size_t i = 0; i < rows(); i++) {
      for (size_t j = 0; j < cols(); j++) {
        this->operator()(i, j) = other(i, j);
      }
    }
  }

  Matrix (const Matrix& other) :
  mStorage(other.rows(), other.cols()){
    memcpy (data(), other.data(), sizeof (ScalarType) * rows() * cols());
  }

  Matrix& operator=(const Matrix& other) {
    if (&other != this) {
      mStorage.resize(other.rows(), other.cols());
      memcpy (data(), other.data(), sizeof (ScalarType) * rows() * cols());
    }
    return *this;
  }

  //
  // Constructor for vectors
  //
  explicit Matrix (
      const ScalarType& v0,
      const ScalarType& v1
      ) {
    static_assert (NumRows * NumCols == 2, "Invalid matrix size");

    operator()(0,0) = v0;
    operator()(1,0) = v1;
  }

  Matrix (
      const ScalarType& v0,
      const ScalarType& v1,
      const ScalarType& v2
      ) {
    static_assert (NumRows * NumCols == 3, "Invalid matrix size");

    operator()(0,0) = v0;
    operator()(1,0) = v1;
    operator()(2,0) = v2;
  }

  Matrix (
      const ScalarType& v0,
      const ScalarType& v1,
      const ScalarType& v2,
      const ScalarType& v3
      ) {
    static_assert (NumRows * NumCols == 4, "Invalid matrix size");

    operator()(0,0) = v0;
    operator()(1,0) = v1;
    operator()(2,0) = v2;
    operator()(3,0) = v3;
  }

  Matrix (
      const ScalarType& v0,
      const ScalarType& v1,
      const ScalarType& v2,
      const ScalarType& v3,
      const ScalarType& v4,
      const ScalarType& v5
      ) {
    static_assert (NumRows * NumCols == 6, "Invalid matrix size");

    operator()(0,0) = v0;
    operator()(1,0) = v1;
    operator()(2,0) = v2;
    operator()(3,0) = v3;
    operator()(4,0) = v4;
    operator()(5,0) = v5;
  }

  //
  // Constructor for matrices
  //
  Matrix (
      const ScalarType& v00,
      const ScalarType& v01,
      const ScalarType& v02,
      const ScalarType& v10,
      const ScalarType& v11,
      const ScalarType& v12,
      const ScalarType& v20,
      const ScalarType& v21,
      const ScalarType& v22
      ) {
    static_assert (NumRows == 3 && NumCols == 3, "Invalid matrix size");

    operator()(0,0) = v00;
    operator()(0,1) = v01;
    operator()(0,2) = v02;

    operator()(1,0) = v10;
    operator()(1,1) = v11;
    operator()(1,2) = v12;

    operator()(2,0) = v20;
    operator()(2,1) = v21;
    operator()(2,2) = v22;
  }

  Matrix (
      const ScalarType& v00,
      const ScalarType& v01,
      const ScalarType& v02,
      const ScalarType& v03,
      const ScalarType& v10,
      const ScalarType& v11,
      const ScalarType& v12,
      const ScalarType& v13,
      const ScalarType& v20,
      const ScalarType& v21,
      const ScalarType& v22,
      const ScalarType& v23,
      const ScalarType& v30,
      const ScalarType& v31,
      const ScalarType& v32,
      const ScalarType& v33
      ) {
    static_assert (NumRows == 4 && NumCols == 4, "Invalid matrix size");

    operator()(0,0) = v00;
    operator()(0,1) = v01;
    operator()(0,2) = v02;
    operator()(0,3) = v03;

    operator()(1,0) = v10;
    operator()(1,1) = v11;
    operator()(1,2) = v12;
    operator()(1,3) = v13;

    operator()(2,0) = v20;
    operator()(2,1) = v21;
    operator()(2,2) = v22;
    operator()(2,3) = v23;

    operator()(3,0) = v30;
    operator()(3,1) = v31;
    operator()(3,2) = v32;
    operator()(3,3) = v33;
  }

  Matrix (
      const ScalarType& v00,
      const ScalarType& v01,
      const ScalarType& v02,
      const ScalarType& v03,
      const ScalarType& v04,
      const ScalarType& v05,

      const ScalarType& v10,
      const ScalarType& v11,
      const ScalarType& v12,
      const ScalarType& v13,
      const ScalarType& v14,
      const ScalarType& v15,

      const ScalarType& v20,
      const ScalarType& v21,
      const ScalarType& v22,
      const ScalarType& v23,
      const ScalarType& v24,
      const ScalarType& v25,

      const ScalarType& v30,
      const ScalarType& v31,
      const ScalarType& v32,
      const ScalarType& v33,
      const ScalarType& v34,
      const ScalarType& v35,

      const ScalarType& v40,
      const ScalarType& v41,
      const ScalarType& v42,
      const ScalarType& v43,
      const ScalarType& v44,
      const ScalarType& v45,

      const ScalarType& v50,
      const ScalarType& v51,
      const ScalarType& v52,
      const ScalarType& v53,
      const ScalarType& v54,
      const ScalarType& v55
        ) {
          static_assert (NumRows == 6 && NumCols == 6, "Invalid matrix size");

          operator()(0,0) = v00;
          operator()(0,1) = v01;
          operator()(0,2) = v02;
          operator()(0,3) = v03;
          operator()(0,4) = v04;
          operator()(0,5) = v05;

          operator()(1,0) = v10;
          operator()(1,1) = v11;
          operator()(1,2) = v12;
          operator()(1,3) = v13;
          operator()(1,4) = v14;
          operator()(1,5) = v15;

          operator()(2,0) = v20;
          operator()(2,1) = v21;
          operator()(2,2) = v22;
          operator()(2,3) = v23;
          operator()(2,4) = v24;
          operator()(2,5) = v25;

          operator()(3,0) = v30;
          operator()(3,1) = v31;
          operator()(3,2) = v32;
          operator()(3,3) = v33;
          operator()(3,4) = v34;
          operator()(3,5) = v35;

          operator()(4,0) = v40;
          operator()(4,1) = v41;
          operator()(4,2) = v42;
          operator()(4,3) = v43;
          operator()(4,4) = v44;
          operator()(4,5) = v45;

          operator()(5,0) = v50;
          operator()(5,1) = v51;
          operator()(5,2) = v52;
          operator()(5,3) = v53;
          operator()(5,4) = v54;
          operator()(5,5) = v55;
        }

  template <typename OtherDerived>
    Matrix& operator+=(const OtherDerived& other) {
      assert (rows() == other.rows() && cols() == other.cols() && "Error: matrix dimensions do not match!");
      for (size_t i = 0; i < rows(); i++) {
        for (size_t j = 0; j < cols(); j++) {
          this->operator()(i,j) += other(i,j);
        }
      }
      return *this;
    }

  Matrix& operator+=(const ScalarType& scalar) {
    assert (rows() == 1 && cols() == 1 && "Error: matrix dimensions do not match!");
    this->operator()(0,0) += scalar;
    return *this;
  }

  template <typename OtherDerived>
    Matrix& operator-=(const OtherDerived& other) {
      assert (rows() == other.rows() && cols() == other.cols() && "Error: matrix dimensions do not match!");
      for (size_t i = 0; i < rows(); i++) {
        for (size_t j = 0; j < cols(); j++) {
            this->operator()(i,j) -= other(i,j);
        }
      }
      return *this;
    }

  Matrix& operator-=(const ScalarType& scalar) {
    assert (rows() == 1 && cols() == 1 && "Error: matrix dimensions do not match!");
    this->operator()(0,0) -= scalar;
    return *this;
  }

  inline ScalarType& operator()(const size_t& i, const size_t& j) {
    return mStorage.coeff(i, j);
  }

  inline const ScalarType& operator()(const size_t& i, const size_t& j) const {
    return mStorage.coeff(i, j);
  }

  ScalarType* data() {
    return mStorage.mData;
  }

  const ScalarType* data() const {
    return mStorage.mData;
  }

  size_t cols() const {
    return mStorage.cols();
  }

  size_t rows() const {
    return mStorage.rows();
  }
};


//
// CommaInitializer
//
template <typename Derived>
struct CommaInitializer {
  typedef typename Derived::value_type value_type;

  private:
    CommaInitializer() {}

    Derived *mParentMatrix;
    unsigned int mRowIndex;
    unsigned int mColIndex;
    bool mElementWasAdded;

  public:

  CommaInitializer(Derived &matrix, const value_type &value) :
    mParentMatrix(&matrix),
    mRowIndex(0),
    mColIndex(0),
    mElementWasAdded(false)
  {
    assert (matrix.rows() > 0 && matrix.cols() > 0);
    mParentMatrix->operator()(0,0) = value;
  }

  CommaInitializer(Derived &matrix, unsigned int row_index, unsigned int col_index) :
    mParentMatrix(&matrix),
    mRowIndex(row_index),
    mColIndex(col_index),
    mElementWasAdded(false)
  {
    assert (matrix.rows() > 0 && matrix.cols() > 0);
  }

  ~CommaInitializer() {
    if (!mElementWasAdded
        && (mColIndex + 1 < mParentMatrix->cols()
          || mRowIndex + 1 < mParentMatrix->rows())) {
      std::cerr
        << "Error: too few elements passed to CommaInitializer Expected "
        << mParentMatrix->rows() * mParentMatrix->cols()
        << " but was given "
        << mRowIndex * mParentMatrix->cols() + mColIndex + 1 << std::endl;
      abort();
    }
  }

  CommaInitializer<Derived> operator, (const value_type &value) {
    mColIndex++;
    if (mColIndex >= mParentMatrix->cols()) {
      mRowIndex++;
      mColIndex = 0;
    }

    if (mRowIndex == mParentMatrix->rows() && mColIndex == 0) {
      std::cerr
        << "Error: too many elements passed to CommaInitializer!Expected "
        << mParentMatrix->rows() * mParentMatrix->cols()
        << " but was given "
        << mRowIndex *mParentMatrix->cols() + mColIndex + 1 << std::endl;
      abort();
    }
    (*mParentMatrix)(mRowIndex, mColIndex) = value;
    mElementWasAdded = true;

    return CommaInitializer(*mParentMatrix, mRowIndex, mColIndex);
  }
};

//
// Transpose
//
template <typename Derived, typename ScalarType, int NumRows, int NumCols>
struct Transpose : public MatrixBase<Transpose<Derived, ScalarType, NumRows, NumCols>, ScalarType, NumRows, NumCols> {
  Derived* mTransposeSource;

  Transpose(Derived* transpose_source) :
    mTransposeSource(transpose_source)
  { }

  Transpose(const Transpose &other) :
    mTransposeSource(other.mTransposeSource)
  { }

  template <typename OtherDerived, typename OtherScalarType, int OtherRows, int OtherCols>
  Matrix<ScalarType, NumRows, OtherCols> operator*(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols>& other) const {
    Matrix<ScalarType, NumRows, OtherCols> result (Matrix<ScalarType, NumRows, OtherCols>::Zero(rows(), other.cols()));

    unsigned int i,j,k;
    unsigned int nrows = rows();
    unsigned int other_ncols = other.cols();
    unsigned int other_nrows = other.rows();

    for (i = 0; i < nrows; i++) {
      for (j = 0; j < other_ncols; j++) {
        for (k = 0; k < other_nrows; k++) {
          result (i,j) += operator()(i,k) * other(k,j);
        }
      }
    }
    return result;
  }

  template <typename OtherDerived, typename OtherScalarType, int OtherRows, int OtherCols>
  Transpose& operator=(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols>& other) {
    if (static_cast<const void*>(this) != static_cast<const void*>(&other)) {
      for (size_t i = 0; i < other.rows(); i++) {
        for (size_t j = 0; j < other.cols(); j++) {
          this->operator()(i,j) = other(i,j);
        }
      }
    }

    return *this;
  }

  size_t rows() const {
    return static_cast<const Derived*>(mTransposeSource)->cols();
  }
  size_t cols() const {
    return static_cast<const Derived*>(mTransposeSource)->rows();
  }

  const ScalarType& operator()(const size_t& i, const size_t& j) const {
    return static_cast<const Derived*>(mTransposeSource)->operator()(j, i);
  }
  ScalarType& operator()(const size_t& i, const size_t& j) {
    return static_cast<Derived*>(mTransposeSource)->operator()(j, i);
  }
};

//
// Block
//
template <typename Derived, typename ScalarType, int NumRows, int NumCols>
struct Block : public MatrixBase<Block<Derived, ScalarType, NumRows, NumCols>, ScalarType, NumRows, NumCols> {
  typedef Block<Derived, ScalarType, NumRows, NumCols> matrix_type;

  Derived* mBlockSource;
  int row_index;
  int col_index;
  int nrows;
  int ncols;

  Block(Derived* block_source, int row_index, int col_index) :
    mBlockSource(block_source),
    row_index(row_index),
    col_index(col_index),
    nrows(NumRows), ncols(NumCols)
  {
    static_assert(NumRows != -1 && NumCols != -1, "Invalid block specifications: unknown number of rows and columns!");
  }

  Block(Derived* block_source, int row_index, int col_index, int num_rows, int num_cols) :
    mBlockSource(block_source),
    row_index(row_index),
    col_index(col_index),
    nrows (num_rows),
    ncols (num_cols)
  {
  }

  Block(const Block &other) :
    mBlockSource(other.mBlockSource),
    row_index(other.row_index),
    col_index(other.col_index)
  { }

  Block& operator=(const Block &other) {
    for (size_t i = 0; i < rows(); i++) {
      for (size_t j = 0; j < cols(); j++) {
        this->operator()(i,j) = other(i,j);
      }
    }
    return *this;
  }

  template <typename OtherDerived, typename OtherScalarType, int OtherRows, int OtherCols>
  Block& operator=(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols>& other) {
    for (size_t i = 0; i < rows(); i++) {
      for (size_t j = 0; j < cols(); j++) {
        this->operator()(i,j) = other(i,j);
      }
    }

    return *this;
  }

  template <typename OtherDerived, typename OtherScalarType, int OtherRows, int OtherCols>
  Block& operator+=(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols>& other) {
    for (size_t i = 0; i < rows(); i++) {
      for (size_t j = 0; j < cols(); j++) {
        this->operator()(i,j) += other(i,j);
      }
    }

    return *this;
  }


  template <typename OtherDerived, typename OtherScalarType, int OtherRows, int OtherCols>
  Matrix<ScalarType, NumRows, OtherCols> operator*(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols>& other) const {
    Matrix<ScalarType, NumRows, OtherCols> result (rows(), other.cols());
    for (size_t i = 0; i < rows(); i++) {
      for (size_t j = 0; j < other.cols(); j++) {
        for (size_t k = 0; k < other.rows(); k++) {
          result (i,j) += operator()(i,k) * other(k,j);
        }
      }
    }
    return result;
  }

  size_t rows() const {
    return nrows;
  }
  size_t cols() const {
    return ncols;
  }

  const ScalarType& operator()(const size_t& i, const size_t& j) const {
    return static_cast<const Derived*>(mBlockSource)->operator()(row_index + i, col_index + j);
  }
  ScalarType& operator()(const size_t& i, const size_t& j) {
    return static_cast<Derived*>(mBlockSource)->operator()(row_index + i,col_index + j);
  }

  template <typename OtherDerived, typename OtherScalarType, int OtherRows, int OtherCols>
  Matrix<ScalarType, NumRows, OtherCols> operator+(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols>& other) const {
    Matrix<ScalarType, NumRows, OtherCols> result (rows(), other.cols());
    for (size_t i = 0; i < rows(); i++) {
      for (size_t j = 0; j < other.cols(); j++) {
        result (i,j) = operator()(i,j) + other(i,j);
      }
    }
    return result;
  }

  private:
  Block() { assert(0 && "Invalid call!"); };

  ScalarType* data() {
    assert("invalid call");
    return NULL;
  }

  const ScalarType* data() const {
    assert("invalid call");
    return NULL;
  }
};

//
// LLT Decomposition
//
template <typename Derived>
class LLT {
public:
    typedef typename Derived::value_type value_type;
    typedef MatrixBase<Derived, value_type, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime> MatrixType;

    LLT() :
            mIsFactorized(false)
    {}

private:
    typedef Matrix<value_type> VectorXd;
    typedef Matrix<value_type> MatrixXXd;
    typedef Matrix<value_type, Derived::RowsAtCompileTime, 1> ColumnVector;

    bool mIsFactorized;
    Derived mL;

public:
    LLT(const Derived &matrix) :
            mIsFactorized(false),
            mL(matrix)
    {
        compute();
    }
    LLT compute() {
      for (unsigned int i = 0; i < mL.rows(); i++) {
        for (unsigned int j = 0; j < mL.rows(); j++) {
          if (j > i) {
            mL(i,j) = 0.;
            continue;
          }
          double s = mL(i,j);
          for (unsigned int k = 0; k < j; k++) {
            s = s - mL(i,k) * mL(j,k);
          }
          if (i > j) {
            mL(i,j) = s / mL(j,j);
          } else if (s > 0.) {
            mL (i,i) = sqrt (s);
          } else {
            std::cerr << "Error computing Cholesky decomposition: matrix not symmetric positive definite!" << std::endl;
            assert (false);
          }
        }
      }


      mIsFactorized = true;

      return *this;
    }
    ColumnVector solve (
        const ColumnVector &rhs
        ) const {
      assert (mIsFactorized);

      ColumnVector y (mL.rows());
      for (unsigned int i = 0; i < mL.rows(); i++) {
        double temp = rhs[i];

        for (unsigned int j = 0; j < i; j++) {
          temp = temp - mL(i,j) * y[j];
        }

        y[i] = temp / mL(i,i);
      }

      ColumnVector x (mL.rows());
      for (int i = mL.rows() - 1; i >= 0; i--) {
        double temp = y[i];

        for (unsigned int j = i + 1; j < mL.rows(); j++) {
          temp = temp - mL(j, i) * x[j];
        }

        x[i] = temp / mL(i,i);
      }

      return x;
    }
    Derived inverse() const {
        assert (mIsFactorized);

        VectorXd rhs_temp = VectorXd::Zero(mL.cols());
        MatrixXXd result (mL.cols(), mL.cols());

        for (unsigned int i = 0; i < mL.cols(); i++) {
            rhs_temp[i] = 1.;

            result.block(0, i, mL.cols(), 1) = solve(rhs_temp);

            rhs_temp[i] = 0.;
        }

        return result;
    }
    Derived matrixL () const {
        return mL;
    }
};


//
// Partial Pivoting LU Decomposition
//
template <typename Derived>
class PartialPivLU {
public:
    typedef typename Derived::value_type value_type;
    typedef MatrixBase<Derived, value_type, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime> MatrixType;
    PartialPivLU() :
            mIsFactorized(false)
    {}
private:
    typedef Matrix<value_type> VectorXd;
    typedef Matrix<value_type> MatrixXXd;
    typedef Matrix<value_type, Derived::RowsAtCompileTime, 1> ColumnVector;
    typedef Matrix<value_type, 1, Derived::ColsAtCompileTime> RowVector;
    bool mIsFactorized;
    unsigned int *mPermutations = nullptr;
    Derived mLU;

public:
    ~PartialPivLU() {
      delete[] mPermutations;
    }

    PartialPivLU(const Derived &matrix) :
            mIsFactorized(false),
            mLU (matrix)
    {
        mPermutations = new unsigned int [matrix.cols() + 1];
        for (unsigned int i = 0; i <= matrix.cols(); i++) {
            mPermutations[i] = i;
        }
        compute(matrix);
    }

    PartialPivLU& compute(const Derived &matrix) {
      unsigned int n = matrix.rows();

      double v_abs;
      RowVector temp_vec;

      unsigned int i,j,k;

      // over all columns
      for (i = 0; i < n; i++) {
        double max_v = 0.0;
        unsigned int max_i = i;

        // Find the row pivoting index
        for (k = i; k < n; k++) {
          if ((v_abs = fabs(mLU(k, i))) > max_v) {
            max_v = v_abs;
            max_i = k;
          }
        }

        if (max_v < std::numeric_limits<double>::epsilon()) {
          std::cerr << "Error: pivoting failed for matrix A = " << std::endl;
          std::cerr << "A = " << matrix << std::endl;
          abort();
        }

        // Perform the permutation
        if (max_i != i) {
          // update permutation vector
          j = mPermutations[i];
          mPermutations[i] = mPermutations[max_i];
          mPermutations[max_i] = j;

          // swap columns
          temp_vec = mLU.block(i,0,1,n);
          mLU.block(i, 0, 1, n) = mLU.block(max_i, 0, 1, n);
          mLU.block(max_i, 0, 1, n) = temp_vec;

          // Increase number of permutations
          mPermutations[n]++;
        }

        // eliminate i'th column of k'th row
        for (k = i+1; k < n; k++) {
          mLU(k,i) = mLU(k,i) / mLU(i,i);

          // iterate over all columns
          for (j = i+1; j < n; j++) {
            mLU(k,j) = mLU(k,j) - mLU(i,j) * mLU(k,i);
          }
        }
      }

      mIsFactorized = true;

      return *this;
    }

    Derived matrixL() const {
      Derived result (Derived::Zero(mLU.rows(), mLU.cols()));

      unsigned int n = mLU.rows();

      for (int i = 0; i < n; i++) {
        for (int j = 0; j < i; j++) {
          result(i,j) = mLU(i,j);
        }

        result(i,i) = 1.0;
      }

      return result;
    }

    Derived matrixU() const {
      Derived result (Derived::Zero(mLU.rows(), mLU.cols()));

      unsigned int n = mLU.rows();

      for (int i = 0; i < n; i++) {
        for (int j = i; j < n; j++) {
          result(i,j) = mLU(i,j);
        }
      }

      return result;
    }

    Derived matrixP() const {
      Derived result(Derived::Zero(mLU.rows(), mLU.cols()));

      unsigned int n = mLU.rows();
      for (int i = 0; i < n; i++) {
        result(i, mPermutations[i]) = 1.0;
      }

      return result;
    }

    ColumnVector solve (
            const ColumnVector &rhs
    ) const {
      assert (mIsFactorized);

      unsigned int n = mLU.rows();

      // Backsolve L^-1 * rhs
      ColumnVector result(n, 1);

      for (unsigned int i = 0; i < n; i++) {
        result[i] = rhs[mPermutations[i]];
        for (unsigned int j = 0; j < i; j++) {
          result[i] = result[i] - result[j] * mLU(i,j);
        }
      }

      // Solve U^-1 * result
      for (int i = n - 1; i >= 0; i--) {
        for (unsigned int j = i + 1; j < n; j++) {
          result[i] = result[i] - result[j] * mLU(i,j);
        }

        result[i] = result[i] / mLU(i,i);
      }

      return result;
    }

    Derived inverse() const {
        assert (mIsFactorized);
        VectorXd rhs_temp = VectorXd::Zero(mLU.cols());
        MatrixXXd result (mLU.cols(), mLU.cols());
        for (unsigned int i = 0; i < mLU.cols(); i++) {
            rhs_temp[i] = 1.;
            result.block(0, i, mLU.cols(), 1) = solve(rhs_temp);
            rhs_temp[i] = 0.;
        }
        return result;
    }
};

//
// QR Decomposition
//
template <typename Derived>
class HouseholderQR {
public:
    typedef typename Derived::value_type value_type;
    typedef MatrixBase<Derived, value_type, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime> MatrixType;

    HouseholderQR() :
            mIsFactorized(false)
    {}

private:
    typedef Matrix<value_type> VectorXd;
    typedef Matrix<value_type> MatrixXXd;
    typedef Matrix<value_type, Derived::RowsAtCompileTime, 1> ColumnVector;

    bool mIsFactorized;
    Matrix<value_type, Derived::RowsAtCompileTime, Derived::RowsAtCompileTime> mQ;
    Derived mR;

public:
    HouseholderQR(const Derived &matrix) :
            mIsFactorized(false),
            mQ(matrix.rows(), matrix.rows())
    {
        compute(matrix);
    }
    HouseholderQR compute(const Derived& matrix) {
        mR = matrix;
        mQ = MatrixType::Identity (mR.rows(), mR.rows());

        for (unsigned int i = 0; i < mR.cols(); i++) {
            unsigned int block_rows = mR.rows() - i;
            unsigned int block_cols = mR.cols() - i;

            MatrixXXd current_block = mR.block(i,i, block_rows, block_cols);
            VectorXd column = current_block.block(0, 0, block_rows, 1);

            value_type alpha = - column.norm();
            if (current_block(0,0) < 0) {
                alpha = - alpha;
            }

            VectorXd v = current_block.block(0, 0, block_rows, 1);
            v[0] = v[0] - alpha;

            MatrixXXd Q (MatrixXXd::Identity(mR.rows(), mR.rows()));

            Q.block(i, i, block_rows, block_rows) = MatrixXXd (Q.block(i, i, block_rows, block_rows))
                                                    - MatrixXXd(v * v.transpose() / (v.squaredNorm() * 0.5));

            mR = Q * mR;

            // Normalize so that we have positive diagonal elements
            if (mR(i,i) < 0) {
                mR.block(i,i,block_rows, block_cols) = MatrixXXd(mR.block(i,i,block_rows, block_cols)) * -1.;
                Q.block(i,i,block_rows, block_rows) = MatrixXXd(Q.block(i,i,block_rows, block_rows)) * -1.;
            }

            mQ = mQ * Q;
        }

        mIsFactorized = true;

        return *this;
    }
    ColumnVector solve (
        const ColumnVector &rhs
        ) const {
      assert (mIsFactorized);

      ColumnVector y = mQ.transpose() * rhs;
      ColumnVector x = ColumnVector::Zero(mR.cols());

      unsigned int ncols = mR.cols();
      for (unsigned int i = ncols - 1; i != 0; i--) {
        value_type z = y[i];

        for (unsigned int j = i + 1; j < ncols; j++) {
          z = z - x[j] * mR(i,j);
        }

        if (fabs(mR(i,i)) < std::numeric_limits<value_type>::epsilon() * 10) {
          std::cerr << "HouseholderQR: Cannot back-substitute as diagonal element is near zero:" << fabs(mR(i,i))<< std::endl;
          abort();
        }
        x[i] = z / mR(i,i);
      }

      assert (!std::isnan(x.squaredNorm()));

      return x;
    }
    Derived inverse() const {
        assert (mIsFactorized);

        VectorXd rhs_temp = VectorXd::Zero(mQ.cols());
        MatrixXXd result (mQ.cols(), mQ.cols());

        for (unsigned int i = 0; i < mQ.cols(); i++) {
            rhs_temp[i] = 1.;

            result.block(0, i, mQ.cols(), 1) = solve(rhs_temp);

            rhs_temp[i] = 0.;
        }

        return result;
    }
    Matrix<value_type> householderQ () const {
        return mQ;
    }
    Derived matrixR () const {
        return mR;
    }
};

template <typename Derived>
class ColPivHouseholderQR {
public:
    typedef typename Derived::value_type value_type;
    typedef MatrixBase<Derived, value_type, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime> MatrixType;


private:
    typedef Matrix<value_type> VectorXd;
    typedef Matrix<value_type> MatrixXXd;
    typedef Matrix<value_type, Derived::RowsAtCompileTime, 1> ColumnVector;

    bool mIsFactorized;
    Matrix<value_type, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime> mQ;
    Derived mR;

    unsigned int *mPermutations;
    value_type mThreshold;
    unsigned int mRank;

public:
    ColPivHouseholderQR():
        mIsFactorized(false) {
        mPermutations = new unsigned int[1];
    }

    ColPivHouseholderQR (const ColPivHouseholderQR& other) {
        mIsFactorized = other.mIsFactorized;
        mQ = other.mQ;
        mR = other.mR;
        mPermutations = new unsigned int[mQ.cols()];
        mThreshold = other.mThreshold;
        mRank = other.mRank;
    }

    ColPivHouseholderQR& operator= (const ColPivHouseholderQR& other) {
        if (this != &other) {
            mIsFactorized = other.mIsFactorized;
            mQ = other.mQ;
            mR = other.mR;
            delete[] mPermutations;
            mPermutations = new unsigned int[mQ.cols()];
            mThreshold = other.mThreshold;
            mRank = other.mRank;
        }

        return *this;
    }

    ColPivHouseholderQR(const MatrixType &matrix) :
            mIsFactorized(false),
            mQ(matrix.rows(), matrix.rows()),
            mThreshold (std::numeric_limits<value_type>::epsilon() * matrix.cols()) {
        mPermutations = new unsigned int [matrix.cols()];
        for (unsigned int i = 0; i < matrix.cols(); i++) {
            mPermutations[i] = i;
        }
        compute(matrix);
    }
    ~ColPivHouseholderQR() {
        delete[] mPermutations;
    }

    ColPivHouseholderQR& setThreshold (const value_type& threshold) {
        mThreshold = threshold;

        return *this;
    }
    ColPivHouseholderQR& compute(const MatrixType &matrix) {
      mR = matrix;
      mQ = MatrixType::Identity (mR.rows(), mR.rows());

      for (unsigned int i = 0; i < mR.cols(); i++) {
        unsigned int block_rows = mR.rows() - i;
        unsigned int block_cols = mR.cols() - i;

        // find and swap the column with the highest norm
        unsigned int col_index_norm_max = i;
        value_type col_norm_max = VectorXd(mR.block(i,i, block_rows, 1)).squaredNorm();

        for (unsigned int j = i + 1; j < mR.cols(); j++) {
          VectorXd column = mR.block(i, j, block_rows, 1);

          if (column.squaredNorm() > col_norm_max) {
            col_index_norm_max = j;
            col_norm_max = column.squaredNorm();
          }
        }

        if (col_norm_max < mThreshold) {
          // if all entries of the column is close to zero, we bail out
          break;
        }


        if (col_index_norm_max != i) {
          VectorXd temp_col = mR.block(0, i, mR.rows(), 1);
          mR.block(0, i, mR.rows(), 1) = mR.block(0, col_index_norm_max, mR.rows(), 1);;
          mR.block(0, col_index_norm_max, mR.rows(), 1) = temp_col;

          unsigned int temp_index = mPermutations[i];
          mPermutations[i] = mPermutations[col_index_norm_max];
          mPermutations[col_index_norm_max] = temp_index;
        }

        MatrixXXd current_block = mR.block(i,i, block_rows, block_cols);
        VectorXd column = current_block.block(0, 0, block_rows, 1);

        value_type alpha = - column.norm();
        if (current_block(0,0) < 0) {
          alpha = - alpha;
        }

        VectorXd v = current_block.block(0, 0, block_rows, 1);
        v[0] = v[0] - alpha;

        MatrixXXd Q (MatrixXXd::Identity(mR.rows(), mR.rows()));

        Q.block(i, i, block_rows, block_rows) = MatrixXXd (Q.block(i, i, block_rows, block_rows))
          - (v * v.transpose()) / (v.squaredNorm() * static_cast<value_type>(0.5));

        mR = Q * mR;

        // Normalize so that we have positive diagonal elements
        if (mR(i,i) < 0) {
          mR.block(i,i,block_rows, block_cols) = MatrixXXd(mR.block(i,i,block_rows, block_cols)) * -1.;
          Q.block(i,i,block_rows, block_rows) = MatrixXXd(Q.block(i,i,block_rows, block_rows)) * -1.;
        }

        mQ = mQ * Q;
      }

      mIsFactorized = true;

      return *this;
    }
    ColumnVector solve (
        const ColumnVector &rhs
        ) const {
    assert (mIsFactorized);

    ColumnVector y = mQ.transpose() * rhs;
    ColumnVector x = ColumnVector::Zero(mR.cols());

    for (int i = mR.cols() - 1; i >= 0; --i) {
      value_type z = y[i];

      for (unsigned int j = i + 1; j < mR.cols(); j++) {
        z = z - x[mPermutations[j]] * mR(i,j);
      }

      if (fabs(mR(i,i)) < std::numeric_limits<value_type>::epsilon() * 10) {
        std::cerr << "HouseholderQR: Cannot back-substitute as diagonal element is near zero:" << fabs(mR(i,i))<< std::endl;
        abort();
      }
      x[mPermutations[i]] = z / mR(i,i);
    }

    assert (!std::isnan(x.squaredNorm()));

    return x;
  }
    Derived inverse() const {
        assert (mIsFactorized);

        VectorXd rhs_temp = VectorXd::Zero(mQ.cols());
        Derived result (mQ.cols(), mQ.cols());

        for (unsigned int i = 0; i < mQ.cols(); i++) {
            rhs_temp[i] = 1.;

            result.block(0, i, mQ.cols(), 1) = solve(rhs_temp);

            rhs_temp[i] = 0.;
        }

        return result;
    }

    Matrix<value_type> householderQ () const {
        return mQ;
    }
    Derived matrixR () const {
        return mR;
    }
    Matrix<value_type> matrixP () const {
        MatrixXXd P = MatrixXXd::Identity(mR.cols(), mR.cols());
        MatrixXXd identity = MatrixXXd::Identity(mR.cols(), mR.cols());
        for (unsigned int i = 0; i < mR.cols(); i++) {
            P.block(0,i,mR.cols(),1) = identity.block(0,mPermutations[i], mR.cols(), 1);
        }
        return P;
    }

    unsigned int rank() const {
        value_type abs_threshold = fabs(mR(0,0)) * mThreshold;

        for (unsigned int i = 1; i < mR.cols(); i++) {
            if (fabs(mR(i,i)) < abs_threshold)
                return i;
        }

        return mR.cols();
    }
};

template <typename Derived, typename ScalarType, int Rows, int Cols>
inline Matrix<ScalarType, Rows, Cols> operator*(const ScalarType& scalar, const MatrixBase<Derived, ScalarType, Rows, Cols> &matrix) {
    return matrix * scalar;
}

template <typename Derived, typename ScalarType, int Rows, int Cols>
inline Matrix<ScalarType, Rows, Cols> operator*(const MatrixBase<Derived, ScalarType, Rows, Cols> &matrix, const ScalarType& scalar) {
    return matrix * scalar;
}

template <typename Derived, typename ScalarType, int Rows, int Cols>
inline Matrix<ScalarType, Rows, Cols> operator/(const MatrixBase<Derived, ScalarType, Rows, Cols> &matrix, const ScalarType& scalar) {
    return matrix * (1.0 / scalar);
}

template <typename Derived, typename ScalarType, int Rows, int Cols>
inline Matrix<ScalarType, Rows, Cols> operator/=(MatrixBase<Derived, ScalarType, Rows, Cols> &matrix, const ScalarType& scalar) {
    return matrix *= (1.0 / scalar);
}

//
// OpenGL Matrices and Quaternions
//

namespace GL {

typedef Matrix<float, 3, 1> Vector3f;
typedef Matrix<float, 3, 3> Matrix33f;

typedef Matrix<float, 4, 1> Vector4f;
typedef Matrix<float, 4, 4> Matrix44f;

inline Matrix33f RotateMat33 (float rot_deg, float x, float y, float z) {
  float c = cosf (rot_deg * M_PI / 180.f);
  float s = sinf (rot_deg * M_PI / 180.f);
  return Matrix33f (
      x * x * (1.0f - c) + c,
      y * x * (1.0f - c) + z * s,
      x * z * (1.0f - c) - y * s,

      x * y * (1.0f - c) - z * s,
      y * y * (1.0f - c) + c,
      y * z * (1.0f - c) + x * s,

      x * z * (1.0f - c) + y * s,
      y * z * (1.0f - c) - x * s,
      z * z * (1.0f - c) + c

      );
}


inline Matrix44f RotateMat44 (float rot_deg, float x, float y, float z) {
  float c = cosf (rot_deg * M_PI / 180.f);
  float s = sinf (rot_deg * M_PI / 180.f);
  return Matrix44f (
      x * x * (1.0f - c) + c,
      y * x * (1.0f - c) + z * s,
      x * z * (1.0f - c) - y * s,
      0.f,

      x * y * (1.0f - c) - z * s,
      y * y * (1.0f - c) + c,
      y * z * (1.0f - c) + x * s,
      0.f,

      x * z * (1.0f - c) + y * s,
      y * z * (1.0f - c) - x * s,
      z * z * (1.0f - c) + c,
      0.f,

      0.f, 0.f, 0.f, 1.f
      );
}

inline Matrix44f TranslateMat44 (float x, float y, float z) {
  return Matrix44f (
      1.f, 0.f, 0.f, 0.f,
      0.f, 1.f, 0.f, 0.f,
      0.f, 0.f, 1.f, 0.f,
        x,   y,   z, 1.f
      );
}

inline Matrix44f ScaleMat44 (float x, float y, float z) {
  return Matrix44f (
        x, 0.f, 0.f, 0.f,
      0.f,   y, 0.f, 0.f,
      0.f, 0.f,   z, 0.f,
      0.f, 0.f, 0.f, 1.f
      );
}

inline Matrix44f Ortho(
    float left, float right,
    float bottom, float top,
    float near, float far) {
  float tx = -(right + left) / (right - left);
  float ty = -(top + bottom) / (top - bottom);
  float tz = -(far + near) / (far - near);
  return Matrix44f(
      2.0f / (right - left), 0.0f, 0.0f, 0.0f,
      0, 2.0f / (top - bottom), 0.0f, 0.0f,
      0.0f, 0.0f, -2.0f / (far - near), 0.0f,
      tx, ty, tz, 1.0f
      );
}

inline Matrix44f Perspective(float fovy, float aspect,
    float near, float far) {
  float x = (fovy * M_PI / 180.0) / 2.0f;
  float f = cos(x) / sin(x);

  return Matrix44f(
      f / aspect, 0.0f, 0.0f, 0.0f,
      0.0f, f, 0.0f, 0.0f,
      0.0f, 0.0f, (far + near) / (near - far), -1.0f,
      0.0f, 0.0f, (2.0f * far * near) / (near - far), 0.0f
      );
}

inline Matrix44f Frustum(float left, float right,
    float bottom, float top,
    float near, float far) {
  float A = (right + left) / (right - left);
  float B = (top + bottom) / (top - bottom);
  float C = -(far + near) / (far - near);
  float D = - (2.0f * far * near) / (far - near);

  return Matrix44f(
      2.0f * near / (right - left), 0.0f, 0.0f, 0.0f,
      0.0f, 2.0f * near / (top - bottom), 0.0f, 0.0f,
      A, B, C, -1.0f,
      0.0f, 0.0f, D, 0.0f
      );
}

inline Matrix44f LookAt(
    const Vector3f& eye,
    const Vector3f& poi,
    const Vector3f& up) {
  Vector3f d = (poi - eye).normalized();
  Vector3f s = d.cross(up.normalized()).normalized();
  Vector3f u = s.cross(d).normalized();

  return TranslateMat44(-eye[0], -eye[1], -eye[2]) * Matrix44f(
      s[0], u[0], -d[0], 0.0f,
      s[1], u[1], -d[1], 0.0f,
      s[2], u[2], -d[2], 0.0f,
      0.0f, 0.0f, 0.0f, 1.0f
      );
}

//
// Quaternion
//
// order: x,y,z,w

class Quaternion : public Vector4f {
  public:
    Quaternion () :
      Vector4f (0.f, 0.f, 0.f, 1.f)
    {}
    Quaternion (const Vector4f vec4) :
      Vector4f (vec4)
    {}
    Quaternion (float x, float y, float z, float w):
      Vector4f (x, y, z, w)
    {}
    // This function is equivalent to multiplicate their corresponding rotation matrices
    Quaternion operator* (const Quaternion &q) const {
      return Quaternion (
          (*this)[3] * q[0] + (*this)[0] * q[3] + (*this)[1] * q[2] - (*this)[2] * q[1],
          (*this)[3] * q[1] + (*this)[1] * q[3] + (*this)[2] * q[0] - (*this)[0] * q[2],
          (*this)[3] * q[2] + (*this)[2] * q[3] + (*this)[0] * q[1] - (*this)[1] * q[0],
          (*this)[3] * q[3] - (*this)[0] * q[0] - (*this)[1] * q[1] - (*this)[2] * q[2]
          );
    }
    Quaternion& operator*=(const Quaternion &q) {
      set (
          (*this)[3] * q[0] + (*this)[0] * q[3] + (*this)[1] * q[2] - (*this)[2] * q[1],
          (*this)[3] * q[1] + (*this)[1] * q[3] + (*this)[2] * q[0] - (*this)[0] * q[2],
          (*this)[3] * q[2] + (*this)[2] * q[3] + (*this)[0] * q[1] - (*this)[1] * q[0],
          (*this)[3] * q[3] - (*this)[0] * q[0] - (*this)[1] * q[1] - (*this)[2] * q[2]
          );
      return *this;
    }

    static Quaternion fromGLRotate (float angle, float x, float y, float z) {
      float st = sinf (angle * M_PI / 360.f);
      return Quaternion (
            st * x,
            st * y,
            st * z,
            cosf (angle * M_PI / 360.f)
            );
    }

    Quaternion normalize() {
      return Vector4f::normalize();
    }

    Quaternion slerp (float alpha, const Quaternion &quat) const {
      // check whether one of the two has 0 length
      float s = sqrt (squaredNorm() * quat.squaredNorm());

      // division by 0.f is unhealthy!
      assert (s != 0.f);

      float angle = acos (dot(quat) / s);
      if (angle == 0.f || std::isnan(angle)) {
        return *this;
      }
      assert(!std::isnan(angle));

      float d = 1.f / sinf (angle);
      float p0 = sinf ((1.f - alpha) * angle);
      float p1 = sinf (alpha * angle);

      if (dot (quat) < 0.f) {
        return Quaternion( ((*this) * p0 - quat * p1) * d);
      }
      return Quaternion( ((*this) * p0 + quat * p1) * d);
    }

    Matrix44f toGLMatrix() const {
      float x = (*this)[0];
      float y = (*this)[1];
      float z = (*this)[2];
      float w = (*this)[3];
      return Matrix44f (
          1 - 2*y*y - 2*z*z,
          2*x*y + 2*w*z,
          2*x*z - 2*w*y,
          0.f,

          2*x*y - 2*w*z,
          1 - 2*x*x - 2*z*z,
          2*y*z + 2*w*x,
          0.f,

          2*x*z + 2*w*y,
          2*y*z - 2*w*x,
          1 - 2*x*x - 2*y*y,
          0.f,

          0.f,
          0.f,
          0.f,
          1.f);
    }

    static Quaternion fromGLMatrix(const Matrix44f &mat) {
      float w = sqrt (1.f + mat(0,0) + mat(1,1) + mat(2,2)) * 0.5f;
      return Quaternion (
          -(mat(2,1) - mat(1,2)) / (w * 4.f),
          -(mat(0,2) - mat(2,0)) / (w * 4.f),
          -(mat(1,0) - mat(0,1)) / (w * 4.f),
          w);
    }

    static Quaternion fromMatrix (const Matrix33f &mat) {
      float w = sqrt (1.f + mat(0,0) + mat(1,1) + mat(2,2)) * 0.5f;
      return Quaternion (
          (mat(2,1) - mat(1,2)) / (w * 4.f),
          (mat(0,2) - mat(2,0)) / (w * 4.f),
          (mat(1,0) - mat(0,1)) / (w * 4.f),
          w);
    }

    static Quaternion fromAxisAngle (const Vector3f &axis, double angle_rad) {
      double d = axis.norm();
      double s2 = std::sin (angle_rad * 0.5) / d;
      return Quaternion (
          axis[0] * s2,
          axis[1] * s2,
          axis[2] * s2,
          std::cos(angle_rad * 0.5)
          );
    }

    static Quaternion fromEulerZYX (const Vector3f &zyx_angles) {
      return Quaternion::fromAxisAngle (Vector3f (0., 0., 1.), zyx_angles[0])
        * Quaternion::fromAxisAngle (Vector3f (0., 1., 0.), zyx_angles[1])
        * Quaternion::fromAxisAngle (Vector3f (1., 0., 0.), zyx_angles[2]);
    }

    static Quaternion fromEulerYXZ (const Vector3f &yxz_angles) {
      return Quaternion::fromAxisAngle (Vector3f (0., 1., 0.), yxz_angles[0])
        * Quaternion::fromAxisAngle (Vector3f (1., 0., 0.), yxz_angles[1])
        * Quaternion::fromAxisAngle (Vector3f (0., 0., 1.), yxz_angles[2]);
    }

    static Quaternion fromEulerXYZ (const Vector3f &xyz_angles) {
      return Quaternion::fromAxisAngle (Vector3f (0., 0., 01.), xyz_angles[2])
        * Quaternion::fromAxisAngle (Vector3f (0., 1., 0.), xyz_angles[1])
        * Quaternion::fromAxisAngle (Vector3f (1., 0., 0.), xyz_angles[0]);
    }

    Vector3f toEulerZYX () const {
      return Vector3f (1.0f, 2.0f, 3.0f
          );
    }

    Vector3f toEulerYXZ() const {
      return Vector3f (
          atan2 (-2.f * (*this)[0] * (*this)[2] + 2.f * (*this)[3] * (*this)[1],
            (*this)[2] * (*this)[2] - (*this)[1] * (*this)[1]
            -(*this)[0] * (*this)[0] + (*this)[3] * (*this)[3]),
          asin (2.f * (*this)[1] * (*this)[2] + 2.f * (*this)[3] * (*this)[0]),
          atan2 (-2.f * (*this)[0] * (*this)[1] + 2.f * (*this)[3] * (*this)[2],
            (*this)[1] * (*this)[1] - (*this)[2] * (*this)[2]
            +(*this)[3] * (*this)[3] - (*this)[0] * (*this)[0]
            )
          );
    };

    Matrix33f toMatrix() const {
      float x = (*this)[0];
      float y = (*this)[1];
      float z = (*this)[2];
      float w = (*this)[3];
      return Matrix33f (
          1 - 2*y*y - 2*z*z,
          2*x*y - 2*w*z,
          2*x*z + 2*w*y,

          2*x*y + 2*w*z,
          1 - 2*x*x - 2*z*z,
          2*y*z - 2*w*x,

          2*x*z - 2*w*y,
          2*y*z + 2*w*x,
          1 - 2*x*x - 2*y*y
      );
    }

    Quaternion conjugate() const {
      return Quaternion (
          -(*this)[0],
          -(*this)[1],
          -(*this)[2],
          (*this)[3]);
    }

    Vector3f rotate (const Vector3f &vec) const {
      Vector3f vn (vec);
      Quaternion vec_quat (vn[0], vn[1], vn[2], 0.f), res_quat;

      res_quat = (*this) * vec_quat;
      res_quat = res_quat * conjugate();

      return Vector3f (res_quat[0], res_quat[1], res_quat[2]);
    }
};

} /* namespace GL */


//
// Stream operators
//
template <typename Derived, typename ScalarType, int Rows, int Cols>
inline std::ostream& operator<<(std::ostream& output, const MatrixBase<Derived, ScalarType, Rows, Cols> &matrix) {
  size_t max_width = 0;
  size_t out_width = output.width();

  // get the widest number
  for (size_t i = 0; i < matrix.rows(); i++) {
    for (size_t j = 0; j < matrix.cols(); j++) {
      std::stringstream out_stream;
      out_stream << matrix(i,j);
      max_width = std::max (out_stream.str().size(),max_width);
    }
  }

  // overwrite width if it was explicitly prescribed
  if (out_width != 0) {
    max_width = out_width;
  }

  for (unsigned int i = 0; i < matrix.rows(); i++) {
    output.width(0);
    output.width(out_width);
    for (unsigned int j = 0; j < matrix.cols(); j++) {
      std::stringstream out_stream;
      out_stream.width (max_width);
      out_stream << matrix(i,j);
      output << out_stream.str();

      if (j < matrix.cols() - 1)
        output << " ";
    }

    if (matrix.rows() > 1 && i < matrix.rows() - 1)
      output << std::endl;
  }
  return output;
}

}