#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <float.h>
#include "linsystem.h"

/*********************************************************************************************************/
linsys ls_init(int rows, int cols){
	linsys ls;
        int i;
        
	ls=(linsys)malloc(sizeof(LinSystem));
	if(ls==NULL){
		fprintf(stderr,"linsys init failed: Not enough memory!\n");
		return NULL;
	}

        ls->extmx=linmx_create(rows,cols);
	ls->luvalid=_FALSE;
        ls->rowscaling=_FALSE;
        ls->colscaling=_FALSE;

	/* init permutations vector*/
	ls->perm=(int *)malloc(rows*sizeof(int));
	if(ls->perm==NULL){
		fprintf(stderr,"LinSys init failed: Not enough memory!\n");
		ls_dispose(ls);
		return NULL;
	}
    for(i=0;i<ls->extmx->rows;++i) ls->perm[i]=i;

    /* init bases*/
    ls->bases=(int *)malloc(rows*sizeof(int));
    if(ls->bases==NULL){
		fprintf(stderr,"LinSys init failed: Not enough memory!\n");
		ls_dispose(ls);
		return NULL;
	}
    memset(ls->bases,0,sizeof(int)*rows);


    /* Az LU m�rix inicializ��a*/
    ls->lu=NULL;

    return ls;

}
/*********************************************************************************************************/
linsys ls_init_copy(linsys ls){
	linsys newls;

	newls=ls_init(ls->extmx->rows,ls->extmx->cols);

	newls->luvalid=ls->luvalid;
      	newls->colscaling=ls->colscaling;
	newls->rowscaling=ls->rowscaling;
        linmx_copy(ls->extmx,newls->extmx);

        memcpy(newls->bases,ls->bases,sizeof(int)*ls->extmx->rows);
        memcpy(newls->lu,ls->lu,sizeof(double)*ls->extmx->rows*ls->extmx->rows);

	return newls;
}

/*********************************************************************************************************/
void ls_dispose(linsys ls){
	int i,j;

	if(ls==NULL) return;
	/*dispose A*/
        linmx_dispose(ls->extmx);
	free(ls->perm);
	free(ls->bases);
	free(ls->lu);
	free(ls);
}
/******************************************************************************/
int ls_lucrout(double m[], int n,int perm[],double eps)
{
        int i,j,k,swaps=1,imax;
        double max,tmpmax;

        if(m==NULL) return 0;


/* For every column*/
        for(j=0;j<n;++j){
                int i=0;
                while(i<=j){
                        for(k=0;k<i;++k) m[i*n+j]-=m[i*n+k]*m[k*n+j];
                        ++i;
                }
                while(i<n){
                        for(k=0;k<j;++k) m[i*n+j]-=m[i*n+k]*m[k*n+j];
                        ++i;
                }
                /* Select pivot element in column*/
                max=fabs(m[j*n+j]);
                imax=j;

                for(k=j+1;k<n;++k){
                        tmpmax=fabs(m[k*n+j]);
                        if(tmpmax>max){
                                max=tmpmax;
                                imax=k;
                        }
                }
                if(max<eps){
                    return 0;/*matrix is singular*/
                }

                /*Swap rows*/
                if(imax!=j){
                        int temp=perm[imax];
                        swaps*=-1;
                        perm[imax]=perm[j];
                        perm[j]=temp;
                        {
                         double* temp=(double *)malloc(n*sizeof(double));
                         if(temp==NULL){/*malloc failed, swap one by one*/
                              for(k=0;k<n;++k){
                                double temp=m[imax*n+k];
                                m[imax*n+k]=m[j*n+k];
                                m[j*n+k]=temp;
                                }
                         }else{
                             memcpy(temp,&m[imax*n],n*sizeof(double));
                             memcpy(&m[imax*n],&m[j*n],n*sizeof(double));
                             memcpy(&m[j*n],temp,n*sizeof(double));
                             free(temp);
                         }
                        }
                }
                /*Divide by pivot element*/
                for(k=j+1;k<n;++k) m[k*n+j]=m[k*n+j]/m[j*n+j];
        }
        return swaps; /*return 1 if even number of swaps were made, -1 if odd*/
}

