/*! \file pimathmatrix.h
 * \ingroup Math
 * \~\brief
 * \~english Math matrix
 * \~russian Математическая матрица
 */
/*
	PIP - Platform Independent Primitives
	PIMathMatrix
	Ivan Pelipenko peri4ko@yandex.ru, Andrey Bychkov work.a.b@yandex.ru

	This program is free software: you can redistribute it and/or modify
	it under the terms of the GNU Lesser General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.

	This program is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU Lesser General Public License for more details.

	You should have received a copy of the GNU Lesser General Public License
	along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#ifndef PIMATHMATRIX_H
#define PIMATHMATRIX_H

#include "pimathvector.h"
#include "pimathcomplex.h"


/// Matrix templated

#define PIMM_FOR for (uint r = 0; r < Rows; ++r) for (uint c = 0; c < Cols; ++c)
#define PIMM_FOR_C for (uint i = 0; i < Cols; ++i)
#define PIMM_FOR_R for (uint i = 0; i < Rows; ++i)

#pragma pack(push, 1)

//! \brief A class that works with square matrix operations, the input data of which are columns, rows and the data type of the matrix
//! @tparam Rows rows number of matrix
//! @tparam Сols columns number of matrix
//! @tparam Type is the data type of the matrix. There are can be basic C++ language data and different classes where the arithmetic operators(=, +=, -=, *=, /=, ==, !=, +, -, *, /)
//!  of the C++ language are implemented
template<uint Rows, uint Cols = Rows, typename Type = double>
class PIP_EXPORT PIMathMatrixT {
	typedef PIMathMatrixT<Rows, Cols, Type> _CMatrix;
	typedef PIMathMatrixT<Cols, Rows, Type> _CMatrixI;
	typedef PIMathVectorT<Rows, Type> _CMCol;
	typedef PIMathVectorT<Cols, Type> _CMRow;
	static_assert(std::is_arithmetic<Type>::value, "Type must be arithmetic");
	static_assert(Rows > 0, "Row count must be > 0");
	static_assert(Cols > 0, "Column count must be > 0");
public:
	/**
	* \brief Constructs PIMathMatrixT that is filled by \a new_value
	*/
	PIMathMatrixT(const Type &new_value = Type()) {PIMM_FOR m[r][c] = new_value;}

	/**
	* \brief Contructs PIMathMatrixT from PIVector
	*/
	PIMathMatrixT(const PIVector<Type> &val) {
		assert(Rows*Cols == val.size());
		int i = 0;
		PIMM_FOR m[r][c] = val[i++];
	}

	/**
	* \brief Contructs PIMathMatrixT from C++11 initializer list
	*/
	PIMathMatrixT(std::initializer_list<Type> init_list) {
		assert(Rows*Cols == init_list.size());
		int i = 0;
		PIMM_FOR m[r][c] = init_list.begin()[i++];
	}

	/**
	* \brief Сreates a matrix whose main diagonal is filled with ones and the remaining elements are zeros
	*
	* @return identity matrix of type PIMathMatrixT
	*/
	static _CMatrix identity() {
		_CMatrix tm = _CMatrix();
		PIMM_FOR tm.m[r][c] = (c == r ? Type(1) : Type(0));
		return tm;
	}

	/**
	* \brief Method which returns number of columns in matrix
	*
	* @return type uint shows number of columns
	*/
	constexpr uint cols() const {return Cols;}

	/**
	* \brief Method which returns number of rows in matrix
	*
	* @return type uint shows number of rows
	*/
	constexpr uint rows() const {return Rows;}

	/**
	* \brief Method which returns the selected column in PIMathVectorT format.
	* If you enter an index out of the border of the matrix there will be "undefined behavior"
	*
	* @param index is the number of the selected column
	* @return column in PIMathVectorT format
	*/
	_CMCol col(uint index) {
		_CMCol tv;
		PIMM_FOR_R tv[i] = m[i][index];
		return tv;
	}

	/**
	* \brief Method which returns the selected row in PIMathVectorT format
	* If you enter an index out of the border of the matrix there will be "undefined behavior"
	*
	* @param index is the number of the selected row
	* @return row in PIMathVectorT format
	*/
	_CMRow row(uint index) {
		_CMRow tv;
		PIMM_FOR_C tv[i] = m[index][i];
		return tv;
	}

	/**
	* \brief Set the selected column in matrix.
	* If you enter an index out of the border of the matrix there will be "undefined behavior"
	*
	* @param index is the number of the selected column
	* @param v is a vector of the type _CMCol that needs to fill the column
	* @return matrix type _CMatrix
	*/
	_CMatrix &setCol(uint index, const _CMCol &v) {
		PIMM_FOR_R m[i][index] = v[i];
		return *this;
	}

	/**
	* \brief Set the selected row in matrix
	* If you enter an index out of the border of the matrix there will be "undefined behavior"
	*
	* @param index is the number of the selected row
	* @param v is a vector of the type _CMCol that needs to fill the row
	* @return matrix type _CMatrix
	*/
	_CMatrix &setRow(uint index, const _CMRow &v) {
		PIMM_FOR_C m[index][i] = v[i];
		return *this;
	}

	/**
	* \brief Method which changes selected rows in a matrix.
	* If you enter an index out of the border of the matrix there will be "undefined behavior"
	*
	* @param rf is the number of the first selected row
	* @param rs is the number of the second selected row
	* @return matrix type _CMatrix
	*/
	_CMatrix &swapRows(uint rf, uint rs) {
		PIMM_FOR_C piSwap<Type>(m[rf][i], m[rs][i]);
		return *this;
	}

	/**
	* \brief Method which changes selected columns in a matrix.
	* If you enter an index out of the border of the matrix there will be "undefined behavior"
	*
	* @param cf is the number of the first selected column
	* @param cs is the number of the second selected column
	* @return matrix type _CMatrix
	*/
	_CMatrix &swapCols(uint cf, uint cs) {
		PIMM_FOR_R piSwap<Type>(m[i][cf], m[i][cs]);
		return *this;
	}

	/**
	* \brief Method which fills the matrix with selected value
	*
	* @param v is a parameter the type and value of which is selected and later filled into the matrix
	* @return filled matrix type _CMatrix
	*/
	_CMatrix &fill(const Type &v) {
		PIMM_FOR m[r][c] = v;
		return *this;
	}

	/**
	* \brief Method which checks if matrix is square
	*
	* @return true if matrix is square, else false
	*/
	constexpr bool isSquare() const { return Rows == Cols; }

	/**
	* \brief Method which checks if main diagonal of matrix consists of ones and another elements are zeros
	*
	* @return true if matrix is identitied, else false
	*/
	bool isIdentity() const {
		PIMM_FOR if ((c == r) ? m[r][c] != Type(1) : m[r][c] != Type(0)) return false;
		return true;
	}

	/**
	* \brief Method which checks if every elements of matrix are zeros
	*
	* @return true if matrix is null, else false
	*/
	bool isNull() const {
		PIMM_FOR if (m[r][c] != Type(0)) return false;
		return true;
	}


	/**
	* \brief Read-only  access to element by \a row and \a col.
	* If you enter an index out of the border of the matrix there will be "undefined behavior"
	*
	* @param row of matrix
	* @param col of matrix
	* @return copy of element of matrix
	*/
	Type at(uint row, uint col) const { return m[row][col]; }

	/**
	* \brief Full access to element by \a row and \a col.
	* If you enter an index out of the border of the matrix there will be "undefined behavior"
	*
	* @param row of matrix
	* @param col of matrix
	* @return element of matrix
	*/
	inline Type & element(uint row, uint col) {return m[row][col];}

	/**
	* \brief Read-only  access to element by \a row and \a col.
	* If you enter an index out of the border of the matrix there will be "undefined behavior"
	*
	* @param row of matrix
	* @param col of matrix
	* @return element of matrix
	*/
	inline const Type & element(uint row, uint col) const {return m[row][col];}

	/**
	* \brief Full access to the matrix row pointer. If you enter an index out of the border of the matrix there will be "undefined behavior"
	*
	* @param row of matrix
	* @return matrix row pointer
	*/
	Type *operator[](uint row) { return m[row]; }

	/**
	* \brief Read-only access to the matrix row pointer. If you enter an index out of the border of the matrix there will be "undefined behavior"
	*
	* @param row of matrix
	* @return matrix row pointer
	*/
	const Type *operator[](uint row) const {return m[row];}

	/**
	* \brief Matrix compare
	*
	* @param sm matrix for compare
	* @return if matrices are equal true, else false
	*/
	bool operator==(const _CMatrix &sm) const {
		PIMM_FOR if (m[r][c] != sm.m[r][c]) return false;
		return true;
	}

	/**
	* \brief Matrix negative compare
	*
	* @param sm matrix for compare
	* @return if matrices are not equal true, else false
	*/
	bool operator!=(const _CMatrix &sm) const { return !(*this == sm); }

	/**
	* \brief Addition assignment with matrix "sm"
	*
	* @param sm matrix for the addition assigment
	*/
	void operator+=(const _CMatrix &sm) {PIMM_FOR m[r][c] += sm.m[r][c];}

	/**
	* \brief Subtraction assignment with matrix "sm"
	*
	* @param sm matrix for the subtraction assigment
	*/
	void operator-=(const _CMatrix &sm) {PIMM_FOR m[r][c] -= sm.m[r][c];}

	/**
	* \brief Multiplication assignment with value "v"
	*
	* @param v value for the multiplication assigment
	*/
	void operator*=(const Type &v) {
		PIMM_FOR m[r][c] *= v;
	}

	/**
	* \brief Division assignment with value "v"
	*
	* @param v value for the division assigment
	*/
	void operator/=(const Type &v) {
		assert(piAbs<Type>(v) > PIMATHVECTOR_ZERO_CMP);
		PIMM_FOR m[r][c] /= v;
	}

	/**
	* \brief Matrix substraction
	*
	* @return the result of matrix substraction
	*/
	_CMatrix operator-() const {
		_CMatrix tm;
		PIMM_FOR tm.m[r][c] = -m[r][c];
		return tm;
	}

	/**
	* \brief Matrix addition
	*
	* @param sm is matrix term
	* @return the result of matrix addition
	*/
	_CMatrix operator+(const _CMatrix &sm) const {
		_CMatrix tm = _CMatrix(*this);
		PIMM_FOR tm.m[r][c] += sm.m[r][c];
		return tm;
	}

	/**
	* \brief Matrix substraction
	*
	* @param sm is matrix subtractor
	* @return the result of matrix substraction
	*/
	_CMatrix operator-(const _CMatrix &sm) const {
		_CMatrix tm = _CMatrix(*this);
		PIMM_FOR tm.m[r][c] -= sm.m[r][c];
		return tm;
	}

	/**
	* \brief Matrix multiplication
	*
	* @param v is value factor
	* @return the result of matrix multiplication
	*/
	_CMatrix operator*(const Type &v) const {
		_CMatrix tm = _CMatrix(*this);
		PIMM_FOR tm.m[r][c] *= v;
		return tm;
	}

	/**
	* \brief Matrix division
	*
	* @param v is value divider
	* @return the result of matrix division
	*/
	_CMatrix operator/(const Type &v) const {
		assert(piAbs<Type>(v) > PIMATHVECTOR_ZERO_CMP);
		_CMatrix tm = _CMatrix(*this);
		PIMM_FOR tm.m[r][c] /= v;
		return tm;
	}

	/**
	* \brief Determinant of the matrix is ​​calculated. Works only with square matrix, nonzero matrices and invertible matrix
	*
	* @param ok is a parameter with which we can find out if the method worked correctly
	* @return matrix determinant
	*/
	Type determinant(bool *ok = 0) const {
		_CMatrix m(*this);
		bool k;
		Type ret = Type(0);
		m.toUpperTriangular(&k);
		if (ok) *ok = k;
		if (!k) return ret;
		ret = Type(1);
		PIMM_FOR if (r == c) ret *= m[r][c];
		return ret;
	}

	/**
	* \brief Trace of the matrix is calculated. Works only with square matrix, nonzero matrices and invertible matrix
	*
	* @return matrix trace
	*/
	Type trace() const {
		static_assert(Rows == Cols, "Works only with square matrix");
		Type ret = Type(0);
		for (uint i = 0; i < Cols; ++i) {
			ret += m[i][i];
		}
		return ret;
	}

	/**
	* \brief Transforming matrix to upper triangular. Works only with square matrix, nonzero matrices and invertible matrix
	*
	* @param ok is a parameter with which we can find out if the method worked correctly
	* @return copy of transformed upper triangular matrix
	*/
	_CMatrix &toUpperTriangular(bool *ok = 0) {
		static_assert(Rows == Cols, "Works only with square matrix");
		_CMatrix smat(*this);
		bool ndet;
		uint crow;
		Type mul;
		for (uint i = 0; i < Cols; ++i) {
			ndet = true;
			for (uint j = 0; j < Rows; ++j) if (smat.m[i][j] != 0) ndet = false;
			if (ndet) {
				if (ok != 0) *ok = false;
				return *this;
			}
		}
		for (uint i = 0; i < Cols; ++i) {
			crow = i;
			while (smat.m[i][i] == Type(0))
				smat.swapRows(i, ++crow);
			for (uint j = i + 1; j < Rows; ++j) {
				mul = smat.m[i][j] / smat.m[i][i];
				for (uint k = i; k < Cols; ++k) smat.m[k][j] -= mul * smat.m[k][i];
			}
			if (i < Cols - 1) {
				if (piAbs<Type>(smat.m[i + 1][i + 1]) < Type(1E-200)) {
					if (ok != 0) *ok = false;
					return *this;
				}
			}
		}
		if (ok != 0) *ok = true;
		memcpy(m, smat.m, sizeof(Type) * Cols * Rows);
		return *this;
	}

	/**
	* \brief Matrix inversion operation. Works only with square matrix, nonzero matrices and invertible matrix
	*
	* @param ok is a parameter with which we can find out if the method worked correctly
	* @return copy of inverted matrix
	*/
	_CMatrix &invert(bool *ok = 0) {
		static_assert(Rows == Cols, "Works only with square matrix");
		_CMatrix mtmp = _CMatrix::identity(), smat(*this);
		bool ndet;
		uint crow;
		Type mul, iddiv;
		for (uint i = 0; i < Cols; ++i) {
			ndet = true;
			for (uint j = 0; j < Rows; ++j) if (smat.m[i][j] != 0) ndet = false;
			if (ndet) {
				if (ok != 0) *ok = false;
				return *this;
			}
		}
		for (uint i = 0; i < Cols; ++i) {
			crow = i;
			while (smat.m[i][i] == Type(0)) {
				++crow;
				smat.swapRows(i, crow);
				mtmp.swapRows(i, crow);
			}
			for (uint j = i + 1; j < Rows; ++j) {
				mul = smat.m[i][j] / smat.m[i][i];
				for (uint k = i; k < Cols; ++k) smat.m[k][j] -= mul * smat.m[k][i];
				for (uint k = 0; k < Cols; ++k) mtmp.m[k][j] -= mul * mtmp.m[k][i];
			}
			if (i < Cols - 1) {
				if (piAbs<Type>(smat.m[i + 1][i + 1]) < Type(1E-200)) {
					if (ok != 0) *ok = false;
					return *this;
				}
			}
			iddiv = smat.m[i][i];
			for (uint j = i; j < Cols; ++j) smat.m[j][i] /= iddiv;
			for (uint j = 0; j < Cols; ++j) mtmp.m[j][i] /= iddiv;
		}
		for (uint i = Cols - 1; i > 0; --i) {
			for (uint j = 0; j < i; ++j) {
				mul = smat.m[i][j];
				smat.m[i][j] -= mul;
				for (uint k = 0; k < Cols; ++k) mtmp.m[k][j] -= mtmp.m[k][i] * mul;
			}
		}
		if (ok != 0) *ok = true;
		memcpy(m, mtmp.m, sizeof(Type) * Cols * Rows);
		return *this;
	}

	/**
	* \brief Matrix inversion operation. Works only with square matrix, nonzero matrices and invertible matrix
	*
	* @param ok is a parameter with which we can find out if the method worked correctly
	* @return inverted matrix
	*/
	_CMatrix inverted(bool *ok = 0) const {
		_CMatrix tm(*this);
		tm.invert(ok);
		return tm;
	}

	/**
	* \brief Matrix transposition operation. Works only with square matrix, nonzero matrices and invertible matrix
	*
	* @return transposed matrix
	*/
	_CMatrixI transposed() const {
		_CMatrixI tm;
		PIMM_FOR tm[c][r] = m[r][c];
		return tm;
	}

	/**
	* \brief Matrix rotation operation. Works only with 2x2 matrix
	*
	* @return rotated matrix
	*/
	_CMatrix rotate(Type angle) {
		static_assert(Rows == 2 && Cols == 2, "Works only with 2x2 matrix");
		Type c = std::cos(angle);
		Type s = std::sin(angle);
		PIMathMatrixT<2u, 2u> tm;
		tm[0][0] = tm[1][1] = c;
		tm[0][1] = -s;
		tm[1][0] = s;
		*this = *this * tm;
		return *this;
	}

