package JSci.maths;

/**
* The DoubleAbstractSquareMatrix class provides default implementations of the standard matrix methods
* without any reference to how the matrix elements are stored.
* A subclass must implement:
* {@link #newInstance(int) newInstance},
* {@link #getElement(int,int) getElement}
* and {@link #setElement(int,int,double) setElement}.
* @note Experimental.
* @version 1.0
* @author Mark Hale
*/
public abstract class DoubleAbstractSquareMatrix extends DoubleAbstractMatrix {
        /** Hints */
        protected static final int LOWER_TRIANGULAR=16;
        protected static final int UPPER_TRIANGULAR=17;
        protected static final int TRIDIAGONAL=18;
        protected static final int DIAGONAL=19;
        /**
        * Constructs an empty matrix.
        * @param size the number of rows/columns.
        */
        public DoubleAbstractSquareMatrix(final int size) {
                super(size,size);
        }
        /**
        * Abstract constructor.
        * Enables an abstract matrix to be constructed from an underlying implementation.
        * @param size the number of rows/columns.
        * @param hint suggestion of the optimal matrix type to use.
        */
        protected abstract DoubleAbstractSquareMatrix newInstance(int size, int hint);
        /**
        * Returns true if this matrix is symmetric.
        */
        public boolean isSymmetric() {
                return this.equals(this.transpose());
        }
        /**
        * Returns the determinant.
        */
        public double det() {
                if(rows()==2) {
                        return getElement(0,0)*getElement(1,1)-getElement(0,1)*getElement(1,0);
                } else {
                        int pivot[]=new int[rows()+1];
                        final DoubleAbstractSquareMatrix lu[]=this.luDecompose(pivot);
                        double det=lu[1].getElement(0,0);
                        for(int i=1;i<rows();i++)
                                det*=lu[1].getElement(i,i);
                        return det*pivot[rows()];
                }
        }
        /**
        * Returns the trace.
        */
        public double trace() {
                double result=getElement(0,0);
                for(int i=1;i<rows();i++)
                        result+=getElement(i,i);
                return result;
        }

//============
// OPERATIONS
//============

// INVERSE

        /**
        * Returns the inverse of this matrix.
        * @return a double square matrix.
        */
        public DoubleAbstractSquareMatrix inverse() {
                int i,j,k;
                double tmpL,tmpU;
                final int N=rows();
                final double arrayL[][]=new double[N][N];
                final double arrayU[][]=new double[N][N];
                int pivot[]=new int[N+1];
                final DoubleAbstractSquareMatrix lu[]=this.luDecompose(pivot);
                arrayL[0][0]=1.0/lu[0].getElement(0,0);
                arrayU[0][0]=1.0/lu[1].getElement(0,0);
                for(i=1;i<N;i++) {
                        arrayL[i][i]=1.0/lu[0].getElement(i,i);
                        arrayU[i][i]=1.0/lu[1].getElement(i,i);
                }
                for(i=0;i<N-1;i++) {
                        for(j=i+1;j<N;j++) {
                                tmpL=tmpU=0.0;
                                for(k=i;k<j;k++) {
                                        tmpL-=lu[0].getElement(j,k)*arrayL[k][i];
                                        tmpU-=arrayU[i][k]*lu[1].getElement(k,j);
                                }
                                arrayL[j][i]=tmpL/lu[0].getElement(j,j);
                                arrayU[i][j]=tmpU/lu[1].getElement(j,j);
                        }
                }
                // matrix multiply arrayU x arrayL
                final DoubleAbstractSquareMatrix inv=newInstance(N,DENSE);
                double sum;
                for(i=0;i<N;i++) {
                        for(j=0;j<i;j++) {
                                sum=0.0;
                                for(k=i;k<N;k++)
                                        sum+=arrayU[i][k]*arrayL[k][j];
                                inv.setElement(i,pivot[j],sum);
                        }
                        for(j=i;j<N;j++) {
                                sum=0.0;
                                for(k=j;k<N;k++)
                                        sum+=arrayU[i][k]*arrayL[k][j];
                                inv.setElement(i,pivot[j],sum);
                        }
                }
                return inv;
        }

// LU DECOMPOSITION

