/* this library contains functions for matrix algebra*/
/* all vectors and arrays in this library are assumed to be*/
/* unit offset, i.e. v[1..n] and map[1..n][1..n]*/

#include <math.h>
#include <stdlib.h>
#include "malgbr.h"
#include "numcip.h"
#define SWAP(a,b) {double temp=(a); double temp2=(b); (a)=temp2; (b)=temp;}

double dpythag(double a, double b);

/* dot: dot computes the dotproduct between two vectors */
double dot(double *thisv, double *thatv,int vdim)
{
	int i;
	double temp;

	temp = 0;
	for (i=1;i<= vdim; i++)
		temp += thisv[i]*thatv[i];
	return temp;
}

/*norm: norm computes the norm of a vector*/
double norm(double *thisv,int vdim)
{
	int i;
	double temp;

	temp = 0;
	for (i=1; i<= vdim; i++)
		temp += thisv[i]*thisv[i];
	return sqrt(temp);
}

/*newvec: newvec normalizes a given vector to unit size*/
void newvec(double *thisvec, double normm,int vdim)
{
	int i;

	for(i=1;i<=vdim; i++)
		thisvec[i]/=normm;
}

/*det: det computes the determint of a given nxn matrix*/
double det(double **matrix, int mdim)
{
	int i,j,k,l;
	double sign;
	double temp;
	
	double **submat;
	extern double **dmatrix(int,int,int,int);
	extern void free_dmatrix(double **,int,int,int,int);

	sign =1.0;
	if (mdim > 2)
		{
		submat=dmatrix(1,mdim-1,1,mdim-1);
		temp = 0;
		for (i=1; i<= mdim; i++)
			{
			if (fabs(matrix[i][1]) >1.0e-25)
				{
				for(k=2; k <= mdim; k++)
					{
					for (l=1; l<=i-1; l++)
						submat[l][k-1]=matrix[l][k];
					for (l=i+1; l<= mdim; l++)
						submat[l-1][k-1]=matrix[l][k];
					}
				temp += sign * matrix[i][1] * det(submat,mdim-1);
				}
			sign = -sign;
			}
		free_dmatrix(submat,1,mdim-1,1,mdim-1);
		return temp;
		}
	else
		return matrix[1][1]*matrix[2][2] - matrix[1][2]*matrix[2][1];
}

/*det_f: det_f computes the determint of a given nxn matrix in float*/
float det_f(float **matrix, int mdim)
{
	int i,j,k,l;
	float sign;
	float temp;
	
	float **submat;

	sign =1.0;
	if (mdim > 2)
		{
		submat=n2matrix(1,mdim-1,1,mdim-1);
		temp = 0;
		for (i=1; i<= mdim; i++)
			{
			if (fabs(matrix[i][1]) >1.0e-25)
				{
				for(k=2; k <= mdim; k++)
					{
					for (l=1; l<=i-1; l++)
						submat[l][k-1]=matrix[l][k];
					for (l=i+1; l<= mdim; l++)
						submat[l-1][k-1]=matrix[l][k];
					}
				temp += sign * matrix[i][1] * det_f(submat,mdim-1);
				}
			sign = -sign;
			}
		free_n2matrix(submat,1,mdim-1,1,mdim-1);
		return temp;
		}
	else
		return matrix[1][1]*matrix[2][2] - matrix[1][2]*matrix[2][1];
}

/*matrixpro: matrixpro computes the product of two matrices*/
void matrixpro(double **product, double **left, double **right, int mdim)
{
	int i,j,k;
	double temp;

	for (i=1; i <= mdim; i++)
		for (j=1; j <= mdim; j++)
			{
			temp = 0;
			for (k=1; k <= mdim; k++)
				temp += left[i][k]*right[k][j];
			product[i][j]=temp;
			}

}

