// simeq_newton.java  solve nonlinear system of equations
//                    method: newton iteration using Jacobian
//
//  Given problem  A X = Y  where x may have terms x1, x2, x3, x4, x1*x2, x3*x4
//                 A is 6 by 6 matrix of reals (could be complex)
//                 Y is vector of reals (could be complex)
//                 independent unknowns are x1, x2, x3, x4
//
//  for testing, generate A using pseudo random numbers
//               choose x1=1.1 x2=1.2 x3=1.4 x4 =1.5, compute x1*x2, x3*x4
//               compute terms of Y using Y = A X
//
//  Solve by initial guess at values of x1, x2, x3, x4 computing x1*x2, x3*x4
//    X_next = X_initial - J_initial^-1 * (A *  X_initial - Y)
//    in general X_next = X_prev - (J_prev^-1 * (A * X_prev - Y))*b
//                                 where 0 < b < 1, often 0.5, for stability
//
//  solved when  abs sum each row A * X_next -Y < epsilon
//
//  It may stall, stop if abs(X_next-X_prev)<epsilon
//  It may diverge, stop, indicate no solution
//                        (or try a different initial guess)
//  It may oscillate, stop, indicate no solution
//                        (or try a different initial guess)
//
//
//  The matrix equation for this case is:
//
//   x1     x2     x3     x4     x1*x2  x3*x4
//
// | A1,1   A1,2   A1,3   A1,4   A1,5   A1,6 | | x1    | |y1|
// | A2,1   A2,2   A2,3   A2,4   A2,5   A2,6 | | x2    | |y2|
// | A3,1   A3,2   A3,3   A3,4   A3,5   A3,6 | | x3    | |y3|
// | A4,1   A4,2   A4,3   A4,4   A4,5   A4,6 |*| x4    |=|y4|
// | A5,1   A5,2   A5,3   A5,4   A5,5   A5,6 | | x1*x2 | |y5|
// | A6,1   A6,2   A6,3   A6,4   A6,5   A6,6 | | x3*x4 | |y6|

//  The Jacobian, J is the numerically computed derivatives based on A, X_prev
//  The derivative coefficients may be computed once based on A and the
//  unknown variables. The numeric value plugs in the X_prev values.
//
//  J1,1 = A1,1 + A1,5*x2   deriv Row 1 wrt x1
//  J1,2 = A1,2 + A1,5*x1   deriv Row 1 wrt x2
//  J1,3 = A1,3 + A1,6*x4   deriv Row 1 wrt x3
//  J1,4 = A1,4 + A1,6*x3   deriv Row 1 wrt x4
//  J1,5 = A1,5             deriv Row 1 wrt x1, wrt x2
//  J1,6 = A1,6             deriv Row 1 wrt x3, wrt x4
//
//  J2,1 = A2,1 + A2,5*x2   deriv Row 2 wrt x1
//  J2,2 = A2,2 + A2,5*x1   deriv Row 2 wrt x2
//  J2,3 = A2,3 + A2,6*x4   deriv Row 2 wrt x3
//  J2,4 = A2,4 + A2,6*x3   deriv Row 2 wrt x4
//  J2,5 = A2,5             deriv Row 2 wrt x1, wrt x2
//  J2,6 = A2,6             deriv Row 2 wrt x3, wrt x4
//
//  J3,1 = A3,1 + A3,5*x2   deriv Row 3 wrt x1
//  J3,2 = A3,2 + A3,5*x1   deriv Row 3 wrt x2
//  J3,3 = A3,3 + A3,6*x4   deriv Row 3 wrt x3
//  J3,4 = A3,4 + A3,6*x3   deriv Row 3 wrt x4
//  J3,5 = A3,5             deriv Row 3 wrt x1, wrt x2
//  J3,6 = A3,6             deriv Row 3 wrt x3, wrt x4
//
//  etc.
//
//  J6,1 = A6,1 + A6,5*x2   deriv Row 6 wrt x1
//  J6,2 = A6,2 + A6,5*x1   deriv Row 6 wrt x2
//  J6,3 = A6,3 + A6,6*x4   deriv Row 6 wrt x3
//  J6,4 = A6,4 + A6,6*x3   deriv Row 6 wrt x4
//  J6,5 = A6,5             deriv Row 6 wrt x1, wrt x2
//  J6,6 = A6,6             deriv Row 6 wrt x3, wrt x4
//
//  Code or a data structure must be available to know the equation
//  of the entries in the Y vector. Symbolic computation of
//  derivatives is assumed to be available, when needed.
//

public class simeq_newton
{
  int n = 6; // number of equations
  double A[][] = new double[n][n];
  double Ja[][] = new double[n][n]; // inverted in place
  double Y[] = new double[n];
  double X_prev[] = new double[n];
  double X_temp[] = new double[n];
  double X_tmp2[] = new double[n];
  double X_next[] = new double[n];
  double X_soln[] = new double[n]; // usually not known. test case
  double Ja_test[][] = new double[n][n]; // just for debug
  double Ja_ident[][] = new double[n][n]; // just for debug


  // derivatives hand coded to load Ja