private:
	Type m[Rows][Cols];
};

#pragma pack(pop)


#ifdef PIP_STD_IOSTREAM
template<uint Rows, uint Cols, typename Type>
inline std::ostream & operator <<(std::ostream & s, const PIMathMatrixT<Rows, Cols, Type> & m) {
	s << "{";
	for (uint r = 0; r < Rows; ++r) {
		for (uint c = 0; c < Cols; ++c) {
			s << m[r][c];
			if (c < Cols - 1 || r < Rows - 1) s << ", ";
		}
		if (r < Rows - 1) s << std::endl << " ";
	}
	s << "}";
	return s;
}
#endif

/**
* \brief Add matrix "m" at the end of matrix and return reference to matrix
*
* @param s PICout type
* @param m PIMathMatrixT type
* @return bitwise left PICout
*/
template<uint Rows, uint Cols, typename Type>
inline PICout operator<<(PICout s, const PIMathMatrixT<Rows, Cols, Type> &m) {
	s << "{";
	for (uint r = 0; r < Rows; ++r) {
		for (uint c = 0; c < Cols; ++c) {
			s << m[r][c];
			if (c < Cols - 1 || r < Rows - 1) s << ", ";
		}
		if (r < Rows - 1) s << PICoutManipulators::NewLine << " ";
	}
	s << "}";
	return s;
}