        /**
        * Returns the LU decomposition of this matrix.
        * @param pivot an empty array of length <code>rows()+1</code>
        * to hold the pivot information (null if not interested).
        * The last array element will contain the parity.
        * @return an array with [0] containing the L-matrix
        * and [1] containing the U-matrix.
        */
        public DoubleAbstractSquareMatrix[] luDecompose(int pivot[]) {
                int i,j,k,pivotrow;
                final int N=rows();
                final DoubleAbstractSquareMatrix L=newInstance(N,LOWER_TRIANGULAR);
                final DoubleAbstractSquareMatrix U=newInstance(N,UPPER_TRIANGULAR);
                final double buf[]=new double[N];
                double tmp,max;
                if(pivot==null)
                        pivot=new int[N+1];
                for(i=0;i<N;i++)
                        pivot[i]=i;
                pivot[N]=1;
        // LU decomposition to arrayU
                for(j=0;j<N;j++) {
                        for(i=0;i<j;i++) {
                                tmp=getElement(pivot[i],j);
                                for(k=0;k<i;k++)
                                        tmp-=U.getElement(i,k)*U.getElement(k,j);
                                U.setElement(i,j,tmp);
                        }
                        max=0.0;
                        pivotrow=j;
                        for(i=j;i<N;i++) {
                                tmp=getElement(pivot[i],j);
                                for(k=0;k<j;k++)
                                        tmp-=U.getElement(i,k)*U.getElement(k,j);
                                U.setElement(i,j,tmp);
                        // while we're here search for a pivot for U[j][j]
                                tmp=Math.abs(tmp);
                                if(tmp>max) {
                                        max=tmp;
                                        pivotrow=i;
                                }
                        }
                // swap row j with pivotrow
                        if(pivotrow!=j) {
                                System.arraycopy(arrayU[j],0,buf,0,j+1);
                                System.arraycopy(arrayU[pivotrow],0,arrayU[j],0,j+1);
                                System.arraycopy(buf,0,arrayU[pivotrow],0,j+1);
                                k=pivot[j];
                                pivot[j]=pivot[pivotrow];
                                pivot[pivotrow]=k;
                                // update parity
                                pivot[N]=-pivot[N];
                        }
                // divide by pivot
                        tmp=U.getElement(j,j);
                        for(i=j+1;i<N;i++)
                                U.setElement(i,j,U.getElement(i,j)/tmp);
                }
                // move lower triangular part to arrayL
                for(j=0;j<N;j++) {
                        L.setElement(j,j,1.0);
                        for(i=j+1;i<N;i++) {
                                L.setElement(i,j,U.getElement(i,j));
                                U.setElement(i,j,0.0);
                        }
                }
                final DoubleAbstractSquareMatrix lu[]=new DoubleAbstractSquareMatrix[2];
                lu[0]=L;
                lu[1]=U;
                return lu;
        }

// CHOLESKY DECOMPOSITION

        /**
        * Returns the Cholesky decomposition of this matrix.
        * Matrix must be symmetric and positive definite.
        * @return an array with [0] containing the L-matrix and [1] containing the U-matrix.
        */
        public DoubleAbstractSquareMatrix[] choleskyDecompose() {
                int i,j,k;
                final int N=rows();
                final DoubleAbstractSquareMatrix L=newInstance(N,LOWER_TRIANGULAR);
                final DoubleAbstractSquareMatrix U=newInstance(N,UPPER_TRIANGULAR);
                double tmp=Math.sqrt(getElement(0,0));
                L.setElement(0,0,tmp);
                U.setElement(0,0,tmp);
                for(i=1;i<N;i++) {
                        L.setElement(i,0,getElement(i,0)/tmp);
                        U.setElement(0,i,getElement(i,0)/tmp);
                }
                for(j=1;j<N;j++) {
                        tmp=getElement(j,j);
                        for(i=0;i<j;i++)
                                tmp-=L.getElement(j,i)*L.getElement(j,i);
                        L.setElement(j,j,Math.sqrt(tmp));
                        U.setElement(j,j,Math.sqrt(tmp));
                        for(i=j+1;i<N;i++) {
                                tmp=getElement(i,j);
                                for(k=0;k<i;k++)
                                        tmp-=L.getElement(j,k)*U.getElement(k,i);
                                L.setElement(i,j,tmp/U.getElement(j,j));
                                U.setElement(j,i,tmp/U.getElement(j,j));
                        }
                }
                final DoubleAbstractSquareMatrix lu[]=new DoubleAbstractSquareMatrix[2];
                lu[0]=L;
                lu[1]=U;
                return lu;
        }

// QR DECOMPOSITION