/******************************************************************************/
int ls_solve(linsys ls,double b[]){
        int i,j;
        double* lu=ls->lu;

        if(lu==NULL) return 0;

        /*U"=Q*U'*Pcol
          Q*L^-1*B'*Pcol=U"
          B'=L*Q^-1*U"*Pcol^-1

          L*Q^-1*U"*Pcol^-1*x =b
          1. step
             Ly=b
             U"*z=Qy
             x=Pcol*z
         */
         ls_applypermut(ls,b);
        /*Ly=b*/
        /*Forward substitution*/
        ls_forwardsub(ls,b);
        for(i=0;i<ls->num_eta;++i){
            if(ls->Q[i].swap==_TRUE){
                double temp=b[ls->Q[i].col];
                b[ls->Q[i].col]=b[ls->Q[i].col+1];
                b[ls->Q[i].col+1]=temp;
            }
            b[ls->Q[i].col+1]+=b[ls->Q[i].col]*ls->Q[i].val;
        }
        /*Ux=y*/
        /*Backward substitution*/
        ls_backwardsub(ls,b);

        if(ls->colscaling==_TRUE){
                for(i=0;i<ls->extmx->rows;++i){
                               b[i]/=linmx_getcolfactor(ls->extmx,ls->bases[i]);
                }
        }
        return 1;
}

int ls_create_lu(linsys ls,int newbase[]){
        int i=0,j,k;

        if(ls->lu!=NULL) free(ls->lu);
        ls->lu=(double*)malloc(ls->extmx->rows*ls->extmx->rows*sizeof(double));
        if (ls->lu==NULL) return 0;


        for(k=0;k<ls->extmx->rows;++k)
                ls->perm[k]=k;
        for(k=0;k<ls->extmx->cols;++k){
                if (newbase[k]!=0){
                        if(i>=ls->extmx->rows){
                                free(ls->lu);
                                ls->lu=NULL;
                                return 0; /*too many base vectors selected*/
                        }
                        double* col=linmx_getcol(ls->extmx,k+1); /*1 based indexing*/
                        for(j=0;j<ls->extmx->rows;++j){
                                ls->lu[ls->extmx->rows*j+i]=col[j];
                        }
                        free(col);
                        ls->bases[i]=k+1;
                        ++i;
                }
        }
        if(!ls_lucrout(ls->lu,ls->extmx->rows,ls->perm,ls->extmx->eps)){
                free(ls->lu);
                ls->lu=NULL;
                return 0;
        }
        ls->luvalid=_TRUE;
        ls->num_updates=0;
        ls->num_eta=0;

        return 1;
}