/**
* \brief Multiplying matrices by each other. If you enter an index out of the border of the matrix there will be "undefined behavior"
*
* @param fm first matrix multiplier
* @param sm second matrix multiplier
* @return matrix that is the result of multiplication
*/
template<uint CR, uint Rows0, uint Cols1, typename Type>
inline PIMathMatrixT<Rows0, Cols1, Type> operator*(const PIMathMatrixT<Rows0, CR, Type> &fm,
												   const PIMathMatrixT<CR, Cols1, Type> &sm) {
	PIMathMatrixT<Rows0, Cols1, Type> tm;
	Type t;
	for (uint j = 0; j < Rows0; ++j) {
		for (uint i = 0; i < Cols1; ++i) {
			t = Type(0);
			for (uint k = 0; k < CR; ++k)
				t += fm[j][k] * sm[k][i];
			tm[j][i] = t;
		}
	}
	return tm;
}

/**
* \brief Multiplying matrix and vector. If you enter an index out of the border of the matrix there will be "undefined behavior"
*
* @param fm first matrix multiplier
* @param sv second vector multiplier
* @return vector that is the result of multiplication
*/
template<uint Cols, uint Rows, typename Type>
inline PIMathVectorT<Rows, Type> operator*(const PIMathMatrixT<Rows, Cols, Type> &fm,
										   const PIMathVectorT<Cols, Type> &sv) {
	PIMathVectorT<Rows, Type> tv;
	Type t;
	for (uint j = 0; j < Rows; ++j) {
		t = Type(0);
		for (uint i = 0; i < Cols; ++i)
			t += fm[j][i] * sv[i];
		tv[j] = t;
	}
	return tv;
}