/*comvec: comvec orthogonalizes a vector with respect to a given subspace*/
void comvec(double *thisvec, double **thatbas, int k,int vdim)
{
	int i,j;
	double *coef;
	extern double *dvector(int,int);
	extern void free_dvector(double *,int,int);

	coef=dvector(1,k);
	for (i=1; i<= k; i++)
		coef[i]= dot(thisvec,thatbas[i],vdim);
	for (i=1; i<= k; i++)
		for (j=1; j <= vdim; j++)
			thisvec[j]-= coef[i]*thatbas[i][j];
	free_dvector(coef,1,k);
}

/*GramSch: GramSchmidt orthogonalizes a given set ob basis*/
void GramSch(double **mybas, double *beta,int dir,int vdim)
{
	int i;

	beta[1]= norm(mybas[1],vdim);
	newvec(mybas[1],beta[1],vdim);
	for (i=2; i <= dir; i++)
		{
		comvec(mybas[i],mybas,i-1,vdim);
		beta[i]=norm(mybas[i],vdim);
		newvec(mybas[i],beta[i],vdim);
		}

}

/*matInv: matInv uses Gauss Jordan elimination to find the inverse of a given matrix*/
void matInv(double **a, int n)
{
	int *indxc,*indxr,*ipiv;
	int i,icol,irow,j,k,l,ll;
	double big, dum,dum1,pivinv;
	
	indxc=ivector(1,n);
	indxr=ivector(1,n);
	ipiv=ivector(1,n);
	for (j=1; j<=n;j++) ipiv[j]=0;
	for (i=1; i<=n;i++)
		{
		big=0.0;
		for (j=1; j<=n;j++)
			if (ipiv[j] != 1)
				for (k=1;k<=n;k++)
					{
					if (ipiv[k] == 0)
						{
						if (fabs(a[j][k]) >= big)
							{
							big=fabs(a[j][k]);
							irow=j;
							icol=k;
							}
						}
					else if (ipiv[k] >1) nrerror("matInv: Singular Matrix-1");
					}
		++(ipiv[icol]);
		if (irow != icol)
			{
			for (l=1; l<=n; l++)
				SWAP(a[irow][l],a[icol][l]);
			
			}
		indxr[i]=irow;
		indxc[i]=icol;
		if (a[icol][icol] == 0.0)
			nrerror("matInv: Singular Matrix-2");
		pivinv=1.0/a[icol][icol];
		a[icol][icol]=1.0;
		for (l=1;l<=n;l++)
			a[icol][l] *= pivinv;
		for (ll=1;ll<=n;ll++)
			if (ll != icol)
				{
				dum=a[ll][icol];
				a[ll][icol]=0.0;
				for (l=1;l<=n;l++)
					a[ll][l] -= a[icol][l]*dum;
				}
		}
	for (l=n;l>=1;l--)
		if (indxr[l] != indxc[l])
			{
			for (k=1;k<=n;k++)
				SWAP(a[k][indxr[l]],a[k][indxc[l]]);
			}
	free_ivector(ipiv,1,n);
	free_ivector(indxr,1,n);
	free_ivector(indxc,1,n);
}

/*eqvector assigns the value of one vector to another*/
void eqvector(double *take, double *give, int n)
{
	int i;
	
	for (i=1; i<=n; i++)
		take[i]=give[i];
}

/*eqmatrix assigns the value of one matrix to another*/
void eqmatrix(double **take, double **give, int rl,int cl)
{
	int i,j;
	
	for (i=1; i<=rl; i++)
		for (j=1; j<=cl; j++)
			take[i][j] = give[i][j];
}

/*transpost */
void transpost(double **matrix, int n)
{
	int i,j;
	
	for (i=1; i<=n; i++)
		for (j=1; j<=n; j++)
			if (i != j)
				SWAP(matrix[i][j],matrix[j][i]);
}

/*delta computes the distance between two vectors*/
double delta(double *thisvec, double *thatvec, int n)
{
	int i;
	double temp, diff;
	
	temp=0.0;
	for (i=1; i<=n; i++)
		{
		diff=thisvec[i]-thatvec[i];
		temp+=diff*diff;
		}
		
	return sqrt(temp);
}