void ls_scaleHall(linsys ls,int rowscale,int colscale){
       int i;
       double factorrow=1,factorcol=1;
       if((!rowscale)&&(!colscale)){
           ls->rowscaling=_FALSE;
           ls->colscaling=_FALSE;
           return;
       }
       do{
       if(colscale){
         for(i=0;i<ls->extmx->cols;++i){
            double* col;
            col=linmx_getcol(ls->extmx,i+1);
            factorcol=Hallfactor(col,ls->extmx->rows);
            linmx_setcol(ls->extmx,i+1,col);
            free(col);
            linmx_updatecolfactor(ls->extmx,i+1,factorcol);
         }
       }
       if(rowscale){
         for(i=0;i<ls->extmx->rows;++i){
            double* row;
            row=linmx_getrow(ls->extmx,i+1);
            factorrow=Hallfactor(row,ls->extmx->cols);
            linmx_setrow(ls->extmx,i+1,row);
            free(row);
            linmx_updaterowfactor(ls->extmx,i+1,factorrow);
            linmx_setrhs(ls->extmx,i+1,linmx_getrhs(ls->extmx,i+1)/factorrow);
         }
       }} while((fabs(factorcol-1)>SCALE_DELTA)||(fabs(factorrow-1)>SCALE_DELTA));

       if(colscale){
           ls->colscaling=_TRUE;
       }else{
           ls->colscaling=_FALSE;
       }
       if(rowscale){
           ls->rowscaling=_TRUE;
       }else{
           ls->rowscaling=_FALSE;
       }
}
void ls_scaleGondzio(linsys ls,int rowscale, int colscale){
       int i;
       double f1;
       double f2;
       double factorrow=1,factorcol=1;
       if((!rowscale)&&(!colscale)){
           ls->rowscaling=_FALSE;
           ls->colscaling=_FALSE;
           return;
       }
       do{
       if(colscale){
         for(i=0;i<ls->extmx->cols;++i){
            double* col;
            col=linmx_getcol(ls->extmx,i+1);
            factorcol=Hallfactor(col,ls->extmx->rows);
            linmx_setcol(ls->extmx,i+1,col);
            free(col);
            linmx_updatecolfactor(ls->extmx,i+1,factorcol);
         }
       }
       if(rowscale){
         for(i=0;i<ls->extmx->rows;++i){
            double* row;
            row=linmx_getrow(ls->extmx,i+1);
            factorrow=Gondziofactor(row,ls->extmx->cols);
            linmx_setrow(ls->extmx,i+1,row);
            free(row);
            linmx_updaterowfactor(ls->extmx,i+1,factorrow);
            linmx_setrhs(ls->extmx,i+1,linmx_getrhs(ls->extmx,i+1)/factorrow);
         }

       }
       f1=fabs(factorcol-1);
       f2=fabs(factorrow-1);
       } while((f1>SCALE_DELTA)||(f2>SCALE_DELTA));

       if(colscale){
           ls->colscaling=_TRUE;
       }else{
           ls->colscaling=_FALSE;
       }
       if(rowscale){
           ls->rowscaling=_TRUE;
       }else{
           ls->rowscaling=_FALSE;
       }
}

double Hallfactor(double v[],int n){
       int i=0;
       int count=0;
       double proj=1;
       double factor;
       for(i=0;i<n;++i){
            if(v[i]!=0.0) {
                     proj*=fabs(v[i]);
                     ++count;
            }
       }
       if (count==0) return 1; /*0vector*/
       factor=pow(proj,1.0/count);

       for(i=0;i<n;++i){
             v[i]/=factor;
       }
       return factor;
}
double Gondziofactor(double v[],int n){

       double max=-DBL_MAX;
       double min= DBL_MAX;
       int i;
       for(i=0;i<n;++i){
             if(v[i]){
                      if(v[i]<min) min=v[i];
                      if(v[i]>max) max=v[i];
             }
       }
       if(min==DBL_MAX) return 1; /* 0 vector*/
       double factor=sqrt(max*min);
       for(i=0;i<n;++i){
             v[i]/=factor;
       }
       return factor;
}

