#define EXPLICIT_ONE_AND_ZERO	1
#define PARTIAL_PIVOT			1

typedef double real;

/*
Assumes matrices are stored in row-order, so an element
m_ij is referenced as follows: M[i*dim+j], where dim is
the dimension, N, of the matrix.

To invert an NxN matrix, use one of these (note that if the matrix
is singular, both will return false):

	LUDecompInvert(real* input, int dim, real* output)
	GaussJInvert(real* input, int dim, real* output)

To solve the linear equation Ax=b for x, use one of these (note that
if the matrix is singular, both will return false, eventhough a solution
may exist):

	LUDecompSolve(real* matrix, int dim, real* vec)
	GaussJSolve(real* matrix, int dim, real* vec, real* out)

To find the LU decomposition (returns true if matrix was non-singular):

	LUDecomp(real* input, int dim, real* L, real* U, 
			 int* permutation, int& sign)

Forward/backward substitution:

	ForwardSubstitute(real* L, int dim, real* vec, real* out, 
					  int* permutation = NULL)
	BackSubstitute(real* U, int dim, real* vec, real* out, 
				   int* permutation = NULL)

NOTE: The Gauss-Jordan routines *overwrite* the original matrix

Copyright (c) 2002-2005, Willem H. de Boer. All rights reserved.

*/
namespace SMM /*{S}quare {M}atrix {M}ath*/
{
	inline real ABS(real a)
	{
		if (a < 0.0)
			return -a;

		return a;
	}

	inline void SWAP(real& a, real& b)
	{
		real tmp = b;
		b = a;
		a = tmp;
	}

	// Calculate the following sum
	// \sum_{k=0}^{num} L_{ik}*U{kj}
	//
	inline real LUSUM(int dim, real* L, int i, real* U, int j, int num)
	{
//		ASSERT(i >= 0 && i < dim);
//		ASSERT(j >= 0 && j < dim);

		if (num <= 0)
			return 0.0;

		real result = 0.0;
		for (int k = 0; k < num; ++k)
		{
			result += (L[i*dim+k] * U[k*dim+j]);
		}

		return result;
	}

	// [matrix] will be overridden
	void Transpose(real* matrix, int dim)
	{
		for (int i = 0; i < dim; ++i)
		{
			for (int j = 0; j < i; ++j)
			{
				SWAP(matrix[i*dim+j], matrix[j*dim+i]);
			}
		}
	}

    // Calculate matrix product AB and store result in [out]
    void Multiply(real* A, real* B, real* out, int dim)
    {
	    int k = 0;
	    for (int i = 0; i < dim; ++i)
	    {
		    for (int j = 0; j < dim; ++j)
		    {
			    out[i*dim+j] = 0.0;
			    for (int k = 0; k < dim; ++k)
			    {
				    out[i*dim+j] += (A[i*dim+k] * B[k*dim+j]);
			    }
		    }
	    }
    }

    // Calculate vector Av and store in [out]
    void MatVecMult(real* A, real* v, real* out, int dim)
    {
	    for (int i = 0; i < dim; ++i)
	    {
		    out[i] = 0.0;
		    for (int j = 0; j < dim; ++j)
			    out[i] += (A[i*dim+j] * v[j]);
	    }
    }

	// x_i = a_{ii}^{-1} (b_i - \sum{j=i+1}^dim a_ij x_j)
	bool BackSubstitute(real* U, int dim, real* vec, real* out, int* permutation = NULL)
	{
		int i,j;

		if (permutation)
		{
			i = dim-1;
			out[i] = vec[permutation[i]] / U[i*dim+i];

			for (i = dim-2; i >= 0; --i)
			{
				// FIXME: We are recalculating sum from scratch for every row
				// but we could just as well use the result from the previous row
				// plus the U_ij*x_j from this row.
				real sum = 0.0;
				for (j = i+1; j < dim; ++j)
					sum += (U[i*dim+j] * out[j]);
				out[i] = (vec[permutation[i]] - sum) / U[i*dim+i];
			}
		}
		else
		{
			i = dim-1;
			out[i] = vec[i] / U[i*dim+i];

			for (i = dim-2; i >= 0; --i)
			{
				// FIXME: We are recalculating sum from scratch for every row
				// but we could just as well use the result from the previous row
				// plus the U_ij*x_j from this row.
				real sum = 0.0;
				for (j = i+1; j < dim; ++j)
					sum += (U[i*dim+j] * out[j]);
				out[i] = (vec[i] - sum) / U[i*dim+i];
			}
		}

		return true;
	}

