#include "MpUtil.h"
#include "f2c.h"
#include "CBlas.h"
#include "CLapack.h"

#include "nr3.h"
#include "gaussj.h"
#include "ludcmp.h"
#include "cholesky.h"

int main()
{
	Print("Begin");

	Timer tm;

	int n = 500;		// Size of Matrix
	int MaxIter = 1;	// Number of Replications
	long RandomSeed = -239047234;

	Timer tm0;
	Timer tm1;
	Timer tm2;
	Timer tm3;
	Timer tm4;
	Timer tm5;
	Timer tm6;
	Timer tm7;
	Timer tm8;

	for(int Iter=1; Iter<=MaxIter; Iter++)
	{

		/* Generate Random Matrices */

			tm0.Start();

			SymmetricMatrix<double> A = RandomNormalSymmetricMatrixSpd(n,RandomSeed);

			tm0.Stop();

		/* Invert General Matrix Using LAPACK LU Decomposition */

			tm1.Name = "Time LAPACK LU Inverse";
			Matrix<double> A1(n,n);
			Copy(A1,A);
			Vector<int> Piv1(n);

			tm1.Start();

			/* Preform LU Decomposition */
			int Info1;
			dgetrf_(&n,&n,A1.Begin(),&n,Piv1.Begin(),&Info1);

			/* Check for Singularity */
			if(Info1)
			{
		   		Print("Print: LAPACK ERR1");
		   		PauseExecution();
		   	}

		   	/* Determine Block Size for Inverse */
			int c__1 = 1;
			int c_n1 = -1;
			char uplo = 'u';
			int nb = ilaenv_(&c__1,"DGETRI",&uplo,&n,&c_n1,&c_n1,&c_n1,(ftnlen)6,(ftnlen)1);
			Print("nb",nb);

			/* Compute the Inverse */
			int Info1_2;
			int nWork1 = n * nb;
			Vector<double> Work1(nWork1);
		   	dgetri_(&n,A1.Begin(),&n,Piv1.Begin(),Work1.Begin(),&nWork1,&Info1_2);
		   	if(Info1_2)
		   	{
		   		Print("Error in LUDecompObj::Inverse");
		   		Print("Matrix is Singular");
		   		Print("Info1_2",Info1_2);
		   		HaltExecution();
		   	}

		   	tm1.Stop();


		/* Invert Symmetric Matrix Using LAPACK BK Decomposition */

			tm2.Name = "Time LAPACK BK Inverse";
			SymmetricMatrixLU<double> A2(n,n);
			Copy(A2,A);
			Vector<int> Piv2(n);

			tm2.Start();

			/* Preform BK Decomposition */
		   	uplo = 'u';

			/* Preform Decomposition */
			int Info2;
			int nb2 = ilaenv_(&c__1,"DSYTRF",&uplo,&n,&c_n1,&c_n1,&c_n1,(ftnlen)6,(ftnlen)1);
			int LenWork2 = n * nb2;
			Vector<double> Work2(LenWork2);
			dsytrf_(&uplo,&n,A2.Begin(),&n,Piv2.Begin(),Work2.Begin(),&LenWork2,&Info2);

			/* Check for Singularity */
			if(Info2)
			{
		   		Print("Print: LAPACK ERR1");
		   		PauseExecution();
		   	}

			/* Compute the Inverse */
			int Info2_2;
			int nWork2 = n;
			Vector<double> Work2_2(nWork2);
		   	dsytri_(&uplo,&n,A2.Begin(),&n,Piv2.Begin(),Work2_2.Begin(),&Info2_2);
		   	if(Info2_2)
		   	{
		   		Print("Error in LUDecompObj::Inverse");
		   		Print("Matrix is Singular");
		   		Print("Info2_2",Info2_2);
		   		HaltExecution();
		   	}

		   	tm2.Stop();

		/* Invert Symmetric Matrix Using LAPACK Cholesky Decomposition */

			tm3.Name = "Time LAPACK Cholesky Decomposition";
			SymmetricMatrixLU<double> A3(n,n);
			Copy(A3,A);
			Vector<int> Piv3(n);

			tm3.Start();

			/* Preform Cholesky Decomposition */
			int Info3;
			dpotrf_(&uplo,&n,A3.Begin(),&n,&Info3);

			/* Check for Singularity */
			if(Info3)
			{
		   		Print("Print: LAPACK ERR1");
		   		PauseExecution();
		   	}

			/* Compute the Inverse */
			int Info3_2;
			dpotri_(&uplo,&n,A3.Begin(),&n,&Info3_2);

		   	if(Info3_2)
		   	{
		   		Print("Error in LUDecompObj::Inverse");
		   		Print("Matrix is Singular");
		   		Print("Info3_2",Info3_2);
		   		HaltExecution();
		   	}

		   	tm3.Stop();


		/* Inverse using NR in C++, Guass-Jordan Elimination */

			tm4.Name = "Time NR in C++ GJ Inverse";
			MatDoub_IO A4(n,n);
			for(int i=0; i<n; i++) for(int j=0; j<n; j++) A4[i][j] = A(i,j);

			tm4.Start();

			gaussj(A4);

			Matrix<double> A4b(n,n);
			for(int i=0; i<n; i++) for(int j=0; j<n; j++) A4b(i,j) = A4[i][j];

			tm4.Stop();

		/* Inverse using NR in C++, LU Decomposition */

			tm5.Name = "Time NR in C++ LU Inverse";
			MatDoub_IO A5(n,n);
			for(int i=0; i<n; i++) for(int j=0; j<n; j++) A5[i][j] = A(i,j);
			MatDoub_IO A5_2(n,n);
			for(int i=0; i<n; i++) for(int j=0; j<n; j++) A5_2[i][j] = A(i,j);

			tm5.Start();

			LUdcmp lu5(A5);
			lu5.inverse(A5_2);

			Matrix<double> A5b(n,n);
			for(int i=0; i<n; i++) for(int j=0; j<n; j++) A5b(i,j) = A5_2[i][j];

			tm5.Stop();

		/* Inverse using NR in C++, Cholesky Decomposition */

			tm6.Name = "Time NR in C++ Cholesky Inverse";
			MatDoub_IO A6(n,n);
			for(int i=0; i<n; i++) for(int j=0; j<n; j++) A6[i][j] = A(i,j);
			MatDoub_IO A6_2(n,n);
			for(int i=0; i<n; i++) for(int j=0; j<n; j++) A6_2[i][j] = A(i,j);

			tm6.Start();

			Cholesky lu6(A6);
			lu6.inverse(A6_2);

			Matrix<double> A6b(n,n);
			for(int i=0; i<n; i++) for(int j=0; j<n; j++) A6b(i,j) = A6_2[i][j];

			tm6.Stop();

		/* My LU Routine */

			tm7.Name = "My LU Routine";
			tm7.Start();

			LUDecompObj<double> lu(A);
			Matrix<double> A7 = lu.Inverse();

			tm7.Stop();

		/* My Cholesky Routine */

			tm8.Name = "My Cholesky Routine";
			tm8.Start();

			CholeskyDecompObj<double> cd(A);
			SymmetricMatrixLU<double> A8 = cd.Inverse();

			tm8.Stop();

		/* Make Sure Everything Worked */

		Print("NormLInf(A1 - A2)",NormLInf(A1 - A2));
		Print("NormLInf(A1 - A3)",NormLInf(A1 - A3));
		Print("NormLInf(A1 - A4b)",NormLInf(A1 - A4b));
		Print("NormLInf(A1 - A5b)",NormLInf(A1 - A5b));
		Print("NormLInf(A1 - A6b)",NormLInf(A1 - A6b));
		Print("NormLInf(A1 - A7)",NormLInf(A1 - A7));
		Print("NormLInf(A1 - A8)",NormLInf(A1 - A8));

	}


	Print("End");
	return 0;
}