#include <iostream>


template <size_t N,size_t M>
void printMatrix(const double mat[N][M])
{
	for(size_t i = 0; i < N; ++i)
	{
		for(size_t j = 0; j < M; ++j)
			std::cout << mat[i][j] << " ";

		std::cout << std::endl;
	}
	std::cout << std::endl;
}


/*
	Vertauscht zwei Zeilen in einer NxM Matrix.

	line1, line2 - Index der Zeilen, die vertauscht werden sollten.

	return: false, falls line1 oder line2 nicht in der Matrix liegen.
*/
template <size_t N,size_t M>
bool swapLine(double mat[N][M], size_t line1, size_t line2)
{
	if(line1 >= N || line2 >= N)
		return false;

	for(size_t i = 0; i < M; ++i)
	{
		double t = mat[line1][i];
		mat[line1][i] = mat[line2][i];
		mat[line2][i] = t;
	}
	
	return true;
}

/*
	Invertiert eine NxN Matrix mit Hilfe des Gauß-Jordan-Algorithmus.

	mat - Matrix die Invertiert werden soll.
	inv - Die Inverse der Matrix mat.

	return: false, falls die Matrix nicht invertierbar ist.
*/
template <size_t N>
bool invertMatrix(const double mat[N][N], double inv[N][N])
{
	// Eine Nx2N Matrix für den Gauß-Jordan-Algorithmus aufbauen
	double A[N][2*N];
	for(size_t i = 0; i < N; ++i)
	{
		for(size_t j = 0; j < N; ++j)
			A[i][j] = mat[i][j];
		for(size_t j = N; j < 2*N; ++j)
			A[i][j] = (i==j-N) ? 1.0 : 0.0;
	}

	// Gauß-Algorithmus.
	for(size_t k = 0; k < N-1; ++k)
	{
		// Zeilen vertauschen, falls das Pivotelement eine Null ist
		if(A[k][k] == 0.0)
		{
			for(size_t i = k+1; i < N; ++i)
			{
				if(A[i][k] != 0.0)
				{
					swapLine<N, 2*N>(A,k,i);
					break;
				}
				else if(i==N-1)
					return false; // Es gibt kein Element != 0
			}
		}

		// Einträge unter dem Pivotelement eliminieren
		for(size_t i = k+1; i < N; ++i)
		{
			double p = A[i][k]/A[k][k];		
			for(size_t j = k; j < 2*N; ++j)
				A[i][j] -= A[k][j]*p;
		}
	}

	// Determinante der Matrix berechnen
	double det = 1.0;
	for(size_t k = 0; k < N; ++k)
		det *= A[k][k];

	if(det == 0.0)	// Determinante ist =0 -> Matrix nicht invertierbar
		return false;

	// Jordan-Teil des Algorithmus durchführen
	for(size_t k = N-1; k > 0; --k)
	{
		for(int i = k-1; i >= 0; --i)
		{
			double p = A[i][k]/A[k][k];
			for(size_t j = k; j < 2*N; ++j)
				A[i][j] -= A[k][j]*p;
		}
	}

	// Einträge in der linker Matrix auf 1 normieren und in inv schreiben
	for(size_t i = 0; i < N; ++i)
	{
		const double f = A[i][i];
		for(size_t j = N; j < 2*N; ++j)
			inv[i][j-N] = A[i][j]/f;
	}

	return true;
}

int main()
{
	double result2[2][2];
	double result3[3][3];
	double result5[5][5];

	
	/*
	   0,4  -0,33333   0,1 

	  -0,2   0         0,2 

	   0,2  -0,33333  -0,2 
    */
	double A[3][3] = {{2,-3,-2},{0,-3,-3},{2,2,-2}};
	invertMatrix<3>(A, result3);
	printMatrix<3,3>(result3);
	
	/*
		-0,33333   0,4   0,1 

		0        -0,2   0,2 

		-0,33333   0,2  -0,2 
	*/
	double B[3][3] = {{0,-3,-3},{2,-3,-2},{2,2,-2}};
	invertMatrix<3>(B, result3);
	printMatrix<3,3>(result3);
	

	/*
	
	  -0,26316   0,21053 

	  -0,05263  -0,15789 
	*/
	double C[2][2] = {{-3,-4},{1,-5}};
	invertMatrix<2>(C, result2);
	printMatrix<2,2>(result2);


	/*
	   0,53774  -1,11321   0,03774   0,53774  -0,49057 

	  -0,03145   0,09434  -0,03145   0,13522   0,24214 

	  -0,09434   0,28302  -0,09434  -0,09434   0,22642 

	   0,01258  -0,03774   0,01258  -0,15409   0,00314 

	   0,77673  -0,83019   0,27673   0,61006  -0,43082 
	*/

	double D[5][5] = {{0,-3,7,1,2},{-1,-3,3,-4,1},{-3,5,-10,4,1},{0,1,-1,-5,0},{0,5,-1,5,0}};
	invertMatrix<5>(D, result5);
	printMatrix<5,5>(result5);


	std::cin.get();

	return 0;
}