	// x_i = a_{ii}^{-1} (b_i - \sum{j=1}^{i-1} a_ij x_j)
	bool ForwardSubstitute(real* L, int dim, real* vec, real *out, int* permutation = NULL)
	{
		int i,j;

		if (permutation)
		{
			out[0] = vec[permutation[0]] / L[0];

			for (i = 1; i < dim; ++i)
			{
				// FIXME: We are recalculating sum from scratch for every row
				// but we could just as well use the result from the previous row
				// and add the the U_ij*x_j of this row.
				real sum = 0.0;
				for (j = 0; j < i; ++j)
					sum += (L[i*dim+j] * out[j]);
				out[i] = (vec[permutation[i]] - sum) / L[i*dim+i];
			}
		}
		else
		{
			out[0] = vec[0] / L[0];

			for (i = 1; i < dim; ++i)
			{
				// FIXME: We are recalculating sum from scratch for every row
				// but we could just as well use the result from the previous row
				// and add the the U_ij*x_j of this row.
				real sum = 0.0;
				for (j = 0; j < i; ++j)
					sum += (L[i*dim+j] * out[j]);
				out[i] = (vec[i] - sum) / L[i*dim+i];
			}
		}

		return true;

	}

	// LU Decompose the [dim]x[dim] matrix [input] into [L] and [U],
	// using the formulas found at 
	// http://csep10.phys.utk.edu/guidry/phys594/lectures/linear_algebra/lanotes/node3.html
	// (which is Crout's method I believe),  and partial pivoting.
	//
	// U_{1j} = A_{1j}, 1 <= j <= dim
	//
	// L_{ij} = U_{jj}^{-1} (A_{ij} - \sum_{k=1}^{j-1} L_{ik} U_{kj}),	j <= i-1
	// U_{ij} = A_{ij} - \sum_{k=1}^{i-1} L_{ik} U_{kj},				j >= i
	bool LUDecomp(real* input, int dim, real* L, real* U, int* permutation, int& sign)
	{
		bool singular = false;
		int i, j, k;
		// Make L and U identity matrices
		for (i = 0; i < (dim*dim); ++i)
			L[i] = U[i] = 0.0;
		for (i = 0; i < (dim*dim); i += (dim+1))
			L[i] = U[i] = 1.0;

		// First row of U is just the first row of [input]
		for (k = 0; k < dim; ++k)
			U[k] = input[k];

		// Set sign to "even"
		sign = 1;
		// Standard permutation
		for (i = 0; i < dim; ++i)
			permutation[i] = i;

		// Now for the second and remaining rows
		i = 1;
		while (i < dim)
		{
#if PARTIAL_PIVOT==1
			// Find biggest absolute pivot in current column to place in
			// current diagonal
			int biggestrow, row;
			biggestrow = row = i;
			real biggest = ABS(input[i*dim+i]);
			while (row < dim)
			{
				if (ABS(input[row*dim+i]) > biggest)
				{
					biggest = ABS(input[row*dim+i]);
					biggestrow = row;
				}
				++row;
			}

			if (i != biggestrow)
			{
				// Change sign to reflect the number of swaps we've done
				// up till now
				sign = -sign;
				// Update the permutation index vector
				permutation[i] = biggestrow;
				permutation[biggestrow] = i;

				// Swap rows
				for (k = 0; k < dim; ++k)
					SWAP(input[biggestrow*dim+k], input[i*dim+k]);
			}
#endif
			j = 0;
			k = i*dim;
			while (j <= (i-1))
			{
				if (U[j*dim+j] == 0.0)
				{
					// We've just found out that the matrix we're decomposing
					// is singular. No worries, just continue what we were doing
					singular = true;
					break;
				}

				L[k+j] = (input[k+j] - LUSUM(dim, L, i, U, j, j)) / U[j*dim+j];
				++j;
			}
			while (j < dim)
			{
				U[k+j] = input[k+j] - LUSUM(dim, L, i, U, j, i);
				++j;
			}

			++i;
		}

		return !singular;
	}

