/*
 * 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();
}

}

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 Poly::Matrix<T,M>::size_type& size);

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;

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:

    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&&) = 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() = default;

    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 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 typename std::vector<T>::iterator begin() noexcept;
    inline typename std::vector<T>::const_iterator begin() const noexcept;
    inline typename std::vector<T>::iterator end() noexcept;
    inline typename std::vector<T>::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 Matrix<T,M>& rhs);
    void applyInverse();
    Matrix<T,M> getInverse();
    Matrix<T,M> getExp();
    void applyConjugate();
    void applyConjugateTranspose();
    std::vector<T> vectorize(const VECTORIZATION_MODE& mode = VECMODE_FULL) const;
    const T& at(size_type row, size_type column) const;
    T& at(size_type row, size_type column);
    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();
    const size_type& rows() const;
    const size_type& columns() const;
    void clear();
    void applyTranspose();
    T getDeterminant();
    Matrix<T,M> getSVD();
    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);

    //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(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(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>
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>::applyTranspose()
{
    *this = Transpose(*this);
}

template <typename T, int M>
void Matrix<T,M>::applyConjugateTranspose()
{
    *this = ConjugateTranspose(*this);
}

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._rows; j++)
        {
            os << src.at(i,j) << "\t";
        }
        os << std::endl;
    }
    return os;
}
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)
{
#ifdef POLYMATH_DEBUG
    if((lhs.columns() != rhs.columns()) || (lhs.rows() != rhs.rows()))
    {
        throw std::length_error("Invalid matrix sizes for addition");
    }
#endif
    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;
#ifdef POLYMATH_DEBUG
    if((lhs.columns() != rhs.columns()) || (lhs.rows() != rhs.rows()))
    {
        throw std::length_error("Invalid matrix sizes for subtraction");
    }
#endif
    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)
{
#ifdef POLYMATH_DEBUG
    if((lhs.columns() != rhs.columns()) || (lhs.rows() != rhs.rows()))
    {
        throw std::length_error("Invalid matrix sizes for addition");
    }
#endif
    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)
{
#ifdef POLYMATH_DEBUG
    if((lhs.columns() != rhs.columns()) || (lhs.rows() != rhs.rows()))
    {
        throw std::length_error("Invalid matrix sizes for addition");
    }
#endif
    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)
{
#ifdef POLYMATH_DEBUG
    if((lhs.columns() != rhs.columns()) || (lhs.rows() != rhs.rows()))
    {
        throw std::length_error("Invalid matrix sizes for addition");
    }
#endif
    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)
{
#ifdef POLYMATH_DEBUG
    if((lhs.columns() != rhs.columns()) || (lhs.rows() != rhs.rows()))
    {
        throw std::length_error("Invalid matrix sizes for addition");
    }
#endif
    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>
void Matrix<T,M>::applyInverse()
{
    if(rows() != columns())
    {
        throw std::length_error("Matrix inverse is only for square matrices.");
    }
    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>::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>::getInverse()
{
    Matrix<T,M> tmpMat = *this;
    tmpMat.applyInverse();
    return tmpMat;
}

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

template <typename T, int M>
void Matrix<T,M>::applyConjugate()
{
    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>
std::vector<T> Matrix<T, M>::vectorize(const VECTORIZATION_MODE &mode) const
{
    std::vector<T> output;
    if(M == ColMaj)
    {
        if(mode == Poly::VECMODE_FULL)
        {
            return _matEls;
        }
        else if(mode == Poly::VECMODE_UPPER_TRIANGULAR_NO_DIAGONAL)
        {
#ifdef POLYMATH_DEBUG
            if(this->columns() != this->rows())
            {
                throw std::length_error("You cannot vectorize a non-square matrix to upper/lower triangulars.");
            }
#endif
            output.resize((this->columns()*(this->columns()-1))/2);
            typename std::vector<T>::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)
        {
#ifdef POLYMATH_DEBUG
            if(this->columns() != this->rows())
            {
                throw std::length_error("You cannot vectorize a non-square matrix to upper/lower triangulars.");
            }
#endif
            output.resize((this->columns()*(this->columns()+1))/2);
            typename std::vector<T>::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)
        {
#ifdef POLYMATH_DEBUG
            if(this->columns() != this->rows())
            {
                throw std::length_error("You cannot vectorize a non-square matrix to upper/lower triangulars.");
            }
#endif
            output.resize((this->columns()*(this->columns()-1))/2);
            typename std::vector<T>::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)
        {
#ifdef POLYMATH_DEBUG
            if(this->columns() != this->rows())
            {
                throw std::length_error("You cannot vectorize a non-square matrix to upper/lower triangulars.");
            }
#endif
            output.resize((this->columns()*(this->columns()+1))/2);
            typename std::vector<T>::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)
        {
            return _matEls;
        }
        else if(mode == Poly::VECMODE_LOWER_TRIANGULAR_NO_DIAGONAL)
        {
#ifdef POLYMATH_DEBUG
            if(this->columns() != this->rows())
            {
                throw std::length_error("You cannot vectorize a non-square matrix to upper/lower triangulars.");
            }
#endif
            output.resize((this->columns()*(this->columns()-1))/2);
            typename std::vector<T>::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)
        {
#ifdef POLYMATH_DEBUG
            if(this->columns() != this->rows())
            {
                throw std::length_error("You cannot vectorize a non-square matrix to upper/lower triangulars.");
            }
#endif
            output.resize((this->columns()*(this->columns()+1))/2);
            typename std::vector<T>::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)
        {
#ifdef POLYMATH_DEBUG
            if(this->columns() != this->rows())
            {
                throw std::length_error("You cannot vectorize a non-square matrix to upper/lower triangulars.");
            }
#endif
            output.resize((this->columns()*(this->columns()-1))/2);
            typename std::vector<T>::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)
        {
#ifdef POLYMATH_DEBUG
            if(this->columns() != this->rows())
            {
                throw std::length_error("You cannot vectorize a non-square matrix to upper/lower triangulars.");
            }
#endif
            output.resize((this->columns()*(this->columns()+1))/2);
            typename std::vector<T>::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>::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>
typename std::vector<T>::iterator Matrix<T,M>::begin() noexcept
{
    return this->_matEls.begin();
}

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

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

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

template <typename T, int M>
T Matrix<T,M>::getDeterminant()
{
    T det = static_cast<T>(1);
    T val;
#ifdef POLYMATH_DEBUG
    if(this->rows() != this->columns())
    {
        throw std::length_error("Determinant is for square matrices only.");
    }
#endif
    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;
            this->doMultiplyRowWith(val,i);
            for (int j = i + 1; j < sideLength; j++)
            {
                val = -((*this)[j][i]);
                this->doMultiplyAndAddToRow(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>::getSVD()
{
    Matrix<T,M> U, S, V;
    S = (*this);
    std::cout<<S.rows()<<"\t"<<S.columns()<<std::endl;
    std::vector<T> ret;
    U = Matrix<T,M>::identity(S.rows());
    V = Matrix<T,M>::identity(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;
            //val = 1.0/((S)[i][i]);
            //S.doMultiplyRowWith(val,i);
            //sideMat.doMultiplyRowWith(1/val,i);
            for (size_type j = i + 1; j < S.rows(); j++)
            {
                val = -S[j][i]/S[i][i];
                S.doMultiplyAndAddToRow(val,i,j);
                U.doMultiplyAndAddToRow(-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>
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.columns(),matrix.rows());
    for(typename Poly::Matrix<T,M>::size_type i = 0; i < matrix.rows(); i++)
    {
        for(typename Poly::Matrix<T,M>::size_type j = 0; j < matrix.columns(); j++)
        {
            res.at(j,i) = matrix.at(i,j);
        }
    }
    return res;
}

template <typename T, int M>
Matrix<T,M> ConjugateTranspose(const Matrix<T,M> &matrix)
{
    Matrix<T,M> res(matrix.columns(),matrix.rows());
    for(typename Poly::Matrix<T,M>::size_type i = 0; i < matrix.rows(); i++)
    {
        for(typename Poly::Matrix<T,M>::size_type j = 0; j < matrix.columns(); j++)
        {
            res.at(j,i) = std::conj(matrix.at(i,j));
        }
    }
    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.applyConjugateTranspose();
        }
        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.applyConjugateTranspose();
        }
        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.applyConjugateTranspose();
        }
        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.applyConjugateTranspose();
        }
        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
    {
    }
}

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)
{
#ifdef POLYMATH_DEBUG
    if(input_matrix.columns() != input_matrix.rows())
    {
        throw std::length_error("Matrix exponential can only be taken for square matrices");
    }
#endif
    if(type == Poly::MATRIX_HERMITIAN)
    {
        auto input = input_matrix;
        Poly::Matrix<std::complex<T>, M> matrix_exp(input_matrix.rows(),input_matrix.columns(),T());
        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
    {
        throw std::logic_error("The matrix type requested is not supported.");
    }
}

}

#endif	/* Matrix_H */