/**
* \brief Multiplying vector and matrix. If you enter an index out of the border of the matrix there will be "undefined behavior"
*
* @param sv first vector multiplier
* @param fm second matrix multiplier
* @return vector that is the result of multiplication
*/
template<uint Cols, uint Rows, typename Type>
inline PIMathVectorT<Cols, Type> operator*(const PIMathVectorT<Rows, Type> &sv,
										   const PIMathMatrixT<Rows, Cols, Type> &fm) {
	PIMathVectorT<Cols, Type> tv;
	Type t;
	for (uint j = 0; j < Cols; ++j) {
		t = Type(0);
		for (uint i = 0; i < Rows; ++i)
			t += fm[i][j] * sv[i];
		tv[j] = t;
	}
	return tv;
}

/**
* \brief Multiplying value of type Type and matrix
*
* @param x first multiplier of type Type
* @param fm second matrix multiplier
* @return matrix that is the result of multiplication
*/
template<uint Cols, uint Rows, typename Type>
inline PIMathMatrixT<Rows, Cols, Type> operator*(const Type &x, const PIMathMatrixT<Rows, Cols, Type> &v) {
	return v * x;
}


typedef PIMathMatrixT<2u, 2u, int> PIMathMatrixT22i;
typedef PIMathMatrixT<3u, 3u, int> PIMathMatrixT33i;
typedef PIMathMatrixT<4u, 4u, int> PIMathMatrixT44i;
typedef PIMathMatrixT<2u, 2u, double> PIMathMatrixT22d;
typedef PIMathMatrixT<3u, 3u, double> PIMathMatrixT33d;
typedef PIMathMatrixT<4u, 4u, double> PIMathMatrixT44d;