	// Find the determinant of [matrix] using LU factorisation and
	// partial pivoting.
	real Determinant(real* matrix, int dim)
	{
		real* L = new real[dim * dim];
		real* U = new real[dim * dim];
		int* perm = new int[dim];
		int sign;

		if (!LUDecomp(matrix, dim, L, U, perm, sign))
			return 0.0;	// singular

		real det = 1.0;
		// The determinant, det(A) = det(L)det(U) is the product of the diagonal
		// elements of L and U (diagonal elements of L are 1.0), times -1^{num_swaps_performed}
		for (int i = 0; i < dim; ++i)
		{
			det *= (/*L[i*dim+i] **/ U[i*dim+i]);
		}

		delete [] L; delete [] U; delete [] perm;

		return det * (real)sign;
	}

	// Solve the following linear system using LU factorisation, and partial
	// pivoting.
	//
	// [matrix] * x = [vec],	where [matrix] is a real [dim]X[dim] matrix
	// 
	// [out] will contain the answer x upon exit.
	bool LUDecompSolve(real* matrix, int dim, real* vec, real* out)
	{
		real* L = new real[dim*dim];
		real* U = new real[dim*dim];
		real* tmp = new real[dim];
		int* perm = new int[dim];
		int sign;

		if (!LUDecomp(matrix, dim, L, U, perm, sign))
			// Well, the matrix was singular, so depending on whether or not
			// [vec] is in the column space of [matrix], we might find a solution
			// but this function isn't gonna spit it out for ya.
			return false;

		ForwardSubstitute(L, dim, vec, tmp, perm);
		BackSubstitute(U, dim, tmp, out);

		delete [] L;
		delete [] U;
		delete [] tmp;
		delete [] perm;

		return true;
	}


	// Accepts an [dim]x[dim] real matrix, and computes its inverse,
	// using LU decomposition, and partial pivoting.
	//
	// [output] will contain the answer upon exit.
	bool LUDecompInvert(real* input, int dim, real* output)
	{
		int i;
		real* L = new real[dim*dim];
		real* U = new real[dim*dim];
		real* Id = new real[dim*dim];
		real* tmp = new real[dim];
		int* perm = new int[dim];
		int sign;

		// Make Id into the identity matrix
		for (i = 0; i < (dim*dim); ++i)
			Id[i] = 0.0;
		for (i = 0; i < (dim*dim); i += (dim+1))
			Id[i] = U[i] = 1.0;

		if (!LUDecomp(input, dim, L, U, perm, sign))
			// Well, the matrix was singular, so its inverse does not exist.
			return false;

		for (i = 0; i < dim; ++i)
		{
			ForwardSubstitute(L, dim, &Id[i*dim], tmp, perm);
			BackSubstitute(U, dim, tmp, &output[i*dim]);
		}

		Transpose(output, dim);

		delete [] L;
		delete [] U;
		delete [] Id;
		delete [] tmp;
		delete [] perm;

		return true;
	}


