#include<stdio.h>
#include<math.h>
#include "arrays.hpp"  /* dynamic memory allocation routines */

int LUfactor1(double **A,int n);
void LUsolve1(double **A,double *b,int n);
int LUfactor(double **A,int *P,int n);
void LUsolve(double **A,int *P,double *b,int n);

main()
{
	double **A;
	double *b;
	int *P;
	int n;
	int i,j,k;

	n=4;
	b=vector(n);	/* allocate matrix/vector memory */
	A=matrix(n,n);

	A[0][0]=1.0; A[0][1]=4.0; A[0][2]=5.0; A[0][3]=6.0;
	A[1][0]=1.0; A[1][1]=1.0; A[1][2]=1.0; A[1][3]=1.0;
	A[2][0]=3.0; A[2][1]=4.0; A[2][2]= -1.0; A[2][3]=4.0;
	A[3][0]=6.0; A[3][1]=4.0; A[3][2]=3.0; A[3][3]=3.0;

	b[0]=1.0;
	b[1]=1.0;
	b[2]=1.0;
	b[3]=1.0;
/*----------------------------factor,solve--------*/
	LUfactor1(A,n);
	LUsolve1(A,b,n);
/*----------------------------print LU,b solution--*/
	for(i=0;i<n;++i)
	{
		for(j=0;j<n;++j)
		    printf("%f\t",A[i][j]);
				printf("%f\n",b[i]);
	}
	printf("\n\n");
	freematrix(A);  /* release allocated memory */
	freevector(b);
/*==================NOW with pivoting....===========*/
	n=3;
	b=vector(n);	/* allocate matrix/vector memory */
	A=matrix(n,n);
	P=ivector(n); /* NEW: integer vector -- row indices */

	A[0][0]=0.0; A[0][1]=1.0; A[0][2]=2.0;
	A[1][0]=3.0; A[1][1]=2.0; A[1][2]=1.0;
	A[2][0]=1.0; A[2][1]=5.0; A[2][2]=7.0;

	b[0]=8.0;
	b[1]=10.0;
	b[2]=32.0;

	LUfactor(A,P,n);
	LUsolve(A,P,b,n);
/*----------------------------print LU,b,P solution--*/
	for(i=0;i<n;++i)
	{
		for(j=0;j<n;++j)
		    printf("%f\t",A[i][j]);
				printf("%f\t%d\n",b[i],P[i]);
	}
	freematrix(A);
	freevector(b);
	freevector(P);
}
/*-------------------------------------------------------------------*/
int LUfactor1(double **A,int n) /* factor A=LU, but no pivoting */
{   
	int i,j,k;
	double Ajj,Lij;
	for(j=0;j<n;++j)     		/* go column by column j=0..n-1 */
	{
		Ajj=A[j][j];
		if(fabs(Ajj)<1e-15)
		{
			printf("ZERO Diagonal element zero -- LU fails\n");
			return(-1); /* error signal ! */
		}
		for(i=j+1;i<n;++i) 		/* zero-out sub-diag rows */
		{
			Lij=A[i][j]/Ajj;	/* calculate L-factors */
			A[i][j]=Lij;		/* store Lij entries */	
			for(k=j+1;k<n;++k)  	/* row subtraction */
				A[i][k]-=Lij*A[j][k];
		}
	}
	return(0); /* no error */
} /* replaces matrix A[i][j] with LU factorization */

void LUsolve1(double **A,double *b,int n) /* A=L\U, LUx=b, Ly=b, Ux=y */
{
	double **L,**U;
	double *x,*y;
	double rowsum;
	int i,j;
	y=x=b; 		/* aliases, alternate names for the same vector */
	L=U=A; 		/* aliases for parts of the A matrix=L\U */
	y[0]=b[0]; 			/* forward substitution Ly=b */
	for(i=1;i<n;++i)
	{
		y[i]=b[i];
		rowsum=0.0;
		for(j=0;j<i;++j)
			rowsum+=L[i][j]*y[j];
		y[i]-=rowsum;
	}
	x[n-1]=y[n-1]/U[n-1][n-1]; 	/* back substitution Ux=y */
	for(i=n-2;i>=0;--i)
	{
		rowsum=0.0;
		for(j=n-1;j>i;--j)
			rowsum+=U[i][j]*x[j];
		x[i]=(y[i]-rowsum)/U[i][i];
	}
}  /* replaces b[i] vector with solution x[i] */
/*-------------------------------------------------------------------*/
int LUfactor(double **A,int *P,int n) /* factor PA=LU WITH PIVOTING */
{   
/* HINT :  A[P[i]][j]...... */
}
void LUsolve(double **A,int *P,double *b,int n)/*PA=L\U,LUx=Pb,Ly=Pb,Ux=y*/
{
/* HINT: b[P[i]].... */
}
/*--------------------------------------------------------------------------*/