/*ludcmp and lubsksb combined solve a given linear matrix equation: Ax=B
  ludcmp proforms a lu decomposition on the matrix A and lubsksb backsubstitutes
  it back to solve for the vector x*/
  
void ludcmp(double **a, int n, int *indx, double *d)
{
	int i,imax,j,k;
	double big,dum,sum,temp;
	double *vv;
	
	vv=dvector(1,n);
	*d=1.0;
	for (i=1;i<=n;i++)
		{
		big=0.0;
		for (j=1;j<=n;j++)
			if((temp=fabs(a[i][j]))> big)
				big=temp;
		if (big==0.0)
			nrerror("Singular matrix in routine ludcmp");
		vv[i]=1.0/big;
		} 
	for(j=1;j<=n;j++)
		{
		for(i=1;i<j;i++)
			{
			sum=a[i][j];
			for (k=1;k<i;k++)
				sum -= a[i][k]*a[k][j];
			a[i][j]=sum;
			}
		big=0.0;;
		for(i=j;i<=n;i++)
			{
			sum=a[i][j];
			for(k=1;k<j;k++)
				sum -=a[i][k]*a[k][j];
			a[i][j]=sum;
			if((dum=vv[i]*fabs(sum)) >=big)
				{
				big=dum;
				imax=i;
				}
			}
		if (j != imax)
			{
			for (k=1;k<=n;k++)
				{
				dum=a[imax][k];
				a[imax][k]=a[j][k];
				a[j][k]=dum;
				}
			*d = -(*d);
			vv[imax]=vv[j];
			}
		indx[j]=imax;
		if(a[j][j] == 0.0)
			a[j][j] = 1.0e-20;
		if (j != n)
			{
			dum = 1.0/(a[j][j]);
			for(i=j+1;i<=n;i++)
				a[i][j] *=dum;
			}
		}
	free_dvector(vv,1,n);
}

void lubksb(double **a, int n, int *indx, double b[])
{
	int i, ii=0,ip,j;
	double sum;
	
	for (i=1;i<=n;i++)
		{
		ip=indx[i];
		sum=b[ip];
		b[ip]=b[i];
		if (ii)
			for (j=ii;j<=i-1;j++)
				sum -=a[i][j]*b[j];
		else if (sum)
			ii=i;
		b[i]=sum;
		}
	for(i=n;i>=1;i--)
		{
		sum=b[i];
		for (j=i+1;j<=n;j++)
			sum -=a[i][j]*b[j];
		b[i]=sum/a[i][i];
		}
}

void vec_matrix_pro(double *result,double **matrix,double *vector,int dim)
{
	int i,j;
	double temp;
	
	for(i=1;i<=dim;i++)
		{
		temp=0.0;
		for(j=1;j<=dim;j++)
			temp += matrix[i][j]*vector[j];
		result[i] = temp;
		}
}

void vec_matrix_pro_f(float *result,float **matrix,float *vector,int dim)
{
	int i,j;
	float temp;
	
	for(i=1;i<=dim;i++)
		{
		temp=0.0;
		for(j=1;j<=dim;j++)
			temp += matrix[i][j]*vector[j];
		result[i] = temp;
		}
}