	// Accepts an [dim]x[dim] real matrix, and computes its inverse,
	// using Gauss-Jordan elimination, and partial pivoting.
	//
	// NOTE:
	// [input] and [output] will be overwritten, and [output] will
	// contain the answer upon exit.
	bool GaussJInvert(real* input, int dim, real* output)
	{
//		ASSERT(dim > 0);

		int i, j, k;
		// Make output the identity matrix
		for (i = 0; i < (dim*dim); ++i)
			output[i] = 0.0;
		for (i = 0; i < (dim*dim); i += (dim+1))
			output[i] = 1.0;

		i = 0; j = 0;
		int diag, row, biggestrow;
		for (i = 0; i < dim; ++i, ++j)
		{
			diag = (i*dim)+j;

#if (PARTIAL_PIVOT == 1)
			real biggest = ABS(input[i*dim + j]);;
			// Search for the biggest absolute value
			// in the current column, starting from the
			// current row.
			row = i, biggestrow = i;
			while (row < dim)
			{
				if (ABS(input[row*dim + j]) > biggest)
				{
					biggest = ABS(input[row*dim + j]);
					biggestrow = row;
				}
				++row;
			}

			if (biggest == 0.0)
				// Matrix is singular: The inverse of [input] does not exist
				return false;

			if (i != biggestrow)
			{
				// Swap rows
				for (k = 0; k < dim; ++k)
				{
					SWAP(input[biggestrow*dim + k], input[i*dim + k]);
					SWAP(output[biggestrow*dim + k], output[i*dim + k]);
				}
			}
#endif

			// Scale current row so that the current diagonal element
			// is 1.0
			real scale = 1.0 / input[diag];
#if (EXPLICIT_ONE_AND_ZERO == 1)
			// Let's explicitly set the column to 1.0
			input[diag] = 1.0;
			for (k = j+1; k < dim; ++k)
				input[i*dim+k] *= scale;
#else
			for (k = j; k < dim; ++k)
				input[i*dim+k] *= scale;
#endif
			for (k = 0; k < dim; ++k)
				output[i*dim+k] *= scale;

			// For all the other rows, apart from the current one, subtract
			// the right amount from it so as to make the current column
			// zeros.
			row = 0;
			while (row < dim)
			{
				if (row == i)
				{
					++row;
					continue;
				}

				scale = input[row*dim+j] / input[i*dim+j];
#if (EXPLICIT_ONE_AND_ZERO == 1)
				// Let's explicitly set the columns to zero
				input[row*dim+j] = 0.0;
				for (k = j+1; k < dim; ++k)
					input[row*dim+k] -= (input[i*dim+k] * scale);
#else
				for (k = j; k < dim; ++k)
					input[row*dim+k] -= (input[i*dim+k] * scale);
#endif
				for (k = 0; k < dim; ++k)
					output[row*dim+k] -= (output[i*dim+k] * scale);


				++row;
			}
		}

		return true;
	}

	// Solve the following linear system using Gauss-Jordan elimination
	// and partial pivoting:
	// [matrix] * x = [vec],	where [matrix] is a real [dim]X[dim] matrix
	// 
	// NOTE:
	// [matrix] and [vec] will be overwritten, and [vec] will contain the
	// answer x upon exit.
	bool GaussJSolve(real* matrix, int dim, real* vec)
	{
//		ASSERT(dim > 0);

		real* input = matrix;
		real* output = vec;

		int i, j, k;
		i = 0; j = 0;
		int diag, row, biggestrow;
		for (i = 0; i < dim; ++i, ++j)
		{
			diag = (i*dim)+j;

#if (PARTIAL_PIVOT == 1)
			real biggest = ABS(input[i*dim + j]);
			// Search for the biggest absolute value
			// in the current column, starting from the
			// current row, and working downwards.
			row = i, biggestrow = i;
			while (row < dim)
			{
				if (ABS(input[row*dim + j]) > biggest)
				{
					biggest = ABS(input[row*dim + j]);
					biggestrow = row;
				}
				++row;
			}

			if (biggest == 0.0)
				// Matrix is singular, the inverse does not exist, and therefore
				// there is no unique solution for x. There may be infinitely many
				// but this routine is not going to give them to ya..
				return false;

			if (i != biggestrow)
			{
				// Swap rows
				for (k = 0; k < dim; ++k)
				{
					SWAP(input[biggestrow*dim + k], input[i*dim + k]);
				}
				SWAP(output[biggestrow], output[i]);
			}
#endif

			// Scale current row so that the current diagonal element
			// is 1.0
			real scale = 1.0 / input[diag];
#if (EXPLICIT_ONE_AND_ZERO == 1)
			// Let's explicitly set the column to 1.0
			input[diag] = 1.0;
			for (k = j+1; k < dim; ++k)
				input[i*dim+k] *= scale;
#else
			for (k = j; k < dim; ++k)
				input[i*dim+k] *= scale;
#endif
			output[i] *= scale;

			// For all the other rows, apart from the current one, subtract
			// the right amount from it so as to make the current column
			// zeros.
			row = 0;
			while (row < dim)
			{
				if (row == i)
				{
					++row;
					continue;
				}

				scale = input[row*dim+j] / input[i*dim+j];
#if (EXPLICIT_ONE_AND_ZERO == 1)
				// Let's explicitly set the columns to zero
				input[row*dim+j] = 0.0;
				for (k = j+1; k < dim; ++k)
					input[row*dim+k] -= (input[i*dim+k] * scale);
#else
				for (k = j; k < dim; ++k)
					input[row*dim+k] -= (input[i*dim+k] * scale);
#endif
				output[row] -= (output[i] * scale);


				++row;
			}
		}

		return true;
	}
}

//
// End.