//
// Demonstrates linear least squares solution
// using a Gauss-Jordan matrix inverter.
// See Press et al "Numerical Recipes" 2nd edition sec.15.4
//
// M.Lampton UCB SSL Feb 2010

public class TestLLSQ 
{
    public static void main(String[] args) 
    {
        Solver mySolver = new Solver(); 
        mySolver.solve(); 
    }
}

class Solver
{
    static int MCOEFS = 3; 
    static int NPTS = 100;

    public void solve()
    {
        double coefs[]   = new double[MCOEFS];  
        double goals[]   = new double[NPTS]; 
        double basis[][] = new double[NPTS][MCOEFS];

        //---set up the goals------
        for (int i=0; i<NPTS; i++)
          goals[i] = 1.0; 

        //--set up the basis functions----
        for (int i=0; i<NPTS; i++)
        {
            basis[i][0] = 1.0 + Math.cos(i) + Math.sin(i); 
            basis[i][1] = Math.cos(i); 
            basis[i][2] = Math.sin(i); 
        }

        //---solve-----------------------
        double det = doLLSQ(NPTS, MCOEFS, basis, goals, coefs); 

        //----diagnose---------------
        double sos = 0.0; 
        for (int i=0; i<NPTS; i++)  
        {
           double err = -goals[i];
           for (int j=0; j<MCOEFS; j++)
             err += coefs[j]*basis[i][j]; 
           sos += err*err; 
        }
        for (int k=0; k<MCOEFS; k++)
          System.out.println("Coefs:  "+coefs[k]);
        System.out.println("Det:    "+det); 
        System.out.println("SOS:    "+sos); 
    }


    double doLLSQ(int N,           // how many data points
                  int M,           // how many coefs
                  double a[][],    // design matrix[N][M]
                  double b[],      // the fit goals[N]
                  double coefs[])  // the fitted coefs[M].
    {
        double alfa[][] = new double[M][M]; 
        double beta[]   = new double[M];  

        //---set up beta[j]-------
        for (int j=0; j<M; j++)
        {
            beta[j] = 0.0; 
            for (int i=0; i<N; i++)
              beta[j] += a[i][j]*b[i]; 
        }

        //--set up alfa[][]-----------
        for (int k=0; k<M; k++)
          for (int j=0; j<M; j++)
          {
              alfa[j][k] = 0.0; 
              for (int i=0; i<N; i++)
                alfa[j][k] += a[i][j]*a[i][k]; 
          }
 
        //----invert alfa--------
        double det = gaussj(alfa, M); 

        //----evaluate coefs[]-----------
        for (int k=0; k<M; k++)
        {
            coefs[k] = 0.0; 
            for (int j=0; j<M; j++)
              coefs[k] += alfa[j][k]*beta[j]; 
        }
        return det;
    }
              



    //------------matrix inverter utility------------------------

    private double gaussj( double[][] a, int N )
    // Inverts the double array a[N][N] by Gauss-Jordan method
    // M.Lampton UCB SSL (c)2003, 2005
    {
        double det = 1.0, big, save;
        int i,j,k,L;
        int[] ik = new int[100];
        int[] jk = new int[100];
        for (k=0; k<N; k++)
        {
            big = 0.0;
            for (i=k; i<N; i++)
              for (j=k; j<N; j++)          // find biggest element
                if (Math.abs(big) <= Math.abs(a[i][j]))
                {
                    big = a[i][j];
                    ik[k] = i;
                    jk[k] = j;
                }
            if (big == 0.0) return 0.0;
            i = ik[k];
            if (i>k)
              for (j=0; j<N; j++)          // exchange rows
              {
                  save = a[k][j];
                  a[k][j] = a[i][j];
                  a[i][j] = -save;
              }
            j = jk[k];
            if (j>k)
              for (i=0; i<N; i++)
              {
                  save = a[i][k];
                  a[i][k] = a[i][j];
                  a[i][j] = -save;
              }
            for (i=0; i<N; i++)            // build the inverse
              if (i != k)
                a[i][k] = -a[i][k]/big;
            for (i=0; i<N; i++)
              for (j=0; j<N; j++)
                if ((i != k) && (j != k))
                  a[i][j] += a[i][k]*a[k][j];
            for (j=0; j<N; j++)
              if (j != k)
                a[k][j] /= big;
            a[k][k] = 1.0/big;
            det *= big;                    // bomb point
        }                                  // end k loop
        for (L=0; L<N; L++)
        {
            k = N-L-1;
            j = ik[k];
            if (j>k)
              for (i=0; i<N; i++)
              {
                  save = a[i][k];
                  a[i][k] = -a[i][j];
                  a[i][j] = save;
              }
            i = jk[k];
            if (i>k)
              for (j=0; j<N; j++)
              {
                  save = a[k][j];
                  a[k][j] = -a[i][j];
                  a[i][j] = save;
              }
        }
        return det;
    }
} //----------end of TestLLSQ------------------