template<typename Type>
class PIMathMatrix;

#undef PIMM_FOR
#undef PIMM_FOR_C
#undef PIMM_FOR_R





/// Matrix

#define PIMM_FOR for (uint r = 0; r < _V2D::rows_; ++r) for (uint c = 0; c < _V2D::cols_; ++c)
#define PIMM_FOR_A for (uint i = 0; i < _V2D::mat.size(); ++i)
#define PIMM_FOR_C for (uint i = 0; i < _V2D::cols_; ++i)
#define PIMM_FOR_R for (uint i = 0; i < _V2D::rows_; ++i)

//! \brief A class that works with matrix operations, the input data of which is the data type of the matrix
//!  @tparam There are can be basic C++ language data and different classes where the arithmetic operators(=, +=, -=, *=, /=, ==, !=, +, -, *, /)
//!  of the C++ language are implemented
template<typename Type>
class PIP_EXPORT PIMathMatrix : public PIVector2D<Type> {
	typedef PIVector2D<Type> _V2D;
	typedef PIMathMatrix<Type> _CMatrix;
public:
	/**
	* \brief Constructor of class PIMathMatrix, which creates a matrix
	*
	* @param cols is number of matrix column uint type
	* @param rows is number of matrix row uint type
	* @param f is type of matrix elements
	*/
	PIMathMatrix(const uint cols = 0, const uint rows = 0, const Type &f = Type()) { _V2D::resize(rows, cols, f); }

	/**
	* \brief Constructor of class PIMathMatrix, which creates a matrix
	*
	* @param cols is number of matrix column uint type
	* @param rows is number of matrix row uint type
	* @param val is PIVector<Type> of matrix elements
	*/
	PIMathMatrix(const uint cols, const uint rows, const PIVector<Type> &val) {
		_V2D::resize(rows, cols);
		int i = 0;
		PIMM_FOR _V2D::element(r, c) = val[i++];
	}

	/**
	* \brief Constructor of class PIMathMatrix, which creates a matrix
	*
	* @param val is PIVector<Type> of PIVector, which creates matrix
	*/
	PIMathMatrix(const PIVector<PIVector<Type> > &val) {
		if (!val.isEmpty()) {
			_V2D::resize(val.size(), val[0].size());
			for (uint r = 0; r < _V2D::rows_; ++r) {
				assert(val[r].size() == _V2D::cols_);
				for (uint c = 0; c < _V2D::cols_; ++c)
					_V2D::element(r, c) = val[r][c];
			}
		}
	}

	/**
	* \brief Constructor of class PIMathMatrix, which creates a matrix
	*
	* @param val is PIVector2D<Type>, which creates matrix
	*/
	PIMathMatrix(const PIVector2D<Type> &val) {
		if (!val.isEmpty()) {
			_V2D::resize(val.rows(), val.cols());
			PIMM_FOR _V2D::element(r, c) = val.element(r, c);
		}
	}

	/**
	* \brief Creates a matrix whose main diagonal is filled with ones and the remaining elements are zeros
	*
	* @param cols is number of matrix column uint type
	* @param rows is number of matrix row uint type
	* @return identity matrix(cols,rows)
	*/
	static _CMatrix identity(const uint cols, const uint rows) {
		_CMatrix tm(cols, rows);
		for (uint r = 0; r < rows; ++r) for (uint c = 0; c < cols; ++c) tm.element(r, c) = (c == r ? Type(1) : Type(0));
		return tm;
	}

	/**
	* \brief Creates a row matrix of every element that is equal to every element of the vector
	*
	* @param val is the vector type PIMathVector
	* @return row matrix of every element that is equal to every element of the vector
	*/
	static _CMatrix matrixRow(const PIMathVector<Type> &val) {return _CMatrix(val.size(), 1, val.toVector());}

	/**
	* \brief Creates a column matrix of every element that is equal to every element of the vector
	*
	* @param val is the vector type PIMathVector
	* @return column matrix of every element that is equal to every element of the vector
	*/
	static _CMatrix matrixCol(const PIMathVector<Type> &val) {return _CMatrix(1, val.size(), val.toVector());}

	/**
	* \brief Set the selected column in matrix. If there are more elements of the vector than elements in the 	column of the matrix
	* or index larger than the number of columns otherwise there will be "undefined behavior"
	*
	* @param index is the number of the selected column
	* @param v is a vector of the type _CMCol that needs to fill the column
	* @return matrix type _CMatrix
	*/
	_CMatrix &setCol(uint index, const PIMathVector<Type> &v) {
		assert(_V2D::rows() == v.size());
		PIMM_FOR_R _V2D::element(i, index) = v[i];
		return *this;
	}

	/**
	* \brief Set the selected row in matrix. If there are more elements of the vector than elements in the row of the matrix,
	* or index larger than the number of rows otherwise there will be "undefined behavior"
	* @param index is the number of the selected row
	* @param v is a vector of the type _CMCol that needs to fill the row
	* @return matrix type _CMatrix
	*/
	_CMatrix &setRow(uint index, const PIMathVector<Type> &v) {
		assert(_V2D::cols() == v.size());
		PIMM_FOR_C _V2D::element(index, i) = v[i];
		return *this;
	}

