Polymath/include/internal/Matrix.h

/*
 * File:   Matrix.h
 * Author: Samer Afach
 *
 * Created on 01. Oktober 2016, 17:12
 */

#include <vector>
#include <iostream>
#include <cmath>
#include <exception>
#include <stdexcept>
#include <functional>
#include <complex>
#include <memory>
#include <algorithm>
#include <chrono>
#include <iomanip>

#include "LapackAdapters.h"
#include "StdAdapters.h"


#ifndef MATRIX_H
#define	MATRIX_H

#define ColMaj 0
#define RowMaj 1


/**
  This class is an implementation of a matrix in mathematics. It can be used as a vector implementation as well.
  */

namespace Poly
{
extern std::string ComplexUnit;

template <typename T, int M = ColMaj>
class Matrix;
}


namespace std
{
template <typename T>
std::string to_string(std::complex<T> value)
{
    std::stringstream sstr;

    sstr << value.real();
    sstr << "+";
    sstr << value.imag();
    sstr << Poly::ComplexUnit;
    return sstr.str();
}

template <typename T, int M>
Poly::Matrix<T,M> real(const Poly::Matrix<std::complex<T>,M>& mat)
{
    Poly::Matrix<T,M> result(mat.rows(),mat.columns());
    std::transform(mat.begin(),mat.end(),result.begin(),[](const std::complex<T>& elem) -> T {return elem.real();});
}
template <typename T, int M>
Poly::Matrix<T,M> imag(const Poly::Matrix<std::complex<T>,M>& mat)
{
    Poly::Matrix<T,M> result(mat.rows(),mat.columns());
    std::transform(mat.begin(),mat.end(),result.begin(),[](const std::complex<T>& elem) -> T {return elem.imag();});
}

template <typename T, int M>
void swap(Poly::Matrix<T,M>&__a, Poly::Matrix<T,M>&__b)
{
    std::swap(__a._rows,__b._rows);
    std::swap(__a._columns,__b._columns);
    std::swap(__a._matEls,__b._matEls);

}
}

namespace Poly
{

template <typename T>
struct ExtractType;

template <template <typename U> class T, typename U>
struct ExtractType<T<U>>
 { using SubType = U; };

template <typename T>
typename std::enable_if<!std::is_class<T>::value,
                        std::string >::type
to_string(const T& a_value, int precision)
{
    std::ostringstream out;
    out << std::setprecision(precision) << a_value;
    return out.str();
}

template <typename T, typename U = typename ExtractType<T>::SubType>
typename std::enable_if<std::is_same<T, std::complex<U>>::value &&
                        std::is_class<T>::value,
                        std::string >::type
to_string(const T& a_value, int precision)
{
    std::ostringstream out;
    out << std::setprecision(precision) << a_value.real();
    out << "+";
    out << std::setprecision(precision) << a_value.imag();
    out << Poly::ComplexUnit;
    return out.str();
}


typedef int MATRIX_TYPE;
const static MATRIX_TYPE MATRIX_GENERAL       = 0;
const static MATRIX_TYPE MATRIX_HERMITIAN     = 1;
const static MATRIX_TYPE MATRIX_ANTIHERMITIAN = 2;
const static MATRIX_TYPE MATRIX_SYMMETRIC     = 3;
const static MATRIX_TYPE MATRIX_ANTISYMMETRIC = 4;

typedef int VECTORIZATION_MODE;
const static VECTORIZATION_MODE VECMODE_FULL = 0;
const static VECTORIZATION_MODE VECMODE_UPPER_TRIANGULAR_WITH_DIAGONAL = 1;
const static VECTORIZATION_MODE VECMODE_UPPER_TRIANGULAR_NO_DIAGONAL = 2;
const static VECTORIZATION_MODE VECMODE_LOWER_TRIANGULAR_WITH_DIAGONAL = 3;
const static VECTORIZATION_MODE VECMODE_LOWER_TRIANGULAR_NO_DIAGONAL = 4;

typedef int EIGENS_METHOD;
const static EIGENS_METHOD METHOD_DEFAULT = 0;
const static EIGENS_METHOD METHOD_DIVIDE_CONQUER = 1;


template <typename T, int M>
void MultiplyMatrices(const Matrix<T,M> &matrixLHS, const Matrix<T,M> &matrixRHS, Matrix<T,M> &result);
template <typename T, int M>
void MultiplyMatrices(const Matrix<T,M>& lhs, const Matrix<T,M>& rhs, Matrix<T,M> &to_add_then_result, T alpha, T beta);
template <typename T, int M>
Matrix<T,M> IdentityMatrix(const typename Matrix<T,M>::size_type& size);
template <typename T, typename U, int M>
void _check_equal_matrices_sizes(const Matrix<T,M>&, const Matrix<U,M>&);

template <typename T, int M>
class Matrix
{
public:
    typedef T value_type;
    typedef T& reference;
    typedef const T& const_reference;
    typedef long size_type;
    typedef size_type difference_type;
    typedef typename std::vector<T>::iterator iterator;
    typedef typename std::vector<T>::const_iterator const_iterator;

private:
    std::vector<T> _matEls;
    size_type _rows;
    size_type _columns;
    inline size_type RowColumnToElement(const size_type &row, const size_type &column, const size_type &TotalRows, const size_type &TotalColumns);
    inline size_type RowColumnToElement(const size_type &row, const size_type &column, const size_type &TotalRows, const size_type &TotalColumns) const;
    inline size_type SizeToReserve(const size_type &rows, const size_type &columns);
    inline size_type SizeToReserve(const size_type &rows, const size_type &columns) const;
    void invert_manual();


public:
    void _check_square_matrix() const;

    Matrix<T, M>();
    Matrix<T, M>(const size_type& rows, const size_type& columns, const T& initialValue = T());
    Matrix<T, M>(const size_type& rows, const size_type& columns, const std::initializer_list<T>& init_list);
    Matrix<T, M>(const Matrix<T, M>& src) = default;
    Matrix<T, M>(Matrix<T, M>&&) = default;
    ~Matrix() = default;

    //copy constructor from real to complex
    template <typename U, typename = typename std::enable_if<std::is_same<T, std::complex<U>>::value>::type>
        Matrix(const Matrix<U, M> & src);


    Matrix<T, M> &operator=(const Matrix<T,M>& rhs) = default;

    //operator= from real to complex
    template <typename U>
    typename std::enable_if<std::is_same<T, std::complex<U>>::value, Matrix<T,M> &>::type
        operator=(const Matrix<U, M> & rhs);

    void copyFrom(const Matrix<T, M> & rhs);

    template <typename U>
    typename std::enable_if<std::is_same<T, std::complex<U>>::value, void>::type
        copyFrom(const Matrix<U, M> & rhs);

    void initialize(const std::initializer_list<T>& list, Matrix<T,M>::size_type start_row = 0, Matrix<T,M>::size_type start_column = 0);
    bool operator==(const Matrix<T,M>& rhs);
    bool operator!=(const Matrix<T,M>& rhs);
    template <typename _T, int _M>
        friend Matrix<_T,_M> operator*(const Matrix<_T,_M>& lhs, const Matrix<_T,_M>& rhs);
    template <typename _T, int _M>
        friend Matrix<_T,_M> operator+(const Matrix<_T,_M>& lhs, const Matrix<_T,_M>& rhs);
    template <typename _T, int _M>
        friend Matrix<_T,_M> operator-(const Matrix<_T,_M>& lhs, const Matrix<_T,_M>& rhs);

    template <typename _T, int _M>
        friend Matrix<_T,_M> operator*(const _T& lhs, const Matrix<_T,_M>& rhs);
    template <typename _T, int _M>
        friend Matrix<_T,_M> operator*(const Matrix<_T,_M>& lhs, const _T& rhs);
    template <typename _T, int _M>
        friend Matrix<_T,_M> operator/(const Matrix<_T,_M>& lhs, const _T& rhs);
    template <typename _T, int _M>
        friend void std::swap(Poly::Matrix<_T,_M>&__a, Poly::Matrix<_T,_M>&__b);


   template <typename _T, int _M>
       friend Matrix<std::complex<_T>,_M> operator*(const Matrix<std::complex<_T>,_M>& lhs, const _T& rhs);
   template <typename _T, int _M>
       friend Matrix<std::complex<_T>,_M> operator*(const _T& lhs, const Matrix<std::complex<_T>,_M>& rhs);

   template <typename _T, int _M>
       friend Matrix<std::complex<_T>,_M> operator*(const std::complex<_T>& lhs, const Matrix<_T,_M>& rhs);
   template <typename _T, int _M>
       friend Matrix<std::complex<_T>,_M> operator*(const Matrix<_T,_M>& lhs, const std::complex<_T>& rhs);
   template <typename _T, int _M>
       friend Matrix<std::complex<_T>,_M> operator+(const Matrix<std::complex<_T>,_M>& lhs, const Matrix<_T,_M>& rhs);
   template <typename _T, int _M>
       friend Matrix<std::complex<_T>,_M> operator+(const Matrix<_T,_M>& lhs, const Matrix<std::complex<_T>,_M>& rhs);
   template <typename _T, int _M>
       friend Matrix<std::complex<_T>,_M> operator-(const Matrix<std::complex<_T>,_M>& lhs, const Matrix<_T,_M>& rhs);
   template <typename _T, int _M>
       friend Matrix<std::complex<_T>,_M> operator-(const Matrix<_T,_M>& lhs, const Matrix<std::complex<_T>,_M>& rhs);

