// QRDecomposition.java
// A = Q * R  R is upper triangular and Q^T * Q = 1
// to solve A * X = Y   R * X = Q * Y  solve for X by back substitution
// not very efficient for solving linear equations

import java.io.*;
import java.text.*;

public strictfp class QRDecomposition
{
  public QRDecomposition() // constructor runs test on class
  {
    System.out.println("testing QRDecomposition.java");
    DecimalFormat f4 = new DecimalFormat("0.000");
    DecimalFormat f8 = new DecimalFormat(" ,##0.0000000;-,##0.0000000");
    int n = 4;
    double A[][]  = new double[n][n];
    double At[][] = new double[n][n]; // saved version of A
    double Ac[][] = new double[n][n]; // for checking Q * R = At
    double Q[][]  = new double[n][n]; // not needed for solve
    double R[][]  = new double[n][n]; // not needed for solve
    double C[]    = new double[n];   
    double D[]    = new double[n];    // diagonal of R
    double Y[]    = new double[n];    // RHS
    double X[]    = new double[n];    // unknown solution
    double Xt[]   = new double[n];    // saved solution
    double sumerr, err;
    boolean sing = true;

    // build A, Hilbert matrix, and X, and Y  A * Xt = Y
    System.out.println("A matrix of A * X = Y");
    for(int i=0; i<n; i++)
    {
      for(int j=0; j<n; j++)
      {
     	A[i][j] = 1.0/(double)(i+1+j+1);
        System.out.print(f4.format(A[i][j])+"  ");
        At[i][j] = A[i][j];
      }
      System.out.println(" ");
    }
    System.out.println(" ");
    System.out.println("Expected solution Xt = ");
    for(int i=0; i<n; i++)
    {
      Xt[i] = (double)(i+1); // yes, 1, 2, 3, 4
      System.out.print(f4.format(Xt[i])+"  ");
    }
    System.out.println(" ");
    System.out.println(" ");
    System.out.println("right hand side Y = ");
    for(int i=0; i<n; i++)
    {
      Y[i]=0.0;
      for(int j=0; j<n; j++)
      {
	Y[i] = Y[i] + A[i][j]*Xt[j];
      }
      System.out.print(f4.format(Y[i])+"  ");
    }
    System.out.println(" ");
    System.out.println(" ");

    sing = QRDecomp(A, C, D); // R is upper triangular in A
                              // D is diagonal of R
                              // Q is lower triangular including diagonal in A

    System.out.println("sing="+sing+"  A =");
    for(int i=0; i<n; i++)
    {
      for(int j=0; j<n; j++)
      {
        System.out.print(f8.format(A[i][j])+"  ");
      }
      System.out.println(" ");
    }
    System.out.println(" ");

    System.out.println("C =");
    for(int i=0; i<n; i++)
    {
      System.out.println(C[i]);
    }
    System.out.println(" ");
    System.out.println("D =");
    for(int i=0; i<n; i++)
    {
      System.out.println(D[i]);
    }
    System.out.println(" ");


    System.out.println("R =");
    for(int i=0; i<n; i++)
    {
      for(int j=0; j<n; j++)
      {
	if(j>i)  R[i][j] = A[i][j];
	if(j==i) R[i][j] = D[i];
	if(j<i)  R[i][j] = 0.0;
        System.out.print(f8.format(R[i][j])+"  ");
      }
      System.out.println(" ");
    }
    System.out.println(" ");
    System.out.println("Q = not really computed");
    for(int i=0; i<n; i++)
    {
      for(int j=0; j<n; j++)
      {
	if(j>i)  Q[i][j] = 0.0;
	if(j==i) Q[i][j] = A[i][j];
	if(j<i)  Q[i][j] = A[i][j];
        System.out.print(f8.format(Q[i][j])+"  ");
      }
      System.out.println(" ");
    }
    System.out.println(" ");

    // check A = Q * R
    System.out.println("check A =  Q * R , yet bad Q");
    sumerr = 0.0;
    for(int i=0; i<n; i++)
    {
      for(int j=0; j<n; j++)
      {
     	Ac[i][j] = 0.0;
        for(int k=0; k<n; k++)
	{
	  Ac[i][j] = Ac[i][j] + Q[i][k]*R[k][j];
	}
        err = Ac[i][j]-At[i][j];
        sumerr = sumerr + Math.abs(err);
        // System.out.println("err["+i+"]["+j+"]="+err);
      }
    }
    System.out.println("average error = "+(sumerr/(double)(n*n)));
    System.out.println(" ");

    QRSolve(A, C, D, Y);  // RSolve called from in here
                          // A, C, D from QRDecomposition
                          // Y over written with solution X
    // check
    for(int i=0; i<n; i++)
    {
      X[i] = Y[i];
      System.out.println(X[i]+"  error = "+(Y[i]-Xt[i]));
    }

    System.out.println("test QRDecomposition.java finished");
  } // end test

  // returns true of signular, false is good
  public boolean QRDecomp(double A[][], double C[], double D[])
  {
    boolean sing = false;
    double scale, sigma, sum, tau;
    int n = A.length;

    System.out.println("running QRDecomp");
    scale = 0.0;
    for(int k=0; k<n-1; k++) // 17
    {
      for(int i=k; i<n; i++) // 11
      {
	  scale = Math.max(scale, Math.abs(A[i][k]));
      } // 11
      if(scale < 1.0E-16)
      {
	sing = true;
        C[k] = 0.0;
        D[k] = 0.0;
      }
      else
      {
	for(int i=k; i<n; i++) // 12
	{
	  A[i][k] = A[i][k]/scale;
        } // 12
        sum = 0.0;
        for(int i=k; i<n; i++) // 13
	{
	  sum += A[i][k]*A[i][k];
	} // 13
        sigma = Math.sqrt(sum)*A[k][k]/Math.abs(A[k][k]);
        A[k][k] += sigma;
	C[k] = sigma*A[k][k];
	D[k] = -scale*sigma;
        for(int j=k+1; j<n; j++) // 16
        {
	  sum = 0.0;
          for(int i=k; i<n; i++) // 14
	  {
	    sum += A[i][k]*A[i][j];
	  } // 14
          tau = sum/C[k];
          for(int i=k; i<n; i++) // 15
          {
	      A[i][j] = A[i][j]-tau*A[i][k];
	  } // 15
	} //16
      } // end else
    } // 17
    D[n-1] = A[n-1][n-1];
    if(Math.abs(D[n-1])<1.0E-16) sing = true;
    return sing;
  }

  public void QRSolve(double A[][], double C[], double D[], double B[])
                                                                  // B is Y
  {
    int n = A.length;
    double sum, tau;

    System.out.println("running QRSolve");
    for(int j=0; j<n-1; j++) // 13
    {
      sum = 0.0;
      for(int i=j; i<n; i++) // 11
      {
	sum = sum + A[i][j]*B[i];
      } //11
      tau = sum/C[j];
      for(int i=j; i<n; i++) // 12
      {
	B[i] = B[i]-tau*A[i][j];
      } // 12
    } // 13

    RSolve(A, D, B);
  }

  public void RSolve(double A[][], double D[], double B[]) // B is Y
  {
    int n = A.length;
    double sum;

    System.out.println("running RSolve");
    B[n-1] = B[n-1]/D[n-1];
    for(int i=n-2; i>=0; i--) // 12
    {
      sum = 0.0;
      for(int j=i+1; j<n; j++) // 11
      {
	sum = sum + A[i][j]*B[j];
      } //11
      B[i] = (B[i]-sum)/D[i];
    } //12
  }

  public static void main(String args[])
  {
    new QRDecomposition(); // run test on class
  }
} // end class QRDecomposition