        /**
        * Returns the QR decomposition of this matrix.
        * Based on the code from <a href="http://math.nist.gov/javanumerics/jama/">JAMA</a> (public domain).
        * @return an array with [0] containing the Q-matrix and [1] containing the R-matrix.
        * @planetmath QRDecomposition
        */
        public DoubleSquareMatrix[] qrDecompose() {
                int i, j, k;
                final int N=numRows;
                final double array[][]=new double[N][N];
                final double arrayQ[][]=new double[N][N];
                final double arrayR[][]=new double[N][N];
                // copy matrix
                for(i=0;i<N;i++) {
                        array[i][0]=getElement(i,0);
                        for(j=1;j<N;j++)
                                array[i][j]=getElement(i,j);
                }
                for(k=0; k<N; k++) {
                        // compute l2-norm of kth column
                        double norm = array[k][k];
                        for(i=k+1; i<N; i++)
                                norm = ExtraMath.hypot(norm, array[i][k]);
                        if(norm != 0.0) {
                                // form kth Householder vector
                                if(array[k][k] < 0.0)
                                        norm = -norm;
                                for(i=k; i<N; i++)
                                        array[i][k] /= norm;
                                array[k][k] += 1.0;
                                // apply transformation to remaining columns
                                for(j=k+1; j<N; j++) {
                                        double s=array[k][k]*array[k][j];
                                        for(i=k+1; i<N; i++)
                                                s += array[i][k]*array[i][j];
                                        s /= array[k][k];
                                        for(i=k; i<N; i++)
                                                array[i][j] -= s*array[i][k];
                                }
                        }
                        arrayR[k][k] = -norm;
                }
                for(k=N-1; k>=0; k--) {
                        arrayQ[k][k] = 1.0;
                        for(j=k; j<N; j++) {
                                if(array[k][k] != 0.0) {
                                        double s = array[k][k]*arrayQ[k][j];
                                        for(i=k+1; i<N; i++)
                                                s += array[i][k]*arrayQ[i][j];
                                        s /= array[k][k];
                                        for(i=k; i<N; i++)
                                                arrayQ[i][j] -= s*array[i][k];
                                }
                        }
                }
                for(i=0; i<N; i++) {
                        for(j=i+1; j<N; j++)
                                arrayR[i][j] = array[i][j];
                }
                final DoubleSquareMatrix qr[]=new DoubleSquareMatrix[2];
                qr[0]=new DoubleSquareMatrix(arrayQ);
                qr[1]=new DoubleSquareMatrix(arrayR);
                return qr;
        }

// SINGULAR VALUE DECOMPOSITION