	/**
	* \brief Method which replace selected columns in a matrix. You cannot use an index larger than the number of columns,
	*  otherwise there will be "undefined behavior"
	*
	* @param r0 is the number of the first selected row
	* @param r1 is the number of the second selected row
	* @return matrix type _CMatrix
	*/
	_CMatrix &swapCols(uint r0, uint r1) {
		PIMM_FOR_C piSwap<Type>(_V2D::element(i, r0), _V2D::element(i, r1));
		return *this;
	}

	/**
	* \brief Method which replace selected rows in a matrix. You cannot use an index larger than the number of rows,
	*  otherwise there will be "undefined behavior"
	*
	* @param c0 is the number of the first selected row
	* @param c1 is the number of the second selected row
	* @return matrix type _CMatrix
	*/
	_CMatrix &swapRows(uint c0, uint c1) {
		PIMM_FOR_R piSwap<Type>(_V2D::element(c0, i), _V2D::element(c1, i));
		return *this;
	}

	/**
	* \brief Method which fills the matrix with selected value
	*
	* @param v is a parameter the type and value of which is selected and later filled into the matrix
	* @return filled matrix type _CMatrix
	*/
	_CMatrix &fill(const Type &v) {
		PIMM_FOR_A _V2D::mat[i] = v;
		return *this;
	}

	/**
	* \brief Method which checks if matrix is square
	*
	* @return true if matrix is square, else false
	*/
	bool isSquare() const { return _V2D::cols_ == _V2D::rows_; }

	/**
	* \brief Method which checks if main diagonal of matrix consists of ones and another elements are zeros
	*
	* @return true if matrix is identity, else false
	*/
	bool isIdentity() const {
		PIMM_FOR if ((c == r) ? _V2D::element(r, c) != Type(1) : _V2D::element(r, c) != Type(0))return false;
		return true;
	}

	/**
	* \brief Method which checks if every elements of matrix are zeros
	*
	* @return true if matrix elements equal to zero, else false
	*/
	bool isNull() const {
		PIMM_FOR_A if (_V2D::mat[i] != Type(0)) return false;
		return true;
	}

	/**
	* \brief Method which checks if matrix is empty
	*
	* @return true if matrix is valid, else false
	*/
	bool isValid() const { return !PIVector2D<Type>::isEmpty(); }

	/**
	* \brief Addition assignment with matrix "sm"
	*
	* @param sm matrix for the addition assigment
	*/
	void operator+=(const _CMatrix &sm) {
		assert(_V2D::rows() == sm.rows());
		assert(_V2D::cols() == sm.cols());
		PIMM_FOR_A _V2D::mat[i] += sm.mat[i];
	}

	/**
	* \brief Subtraction assignment with matrix "sm"
	*
	* @param sm matrix for the subtraction assigment
	*/
	void operator-=(const _CMatrix &sm) {
		assert(_V2D::rows() == sm.rows());
		assert(_V2D::cols() == sm.cols());
		PIMM_FOR_A _V2D::mat[i] -= sm.mat[i];
	}

	/**
	* \brief Multiplication assignment with value "v"
	*
	* @param v value for the multiplication assigment
	*/
	void operator*=(const Type &v) {
		PIMM_FOR_A _V2D::mat[i] *= v;
	}

	/**
	* \brief Division assignment with value "v"
	*
	* @param v value for the division assigment
	*/
	void operator/=(const Type &v) {
		assert(piAbs<Type>(v) > PIMATHVECTOR_ZERO_CMP);
		PIMM_FOR_A _V2D::mat[i] /= v;
	}

	/**
	* \brief Matrix substraction
	*
	* @return the result of matrix substraction
	*/
	_CMatrix operator-() const {
		_CMatrix tm(*this);
		PIMM_FOR_A tm.mat[i] = -_V2D::mat[i];
		return tm;
	}

	/**
	* \brief Matrix addition
	*
	* @param sm is matrix term
	* @return the result of matrix addition
	*/
	_CMatrix operator+(const _CMatrix &sm) const {
		_CMatrix tm(*this);
		assert(tm.rows() == sm.rows());
		assert(tm.cols() == sm.cols());
		PIMM_FOR_A tm.mat[i] += sm.mat[i];
		return tm;
	}

	/**
	* \brief Matrix subtraction
	*
	* @param sm is matrix subtractor
	* @return the result of matrix subtraction
	*/
	_CMatrix operator-(const _CMatrix &sm) const {
		_CMatrix tm(*this);
		assert(tm.rows() == sm.rows());
		assert(tm.cols() == sm.cols());
		PIMM_FOR_A tm.mat[i] -= sm.mat[i];
		return tm;
	}

	/**
	* \brief Matrix multiplication
	*
	* @param v is value factor
	* @return the result of matrix multiplication
	*/
	_CMatrix operator*(const Type &v) const {
		_CMatrix tm(*this);
		PIMM_FOR_A tm.mat[i] *= v;
		return tm;
	}

	/**
	* \brief Matrix division
	*
	* @param v is value divider
	* @return the result of matrix division
	*/
	_CMatrix operator/(const Type &v) const {
		assert(piAbs<Type>(v) > PIMATHVECTOR_ZERO_CMP);
		_CMatrix tm(*this);
		PIMM_FOR_A tm.mat[i] /= v;
		return tm;
	}

	/**
	* \brief Determinant of the self matrix is ​​calculated. Works only with square matrix, nonzero matrices and invertible matrix
	*
	* @param ok is a parameter with which we can find out if the method worked correctly
	* @return matrix determinant
	*/
	Type determinant(bool *ok = 0) const {
		_CMatrix m(*this);
		bool k;
		m.toUpperTriangular(&k);
		Type ret = Type(0);
		if (ok) *ok = k;
		if (!k) return ret;
		ret = Type(1);
		for (uint c = 0; c < _V2D::cols_; ++c)
			for (uint r = 0; r < _V2D::rows_; ++r)
				if (r == c)
					ret *= m.element(r, c);
		return ret;
	}