    inline T& operator[](const size_type& index);
    inline const T& operator[](const size_type& index) const;
    inline T& operator()(const size_type& index);
    inline const T& operator()(const size_type& index) const;

    inline iterator begin() noexcept;
    inline const_iterator begin() const noexcept;
    inline iterator end() noexcept;
    inline const_iterator end() const noexcept;

    Matrix<T,M>& operator+=(const Matrix<T,M>& rhs);
    Matrix<T,M>& operator-=(const Matrix<T,M>& rhs);
    Matrix<T,M>& operator*=(const Matrix<T,M>& rhs);
    Matrix<T,M>& operator*=(const T& rhs);
    Matrix<T,M>& operator/=(const T& rhs);
    void elementWiseProduct_inplace(const Matrix<T,M>& rhs);
    Matrix<T,M> elementWiseProduct(const Matrix<T,M>& rhs);
    T trace();
    template <typename _T, int _M>
    friend _T Trace(const Matrix<_T,_M> &rhs);
    void zeros();
    void ones();
    void inverse_inplace();
    Matrix<T,M> inverse();
    Matrix<T,M> getExp();
    void conjugate_inplace();
    void conjugateTranspose_inplace();
    Matrix<T,M> vectorize(const VECTORIZATION_MODE& mode = VECMODE_FULL) const;
    Matrix<T,M> diagonal() const;
    const T& at(size_type row, size_type column) const;
    T& at(size_type row, size_type column);
    T& operator()(size_type row, size_type column);
    const T& operator()(size_type row, size_type column) const;

    T& front();
    const T& front() const;
    void resize(size_type rows, size_type columns, const T &initialValue = T());
    const size_type& rows();
    const size_type& columns();
    typename Matrix<T,M>::size_type size();
    const size_type& rows() const;
    const size_type& columns() const;
    typename Matrix<T,M>::size_type size() const;
    void clear();
    void transpose_inplace();
    T determinant();
    Matrix<T,M> SVD();
    std::string asString(int precision = 7, char open = '[', char close = ']', char sep = ',');
    template <typename _T, int _M>
        friend std::ostream& operator<<(std::ostream &os, const Matrix<_T,_M> &src);
    void print(std::ostream &os = std::cout, std::string header = "") const;
    void raw_print(std::ostream &os = std::cout, std::string header = "") const;
    //converts elements to type _targetType and puts them in a new Matrix
    template <typename _targetType>
        Matrix<_targetType,M> convertElements();
    template <typename _targetType>
        Matrix<_targetType,M> convertElements(const std::function<_targetType(T)> &conversion_rule);