  public simeq_newton()
  {
    double eps = 1.0e-8;
    double b = 1.00; // stability factor
    double resid;  // residual from last iteration
    double presid; // residual from prior to last iteration
    double err;      // error from test solution

    System.out.println("simeq_newton running");
    // build test case
    for(int i=0; i<n; i++) // all subscripts zero based, one less than comments
    {
      for(int j=0; j<n; j++)
      {
	  A[i][j] = Math.random();
      }
    }
    // debug print
    for(int i=0; i<n; i++)
    {
      for(int j=0; j<n; j++)
      {
	System.out.println("A["+i+"]["+j+"]="+A[i][j]);
      }
    }
    System.out.println(" ");
    // choose x1=1.1 x2=1.2 x3=1.4 x4 =1.5, compute x1*x2, x3*x4
    X_soln[0] = 1.1;
    X_soln[1] = 1.2;
    X_soln[2] = 1.4;
    X_soln[3] = 1.5;
    X_soln[4] = X_soln[0]*X_soln[1];
    X_soln[5] = X_soln[2]*X_soln[3];
    // debug print
    for(int i=0; i<n; i++)
    {
      System.out.println("X_soln["+i+"]="+X_soln[i]);
    }
    System.out.println(" ");

    // set up Y
    for(int i=0; i<n; i++)
    {
      Y[i] = 0.0;
      for(int j=0; j<n; j++)
      {
	Y[i] += A[i][j]*X_soln[j];
      }
    }
    // debug print
    for(int i=0; i<n; i++)
    {
      System.out.println("Y["+i+"]="+Y[i]);
    }
    System.out.println(" ");

    // setup

    // initial condition
    X_prev[0] = 1.0;
    X_prev[1] = 1.0;
    X_prev[2] = 1.0;
    X_prev[3] = 1.0;
    X_prev[4] = 1.0;
    X_prev[5] = 1.0;
    // debug print
    for(int i=0; i<n; i++)
    {
      System.out.println("X_prev["+i+"]="+X_prev[i]);
    }
    System.out.println(" ");

    presid = 1.0e12;
    // iterate to find solution
    for(int itr=0; itr<4; itr++)
    {
      // compute residual
      resid = 0.0; 
      for(int i=0; i<n; i++)
      {
        X_temp[i] = 0.0;
        for(int j=0; j<n; j++)
        {
	  X_temp[i] += A[i][j]*X_prev[j];
        }
        X_temp[i] -= Y[i]; // X_temp used later if not solved
        resid += Math.abs(X_temp[i]);
      }
      // debug print
      for(int i=0; i<n; i++)
      {
	System.out.println("residual X_temp["+i+"]="+X_temp[i]);
      }
      System.out.println(" ");
      // for testing, compute error with solution
      err = 0.0;
      for(int i=0; i<n; i++)
      {
	  err += Math.abs(X_prev[i] - X_soln[i]);
      }
      System.out.println("itr "+itr+", prev="+presid+" residual="+resid+
                         ", err="+err);
      // check for convergence
      if(resid<eps) break;

      // compute Jacobian
      for(int i=0; i<n; i++) // all subscripts zero based
      {
        for(int j=0; j<n; j++)
        {
	  Ja[i][j] = A[i][j];
          if(j==0) Ja[i][j] += A[i][4]*X_prev[1];
          if(j==1) Ja[i][j] += A[i][4]*X_prev[0];
          if(j==2) Ja[i][j] += A[i][5]*X_prev[3];
          if(j==3) Ja[i][j] += A[i][5]*X_prev[2];
          Ja_test[i][j] = Ja[i][j]; // just for debug
        }
      }
      // debug print
      for(int i=0; i<n; i++)
      {
        for(int j=0; j<n; j++)
        {
	  System.out.println("Ja["+i+"]["+j+"]="+Ja[i][j]);
        }
      }
      System.out.println(" ");
      // invert Ja
      new invert(Ja);
      // debug print
      for(int i=0; i<n; i++)
      {
        for(int j=0; j<n; j++)
        {
	  System.out.println("Ja_inv["+i+"]["+j+"]="+Ja[i][j]);
        }
      }
      System.out.println(" ");
      // check invert
      for(int i=0; i<n; i++)
      {
        for(int j=0; j<n; j++)
        {
	  Ja_ident[i][j]=0.0;
	  for(int k=0; k<n; k++)
          {
	    Ja_ident[i][j] += Ja_test[i][k]*Ja[k][j];
	  }
        }
      }
      System.out.println(" ");
      for(int i=0; i<n; i++)
      {
        for(int j=0; j<n; j++)
        {
	  System.out.println("Ja_ident["+i+"]["+j+"]="+Ja_ident[i][j]);
        }
      }
      System.out.println(" ");

      // X_next = X_prev - (J_prev^-1 * (A X_prev - Y))*b
      // X_next = X_prev - (J_prev^-1 * (X_temp))*b
      // X_next = X_prev - (X_tmp2)*b

      for(int i=0; i<n; i++)
      {
	X_tmp2[i] = 0.0;
        for(int j=0; j<n; j++)
        {
	  X_tmp2[i] += Ja[i][j]*X_temp[j]; // transpose J_inv
        }
      }
      // debug print
      for(int i=0; i<n; i++)
      {
	System.out.println("change X_tmp2["+i+"]="+X_tmp2[i]);
      }
      System.out.println(" ");

      for(int i=0; i<n; i++)
      {
        X_next[i] = X_prev[i] - b*X_tmp2[i];
      }
      for(int i=0; i<n; i++)
      {
        System.out.println("X_next["+i+"]="+X_next[i]);
      }
      System.out.println(" ");

      // iterate
      presid = resid; 
      for(int i=0; i<n; i++)
      {
	  X_prev[i] = X_next[i];
      }
      System.out.println(" ");

    } // end iteration    
    System.out.println("simeq_newton finished");
  }

  public static void main (String[] args)
  {
    new simeq_newton();
  }
} // end class simeq_newton