#include <math.h>

/* Package contains following functions in addition to function hichol */
int hi_prep(double vd[],unsigned long iv[],unsigned short jv[],double rd[],
            unsigned short ni[],unsigned short ns,unsigned short nm, 
            float v[],float t[],float r[],float s[]);
int hi_chol(float v[],unsigned long iv[],unsigned short jv[],
            unsigned short ir,unsigned short nr,unsigned short mb,
            float t[],unsigned short nm,unsigned short nb);
int cholesky(float t[],unsigned short n);
int hi_solve(float v[],unsigned long iv[],unsigned short jv[],
             unsigned short ni[],unsigned short ns,unsigned short nm,
             float t[],float r[],float s[]);
int hi_res(double vd[],unsigned long iv[],unsigned short jv[],double rd[],
	   double rn[],float s[],float r[],unsigned short nm);


int hichol(float v[],unsigned long iv[],unsigned short jv[],
           unsigned short ni[],unsigned short ns,unsigned short nm, 
           float t[])
        {

        unsigned short i;
        unsigned short nb,ib,nr,mb,np;

        /*
        Performs Cholesky-decomposition of sparse matrices.
        Matrix must be positive definite, symmetric doubly-bordered
        block diagonal. Diagonal blocks can have different sizes.
        Border band may be sparse too, but must consist of blocks with
        vertical extent matching the corresponding diagonal block. 
        The upper triangle of the matrix is stored in row-wise format 
        in v[], except for the upper triangle of the lower right block
        formed by the overlap of both borders, which is stored row-wise
        in t[]. The storage of the former must account for zeros in 
        lower right part; the latter must be filled, i.e. non-sparse.
        The transpose of v[] is vt[]. iv[] indicates position in v[],
        where row i begins, jv[] gives column number of element v[i].
        Dimension of the matrix is nm. There are ns diagonal blocks,
        each with dimension ni[i]. (All arrays [0..N-1].) */


        /* Calculate number of columns in border band of matrix.
        This is the dimension of matrix t[] at the same time. */
        nb=0;
        for(i=1;i<=ns;i++){nb+=ni[i];}
        nb=nm-nb;

        /* Calculate decomposition down to lower right triangle.*/
        ib=1;              /* We start with first block */
        nr=ni[ib];         /* Number of rows in block to operate on */
        mb=iv[2]-iv[1]-nr; /* Number of non-zero columns in border block */
        np=nm-nb;
        for(i=1;i<=np;i++){
                hi_chol(v,iv,jv,i,nr,mb,t,nm,nb); 
                nr-=1;
                if(nr==0){
                        ib+=1;
                        nr=ni[ib];
                        mb=iv[i+2]-iv[i+1]-nr;
                        }
                }

        /* Calculate decomposition of lower right triangle */
        cholesky(t,nb);          

        return 0;
        }

int hi_prep(double vd[],unsigned long iv[],unsigned short jv[],double rd[],
            unsigned short ni[],unsigned short ns,unsigned short nm, 
            float v[],float t[],float r[],float s[])
	{

	unsigned short nb,np;
	unsigned long i,j,ii,jj;
	unsigned long nv;

	/*
	Preparation for HICHOL. Copy vd into v except for lower
	right triangle, which is expanded and copied into t.
	Set r and initialize s. */

        nb=0;
        for(i=1;i<=ns;i++){nb+=ni[i];}
        nb=nm-nb;  /* Number of border columns */
        np=nm-nb;  /* Number of columns in block diagonal part */

	/* Fill in v */
        for(i=1;i<iv[np+1];i++){v[i]=vd[i];}

        /* Prepare matrix t */
	nv=(unsigned long)nb*(nb+1)/2;
        for(i=1;i<nv+1;i++){t[i]=0.0;}
        for(i=np+1;i<nm+1;i++){
                ii=i-np;
                for(j=iv[i];j<iv[i+1];j++){
                        jj=jv[j]-np;
                        t[(ii-1)*nb-(ii-1)*(ii-2)/2+1+jj-ii]=vd[j];
                        }
                }

	/* Copy right hand side to r and initalize s */
	for(i=1;i<=nm;i++){
		r[i]= -rd[i];
		s[i]= 0;
		}

	return 0;
	}

int hi_chol(float v[],unsigned long iv[],unsigned short jv[],
            unsigned short ir,unsigned short nr,unsigned short mb,
            float t[],unsigned short nm,unsigned short nb)
        {

        float v1;
        unsigned long i,j,ii,jj,jjmax,i1,i2;

        /* Work on current row */
        v[iv[ir]]=sqrt(v[iv[ir]]);
	v1=1.0/v[iv[ir]];
        for(i=iv[ir]+1;i<=iv[ir+1]-1;i++){v[i]*=v1;}

        /* Work on remaining rows of current block */
        jj=ii=iv[ir]+1;
        jjmax=iv[ir+1];
        for(i=iv[ir+1];i<=iv[ir+nr]-1;i++){
                v[i]-=v[ii]*v[jj];
                if((jj+=1)==jjmax){jj=++ii;}
                }

        /* Work on rows in lower right triangle */
        i2=iv[ir+1]-1;
        i1=i2-mb+1;
        for(i=i1;i<=i2;i++){
                ii=jv[i]-(nm-nb);         
                for(j=i;j<=i2;j++){
                        jj=jv[j]-(nm-nb); 
                        t[(ii-1)*nb-(ii-1)*(ii-2)/2+1+jj-ii]-=v[i]*v[j];
                        }
                }   
        return 0;
        }