	/**
	* \brief Trace of the matrix is calculated. Works only with square matrix, nonzero matrices and invertible matrix
	*
	* @return matrix trace
	*/
	Type trace() const {
		assert(isSquare());
		Type ret = Type(0);
		for (uint i = 0; i < _V2D::cols_; ++i) {
			ret += _V2D::element(i, i);
		}
		return ret;
	}

	/**
	* \brief Transforming matrix to upper triangular. Works only with square matrix, nonzero matrices and invertible matrix
	*
	* @param ok is a parameter with which we can find out if the method worked correctly
	* @return copy of transformed upper triangular matrix
	*/
	_CMatrix &toUpperTriangular(bool *ok = 0) {
		assert(isSquare());
		_CMatrix smat(*this);
		bool ndet;
		uint crow;
		Type mul;
		for (uint i = 0; i < _V2D::cols_; ++i) {
			ndet = true;
			for (uint j = 0; j < _V2D::rows_; ++j) if (smat.element(i, j) != 0) ndet = false;
			if (ndet) {
				if (ok != 0) *ok = false;
				return *this;
			}
		}
		for (uint i = 0; i < _V2D::cols_; ++i) {
			crow = i;
			while (smat.element(i, i) == Type(0))
				smat.swapRows(i, ++crow);
			for (uint j = i + 1; j < _V2D::rows_; ++j) {
				mul = smat.element(i, j) / smat.element(i, i);
				for (uint k = i; k < _V2D::cols_; ++k) smat.element(k, j) -= mul * smat.element(k, i);
			}
			if (i < _V2D::cols_ - 1) {
				if (PIMathFloatNullCompare(smat.element(i + 1, i + 1))) {
					if (ok != 0) *ok = false;
					return *this;
				}
			}
		}
		if (ok != 0) *ok = true;
		_V2D::mat.swap(smat.mat);
		return *this;
	}

	/**
	* \brief Matrix inversion operation. Works only with square matrix, nonzero matrices and invertible matrix
	*
	* @param ok is a parameter with which we can find out if the method worked correctly
	* @param sv is a vector multiplier
	* @return copy of inverted matrix
	*/
	_CMatrix &invert(bool *ok = 0, PIMathVector<Type> *sv = 0) {
		assert(isSquare());
		_CMatrix mtmp = _CMatrix::identity(_V2D::cols_, _V2D::rows_), smat(*this);
		bool ndet;
		uint crow;
		Type mul, iddiv;
		for (uint i = 0; i < _V2D::cols_; ++i) {
			ndet = true;
			for (uint j = 0; j < _V2D::rows_; ++j) if (smat.element(i, j) != Type(0)) ndet = false;
			if (ndet) {
				if (ok != 0) *ok = false;
				return *this;
			}
		}
		for (uint i = 0; i < _V2D::cols_; ++i) {
			crow = i;
			while (smat.element(i, i) == Type(0)) {
				++crow;
				smat.swapRows(i, crow);
				mtmp.swapRows(i, crow);
				if (sv != 0) sv->swapElements(i, crow);
			}
			for (uint j = i + 1; j < _V2D::rows_; ++j) {
				mul = smat.element(i, j) / smat.element(i, i);
				for (uint k = i; k < _V2D::cols_; ++k) smat.element(k, j) -= mul * smat.element(k, i);
				for (uint k = 0; k < _V2D::cols_; ++k) mtmp.element(k, j) -= mul * mtmp.element(k, i);
				if (sv != 0) (*sv)[j] -= mul * (*sv)[i];
			}
			if (i < _V2D::cols_ - 1) {
				if (PIMathFloatNullCompare(smat.element(i + 1, i + 1))) {
					if (ok != 0) *ok = false;
					return *this;
				}
			}
			iddiv = smat.element(i, i);
			for (uint j = i; j < _V2D::cols_; ++j) smat.element(j, i) /= iddiv;
			for (uint j = 0; j < _V2D::cols_; ++j) mtmp.element(j, i) /= iddiv;
			if (sv != 0) (*sv)[i] /= iddiv;
		}
		for (uint i = _V2D::cols_ - 1; i > 0; --i) {
			for (uint j = 0; j < i; ++j) {
				mul = smat.element(i, j);
				smat.element(i, j) -= mul;
				for (uint k = 0; k < _V2D::cols_; ++k) mtmp.element(k, j) -= mul * mtmp.element(k, i);
				if (sv != 0) (*sv)[j] -= mul * (*sv)[i];
			}
		}
		if (ok != 0) *ok = true;
		PIVector2D<Type>::swap(mtmp);
		return *this;
	}

	/**
	* \brief Matrix inversion operation. Works only with square matrix, nonzero matrices and invertible matrix
	*
	* @param ok is a parameter with which we can find out if the method worked correctly
	* @return inverted matrix
	*/
	_CMatrix inverted(bool *ok = 0) const {
		_CMatrix tm(*this);
		tm.invert(ok);
		return tm;
	}

	/**
	* \brief Matrix transposition operation
	*
	* @return transposed matrix
	*/
	_CMatrix transposed() const {
		_CMatrix tm(_V2D::rows_, _V2D::cols_);
		PIMM_FOR tm.element(c, r) = _V2D::element(r, c);
		return tm;
	}
};