        /**
        * Returns the singular value decomposition of this matrix.
        * Based on the code from <a href="http://math.nist.gov/javanumerics/jama/">JAMA</a> (public domain).
        * @return an array with [0] containing the U-matrix, [1] containing the S-matrix and [2] containing the V-matrix.
        */
        public DoubleAbstractSquareMatrix[] singularValueDecompose() {
                int i,j,k;
                final int N=rows();
                final int Nm1=N-1;
                final double array[][]=new double[N][N];
                final DoubleAbstractSquareMatrix U=newInstance(N,DENSE);
                final DoubleAbstractSquareMatrix S=newInstance(N,DIAGONAL);
                final DoubleAbstractSquareMatrix V=newInstance(N,DENSE);
                final double e[]=new double[N];
                final double work[]=new double[N];
                double tmp;
                // copy matrix
                for(i=0;i<N;i++) {
                        array[i][0]=getElement(i,0);
                        for(j=1;j<N;j++)
                                array[i][j]=getElement(i,j);
                }
                // reduce matrix to bidiagonal form
                for(k=0;k<Nm1;k++) {
                        // compute the transformation for the kth column
                        // compute l2-norm of kth column
                        tmp=array[k][k];
                        for(i=k+1;i<N;i++)
                                tmp=ExtraMath.hypot(tmp,array[i][k]);
                        if(tmp!=0.0) {
                                if(array[k][k]<0.0)
                                        tmp=-tmp;
                                for(i=k;i<N;i++)
                                        array[i][k]/=tmp;
                                array[k][k]+=1.0;
                        }
                        tmp=-tmp;
                        for(j=k+1;j<N;j++) {
                                if(tmp!=0.0) {
                                        // apply the transformation
                                        double t=0.0;
                                        for(i=k;i<N;i++)
                                                t+=array[i][k]*array[i][j];
                                        t/=-array[k][k];
                                        for(i=k;i<N;i++)
                                                array[i][j]+=t*array[i][k];
                                }
                                e[j]=array[k][j];
                        }
                        S.setElement(k,k,tmp);
                        for(i=k;i<N;i++)
                                U.setElement(i,k,array[i][k]);
                        if(k<N-2) {
                                // compute the kth row transformation
                                // compute l2-norm of kth column
                                e[k]=e[k+1];
                                for(i=k+2;i<N;i++)
                                        e[k]=ExtraMath.hypot(e[k],e[i]);
                                if(e[k]!=0.0) {
                                        if(e[k+1]<0.0)
                                                e[k]=-e[k];
                                        for(i=k+1;i<N;i++)
                                                e[i]/=e[k];
                                        e[k+1]+=1.0;
                                }
                                e[k]=-e[k];
                                if(e[k]!=0.0) {
                                        // apply the transformation
                                        for(i=k+1;i<N;i++) {
                                                work[i]=0.0;
                                                for(j=k+1;j<N;j++)
                                                        work[i]+=e[j]*array[i][j];
                                        }
                                        for(j=k+1;j<N;j++) {
                                                double t=-e[j]/e[k+1];
                                                for(i=k+1;i<N;i++)
                                                        array[i][j]+=t*work[i];
                                        }
                                }
                                for(i=k+1;i<N;i++)
                                        V.setElement(i,k,e[i]);
                        }
                }
                // setup the final bidiagonal matrix of order p
                int p=N;
                S.setElement(Nm1,Nm1,array[Nm1][Nm1]);
                e[N-2]=array[N-2][Nm1];
                e[Nm1]=0.0;
                for(i=0;i<N;i++)
                        U.setElement(i,Nm1,0.0);
                U.setElement(Nm1,Nm1,1.0);
                for(k=N-2;k>=0;k--) {
                        if(arrayS[k]!=0.0) {
                                for(j=k+1;j<N;j++) {
                                        double t=U.getElement(k,k)*U.getElement(k,j);
                                        for(i=k+1;i<N;i++)
                                                t+=U.getElement(i,k)*U.getElement(i,j);
                                        t/=-U.getElement(k,k);
                                        for(i=k;i<N;i++)
                                                U.setElement(i,j,U.getElement(i,j)+t*U.getElement(i,k));
                                }
                                for(i=k;i<N;i++)
                                        U.setElement(i,k,-U.getElement(i,k));
                                U.setElement(k,k,U.getElement(k,k)+1.0);
                                for(i=0;i<k-1;i++)
                                        U.setElement(i,k,0.0);
                        } else {
                                for(i=0;i<N;i++)
                                        U.setElement(i,k,0.0);
                                U.setElement(k,k,1.0);
                        }
                }
                for(k=Nm1;k>=0;k--) {
                        if(k<N-2 && e[k]!=0.0) {
                                for(j=k+1;j<N;j++) {
                                        double t=V.getElement(k+1,k)*V.getElement(k+1,j);
                                        for(i=k+2;i<N;i++)
                                                t+=V.getElement(i,k)*V.getElement(i,j);
                                        t/=-V.getElement(k+1,k);
                                        for(i=k+1;i<N;i++)
                                                V.setElement(i,j,V.getElement(i,j)+t*V.getElement(i,k));
                                }
                        }
                        for(i=0;i<N;i++)
                                V.setElement(i,k,0.0);
                        V.setElement(k,k,1.0);
                }
                final double eps=Math.pow(2.0,-52.0);
                int iter=0;
                while(p>0) {
                        int action;
                        // action = 1 if arrayS[p] and e[k-1] are negligible and k<p
                        // action = 2 if arrayS[k] is negligible and k<p
                        // action = 3 if e[k-1] is negligible, k<p, and arrayS[k], ..., arrayS[p] are not negligible (QR step)
                        // action = 4 if e[p-1] is negligible (convergence)
                        for(k=p-2;k>=-1;k--) {
                                if(k==-1)
                                        break;
                                if(Math.abs(e[k])<=eps*(Math.abs(S.getElement(k,k))+Math.abs(S.getElement(k+1,k+1)))) {
                                        e[k]=0.0;
                                        break;
                                }
                        }
                        if(k==p-2) {
                                action=4;
                        } else {
                                int ks;
                                for(ks=p-1;ks>=k;ks--) {
                                        if(ks==k)
                                                break;
                                        double t=(ks!=p ? Math.abs(e[ks]) : 0.0)+(ks!=k+1 ? Math.abs(e[ks-1]) : 0.0);
                                        if(Math.abs(S.getElement(ks,ks))<=eps*t) {
                                                S.setElement(ks,ks,0.0);
                                                break;
                                        }
                                }
                                if(ks==k) {
                                        action=3;
                                } else if(ks==p-1) {
                                        action=1;
                                } else {
                                        action=2;
                                        k=ks;
                                }
                        }
                        k++;
                        switch(action) {
                                // deflate negligible arrayS[p]
                                case 1: {
                                        double f=e[p-2];
                                        e[p-2]=0.0;
                                        for(j=p-2;j>=k;j--) {
                                                double t=ExtraMath.hypot(S.getElement(j,j),f);
                                                final double cs=S.getElement(j,j)/t;
                                                final double sn=f/t;
                                                S.setElement(j,j,t);
                                                if(j!=k) {
                                                        f=-sn*e[j-1];
                                                        e[j-1]*=cs;
                                                }
                                                for(i=0;i<N;i++) {
                                                        t=cs*V.getElement(i,j)+sn*V.getElement(i,p-1);
                                                        V.setElement(i,p-1,-sn*V.getElement(i,j)+cs*V.getElement(i,p-1));
                                                        V.setElement(i,j,t);
                                                }
                                        }
                                        } break;
                                // split at negligible arrayS[k]
                                case 2: {
                                        double f=e[k-1];
                                        e[k-1]=0.0;
                                        for(j=k;j<p;j++) {
                                                double t=ExtraMath.hypot(S.getElement(j,j),f);
                                                final double cs=S.getElement(j,j)/t;
                                                final double sn=f/t;
                                                S.setElement(j,j,t);
                                                f=-sn*e[j];
                                                e[j]*=cs;
                                                for(i=0;i<N;i++) {
                                                        t=cs*arrayU[i][j]+sn*arrayU[i][k-1];
                                                        arrayU[i][k-1]=-sn*arrayU[i][j]+cs*arrayU[i][k-1];
                                                        arrayU[i][j]=t;
                                                }
                                        }
                                        } break;
                                // perform one QR step
                                case 3: {
                                        // calculate the shift
                                        final double scale=Math.max(Math.max(Math.max(Math.max(
                                                Math.abs(S.getElement(p-1,p-1)),
                                                Math.abs(S.getElement(p-2,p-2))),Math.abs(e[p-2])),
                                                Math.abs(S.getElement(k,k))),Math.abs(e[k]));
                                        double sp=S.getElement(p-1,p-1)/scale;
                                        double spm1=S.getElement(p-2,p-2)/scale;
                                        double epm1=e[p-2]/scale;
                                        double sk=S.getElement(k,k)/scale;
                                        double ek=e[k]/scale;
                                        double b=((spm1+sp)*(spm1-sp)+epm1*epm1)/2.0;
                                        double c=(sp*epm1)*(sp*epm1);
                                        double shift=0.0;
                                        if(b!=0.0 || c!=0.0) {
                                                shift=Math.sqrt(b*b+c);
                                                if(b<0.0)
                                                        shift=-shift;
                                                shift=c/(b+shift);
                                        }
                                        double f=(sk+sp)*(sk-sp)+shift;
                                        double g=sk*ek;
                                        // chase zeros
                                        for(j=k;j<p-1;j++) {
                                                double t=ExtraMath.hypot(f,g);
                                                double cs=f/t;
                                                double sn=g/t;
                                                if(j!=k)
                                                        e[j-1]=t;
                                                f=cs*S.getElement(j,j)+sn*e[j];
                                                e[j]=cs*e[j]-sn*S.getElement(j,j);
                                                tmp=S.getElement(j+1,j+1);
                                                g=sn*tmp;
                                                tmp*=cs;
                                                for(i=0;i<N;i++) {
                                                        t=cs*arrayV[i][j]+sn*arrayV[i][j+1];
                                                        arrayV[i][j+1]=-sn*arrayV[i][j]+cs*arrayV[i][j+1];
                                                        arrayV[i][j]=t;
                                                }
                                                t=ExtraMath.hypot(f,g);
                                                cs=f/t;
                                                sn=g/t;
                                                S.setElement(j,j,t);
                                                f=cs*e[j]+sn*tmp;
                                                tmp=-sn*e[j]+cs*tmp;
                                                S.setElement(j+1,j+1,tmp);
                                                g=sn*e[j+1];
                                                e[j+1]*=cs;
                                                if(j<Nm1) {
                                                        for(i=0;i<N;i++) {
                                                                t=cs*arrayU[i][j]+sn*arrayU[i][j+1];
                                                                arrayU[i][j+1]=-sn*arrayU[i][j]+cs*arrayU[i][j+1];
                                                                arrayU[i][j]=t;
                                                        }
                                                }
                                        }
                                        e[p-2]=f;
                                        iter++;
                                        } break;
                                // convergence
                                case 4: {
                                        // make the singular values positive
                                        if(S.getElement(k,k)<=0.0) {
                                                S.setElement(k,k,-S.getElement(k,k));
                                                for(i=0;i<p;i++)
                                                        V.setElement(i,k,-V.getElement(i,k));
                                        }
                                        // order the singular values
                                        while(k<p-1) {
                                                if(S.getElement(k,k)>=S.getElement(k+1,k+1))
                                                        break;
                                                tmp=S.getElement(k,k);
                                                S.setElement(k,k,S.getElement(k+1,k+1));
                                                S.setElement(k+1,k+1,tmp);
                                                if(k<Nm1) {
                                                        for(i=0;i<N;i++) {
                                                                tmp=U.getElement(i,k+1);
                                                                U.setElement(i,k+1,U.getElement(i,k));
                                                                U.setElement(i,k,tmp);
                                                                tmp=V.getElement(i,k+1);
                                                                V.setElement(i,k+1,V.getElement(i,k));
                                                                V.setElement(i,k,tmp);
                                                        }
                                                }
                                                k++;
                                        }
                                        iter=0;
                                        p--;
                                        } break;
                        }
                }
                final DoubleAbstractSquareMatrix svd[]=new DoubleAbstractSquareMatrix[3];
                svd[0]=U;
                svd[1]=S;
                svd[2]=V;
                return svd;
        }
}