int cholesky(float t[],unsigned short n)
        {
        float t1;
        unsigned long ii,jj,i,j,k,l;

        ii=1;
        jj=(unsigned long)n*(n+1)/2;
        for(i=n;i>0;i--){
                t[ii]=sqrt(t[ii]);
		t1=1.0/t[ii];
                for(j=ii+1;j<ii+i;j++){t[j]*=t1;}
                k=l=ii+1;
                for(j=ii+i;j<=jj;j++){
                        t[j]-=t[k]*t[l];
                        if(++l==ii+i){l=++k;}
                        }
                ii+=i;
                }
        return 0;
        }

int hi_solve(float v[],unsigned long iv[],unsigned short jv[],
             unsigned short ni[],unsigned short ns,unsigned short nm,
             float t[],float r[],float s[])
        {

        /* Given the Cholesky decomposition of a matrix as returned
        by HICHOL, solve for right hand side r[] by forward substitution
        and backsubstitution. Right hand side is overwritten with
	solution, which is subtracted from s for improved solution.*/

        float sum;
	unsigned short nb,np;
        unsigned long ii,jj,i,j,k,l;

        nb=0;
        for(i=1;i<=ns;i++){nb+=ni[i];}
        nb=nm-nb;
	np=nm-nb;

        /* Skip leading zeros in right hand side */
        for(ii=1;r[ii]==0;ii++);

        /* Forward substitution in sparse part...*/
        if(ii<=np){
                for(i=ii;i<=np;i++){
                        r[i]/=v[iv[i]];
                        for(j=iv[i]+1;j<iv[i+1];j++){
                                r[jv[j]]-=v[j]*r[i];
                                }
                        }
                }
        /* ...and in triangle */
        if(ii<np+1){ii=np+1;}
        ii-=np;
        for(i=ii;i<=nb;i++){
                r[i+np]/=t[(i-1)*nb-(i-1)*(i-2)/2+1];
                for(j=i+1;j<=nb;j++){
                        r[j+np]-=t[(i-1)*nb-(i-1)*(i-2)/2+1+j-i]*r[i+np];
                        }
                }

        /* Back substitution in sparse part...*/
        for(i=nm;i>np;i--){
                sum=r[i];
                k=i-np;
                for(j=i+1;j<=nm;j++){
                        l=j-np;
                        sum-=t[(k-1)*nb-(k-1)*(k-2)/2+1+l-k]*r[j];
                        }
                r[i]=sum/t[(k-1)*nb-(k-1)*(k-2)/2+1];
                }
        /* ...and in triangle */
        for(i=np;i>0;i--){
                sum=r[i];
                for(j=iv[i]+1;j<=iv[i+1]-1;j++){
                        sum-=v[j]*r[jv[j]];
                        }
                r[i]=sum/v[iv[i]];
                }

        for(i=1;i<=nm;i++){s[i]-=r[i];} 

        return 0;
        }

int hi_res(double vd[],unsigned long iv[],unsigned short jv[],
           double rd[],double rn[],float s[],float r[],unsigned short nm)
	{

	double sum;
	unsigned short i;
	unsigned long j;

	/*
	Calculate residual of vd*s from right hand side rd and
	prepare new right hand side r for calculation of improved
	solution with hi_solve. */ 

        for(i=1;i<=nm;i++){
                sum=-rd[i];
                for(j=iv[i];j<iv[i+1];j++){sum+=vd[j]*s[jv[j]];}
                rn[i]=sum;
                }
        for(i=1;i<=nm;i++){
                for(j=iv[i]+1;j<iv[i+1];j++){rn[jv[j]]+=vd[j]*s[i];}
                r[i]=rn[i];
                }
	return 0;
	}

int hi_pow(double vd[],unsigned long iv[],unsigned short jv[],
           double u[],double w[],double x[],unsigned short nm)
	{

	double sum,ev,tiny;
	unsigned short i,n;
	unsigned long j;

	tiny=1.0e-8;
	sum=0.0;
	n=0;

	for(i=1;i<=nm;i++){w[i]=u[i];}
	do{
		ev=sum;
        	for(i=1;i<=nm;i++){
               		 sum=0.0;
               		 for(j=iv[i];j<iv[i+1];j++){sum+=vd[j]*w[jv[j]];}
               		 x[i]=sum;
               		 }
        	for(i=1;i<=nm;i++){
                	 for(j=iv[i]+1;j<iv[i+1];j++){x[jv[j]]+=vd[j]*w[i];}
               		 }
		sum=0.0;
		for(i=1;i<=nm;i++){sum+=x[i]*x[i];}
		sum=sqrt(sum);
		for(i=1;i<=nm;i++){w[i]=x[i]/sum;}
		n+=1;
		} while(fabs(ev-sum)>tiny);

	ev=sum;
	printf("\nLargest eigenvalue = %lf",ev);
	sum=0.0;
	for(i=1;i<=nm;i++){sum+=w[i];} /* components of u assumed = 1! */
	for(i=1;i<=nm;i++){u[i]-=sum*w[i];}
	return 0;
	}