void gaussj(double **a,int n,double **b,int m)
{
	int *indxc,*indxr,*ipiv;
	int i,icol,irow,j,k,l,ll;
	double big,dum,pivinv,temp;
	
	indxc=ivector(1,n);
	indxr=ivector(1,n);
	ipiv=ivector(1,n);
	for (j=1;j<=n;j++)
		ipiv[j]=0;
	for (i=1;i<=n;i++)
		{
		big=0.0;
		for (j=1;j<=n;j++)
			if (ipiv[j] != 1)
				for (k=1;k<=n;k++)
					{
					if (ipiv[k] == 0)
						{
						if (fabs(a[j][k]) >= big)
							{
							big=fabs(a[j][k]);
							irow=j;
							icol=k;
							}
						}
					else if (ipiv[k] >1)
						nrerror("gaussj: Singular Matrix-1");
					}
		++(ipiv[icol]);
		if (irow != icol)
			{
			for (l=1;l<=n;l++)
				SWAP(a[irow][l],a[icol][l]);
			for (l=1;l<=m;l++)
				SWAP(b[irow][l],b[icol][l]);
			}
		indxr[i]=irow;
		indxc[i]=icol;
		if (a[icol][icol] == 0.0)
			nrerror("gaussj: Singular Matrix-2");
		pivinv=1.0/a[icol][icol];
		a[icol][icol]=1.0;
		for (l=1;l<=n;l++)
			a[icol][l] *= pivinv;
		for (l=1;l<=m;l++)
			b[icol][l] *= pivinv;
		for (ll=1;ll<=n;ll++)
			if (ll != icol)
				{
				dum=a[ll][icol];
				a[ll][icol]=0.0;
				for (l=1;l<=n;l++)
					a[ll][l] -= a[icol][l]*dum;
				for (l=1;l<=m;l++)
					b[ll][l] -= b[icol][l]*dum;
				}
		}
	for (l=n;l>=1;l--)
		{
		if (indxr[l] != indxc[l])
			for (k=1;k<=n;k++)
				SWAP(a[k][indxr[l]],a[k][indxc[l]]);
		}
	free_ivector(ipiv,1,n);
	free_ivector(indxr,1,n);
	free_ivector(indxc,1,n);
}


void dsvbksb(double **u, double w[], double **v, int m, int n, double b[], double x[])
{
	int jj,j,i;
	double s,*tmp;

	tmp=dvector(1,n);
	for (j=1;j<=n;j++) {
		s=0.0;
		if (w[j]) {
			for (i=1;i<=m;i++) s += u[i][j]*b[i];
			s /= w[j];
		}
		tmp[j]=s;
	}
	for (j=1;j<=n;j++) {
		s=0.0;
		for (jj=1;jj<=n;jj++) s += v[j][jj]*tmp[jj];
		x[j]=s;
	}
	free_dvector(tmp,1,n);
}