#ifdef PIP_STD_IOSTREAM
template<typename Type>
inline std::ostream & operator <<(std::ostream & s, const PIMathMatrix<Type> & m) {s << "{"; for (uint r = 0; r < m.rows(); ++r) { for (uint c = 0; c < m.cols(); ++c) { s << m.element(r, c); if (c < m.cols() - 1 || r < m.rows() - 1) s << ", ";} if (r < m.rows() - 1) s << std::endl << " ";} s << "}"; return s;}
#endif

/**
* \brief Inline operator for outputting the matrix to the console
*
* @param s PICout type
* @param the matrix type PIMathMatrix that we print to the console
* @return PIMathMatrix printed to the console
*/
template<typename Type>
inline PICout operator<<(PICout s, const PIMathMatrix<Type> &m) {
	s << "Matrix{";
	for (uint r = 0; r < m.rows(); ++r) {
		for (uint c = 0; c < m.cols(); ++c) {
			s << m.element(r, c);
			if (c < m.cols() - 1 || r < m.rows() - 1) s << ", ";
		}
		if (r < m.rows() - 1) s << PICoutManipulators::NewLine << " ";
	}
	s << "}";
	return s;
}

/**
* \brief Inline operator for serializing a matrix into a PIByteArray
*
* @param s PIByteArray type
* @param v PIMathMatrix type
* @return PIBiteArray serialized PIMathMatrix
*/
template <typename P, typename T>
inline PIBinaryStream<P> & operator <<(PIBinaryStream<P> & s, const PIMathMatrix<T> & v) {
	s << (const PIVector2D<T> &) v;
	return s;
}

/**
* \brief Inline operator to deserialize matrix from PIByteArray
*
* @param s PIByteArray type
* @param v PIMathMatrix type
* @return PIMathMatrix deserialized from PIByteArray
*/
template <typename P, typename T>
inline PIBinaryStream<P> & operator >>(PIBinaryStream<P> & s, PIMathMatrix<T> & v) {
	s >> (PIVector2D<T> &) v;
	return s;
}


/**
* \brief Multiplying matrices by each other. If you enter an index out of the border of the matrix there will be "undefined behavior"
*
* @param fm first matrix multiplier
* @param sm second matrix multiplier
* @return matrix that is the result of multiplication
*/
template<typename Type>
inline PIMathMatrix<Type> operator*(const PIMathMatrix<Type> &fm,
									const PIMathMatrix<Type> &sm) {
	assert(fm.cols() == sm.rows());
	PIMathMatrix<Type> tm(sm.cols(), fm.rows());
	Type t;
	for (uint j = 0; j < fm.rows(); ++j) {
		for (uint i = 0; i < sm.cols(); ++i) {
			t = Type(0);
			for (uint k = 0; k < fm.cols(); ++k)
				t += fm.element(j, k) * sm.element(k, i);
			tm.element(j, i) = t;
		}
	}
	return tm;
}

/**
* \brief Multiplying matrix and vector. If you enter an index out of the border of the matrix there will be "undefined behavior"
*
* @param fm first matrix multiplier
* @param sv second vector multiplier
* @return vector that is the result of multiplication
*/
template<typename Type>
inline PIMathVector<Type> operator*(const PIMathMatrix<Type> &fm,
									const PIMathVector<Type> &sv) {
	assert(fm.cols() == sv.size());
	PIMathVector<Type> tv(fm.rows());
	Type t;
	for (uint j = 0; j < fm.rows(); ++j) {
		t = Type(0);
		for (uint i = 0; i < fm.cols(); ++i)
			t += fm.element(j, i) * sv[i];
		tv[j] = t;
	}
	return tv;
}

/**
* \brief Multiplying vector and matrix. If you enter an index out of the border of the matrix there will be "undefined behavior"
*
* @param sv first vector multiplier
* @param fm second matrix multiplier
* @return vector that is the result of multiplication
*/
template<typename Type>
inline PIMathVector<Type> operator*(const PIMathVector<Type> &sv,
									const PIMathMatrix<Type> &fm) {
	PIMathVector<Type> tv(fm.cols());
	assert(fm.rows() == sv.size());
	Type t;
	for (uint j = 0; j < fm.cols(); ++j) {
		t = Type(0);
		for (uint i = 0; i < fm.rows(); ++i)
			t += fm.element(i, j) * sv[i];
		tv[j] = t;
	}
	return tv;
}

/**
* \brief Multiplying value of type Type and matrix
*
* @param x first multiplier of type Type
* @param v second matrix multiplier
* @return matrix that is the result of multiplication
*/
template<typename Type>
inline PIMathMatrix<Type> operator*(const Type &x, const PIMathMatrix<Type> &v) {
	return v * x;
}

typedef PIMathMatrix<int> PIMathMatrixi;
typedef PIMathMatrix<double> PIMathMatrixd;

/**
* \brief Searching hermitian matrix
*
* @param m conjugate transpose matrix
* @return result of the hermitian
*/
template<typename T>
PIMathMatrix<complex<T> > hermitian(const PIMathMatrix<complex<T> > &m) {
	PIMathMatrix<complex<T> > ret(m);
	for (uint r = 0; r < ret.rows(); ++r)
		for (uint c = 0; c < ret.cols(); ++c)
			ret.element(r, c).imag(-(ret.element(r, c).imag()));
	return ret.transposed();
}

#undef PIMM_FOR
#undef PIMM_FOR_A
#undef PIMM_FOR_C
#undef PIMM_FOR_R

#endif // PIMATHMATRIX_H