int ls_updatelu(linsys ls,int entering, int leaving){
    int i,j,k;
    double* newcol;
    double val;

    for(i=0;i<ls->extmx->rows;++i){
        if(ls->bases[i]==leaving) break;
    }

    /*performing update in-place*/
    /*compute last column: Q*L^-1*entering*/
    newcol=linmx_getcol(ls->extmx,entering);
    ls_applypermut(ls,newcol);
    ls_forwardsub(ls,newcol);

    /*apply updates so far*/
    for(j=0;j<ls->num_eta;++j){
            if(ls->Q[j].swap==_TRUE){
                double temp=newcol[ls->Q[j].col];
                newcol[ls->Q[j].col]=newcol[ls->Q[j].col+1];
                newcol[ls->Q[j].col+1]=temp;
            }
            newcol[ls->Q[j].col+1]+=newcol[ls->Q[j].col]*ls->Q[j].val;
    }

    /*copy non-changing parts*/
    for(j=0;j<i;++j){
        for(k=i;k<ls->extmx->rows-1;++k){
             ls->lu[j*ls->extmx->rows+k]=ls->lu[j*ls->extmx->rows+k+1];
        }
    }
    for(;i<ls->extmx->rows-1;++i){/*for every column*/
        /*update bases vector*/
        ls->bases[i]=ls->bases[i+1];
        ls->Q[ls->num_eta].col=i;

        if((fabs(ls->lu[i*ls->extmx->rows+i+1])<ls->extmx->eps)&&(fabs(ls->lu[(i+1)*ls->extmx->rows+i+1])<ls->extmx->eps)){
        /* new base is singular*/
            ls->luvalid=_FALSE;
            return 0;
        }

        /*select pivot element*/
        if(fabs(ls->lu[i*ls->extmx->rows+i+1])<fabs(ls->lu[(i+1)*ls->extmx->rows+i+1])){
            /*swap*/
            int size=(ls->extmx->rows-i-1)*sizeof(double);
            double* first=&ls->lu[i*ls->extmx->rows+i+1];
            double* second=&ls->lu[(i+1)*ls->extmx->rows+i+1];
            double *temp=(double *)malloc(size);
            double tmp2;

            if(temp==NULL){
               ls->luvalid=_FALSE;
               return 0;
            }
            memcpy(temp,first,size);
            memcpy(first,second,size);
            memcpy(second,temp,size);
            free(temp);
            /*swap entering*/
            tmp2=newcol[i];
            newcol[i]=newcol[i+1];
            newcol[i+1]=tmp2;
            ls->Q[ls->num_eta].swap=_TRUE;
        }else { ls->Q[ls->num_eta].swap=_FALSE;}

        if(fabs(ls->lu[(i+1)*ls->extmx->rows+i+1])<ls->extmx->eps) val=0;
        else val=-ls->lu[(i+1)*ls->extmx->rows+i+1]/ls->lu[i*ls->extmx->rows+i+1];

        ls->Q[ls->num_eta].val=val;

        for(k=i+1;k<ls->extmx->rows;++k){
            ls->lu[i*ls->extmx->rows+k-1]=ls->lu[i*ls->extmx->rows+k];
            ls->lu[(i+1)*ls->extmx->rows+k]=ls->lu[(i+1)*ls->extmx->rows+k]
                                            +val*ls->lu[i*ls->extmx->rows+k];
        }
        newcol[i+1]+=val*newcol[i];
        ++ls->num_eta;
    }

    /*set last col*/
    ls->bases[i]=entering;
    if(fabs(newcol[i])<ls->extmx->eps){
        /*new base is singular*/
        ls->luvalid=_FALSE;
        return 0;
    }
    for(i=0;i<ls->extmx->rows;++i){
        ls->lu[(i+1)*ls->extmx->rows-1]=newcol[i];
    }
    free(newcol);

    ++ls->num_updates;
    ls->luvalid=_TRUE;

    return 1;
}

int ls_varinbase(linsys ls,int varno){
    assert(varno>=1&&varno<=ls->extmx->cols);
    int i;
    for(i=0;i<ls->extmx->rows;++i){
          if(ls->bases[i]==varno) return _TRUE;
    }
    return _FALSE;
}
int ls_applypermut(linsys ls,double v[]){
        double *newb=(double *)malloc(ls->extmx->rows*sizeof(double));
        int i;

        if (newb==NULL) return 0;

        /*permute b*/
        for(i=0;i<ls->extmx->rows;++i){
             newb[i]=v[ls->perm[i]];
        }
        memcpy(v,newb,sizeof(double)*ls->extmx->rows);
        free(newb);
        return 1;
}

void ls_forwardsub(linsys ls, double b[]){
        int i,j;
        int n=ls->extmx->rows;
        double* lu=ls->lu;
        for(i=0;i<n;++i){
                for(j=0;j<i;++j) b[i]-=b[j]*lu[i*n+j];
        }
}

void ls_backwardsub(linsys ls, double b[]){
        int i,j;
        int n=ls->extmx->rows;
        double* lu=ls->lu;

        for(i=n-1;i>=0;--i){
                for(j=i+1;j<n;++j) b[i]-=b[j]*lu[i*n+j];
                b[i]/=lu[i*n+i];
        }
}
