// Coperight (C) Edward Rosten 2009, 2011

//All rights reserved.
//
//Redistribution and use in source and binary forms, with or without
//modification, are permitted provided that the following conditions
//are met:
//1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//2. Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer in the
//   documentation and/or other materials provided with the distribution.
//
//THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND OTHER CONTRIBUTORS ``AS IS''
//AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
//IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
//ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR OTHER CONTRIBUTORS BE
//LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
//CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
//SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
//INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
//CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
//ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
//POSSIBILITY OF SUCH DAMAGE.

#ifndef TOON_INCLUDE_DETERMINANT_H
#define TOON_INCLUDE_DETERMINANT_H
#include <TooN/TooN.h>
#include <cstdlib>
#include <utility>
#ifdef TOON_DETERMINANT_LAPACK
	#include <TooN/LU.h>
#endif

namespace TooN
{
	namespace Internal
	{
		///@internal
		///@brief  Provides the static size for a square matrix. 
		///In
		///the general case, if R != C, then the matrix is not
		///square and so no size is provided. A compile error results.
		///@ingroup gInternal
		template<int R, int C> struct Square
		{
		};
		

		///@internal
		///@brief Provides the static size for a square matrix where both dimensions are the same.
		///@ingroup gInternal
		template<int R> struct Square<R, R>
		{
			static const int Size = R; ///<The size
		};
		
		///@internal
		///@brief Provides the static size for a square matrix where one dimension is static. 
		///The size must be equal to the size of the static dimension.
		///@ingroup gInternal
		template<int R> struct Square<R, Dynamic>
		{
			static const int Size = R; ///<The size
		};
		///@internal
		///@brief Provides the static size for a square matrix where one dimension is static. 
		///The size must be equal to the size of the static dimension.
		///@ingroup gInternal
		template<int C> struct Square<Dynamic, C>
		{
			static const int Size = C; ///<The size
		};
		///@internal
		///@brief Provides the static size for a square matrix where both dimensions are dynamic.
		///The size must be Dynamic.
		///@ingroup gInternal
		template<> struct Square<Dynamic, Dynamic>
		{
			static const int Size = Dynamic; ///<The size
		};
	};


	/** Compute the determinant using Gaussian elimination.
		@param A_ The matrix to find the determinant of.
		@returns determinant.
		@ingroup gLinAlg
	*/
	template<int R, int C, typename Precision, typename Base>
	Precision determinant_gaussian_elimination(const Matrix<R, C, Precision, Base>& A_)
	{
		Matrix<Internal::Square<R,C>::Size, Internal::Square<R,C>::Size,Precision> A = A_;
		TooN::SizeMismatch<R, C>::test(A.num_rows(), A.num_cols());
		using std::swap;
		using std::abs;

		int size=A.num_rows();
		
		//If row operations of the form row_a += alpha * row_b
		//then the determinant is unaffected. However, if a row
		//is scaled, then the determinant is scaled by the same 
		//amount. 
		Precision determinant=1;

		for (int i=0; i<size; ++i) {

			//Find the pivot
			int argmax = i;
			Precision maxval = abs(A[i][i]);
			
			for (int ii=i+1; ii<size; ++ii) {
				Precision v =  abs(A[ii][i]);
				if (v > maxval) {
					maxval = v;
					argmax = ii;
				}
			}
			Precision pivot = A[argmax][i];

			//assert(abs(pivot) > 1e-16);
			
			//Swap the current row with the pivot row if necessary.
			//A row swap negates the determinant.
			if (argmax != i) {
				determinant*=-1;
				for (int j=i; j<size; ++j)
					swap(A[i][j], A[argmax][j]);
			}

			determinant *= A[i][i];

			if(determinant == 0)
				return 0;
			
			for (int u=i+1; u<size; ++u) {
				//Do not multiply out the usual 1/pivot term
				//to avoid division. It causes poor scaling.
				Precision factor = A[u][i]/pivot;

				for (int j=i+1; j<size; ++j)
					A[u][j] = A[u][j] - factor * A[i][j];
			}
		}

		return determinant;
	}
	
	#ifdef TOON_DETERMINANT_LAPACK
		/** Compute the determinant using TooN::LU.
			@param A The matrix to find the determinant of.
			@returns determinant.
			@ingroup gLinAlg
		*/
		template<int R, int C, class P, class B>
		P determinant_LU(const Matrix<R, C, P, B>& A)
		{
			TooN::SizeMismatch<R, C>::test(A.num_rows(), A.num_cols());
			LU<Internal::Square<R,C>::Size, P> lu(A);
			return lu.determinant();
		}
	#endif

	/** 
		Compute the determinant of a matrix using an appropriate method. The
		obvious method is used for 2x2, otherwise
		determinant_gaussian_elimination() or determinant_LU() is used depending
		on the value of \c TOON_DETERMINANT_LAPACK.  See also \ref sConfigLapack.
		@param A The matrix to find the determinant of.
		@returns determinant.
		@ingroup gLinAlg
	*/
	template<int R, int C, class P, class B>
	P determinant(const Matrix<R, C, P, B>& A)
	{
		TooN::SizeMismatch<R, C>::test(A.num_rows(), A.num_cols());
		if(A.num_rows() == 2)
			return A[0][0]*A[1][1] - A[1][0]*A[0][1];
		#if defined TOON_DETERMINANT_LAPACK && TOON_DETERMINANT_LAPACK != -1
			else if(A.num_rows() >= TOON_DETERMINANT_LAPACK)
				return determinant_LU(A);
		#endif
		else
			return determinant_gaussian_elimination(A);
	}
}

#endif