void dsvdcmp(double **a, int m, int n, double w[], double **v)
{
	double dpythag(double a, double b);
	int flag,i,its,j,jj,k,l,nm;
	double anorm,c,f,g,h,s,scale,x,y,z,*rv1;

	rv1=dvector(1,n);
	g=scale=anorm=0.0;
	for (i=1;i<=n;i++) {
		l=i+1;
		rv1[i]=scale*g;
		g=s=scale=0.0;
		if (i <= m) {
			for (k=i;k<=m;k++) scale += fabs(a[k][i]);
			if (scale) {
				for (k=i;k<=m;k++) {
					a[k][i] /= scale;
					s += a[k][i]*a[k][i];
				}
				f=a[i][i];
				g = -SIGN(sqrt(s),f);
				h=f*g-s;
				a[i][i]=f-g;
				for (j=l;j<=n;j++) {
					for (s=0.0,k=i;k<=m;k++) s += a[k][i]*a[k][j];
					f=s/h;
					for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
				}
				for (k=i;k<=m;k++) a[k][i] *= scale;
			}
		}
		w[i]=scale *g;
		g=s=scale=0.0;
		if (i <= m && i != n) {
			for (k=l;k<=n;k++) scale += fabs(a[i][k]);
			if (scale) {
				for (k=l;k<=n;k++) {
					a[i][k] /= scale;
					s += a[i][k]*a[i][k];
				}
				f=a[i][l];
				g = -SIGN(sqrt(s),f);
				h=f*g-s;
				a[i][l]=f-g;
				for (k=l;k<=n;k++) rv1[k]=a[i][k]/h;
				for (j=l;j<=m;j++) {
					for (s=0.0,k=l;k<=n;k++) s += a[j][k]*a[i][k];
					for (k=l;k<=n;k++) a[j][k] += s*rv1[k];
				}
				for (k=l;k<=n;k++) a[i][k] *= scale;
			}
		}
		anorm=DMAX(anorm,(fabs(w[i])+fabs(rv1[i])));
	}
	for (i=n;i>=1;i--) {
		if (i < n) {
			if (g) {
				for (j=l;j<=n;j++) v[j][i]=(a[i][j]/a[i][l])/g;
				for (j=l;j<=n;j++) {
					for (s=0.0,k=l;k<=n;k++) s += a[i][k]*v[k][j];
					for (k=l;k<=n;k++) v[k][j] += s*v[k][i];
				}
			}
			for (j=l;j<=n;j++) v[i][j]=v[j][i]=0.0;
		}
		v[i][i]=1.0;
		g=rv1[i];
		l=i;
	}
	for (i=IMIN(m,n);i>=1;i--) {
		l=i+1;
		g=w[i];
		for (j=l;j<=n;j++) a[i][j]=0.0;
		if (g) {
			g=1.0/g;
			for (j=l;j<=n;j++) {
				for (s=0.0,k=l;k<=m;k++) s += a[k][i]*a[k][j];
				f=(s/a[i][i])*g;
				for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
			}
			for (j=i;j<=m;j++) a[j][i] *= g;
		} else for (j=i;j<=m;j++) a[j][i]=0.0;
		++a[i][i];
	}
	for (k=n;k>=1;k--) {
		for (its=1;its<=30;its++) {
			flag=1;
			for (l=k;l>=1;l--) {
				nm=l-1;
				if ((double)(fabs(rv1[l])+anorm) == anorm) {
					flag=0;
					break;
				}
				if ((double)(fabs(w[nm])+anorm) == anorm) break;
			}
			if (flag) {
				c=0.0;
				s=1.0;
				for (i=l;i<=k;i++) {
					f=s*rv1[i];
					rv1[i]=c*rv1[i];
					if ((double)(fabs(f)+anorm) == anorm) break;
					g=w[i];
					h=dpythag(f,g);
					w[i]=h;
					h=1.0/h;
					c=g*h;
					s = -f*h;
					for (j=1;j<=m;j++) {
						y=a[j][nm];
						z=a[j][i];
						a[j][nm]=y*c+z*s;
						a[j][i]=z*c-y*s;
					}
				}
			}
			z=w[k];
			if (l == k) {
				if (z < 0.0) {
					w[k] = -z;
					for (j=1;j<=n;j++) v[j][k] = -v[j][k];
				}
				break;
			}
			if (its == 30) nrerror("no convergence in 30 dsvdcmp iterations");
			x=w[l];
			nm=k-1;
			y=w[nm];
			g=rv1[nm];
			h=rv1[k];
			f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
			g=dpythag(f,1.0);
			f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
			c=s=1.0;
			for (j=l;j<=nm;j++) {
				i=j+1;
				g=rv1[i];
				y=w[i];
				h=s*g;
				g=c*g;
				z=dpythag(f,h);
				rv1[j]=z;
				c=f/z;
				s=h/z;
				f=x*c+g*s;
				g = g*c-x*s;
				h=y*s;
				y *= c;
				for (jj=1;jj<=n;jj++) {
					x=v[jj][j];
					z=v[jj][i];
					v[jj][j]=x*c+z*s;
					v[jj][i]=z*c-x*s;
				}
				z=dpythag(f,h);
				w[j]=z;
				if (z) {
					z=1.0/z;
					c=f*z;
					s=h*z;
				}
				f=c*g+s*y;
				x=c*y-s*g;
				for (jj=1;jj<=m;jj++) {
					y=a[jj][j];
					z=a[jj][i];
					a[jj][j]=y*c+z*s;
					a[jj][i]=z*c-y*s;
				}
			}
			rv1[l]=0.0;
			rv1[k]=f;
			w[k]=x;
		}
	}
	free_dvector(rv1,1,n);
}

double dpythag(double a, double b)
{
	double absa,absb;
	absa=fabs(a);
	absb=fabs(b);
	if (absa > absb) return absa*sqrt(1.0+DSQR(absb/absa));
	else return (absb == 0.0 ? 0.0 : absb*sqrt(1.0+DSQR(absa/absb)));
}