    //puts all the elements in a new container
    template <typename Container>
        Container convertToContainer();
};

template <typename T, int M>
template <typename _targetType>
Matrix<_targetType, M> Matrix<T,M>::convertElements()
{
    Matrix<_targetType, M> ret(this->rows(),this->columns());
    std::copy(this->begin(), this->end(), ret.begin());
    return ret;
}

template <typename T, int M>
template <typename _targetType>
Matrix<_targetType, M> Matrix<T,M>::convertElements(const std::function<_targetType(T)> &conversion_rule)
{
    Matrix<_targetType, M> ret(this->rows(),this->columns());
    std::transform(this->begin(), this->end(), ret.begin(),conversion_rule);
    return ret;
}

template <typename T, int M>
template <typename Container>
Container Matrix<T,M>::convertToContainer()
{
    Container ret;
    ret.resize(this->rows()*this->columns());
    std::copy(this->begin(), this->end(), ret.begin());
    return ret;
}

template <typename T, int M>
Matrix<T,M>::Matrix()
{
    _rows = 0;
    _columns = 0;
}

template <typename T, int M>
Matrix<T,M>::Matrix(const size_type &rows, const size_type &columns, const T& initialValue)
{
    _rows = 0;
    _columns = 0;
    this->resize(rows,columns,initialValue);
}

template <typename T, int M>
Matrix<T,M>::Matrix(const size_type& rows, const size_type& columns, const std::initializer_list<T>& init_list)
{
    _rows = 0;
    _columns = 0;
    this->resize(rows,columns);
    this->initialize(init_list);
}

template <typename T, int M>
template <typename U, typename>
Matrix<T,M>::Matrix(const Matrix<U, M> & src)
{
    _rows = 0;
    _columns = 0;
    this->copyFrom(src);
}

template <typename T, int M>
inline typename Matrix<T,M>::size_type Matrix<T,M>::RowColumnToElement(const size_type &row, const size_type &column, const size_type &TotalRows, const size_type &TotalColumns)
{
    if (M == RowMaj)
        return row*TotalColumns+column;
    else
        return column*TotalRows+row;
}

template <typename T, int M>
inline typename Matrix<T,M>::size_type Matrix<T,M>::RowColumnToElement(const size_type &row, const size_type &column, const size_type &TotalRows, const size_type &TotalColumns) const
{
    if (M == RowMaj)
        return row*TotalColumns+column;
    else
        return column*TotalRows+row;
}

template <typename T, int M>
inline typename Matrix<T,M>::size_type Matrix<T,M>::SizeToReserve(const size_type &rows, const size_type &columns)
{
    return static_cast<size_type>(rows*columns);
}

template <typename T, int M>
inline typename Matrix<T,M>::size_type Matrix<T,M>::SizeToReserve(const size_type &rows, const size_type &columns) const
{
    return static_cast<size_type>(rows*columns);
}

template <typename T, int M>
void Matrix<T,M>::resize(size_type rows, size_type columns, const T& initialValue)
{
    size_type oldRows = this->rows();
    size_type oldColumns = this->columns();
    size_type newSize = static_cast<size_type>(SizeToReserve(rows,columns));
    Matrix<T,M> oldMat = *this;
    this->_matEls.resize(newSize);
    this->_rows = rows;
    this->_columns = columns;
    for(size_type i = 0; i < oldRows; i++)
    {
        for(size_type j = 0; j < oldColumns; j++)
        {
            this->at(i,j) = oldMat.at(i,j);
        }
    }
    //assign columns with rows higher than the previous row-size
    for(size_type i = oldRows; i < this->rows(); i++)
    {
        for(size_type j = 0; j < this->columns(); j++)
        {
            this->at(i,j) = initialValue;
        }
    }
    //assign rows with columns higher than the previous column-size
    for(size_type i = 0; i < this->rows(); i++)
    {
        for(size_type j = oldColumns; j < this->columns(); j++)
        {
            this->at(i,j) = initialValue;
        }
    }
}

template <typename T, int M>
const typename Matrix<T,M>::size_type& Matrix<T,M>::columns()
{
    return _columns;
}

template <typename T, int M>
const typename Matrix<T,M>::size_type& Matrix<T,M>::columns() const
{
    return _columns;
}

template <typename T, int M>
const typename Matrix<T,M>::size_type& Matrix<T,M>::rows()
{
    return _rows;
}

template <typename T, int M>
const typename Matrix<T,M>::size_type& Matrix<T,M>::rows() const
{
    return _rows;
}

template <typename T, int M>
typename Matrix<T,M>::size_type Matrix<T,M>::size()
{
    return this->_matEls.size();
}

template <typename T, int M>
typename Matrix<T,M>::size_type Matrix<T,M>::size() const
{
    return this->_matEls.size();
}

template <typename T, int M>
void Matrix<T,M>::clear()
{
    _matEls.clear();
    _columns = 0;
    _rows = 0;
}

template <typename T, int M>
bool Matrix<T,M>::operator==(const Matrix<T,M>& rhs)
{
    if(this->_rows != rhs._rows || this->_columns != rhs._columns)
    {
        return 0;
    }
    else
    {
        return (_matEls == rhs._matEls);
    }
}

template <typename T, int M>
bool Matrix<T,M>::operator!=(const Matrix<T,M>& rhs)
{
    return !(*this == rhs);
}

template <typename T, int M>
void Matrix<T,M>::transpose_inplace()
{
    if(this->rows() == 1 || this->columns() == 1)
    {
        std::swap(this->_rows,this->_columns);
    }
    else
    {
        for(typename Poly::Matrix<T,M>::size_type i = 0; i < this->rows(); i++)
        {
            for(typename Poly::Matrix<T,M>::size_type j = i+1; j < this->columns(); j++)
            {
                std::swap(this->at(j,i),this->at(i,j));
            }
        }
    }
}

template <typename T, int M>
void Matrix<T,M>::conjugateTranspose_inplace()
{
    if(this->rows() == 1 || this->columns() == 1)
    {
        std::swap(this->_rows,this->_columns);
    }
    else
    {
        for(typename Poly::Matrix<T,M>::size_type i = 0; i < this->rows(); i++)
        {
            for(typename Poly::Matrix<T,M>::size_type j = i+1; j < this->columns(); j++)
            {
                std::swap(this->at(j,i),this->at(i,j));
            }
        }
    }
    this->conjugate_inplace();
}

template <typename T, int M>
std::ostream& operator<<(std::ostream &os, const Matrix<T,M> &src)
{
    for (typename Matrix<T,M>::size_type i = 0; i < src._rows; i++)
    {
        for (typename Matrix<T,M>::size_type j = 0; j < src._columns; j++)
        {
            os << src.at(i,j) << "\t";
        }
        os << std::endl;
    }
    return os;
}

template <typename T, int M>
Matrix<std::complex<T>,M> operator*(const Matrix<std::complex<T>,M>& lhs, const T& rhs)
{
    Matrix<std::complex<T>,M> result = lhs;
    for(typename Matrix<T,M>::size_type i = 0; i < static_cast<typename Matrix<T,M>::size_type>(lhs._matEls.size()); i++)
    {
        result._matEls[i] *= rhs;
    }
    return result;
}

template <typename T, int M>
Matrix<std::complex<T>,M> operator*(const T& lhs, const Matrix<std::complex<T>,M>& rhs)
{
    Matrix<std::complex<T>,M> result = rhs;
    for(typename Matrix<T,M>::size_type i = 0; i < static_cast<typename Matrix<T,M>::size_type>(rhs._matEls.size()); i++)
    {
        result._matEls[i] *= lhs;
    }
    return result;
}


template <typename T, int M>
Matrix<T,M> operator*(const Matrix<T,M>& lhs, const Matrix<T,M>& rhs)
{
    Matrix<T,M> result;
    Poly::MultiplyMatrices(lhs,rhs,result);
    return result;
}

template <typename T, int M>
Matrix<T,M> operator*(const T& lhs, const Matrix<T,M>& rhs)
{
    Matrix<T,M> result = rhs;
    for(typename Matrix<T,M>::size_type i = 0; i < static_cast<typename Matrix<T,M>::size_type>(rhs._matEls.size()); i++)
    {
        result._matEls[i] *= lhs;
    }
    return result;
}

template <typename T, int M>
Matrix<T,M> operator*(const Matrix<T,M>& lhs, const T& rhs)
{
    Matrix<T,M> result = lhs;
    for(typename Matrix<T,M>::size_type i = 0; i < static_cast<typename Matrix<T,M>::size_type>(lhs._matEls.size()); i++)
    {
        result._matEls[i] *= rhs;
    }
    return result;
}
template <typename T, int M>
Matrix<T,M> operator/(const Matrix<T,M>& lhs, const T& rhs)
{
    Matrix<T,M> result = lhs;
    for(typename Matrix<T,M>::size_type i = 0; i < static_cast<typename Matrix<T,M>::size_type>(lhs._matEls.size()); i++)
    {
        result._matEls[i] /= rhs;
    }
    return result;
}

template <typename T, int M>
Matrix<std::complex<T>,M> operator*(const std::complex<T>& lhs, const Matrix<T,M>& rhs)
{
    Matrix<std::complex<T>,M> result = rhs;
    for(typename Matrix<T,M>::size_type i = 0; i < static_cast<typename Matrix<T,M>::size_type>(rhs._matEls.size()); i++)
    {
        result._matEls[i] *= lhs;
    }
    return result;
}

template <typename T, int M>
Matrix<std::complex<T>,M> operator*(const Matrix<T,M>& lhs, const std::complex<T>& rhs)
{
    Matrix<std::complex<T>,M> result = lhs;
    for(typename Matrix<T,M>::size_type i = 0; i < static_cast<typename Matrix<T,M>::size_type>(lhs._matEls.size()); i++)
    {
        result._matEls[i] *= rhs;
    }
    return result;
}

template <typename T, int M>
Matrix<T,M> operator+(const Matrix<T,M>& lhs, const Matrix<T,M>& rhs)
{
    _check_equal_matrices_sizes(lhs,rhs);
    Matrix<T,M> result(rhs.rows(),rhs.columns());
    std::transform( lhs.begin(), lhs.end(),
                    rhs.begin(), result.begin(),
                    std::plus<T>());
    return result;
}

template <typename T, int M>
Matrix<T,M> operator-(const Matrix<T,M>& lhs, const Matrix<T,M>& rhs)
{
    Matrix<T,M> result;
    _check_equal_matrices_sizes(lhs,rhs);
    result.resize(rhs.rows(),rhs.columns());
    std::transform( lhs.begin(), lhs.end(),
                    rhs.begin(), result.begin(),
                    std::minus<T>());
    return result;
}

template <typename T, int M>
Matrix<std::complex<T>,M> operator+(const Matrix<std::complex<T>,M>& lhs, const Matrix<T,M>& rhs)
{
    _check_equal_matrices_sizes(lhs,rhs);
    Matrix<std::complex<T>,M> result(rhs.rows(),rhs.columns());
    std::transform( lhs.begin(), lhs.end(),
                    rhs.begin(), result.begin(),
                    std::plus<std::complex<T>>());
    return result;
}

template <typename T, int M>
Matrix<std::complex<T>,M> operator+(const Matrix<T,M>& lhs, const Matrix<std::complex<T>,M>& rhs)
{
    _check_equal_matrices_sizes(lhs,rhs);
    Matrix<std::complex<T>,M> result(rhs.rows(),rhs.columns());
    std::transform( lhs.begin(), lhs.end(),
                    rhs.begin(), result.begin(),
                    std::plus<std::complex<T>>());
    return result;
}
template <typename T, int M>
Matrix<std::complex<T>,M> operator-(const Matrix<std::complex<T>,M>& lhs, const Matrix<T,M>& rhs)
{
    _check_equal_matrices_sizes(lhs,rhs);
    Matrix<std::complex<T>,M> result(rhs.rows(),rhs.columns());
    std::transform( lhs.begin(), lhs.end(),
                    rhs.begin(), result.begin(),
                    std::minus<std::complex<T>>());
    return result;
}

template <typename T, int M>
Matrix<std::complex<T>,M> operator-(const Matrix<T,M>& lhs, const Matrix<std::complex<T>,M>& rhs)
{
    _check_equal_matrices_sizes(lhs,rhs);
    Matrix<std::complex<T>,M> result(rhs.rows(),rhs.columns());
    std::transform( lhs.begin(), lhs.end(),
                    rhs.begin(), result.begin(),
                    std::minus<std::complex<T>>());
    return result;
}

template <typename T, int M>
Matrix<T,M>& Matrix<T,M>::operator+=(const Matrix<T,M>& rhs)
{
    (*this) = (*this) + rhs;
    return *this;
}

template <typename T, int M>
Matrix<T,M>& Matrix<T,M>::operator-=(const Matrix<T,M>& rhs)
{
    (*this) = (*this) - rhs;
    return *this;
}

template <typename T, int M>
Matrix<T,M>& Matrix<T,M>::operator*=(const Matrix<T,M>& rhs)
{
    (*this) = (*this) * rhs;
    return *this;
}

template <typename T, int M>
Matrix<T,M>& Matrix<T,M>::operator*=(const T& rhs)
{
    (*this) = (*this) * rhs;
    return *this;
}

template <typename T, int M>
Matrix<T,M>& Matrix<T,M>::operator/=(const T& rhs)
{
    (*this) = (*this) / rhs;
    return *this;
}

template <typename T, int M>
void Matrix<T,M>::elementWiseProduct_inplace(const Matrix<T,M> &rhs)
{
    _check_equal_matrices_sizes(*this,rhs);
    std::transform( this->begin(), this->end(),
                    rhs.begin(), this->begin(),
                    std::multiplies<T>());
}

template <typename T, int M>
Matrix<T,M> Matrix<T,M>::elementWiseProduct(const Matrix<T,M> &rhs)
{
    Matrix<T,M> result(this->rows(),this->columns());
    std::transform( this->begin(), this->end(),
                    rhs.begin(), result.begin(),
                    std::multiplies<T>());
    return result;
}

template <typename T, int M>
T Matrix<T,M>::trace()
{
    _check_square_matrix();
    T result = T(0);
    for(size_type i = 0; i < this->rows(); i++)
    {
        result += this->at(i,i);
    }
    return result;
}

template <typename T, int M>
T Trace(const Matrix<T,M> &rhs)
{
    rhs._check_square_matrix();
    T result = T(0);
    for(typename Matrix<T,M>::size_type i = 0; i < rhs.rows(); i++)
    {
        result += rhs.at(i,i);
    }
    return result;
}

template <typename T, int M>
void Matrix<T,M>::zeros()
{
    std::fill(this->begin(), this->end(), T(0));
}

template <typename T, int M>
void Matrix<T,M>::ones()
{
    std::fill(this->begin(), this->end(), T(1));
}

template <typename T, int M>
void Matrix<T,M>::inverse_inplace()
{
    this->_check_square_matrix();
    if(std::is_same<float,T>::value)
    {
        Xgetri(float,s, tmp_workspace_size)
    }
    else if(std::is_same<double,T>::value)
    {
        Xgetri(double,d, tmp_workspace_size)
    }
    else if(std::is_same<lapack_complex_float,T>::value)
    {
        Xgetri(lapack_complex_float,c, tmp_workspace_size.real())
    }
    else if(std::is_same<lapack_complex_double,T>::value)
    {
        Xgetri(lapack_complex_double,z, tmp_workspace_size.real())
    }
    else
    {
        this->invert_manual();
    }
}

template <typename T, int M>
void Matrix<T,M>::invert_manual()
{
    T val;
    if(this->rows() != this->columns())
    {
        throw std::length_error("Cannot invert a non-square matrix.");
    }
    size_type sideLength = this->rows();
    Matrix<T,M> sideMat = Poly::IdentityMatrix<T,M>(this->rows());
//    std::cout<<(*this)<<std::endl;
    for (int i = 0; i < sideLength; i++)
    {
        if((*this).at(i,i) != static_cast<T>(0))
        {
//            std::cout<<(*this)<<std::endl;
            val = static_cast<T>(1)/((*this).at(i,i));
            MultiplyRowWith(*this,val,i);
            MultiplyRowWith(sideMat,val,i);
            for (int j = i + 1; j < sideLength; j++)
            {
                val = -((*this).at(j,i));
                MultiplyAndAddToRow(*this,val,i,j);
                MultiplyAndAddToRow(sideMat,val,i,j);
            }
        }
        else
        {
            for (int j = i + 1; j < sideLength; j++)
            {
                if((*this).at(j,i) != static_cast<T>(0))
                {
                    SwapRows(*this,j,i);
                    SwapRows(sideMat,j,i);
                    break;
                }
            }
            if((*this).at(i,i) == static_cast<T>(0))
            {
                throw std::runtime_error("Matrix is non invertible");
            }
        }
    }
    for (int i = sideLength - 1; i >= 0; i--)
    {
        if((*this).at(i,i) != static_cast<T>(0))
        {
//            cout<<(*this)<<endl;
            val = static_cast<T>(1)/((*this).at(i,i));
            MultiplyRowWith(*this,val,i);
            MultiplyRowWith(sideMat,val,i);
            for (int j = i - 1; j >= 0; j--)
            {
                val = -((*this).at(j,i));
                MultiplyAndAddToRow(*this,val,i,j);
                MultiplyAndAddToRow(sideMat,val,i,j);
            }
        }
    }
    (*this) = sideMat;
}

template <typename T, int M>
void Matrix<T,M>::_check_square_matrix() const
{
#ifdef POLYMATH_DEBUG
    if(this->columns() != this->rows())
    {
        throw std::length_error("This operation requires square matrices.");
    }
#endif
}

template <typename T, typename U, int M>
void _check_equal_matrices_sizes(const Matrix<T,M>&
#ifdef POLYMATH_DEBUG
                                 mat1
#endif
                                 ,
                                 const Matrix<U,M>&
#ifdef POLYMATH_DEBUG
                                 mat2
#endif
                                 )
{
#ifdef POLYMATH_DEBUG
    if((mat1.columns() != mat2.columns()) || (mat1.rows() != mat2.rows()))
    {
        throw std::length_error("Matrices sizes should be equal");
    }
#endif
}


template <typename T, int M>
void Matrix<T,M>::copyFrom(const Matrix<T, M> &rhs)
{
    this->_rows    = rhs._rows;
    this->_columns = rhs._columns;
    this->_matEls  = rhs._matEls;
}

template <typename T, int M>
template <typename U>
typename std::enable_if<std::is_same<T, std::complex<U>>::value, void>::type
    Matrix<T,M>::copyFrom(const Matrix<U, M> & rhs)
{
    this->resize(rhs.rows(),rhs.columns());
    for(typename Matrix<U,M>::size_type i = 0; i < static_cast<typename Matrix<U,M>::size_type>(_matEls.size()); i++)
    {
        (*this)[i] = rhs[i];
    }
}

template <typename T, int M>
template <typename U>
typename std::enable_if<std::is_same<T, std::complex<U>>::value, Matrix<T, M> &>::type
Matrix<T, M>::operator=(const Matrix<U, M> & rhs)
{
    this->copyFrom(rhs);
    return *this;
}

template <typename T, int M>
void Matrix<T,M>::initialize(const std::initializer_list<T> &list, Matrix<T,M>::size_type start_row, Matrix<T,M>::size_type start_column)
{
    size_type start_from = RowColumnToElement(start_row,start_column,this->rows(),this->columns());
    if(list.size() + start_from > this->_matEls.size()) //if tried to initalize with an array bigger than
    {
        throw std::out_of_range("initialize failed: Out of range.");
    }
    std::copy(list.begin(),list.end(),_matEls.begin()+start_from);
}

template <typename T, int M>
Matrix<T,M> Matrix<T,M>::inverse()
{
    Matrix<T,M> tmpMat = *this;
    tmpMat.inverse_inplace();
    return tmpMat;
}

template <typename T, int M>
Matrix<T,M> Matrix<T,M>::getExp()
{
}

template <typename T, int M>
void Matrix<T,M>::conjugate_inplace()
{
    transform(_matEls.begin(),_matEls.end(), _matEls.begin(), [](std::complex<typename T::value_type>& c) -> std::complex<typename T::value_type> { return std::conj(c); });
}


template <typename T, int M>
Matrix<T, M> Matrix<T,M>::diagonal() const
{
    _check_square_matrix();
    Matrix<T, M> output(this->columns(),1);
    for(long i = 0; i < this->rows(); i++)
    {
        output._matEls[i] = this->at(i,i);
    }
    return output;
}

template <typename T, int M>
Matrix<T, M> Matrix<T, M>::vectorize(const VECTORIZATION_MODE &mode) const
{
    Matrix<T, M> output;
    if(M == ColMaj)
    {
        if(mode == Poly::VECMODE_FULL)
        {
            Matrix<T, M> output(this->rows()*this->columns(),1);
            std::copy(this->begin(),this->end(),output.begin());
            return output;
        }
        else if(mode == Poly::VECMODE_UPPER_TRIANGULAR_NO_DIAGONAL)
        {
            _check_square_matrix();
            output.resize((this->columns()*(this->columns()-1))/2,1);
            typename Matrix<T,M>::const_iterator it = _matEls.begin() + this->columns(); //iterator at rows to skip in every step, starts at second column
            long size_to_copy = 0;
            for(int i = 1; i < this->columns(); i++) //Starts with 1 to skip the diagonal
            {
                std::copy(it, it + i, output.begin() + size_to_copy);
                size_to_copy += i;
                it += this->columns();
            }
        }
        else if(mode == Poly::VECMODE_UPPER_TRIANGULAR_WITH_DIAGONAL)
        {
            _check_square_matrix();
            output.resize((this->columns()*(this->columns()+1))/2,1);
            typename Matrix<T,M>::const_iterator it = _matEls.begin();
            long size_to_copy = 0;
            for(int i = 0; i < this->columns(); i++)
            {
                std::copy(it, it + i + 1, output.begin() + size_to_copy);
                size_to_copy += i + 1;
                it += this->columns();
            }
        }
        else if(mode == Poly::VECMODE_LOWER_TRIANGULAR_NO_DIAGONAL)
        {
            _check_square_matrix();
            output.resize((this->columns()*(this->columns()-1))/2,1);
            typename Matrix<T,M>::const_iterator it = _matEls.begin(); //iterator at rows to skip in every step, starts at second column
            long size_to_copy = 0;
            it += 1;
            for(int i = this->columns() - 1; i > 0 ; i--) //Starts with 1 to skip the diagonal
            {
                std::copy(it, it + i, output.begin() + size_to_copy);
                size_to_copy += i;
                it += this->columns() + 1;
            }
        }
        else if(mode == Poly::VECMODE_LOWER_TRIANGULAR_WITH_DIAGONAL)
        {
            _check_square_matrix();
            output.resize((this->columns()*(this->columns()+1))/2,1);
            typename Matrix<T,M>::const_iterator it = _matEls.begin(); //iterator at rows to skip in every step, starts at second column
            long size_to_copy = 0;
            for(int i = this->columns() - 1; i >= 0 ; i--) //Starts with 1 to skip the diagonal
            {
                std::copy(it, it + i + 1, output.begin() + size_to_copy);
                size_to_copy += i + 1;
                it += this->columns() + 1;
            }
        }
        else
        {
            throw std::range_error("Vectorization mode not supported.");
        }
    }
    else
    {
        if(mode == Poly::VECMODE_FULL)
        {
            Matrix<T, M> output(this->rows()*this->columns(),1);
            std::copy(this->begin(),this->end(),output.begin());
            return output;
        }
        else if(mode == Poly::VECMODE_LOWER_TRIANGULAR_NO_DIAGONAL)
        {
            _check_square_matrix();
            output.resize((this->columns()*(this->columns()-1))/2,1);
            typename Matrix<T,M>::const_iterator it = _matEls.begin() + this->columns(); //iterator at rows to skip in every step, starts at second column
            long size_to_copy = 0;
            for(int i = 1; i < this->columns(); i++) //Starts with 1 to skip the diagonal
            {
                std::copy(it, it + i, output.begin() + size_to_copy);
                size_to_copy += i;
                it += this->columns();
            }
        }
        else if(mode == Poly::VECMODE_LOWER_TRIANGULAR_WITH_DIAGONAL)
        {
            _check_square_matrix();
            output.resize((this->columns()*(this->columns()+1))/2,1);
            typename Matrix<T,M>::const_iterator it = _matEls.begin();
            long size_to_copy = 0;
            for(int i = 0; i < this->columns(); i++)
            {
                std::copy(it, it + i + 1, output.begin() + size_to_copy);
                size_to_copy += i + 1;
                it += this->columns();
            }
        }
        else if(mode == Poly::VECMODE_UPPER_TRIANGULAR_NO_DIAGONAL)
        {
            _check_square_matrix();
            output.resize((this->columns()*(this->columns()-1))/2,1);
            typename Matrix<T,M>::const_iterator it = _matEls.begin(); //iterator at rows to skip in every step, starts at second column
            long size_to_copy = 0;
            it += 1;
            for(int i = this->columns() - 1; i > 0 ; i--) //Starts with 1 to skip the diagonal
            {
                std::copy(it, it + i, output.begin() + size_to_copy);
                size_to_copy += i;
                it += this->columns() + 1;
            }
        }
        else if(mode == Poly::VECMODE_UPPER_TRIANGULAR_WITH_DIAGONAL)
        {
            _check_square_matrix();
            output.resize((this->columns()*(this->columns()+1))/2,1);
            typename Matrix<T,M>::const_iterator it = _matEls.begin(); //iterator at rows to skip in every step, starts at second column
            long size_to_copy = 0;
            for(int i = this->columns() - 1; i >= 0 ; i--) //Starts with 1 to skip the diagonal
            {
                std::copy(it, it + i + 1, output.begin() + size_to_copy);
                size_to_copy += i + 1;
                it += this->columns() + 1;
            }
        }
        else
        {
            throw std::range_error("Vectorization mode not supported.");
        }
    }
    return output;
}

template <typename T, int M>
T& Matrix<T,M>::operator()(size_type row, size_type column)
{
    return this->at(row,column);
}

template <typename T, int M>
const T& Matrix<T,M>::operator()(size_type row, size_type column) const
{
    return this->at(row,column);
}

template <typename T, int M>
T &Matrix<T,M>::at(size_type row, size_type column)
{
#ifdef POLYMATH_DEBUG
    if(row < _rows && column < _columns)
#endif
        return _matEls[RowColumnToElement(row,column,_rows,_columns)];
#ifdef POLYMATH_DEBUG
    else
        throw std::out_of_range("at(): Out of range");
#endif
}

template <typename T, int M>
const T &Matrix<T,M>::at(size_type row, size_type column) const
{
#ifdef POLYMATH_DEBUG
    if(row < _rows && column < _columns)
#endif
        return _matEls[RowColumnToElement(row,column,_rows,_columns)];
#ifdef POLYMATH_DEBUG
    else
        throw std::out_of_range("at(): Out of range");
#endif
}

template <typename T, int M>
T &Matrix<T,M>::front()
{
    return _matEls[0];
}

template <typename T, int M>
const T &Matrix<T,M>::front() const
{
    return _matEls[0];
}

template <typename T, int M>
T& Matrix<T,M>::operator[](const size_type& index)
{
    return _matEls[index];
}

template <typename T, int M>
const T& Matrix<T,M>::operator[](const size_type& index) const
{
    return _matEls[index];
}

template <typename T, int M>
T& Matrix<T,M>::operator()(const size_type& index)
{
    return _matEls[index];
}

template <typename T, int M>
const T& Matrix<T,M>::operator()(const size_type& index) const
{
    return _matEls[index];
}

template <typename T, int M>
typename Matrix<T,M>::iterator Matrix<T,M>::begin() noexcept
{
    return this->_matEls.begin();
}

template <typename T, int M>
typename Matrix<T,M>::const_iterator Matrix<T,M>::begin() const noexcept
{
    return this->_matEls.begin();
}

template <typename T, int M>
typename Matrix<T,M>::iterator Matrix<T,M>::end() noexcept
{
    return this->_matEls.end();
}

template <typename T, int M>
typename Matrix<T,M>::const_iterator Matrix<T,M>::end() const noexcept
{
    return this->_matEls.end();
}

template <typename T, int M>
T Matrix<T,M>::determinant()
{
    this->_check_square_matrix();
    T det = static_cast<T>(1);
    T val;
    size_type sideLength = this->rows();
    for (size_type i = 0; i < sideLength; i++)
    {
        if((*this)[i][i] != 0)
        {
//            cout<<(*this)<<endl;
            val = static_cast<T>(1)/((*this)[i][i]);
            det *= static_cast<T>(1)/val;
            MultiplyRowWith(*this,val,i);
            for (int j = i + 1; j < sideLength; j++)
            {
                val = -((*this)[j][i]);
                MultiplyAndAddToRow(*this,val,i,j);
            }
        }
        else
        {
            for (size_type j = i + 1; j < sideLength; j++)
            {
                if((*this)[j][i] != static_cast<T>(0))
                {
                    this->swapRows(j,i);
                    break;
                }
            }
            if((*this)[i][i] == static_cast<T>(0))
            {
                return static_cast<T>(0);
            }
        }
    }
    return det;
}
template <typename T, int M>
Matrix<T,M> Matrix<T,M>::SVD()
{
    Matrix<T,M> U, S, V;
    S = (*this);
//    std::cout<<S.rows()<<"\t"<<S.columns()<<std::endl;
    std::vector<T> ret;
    U = IdentityMatrix<T,M>(S.rows());
    V = IdentityMatrix<T,M>(S.columns());
    T val;
    for (size_type i = 0; i < S.rows(); i++)
    {
        if(S[i][i] != static_cast<T>(0))
        {
//            std::cout<<"Int S: "<<std::endl<<S<<std::endl<<U<<std::endl<<U*S<<std::endl;
            for (size_type j = i + 1; j < S.rows(); j++)
            {
                val = -S[j][i]/S[i][i];
                MultiplyAndAddToRow(S,val,i,j);
                MultiplyAndAddToRow(U,-val,i,j);
            }
        }
        else
        {
            for (size_type j = i + 1; j < S.rows(); j++)
            {
                if(S[i][j] != static_cast<T>(0))
                {
                    S.swapRows(j,i);
                    U.swapRows(j,i);
                    break;
                }
            }
            if((S)[i][i] == static_cast<T>(0))
            {
                throw std::runtime_error("Matrix is non invertible");
            }
        }
    }
//    std::cout<<"U: "<<std::endl<<U<<std::endl;
//    std::cout<<"S: "<<std::endl<<S<<std::endl;
//    std::cout<<"Start Res: "<<std::endl<<*this<<std::endl;
//    std::cout<<"Res: "<<std::endl<<U*S<<std::endl;
    ret.push_back(U);
    ret.push_back(S);
    ret.push_back(V);
    return ret;
}


template <typename T, int M>
std::string Matrix<T,M>::asString(int precision, char open, char close, char sep)
{
    std::string out;
    out.push_back(open);
    for(long i = 0; i < this->rows(); i++)
    {
        out.push_back(open);
        for(long j = 0; j < this->columns(); j++)
        {
            out.append(Poly::to_string(this->at(i,j),precision));
            if(j != this->columns() - 1)
                out.push_back(sep);
        }
        out.push_back(close);
        if(i != this->rows() - 1)
            out.push_back(sep);
    }
    out.push_back(close);
    return out;
}

template <typename T, int M>
void Matrix<T,M>::print(std::ostream &os, std::string header) const
{
    if(!header.empty())
        std::cout<<header<<std::endl;
    std::cout << *this << std::endl;
}

template <typename T, int M>
void Matrix<T,M>::raw_print(std::ostream &os, std::string header) const
{
    if(!header.empty())
        std::cout<<header<<std::endl;
    std::cout << *this << std::endl;
}

template <typename T, int M>
Matrix<T,M> IdentityMatrix(const typename Poly::Matrix<T,M>::size_type& size)
{
    Matrix<T,M> temp;
    temp.resize(size,size,static_cast<T>(0));
    for (typename Poly::Matrix<T,M>::size_type i = 0; i < size; i++)
    {
        for (typename Poly::Matrix<T,M>::size_type j = 0; j < size; j++)
        {
            if(i == j)
            {
                temp.at(i,j) = static_cast<T>(1);
            }
        }
    }
    return temp;
}

template <typename T, int M>
void ResetMatrix(Matrix<T,M> &matrix, T value)
{
    for(typename Poly::Matrix<T,M>::size_type i = 0; i < matrix._matEls.size(); i++)
    {
        matrix._mat[i] = value;
    }
}

template <typename T, int M>
Matrix<T,M> Transpose(const Matrix<T,M> &matrix)
{
    Matrix<T,M> res(matrix);
    res.transpose_inplace();
    return res;
}

template <typename T, int M>
Matrix<T,M> ConjugateTranspose(const Matrix<T,M> &matrix)
{
    Matrix<T,M> res(matrix);
    res.conjugateTranspose_inplace();
    return res;
}

template <typename T, int M>
Matrix<T,M> Commute(const Matrix<T,M> &matrix1, const Matrix<T,M> &matrix2)
{
    return matrix1*matrix2 - matrix2*matrix1;
}

template <typename T, int M>
Matrix<T,M> AntiCommute(const Matrix<T,M> &matrix1, const Matrix<T,M> &matrix2)
{
    return matrix1*matrix2 + matrix2*matrix1;
}


template <typename T, int M>
Matrix<T,M> SolveLinearSystem(const Matrix<T,M>& matrix, const Matrix<T,M>& rhs)
{
#ifdef POLYMATH_DEBUG
    if(matrix.rows() != matrix.columns())
    {
        throw std::length_error("Inverse::NotSquareMatrix");
    }
#endif
    Matrix<T,M> mat = matrix;
    Matrix<T,M> sol = rhs;
    T val;
    std::vector<T> swapList;
    for (typename Poly::Matrix<T,M>::size_type i = 0; i < matrix.rows(); i++)
    {
        if(mat.at(i,i))
        {
            //            cout<<mat<<endl;
            val = static_cast<T>(1)/(mat.at(i,i));
            MultiplyRowWith(mat,val,i);
            MultiplyRowWith(sol,val,i);
            for (typename Poly::Matrix<T,M>::size_type j = i + 1; j < matrix.columns(); j++)
            {
                val = -(mat.at(j,i));
                MultiplyAndAddToRow(mat,val,i,j);
                MultiplyAndAddToRow(sol,val,i,j);
            }
        }
        else
        {
            for (typename Poly::Matrix<T,M>::size_type j = i + 1; j < matrix.rows(); j++)
            {
                if(mat.at(j,i) != static_cast<T>(0))
                {
                    SwapRows(mat,j,i);
                    SwapRows(sol,j,i);
                    swapList.push_back(i);
                    swapList.push_back(j);
                    break;
                }
            }
            if(mat.at(i,i) == static_cast<T>(0))
            {
                throw std::runtime_error("Inverse::MatrixNonInvertible");
            }
        }
    }
    for (int i = matrix.rows() - 1; i >= 0; i--)
    {
        if(mat.at(i,i) != static_cast<T>(0))
        {
            //            cout<<mat<<endl;
            val = 1.0/(mat.at(i,i));
            MultiplyRowWith(mat,val,i);
            MultiplyRowWith(sol,val,i);
            for (int j = i - 1; j >= 0; j--)
            {
                val = -(mat.at(j,i));
                MultiplyAndAddToRow(mat,val,i,j);
                MultiplyAndAddToRow(sol,val,i,j);
            }
        }
    }
    for(typename Poly::Matrix<T,M>::size_type i = swapList.size() - 1; i >= 0; i-=2)
    {
        SwapRows(sol,swapList[i],swapList[i-1]);
    }
    return sol;
}
template <typename T, int M>
void MultiplyRowWith(Matrix<T,M>& matrix, const T& multiplicand, const typename Poly::Matrix<T,M>::size_type &rowNum)
{
    for(typename Poly::Matrix<T,M>::size_type i = 0; i < matrix.columns(); i++)
    {
        matrix.at(rowNum,i) *= multiplicand;
    }
}

template <typename T, int M>
void MultiplyAndAddToRow(Matrix<T,M>& matrix, const T& multiplicand, const typename Poly::Matrix<T,M>::size_type &rowA, const typename Poly::Matrix<T,M>::size_type &rowB)
{
    for(typename Poly::Matrix<T,M>::size_type i = 0; i < matrix.columns(); i++)
    {
        matrix.at(rowB,i) += matrix.at(rowA,i)*multiplicand;
    }
}

template <typename T, int M>
void SwapRows(Matrix<T,M> &matrix, const typename Poly::Matrix<T,M>::size_type &a, const typename Poly::Matrix<T,M>::size_type &b)
{
//    T temp;
    for(typename Poly::Matrix<T,M>::size_type i = 0; i < matrix.columns(); i++)
    {
        std::swap(matrix.at(a,i),matrix.at(b,i));
//        temp = matrix.at(a,i);
//        matrix.at(a,i) = matrix.at(b,i);
//        matrix.at(b,i) = temp;
    }
}

std::string _lapack_eigens_exception_illegal_value(int info);
std::string _lapack_eigens_exception_converge_error(int info);
std::string _lapack_unsupported_type();
std::string _unsupported_type(const std::string& function_name);


template <typename T, int M>
void _Lapack_multiply_matrix_general(const Matrix<T,M>& lhs, const Matrix<T,M>& rhs, Matrix<T,M> &to_add_then_result, T alpha, T beta)
{
    char transa = 'N';
    char transb = 'N';
    int m = static_cast<int>(lhs.rows());
    int n = static_cast<int>(rhs.columns());
    int k = static_cast<int>(lhs.columns());
    int lda = std::max({1,k});
    int ldb = std::max({1,n});
    int ldc = std::max({1,m});
    if(std::is_same<T,double>::value)
    {
        typedef double TP;
        dgemm_(&transa, &transb,
                    &m, &n, &k, (TP*)&alpha, (TP*)&lhs.front(), &lda, (TP*)&rhs.front(), &ldb, (TP*)&beta, (TP*)&to_add_then_result.front(), &ldc);
    }
    else if(std::is_same<T,lapack_complex_double>::value)
    {
        typedef lapack_complex_double TP;
        zgemm_(&transa, &transb,
                &m, &n, &k, (TP*)&alpha, (TP*)&lhs.front(), &lda, (TP*)&rhs.front(), &ldb, (TP*)&beta, (TP*)&to_add_then_result.front(), &ldc);
    }
    else if(std::is_same<T,float>::value)
    {
        typedef float TP;
        sgemm_(&transa, &transb,
                &m, &n, &k, (TP*)&alpha, (TP*)&lhs.front(), &lda, (TP*)&rhs.front(), &ldb, (TP*)&beta, (TP*)&to_add_then_result.front(), &ldc);
    }
    else if(std::is_same<T,lapack_complex_float>::value)
    {
        typedef lapack_complex_float TP;
        cgemm_(&transa, &transb,
                &m, &n, &k, (TP*)&alpha, (TP*)&lhs.front(), &lda, (TP*)&rhs.front(), &ldb, (TP*)&beta, (TP*)&to_add_then_result.front(), &ldc);
    }
    else
    {
        throw std::logic_error(_lapack_unsupported_type());
    }
}

template <typename T, int M>
void MultiplyMatrices(const Matrix<T,M>& lhs, const Matrix<T,M>& rhs, Matrix<T,M> &to_add_then_result, T alpha, T beta)
{
//    std::cout<<"multiplication: "<<lhs.columns()<<"\t"<<rhs.rows()<<std::endl;
#ifdef POLYMATH_DEBUG
    if(lhs.columns() != rhs.rows())
    {
        throw std::length_error("IncompatibleMatricesSizesForMultiplication");
    }
#endif
    to_add_then_result.resize(lhs.rows(),rhs.columns());
    if(std::is_same<T,double>::value ||
       std::is_same<T,float>::value ||
       std::is_same<T,lapack_complex_double>::value ||
       std::is_same<T,lapack_complex_float>::value)
    {
        _Lapack_multiply_matrix_general(lhs,rhs,to_add_then_result,alpha,beta);
    }
    else
    {
        T comp;
        for (typename Poly::Matrix<T,M>::size_type i = 0; i < lhs.rows(); i++)
        {
            for (typename Poly::Matrix<T,M>::size_type j = 0; j < rhs.columns(); j++)
            {
                comp = 0;
                for (typename Poly::Matrix<T,M>::size_type k = 0; k < lhs.columns(); k++)
                {
                    comp += lhs.at(i,k)*rhs.at(k,j);
                }
                to_add_then_result.at(i,j) = comp;
            }
        }
    }
}

template <typename T, int M>
void MultiplyMatrices(const Matrix<T,M> &matrixLHS,
                      const Matrix<T,M> &matrixRHS,
                      Matrix<T,M> &result)
{
    Poly::MultiplyMatrices(matrixLHS,matrixRHS,result,T(1),T(0));
}

template <typename T, int M = ColMaj>
typename std::enable_if<std::is_integral<T>::value && std::is_scalar<T>::value, Matrix<T,M> >::type
RandomMatrix(const typename Matrix<T,M>::size_type& rows,
             const typename Matrix<T,M>::size_type& columns,
             T min,
             T max,
             long seed = 0,
             const MATRIX_TYPE& type = Poly::MATRIX_GENERAL)
{   
    Matrix<T,M> result(rows,columns);
    if(std::is_integral<T>::value)
    {
        std::uniform_int_distribution<T> distribution(min,max);
        std::default_random_engine generator(seed);
        std::generate(result.begin(), result.end(), [&]() { return distribution(generator); });
    }

    if(type == MATRIX_SYMMETRIC || type == MATRIX_HERMITIAN)
    {
        result = result/T(2) + Poly::Transpose<T,M>(result/T(2));
    }
    else if(type == MATRIX_ANTISYMMETRIC || type == MATRIX_ANTIHERMITIAN)
    {
        result = result/T(2) - Poly::Transpose<T,M>(result/T(2));
    }
    else if(type == MATRIX_GENERAL) {}
    else
    {
        throw std::logic_error(_unsupported_type(__func__).c_str());
    }
    return result;
}

template <typename T, int M = ColMaj>
typename std::enable_if<std::is_floating_point<T>::value && std::is_scalar<T>::value,Matrix<T,M> >::type
RandomMatrix(const typename Matrix<T,M>::size_type& rows,
             const typename Matrix<T,M>::size_type& columns,
             T min,
             T max,
             long seed = 0,
             const MATRIX_TYPE& type = Poly::MATRIX_GENERAL)
{
    Matrix<T,M> result(rows,columns);
    if(std::is_floating_point<T>::value)
    {
        std::uniform_real_distribution<T> distribution(min,max);
        std::default_random_engine generator(seed);
        std::generate(result.begin(), result.end(), [&]() { return distribution(generator); });
    }

    if(type == MATRIX_SYMMETRIC || type == MATRIX_HERMITIAN)
    {
        result = result/T(2) + Poly::Transpose<T,M>(result/T(2));
    }
    else if(type == MATRIX_ANTISYMMETRIC || type == MATRIX_ANTIHERMITIAN)
    {
        result = result/T(2) - Poly::Transpose<T,M>(result/T(2));
    }
    else if(type == MATRIX_GENERAL) {}
    else
    {
        throw std::logic_error(_unsupported_type(__func__));
    }
    return result;
}

template <typename T,
          int M = ColMaj,
          typename U = typename ExtractType<T>::SubType>
typename std::enable_if<std::is_same<T, std::complex<U>>::value,
                        Matrix<T,M> >::type
RandomMatrix(const typename Matrix<T,M>::size_type& rows,
             const typename Matrix<T,M>::size_type& columns,
             typename ExtractType<T>::SubType min,
             typename ExtractType<T>::SubType max,
             long seed = 0,
             const MATRIX_TYPE& type = Poly::MATRIX_GENERAL)
{
    Matrix<T,M> result(rows,columns);

    if(type == MATRIX_SYMMETRIC)
    {
        result =                      RandomMatrix<U>(rows,columns,min,max,seed,  MATRIX_SYMMETRIC) +
                 std::complex<U>(0,1)*RandomMatrix<U>(rows,columns,min,max,seed+1,MATRIX_SYMMETRIC);
    }
    else if(type == MATRIX_HERMITIAN)
    {
        result =                      RandomMatrix<U>(rows,columns,min,max,seed,  MATRIX_SYMMETRIC) +
                 std::complex<U>(0,1)*RandomMatrix<U>(rows,columns,min,max,seed+1,MATRIX_ANTISYMMETRIC);
    }
    else if(type == MATRIX_ANTISYMMETRIC)
    {
        result =                      RandomMatrix<U>(rows,columns,min,max,seed,  MATRIX_ANTISYMMETRIC) +
                 std::complex<U>(0,1)*RandomMatrix<U>(rows,columns,min,max,seed+1,MATRIX_ANTISYMMETRIC);
    }
    else if(type == MATRIX_ANTIHERMITIAN)
    {
        result =                      RandomMatrix<U>(rows,columns,min,max,seed,  MATRIX_ANTISYMMETRIC) +
                 std::complex<U>(0,1)*RandomMatrix<U>(rows,columns,min,max,seed+1,MATRIX_SYMMETRIC);
    }
    else if(type == MATRIX_GENERAL)
    {
        result =                      RandomMatrix<U>(rows,columns,min,max,seed,  MATRIX_GENERAL) +
                 std::complex<U>(0,1)*RandomMatrix<U>(rows,columns,min,max,seed+1,MATRIX_GENERAL);
    }
    else
    {
        throw std::logic_error(_unsupported_type(__func__));
    }
    return result;
}

/**
 * calculate eigenvalues and vectors
 */
template <typename T, int M>
int _Lapack_EigenVals_hermitian_divconq_double(Poly::Matrix<T>& eigenValues, Poly::Matrix<std::complex<T>, M> &input, bool with_eigenvectors)
{
        typedef double TP;
        typedef std::complex<double> CX_TP;
        lapack_int n = static_cast<int>(input.rows());
        int info = 0;
        int lda = std::max({1,n});
        std::unique_ptr<TP[]> rwork(new TP[std::max({1,3*n-2})]);
        int lwork = -1;
        int lrwork = -1;
        int liwork = -1;
        CX_TP tmp_lwork_size;
        TP    tmp_rwork_size;
        int   tmp_iwork_size;
        eigenValues.resize(n,1);
        char jobz = (with_eigenvectors ? 'V' : 'N');
        char uplo = 'U';

        //query optimal work size
        zheevd_(&jobz, &uplo, &n, (CX_TP*)&input.front(), &lda, (TP*)&eigenValues.front(),
                (CX_TP*)&tmp_lwork_size, &lwork,
                (TP*)   &tmp_rwork_size, &lrwork,
                &tmp_iwork_size, &liwork, &info);


        lwork = static_cast<unsigned long>(tmp_lwork_size.real());
        std::unique_ptr<CX_TP[]> workspace(new CX_TP[lwork]);
        lrwork = static_cast<unsigned long>(tmp_rwork_size);
        std::unique_ptr<TP[]> rworkspace(new TP[lrwork]);
        liwork = static_cast<unsigned long>(tmp_iwork_size);
        std::unique_ptr<int[]> iworkspace(new int[liwork]);

        //solve eigen problem
        zheevd_(&jobz, &uplo, &n, (CX_TP*)&input.front(), &lda, (TP*)&eigenValues.front(),
                (CX_TP*)workspace.get(), &lwork,
                (TP*)   rworkspace.get(), &lrwork,
                iworkspace.get(), &liwork, &info);
        if(M != ColMaj)
        {
            input.conjugateTranspose_inplace();
        }
        if(info > 0)
        {
            throw std::runtime_error(_lapack_eigens_exception_converge_error(info));
        }
        if(info < 0)
        {
            throw std::runtime_error(_lapack_eigens_exception_illegal_value(info));
        }
        return info;
}

template <typename T, int M>
int _Lapack_EigenVals_hermitian_divconq_float(Poly::Matrix<T>& eigenValues, Poly::Matrix<std::complex<T>, M> &input, bool with_eigenvectors)
{
        typedef float TP;
        typedef std::complex<float> CX_TP;
        lapack_int n = static_cast<int>(input.rows());
        int info = 0;
        int lda = std::max({1,n});
        std::unique_ptr<TP[]> rwork(new TP[std::max({1,3*n-2})]);
        int lwork = -1;
        int lrwork = -1;
        int liwork = -1;
        CX_TP tmp_lwork_size;
        TP    tmp_rwork_size;
        int   tmp_iwork_size;
        eigenValues.resize(n,1);
        char jobz = (with_eigenvectors ? 'V' : 'N');
        char uplo = 'U';

        //query optimal work size
        cheevd_(&jobz, &uplo, &n, (CX_TP*)&input.front(), &lda, (TP*)&eigenValues.front(),
                (CX_TP*)&tmp_lwork_size, &lwork,
                (TP*)   &tmp_rwork_size, &lrwork,
                &tmp_iwork_size, &liwork, &info);


        lwork = static_cast<unsigned long>(tmp_lwork_size.real());
        std::unique_ptr<CX_TP[]> workspace(new CX_TP[lwork]);
        lrwork = static_cast<unsigned long>(tmp_rwork_size);
        std::unique_ptr<TP[]> rworkspace(new TP[lrwork]);
        liwork = static_cast<unsigned long>(tmp_iwork_size);
        std::unique_ptr<int[]> iworkspace(new int[liwork]);

        //solve eigen problem
        cheevd_(&jobz, &uplo, &n, (CX_TP*)&input.front(), &lda, (TP*)&eigenValues.front(),
                (CX_TP*)workspace.get(), &lwork,
                (TP*)   rworkspace.get(), &lrwork,
                iworkspace.get(), &liwork, &info);
        if(M != ColMaj)
        {
            input.conjugateTranspose_inplace();
        }
        if(info > 0)
        {
            throw std::runtime_error(_lapack_eigens_exception_converge_error(info));
        }
        if(info < 0)
        {
            throw std::runtime_error(_lapack_eigens_exception_illegal_value(info));
        }
        return info;
}

template <typename T, int M>
int _Lapack_EigenVals_hermitian_double(Poly::Matrix<T>& eigenValues, Poly::Matrix<std::complex<T>, M> &input, bool with_eigenvectors)
{
        typedef double TP;
        typedef std::complex<double> CX_TP;
        lapack_int n = static_cast<int>(input.rows());
        int info = 0;
        int lda = std::max({1,n});
        std::unique_ptr<TP[]> rwork(new TP[std::max({1,3*n-2})]);
        int lwork = -1;
        eigenValues.resize(n,1);
        CX_TP tmp_work_size;
        char jobz = (with_eigenvectors ? 'V' : 'N');
        char uplo = 'U';

        //query optimal work size
        zheev_(&jobz, &uplo, &n, (CX_TP*)&input.front(), &lda, (TP*)&eigenValues.front(), (CX_TP*)&tmp_work_size, &lwork, rwork.get(), &info);
        lwork = static_cast<unsigned long>(tmp_work_size.real());
        std::unique_ptr<CX_TP[]> workspace(new CX_TP[lwork]);

        //solve eigen problem
        zheev_(&jobz, &uplo, &n, (CX_TP*)&input.front(), &lda, (TP*)&eigenValues.front(), (CX_TP*)workspace.get(), &lwork, rwork.get(), &info);
        if(M != ColMaj)
        {
            input.conjugateTranspose_inplace();
        }
        if(info > 0)
        {
            throw std::runtime_error(_lapack_eigens_exception_converge_error(info));
        }
        if(info < 0)
        {
            throw std::runtime_error(_lapack_eigens_exception_illegal_value(info));
        }
        return info;
}

template <typename T, int M>
int _Lapack_EigenVals_hermitian_float(Poly::Matrix<T>& eigenValues, Poly::Matrix<std::complex<T>, M > &input, bool with_eigenvectors)
{
        typedef float TP;
        typedef std::complex<float> CX_TP;
        lapack_int n = static_cast<int>(input.rows());
        int info = 0;
        int lda = std::max({1,n});
        std::unique_ptr<TP[]> rwork(new TP[std::max({1,3*n-2})]);
        int lwork = -1;
        eigenValues.resize(n,1);
        CX_TP tmp_work_size;
        char jobz = (with_eigenvectors ? 'V' : 'N');
        char uplo = 'U';

        //query optimal work size
        cheev_(&jobz, &uplo, &n, (CX_TP*)&input.front(), &lda, (TP*)&eigenValues.front(), (CX_TP*)&tmp_work_size, &lwork, rwork.get(), &info);
        lwork = static_cast<unsigned long>(tmp_work_size.real());
        std::unique_ptr<CX_TP[]> workspace(new CX_TP[lwork]);

        //solve eigen problem
        cheev_(&jobz, &uplo, &n, (CX_TP*)&input.front(), &lda, (TP*)&eigenValues.front(), (CX_TP*)workspace.get(), &lwork, rwork.get(), &info);
        if(M != ColMaj)
        {
            input.conjugateTranspose_inplace();
        }
        if(info > 0)
        {
            throw std::runtime_error(_lapack_eigens_exception_converge_error(info));
        }
        if(info < 0)
        {
            throw std::runtime_error(_lapack_eigens_exception_illegal_value(info));
        }
        return info;
}

template <typename T, int M>
void
EigenVecsVals(Poly::Matrix<T,M>& eigenValues,
              Poly::Matrix<std::complex<T>,M>& eigenVectors,
              const Poly::Matrix<std::complex<T>, M > &input_matrix,
              const MATRIX_TYPE &type,
              const EIGENS_METHOD &method = METHOD_DEFAULT)
{
    if(type == Poly::MATRIX_HERMITIAN)
    {
        if(std::is_same<double,T>::value)
        {
            eigenVectors = input_matrix;
            if(method == METHOD_DEFAULT)
            {
                _Lapack_EigenVals_hermitian_double(eigenValues,eigenVectors,true);
            }
            else if(method == METHOD_DIVIDE_CONQUER)
            {
                _Lapack_EigenVals_hermitian_divconq_double(eigenValues,eigenVectors,true);
            }
            else
            {
                throw std::range_error("Unknown eigenvalues/vectors decomposition method.");
            }
        }
        else if(std::is_same<float,T>::value)
        {
            eigenVectors = input_matrix;
            if(method == METHOD_DEFAULT)
            {
                _Lapack_EigenVals_hermitian_float(eigenValues,eigenVectors,true);
            }
            else if(method == METHOD_DIVIDE_CONQUER)
            {
                _Lapack_EigenVals_hermitian_divconq_float(eigenValues,eigenVectors,true);
            }
            else
            {
                throw std::range_error("Unknown eigenvalues/vectors decomposition method.");
            }
        }
        else
        {
            throw std::logic_error("The type requested is not supported.");
        }
    }
    else
    {
        throw std::logic_error("The type requested is not supported.");
    }
}

template <typename T, int M>
Poly::Matrix<T,M>
EigenVals(const Poly::Matrix<std::complex<T>,
          M > &input_matrix,
          const MATRIX_TYPE &type,
          const EIGENS_METHOD &method = METHOD_DEFAULT)
{
    if(type == Poly::MATRIX_HERMITIAN)
    {
        if(std::is_same<double,T>::value)
        {
            auto input = input_matrix; //copying is necessary because the LAPACK function overwrites matrix values
            Poly::Matrix<T,M> eigenValues;
            if(method == METHOD_DEFAULT)
            {
                _Lapack_EigenVals_hermitian_double(eigenValues,input,false);
            }
            else if(method == METHOD_DIVIDE_CONQUER)
            {
                _Lapack_EigenVals_hermitian_divconq_double(eigenValues,input,true);
            }
            else
            {
                throw std::range_error("Unknown eigenvalues/vectors decomposition method.");
            }
            return eigenValues;
        }
        else if(std::is_same<float,T>::value)
        {
            auto input = input_matrix; //copying is necessary because the LAPACK function overwrites matrix values
            Poly::Matrix<T,M> eigenValues;
            if(method == METHOD_DEFAULT)
            {
                _Lapack_EigenVals_hermitian_float(eigenValues,input,false);
            }
            else if(method == METHOD_DIVIDE_CONQUER)
            {
                _Lapack_EigenVals_hermitian_divconq_float(eigenValues,input,true);
            }
            else
            {
                throw std::range_error("Unknown eigenvalues/vectors decomposition method.");
            }
            return eigenValues;
        }
        else
        {
            throw std::logic_error("The type requested is not supported.");
        }
    }
    else
    {
        throw std::logic_error("The matrix type requested is not supported.");
    }
}

template <typename T, int M>
Poly::Matrix<std::complex<T>, M >
MatrixExp(const Poly::Matrix<std::complex<T>, M > &input_matrix, const MATRIX_TYPE &type)
{
    input_matrix._check_square_matrix();
    if(type == Poly::MATRIX_HERMITIAN)
    {
        auto input = input_matrix;
        Poly::Matrix<std::complex<T>, M> matrix_exp(input_matrix.rows(),input_matrix.columns());
        Poly::Matrix<T, M> eigenVals;
        Poly::Matrix<std::complex<T>, M> eigenVecs;
        EigenVecsVals(eigenVals,eigenVecs,input,type);
        for(long i = 0; i < matrix_exp.rows(); i++)
        {
            matrix_exp.at(i,i) = T(std::exp(eigenVals[i]));
        }
        return eigenVecs*matrix_exp*Poly::ConjugateTranspose(eigenVecs);
    }
    else if(type == Poly::MATRIX_ANTIHERMITIAN)
    {
        auto input = std::complex<T>(0,1)*input_matrix; //convert the matrix to Hermitian
        Poly::Matrix<std::complex<T>, M> matrix_exp(input_matrix.rows(),input_matrix.columns());
        Poly::Matrix<T, M> eigenVals;
        Poly::Matrix<std::complex<T>, M> eigenVecs;
        EigenVecsVals(eigenVals,eigenVecs,input,Poly::MATRIX_HERMITIAN);
        Poly::Matrix<std::complex<T>, M> eigenValsComplex = std::complex<T>(0,-1)*eigenVals; //necessary for anti-hermitian matrices to reverse the multiplication by i in the beginning
        for(long i = 0; i < matrix_exp.rows(); i++)
        {
            matrix_exp.at(i,i) = std::exp(eigenValsComplex[i]);
        }
        return eigenVecs*matrix_exp*Poly::ConjugateTranspose(eigenVecs);
    }
    else
    {
        throw std::logic_error("The matrix type requested is not supported.");
    }
}

template <typename T, int M>
Poly::Matrix<T, M>
KroneckerProduct(const Poly::Matrix<T, M> &mat1, const Poly::Matrix<T, M> &mat2)
{
    typedef typename Poly::Matrix<T,M>::size_type size_type;
    Poly::Matrix<T,M> result(mat1.rows()*mat2.rows(),mat1.columns()*mat2.columns());

    for(size_type i = 0; i < mat1.rows(); i++)
    {
        for(size_type j = 0; j < mat1.columns(); j++)
        {
            for(size_type k = 0; k < mat2.rows(); k++)
            {
                for(size_type l = 0; l < mat2.columns(); l++)
                {
                    result.at(mat2.rows()*i+k,mat2.columns()*j+l) = mat1.at(i,j)*mat2.at(k,l);
                }
            }
        }
    }
    return result;
}

}
#endif	/* Matrix_H */