/* simeq_newton2.c  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, x1*x3, x1*x4, x2*x3, x2*x4, x3*x4       */
/*                 A is 10 by 10 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 products     */
/*               compute terms of Y using Y = A X                          */
/*                                                                         */
/*  Solve by initial guess at values of x1, x2, x3, x4 computing products  */
/*    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,10  | | x1    | |y1 |        */
/* | A2,1   A2,2   A2,3   A2,4   A2,5 ...  A2,10  | | x2    | |y2 |        */
/* | A3,1   A3,2   A3,3   A3,4   A3,5 ...  A3,10  | | x3    | |y3 |        */
/* | A4,1   A4,2   A4,3   A4,4   A4,5 ...  A4,10  |*| x4    |=|y4 |        */
/* | A5,1   A5,2   A5,3   A5,4   A5,5 ...  A5,10  | | x1*x2 | |y5 |        */
/* | A6,1   A6,2   A6,3   A6,4   A6,5 ...  A6,10  | | x1*x3 | |y6 |        */
/* | ...                                          | |       | |   |        */
/* | A10,1  A10,2  A10,3  A10,4  A10,5 ... A10,10 | | x3*x4 | |y10|        */
/*                                                                         */
/*  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 + A1,6*x3 + A1,7*x4   deriv Row 1 wrt x1         */
/*  J1,2 = A1,2 + A1,5*x1 + A1,8*x3 + A1,9*x4   deriv Row 1 wrt x2         */
/*  J1,3 = A1,3 + A1,6*x1 + A1,8*x2 + A1,10*x4  deriv Row 1 wrt x3         */
/*  J1,4 = A1,4 + A1,7*x1 + A1,9*x2 + A1,10*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 x1, wrt x3 */
/*  J1,7 = A1,7                                 deriv Row 1 wrt x1, wrt x4 */
/*  J1,8 = A1,8                                 deriv Row 1 wrt x2, wrt x3 */
/*  J1,9 = A1,9                                 deriv Row 1 wrt x2, wrt x4 */
/*  J1,10 = A1,10                               deriv Row 1 wrt x3, wrt x4 */
/*                                                                         */
/*  etc.                                                                   */
/*                                                                         */
/*  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.                   */
/*                                                                         */

#include <stdio.h>
#include <stdlib.h>
#include "udrnrt.h"
#include "invert.h"

#undef  abs
#define abs(a)  ((a)<0.0?(-(a)):(a))

#define n 10  /* number of equations */

int main(int argc, char *argv[])
{
  double A[n][n];
  double Ja[n][n];  /* inverted in place */
                    /* derivatives hand coded to load Ja */
  double Y[n];
  double X_prev[n];
  double X_temp[n];
  double X_tmp2[n];
  double X_next[n];
  double X_soln[n]; /* usually not known. test case */
  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 */
  int i, j, itr;

  printf("simeq_newton2.c running \n");
  /* build test case, using a random A matrix */
  for(i=0; i<n; i++) /* all subscripts zero based, */
                     /* one less than comments     */
  {
    for(j=0; j<n; j++)
    {
      A[i][j] = udrnrt();
    }
  }
  printf("random A generated \n");
  /* 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]; /* nonlinear terms */
  X_soln[5] = X_soln[0]*X_soln[2];
  X_soln[6] = X_soln[0]*X_soln[3];
  X_soln[7] = X_soln[1]*X_soln[2];
  X_soln[8] = X_soln[1]*X_soln[3];
  X_soln[9] = X_soln[2]*X_soln[3];
  /* debug print */
  for(i=0; i<n; i++)
  {
    printf("X_soln[%d]=%f \n", i, X_soln[i]);
  }
  printf("\n");

  /* set up Y, for test we know solution */
  for(i=0; i<n; i++)
  {
    Y[i] = 0.0;
    for(j=0; j<n; j++)
    {
      Y[i] += A[i][j]*X_soln[j];
    }
  }
  /* debug print */
  for(i=0; i<n; i++)
  {
    printf("Y[%d]=%f \n", i, Y[i]);
  }
  printf("\n");

  /* setup */

  /* initial condition */
  for(i=0; i<n; i++) X_prev[i] = 1.0;
  /* debug print */
  for(i=0; i<n; i++)
  {
    printf("X_prev[%d]=%f \n", i, X_prev[i]);
  }
  printf("\n");

  presid = 1.0e12;
  /* iterate to find solution */
  for(itr=0; itr<10; itr++)
  {
    /* compute residual */
    resid = 0.0; 
    for(i=0; i<n; i++)
    {
      X_temp[i] = 0.0;
      for(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 += abs(X_temp[i]);
    }
    /* debug print */
    for(i=0; i<n; i++)
    {
      printf("residual X_temp[%d]=%f \n", i, X_temp[i]);
    }
    printf("\n");
    /* for testing, compute error with solution */
    err = 0.0;
    for(i=0; i<n; i++)
    {
      err += abs(X_prev[i] - X_soln[i]);
    }
    printf("itr %d, prevres=%g, residual=%g, err=%g \n",
           itr, presid, resid, err);
    /* check for convergence */
    if(resid<eps) break;

    /* compute Jacobian */
    for(i=0; i<n; i++) /* all subscripts zero based */
    {
      for(j=0; j<n; j++)
      {
        Ja[i][j] = A[i][j];
        if(j==0) Ja[i][j] += A[i][4]*X_prev[1] + A[i][5]*X_prev[2] +
      	                     A[i][6]*X_prev[3];
        if(j==1) Ja[i][j] += A[i][4]*X_prev[0] + A[i][7]*X_prev[2] +
                             A[i][8]*X_prev[3];
        if(j==2) Ja[i][j] += A[i][5]*X_prev[0] + A[i][7]*X_prev[1] +
                             A[i][9]*X_prev[3];
        if(j==3) Ja[i][j] += A[i][6]*X_prev[0] + A[i][8]*X_prev[1] +
                             A[i][9]*X_prev[2];
      }
    }
    printf("Ja computed \n");
    /* invert Ja */
    invert(n, (double *)Ja);
    printf("Ja inverted \n");

    /* 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(i=0; i<n; i++)
    {
      X_tmp2[i] = 0.0;
      for(j=0; j<n; j++)
      {
        X_tmp2[i] += Ja[i][j]*X_temp[j]; 
      }
    }
    /* debug print */
    for(i=0; i<n; i++)
    {
      printf("change X_tmp2[%d]=%f \n", i, X_tmp2[i]);
    }
    printf("\n");

    for(i=0; i<n; i++)
    {
      X_next[i] = X_prev[i] - b*X_tmp2[i];
    }
    for(i=0; i<n; i++)
    {
      printf("X_next[%d]=%f \n", i, X_next[i]);
    }
    printf("\n");

    /* iterate */
    presid = resid; 
    for(i=0; i<n; i++)
    {
      X_prev[i] = X_next[i];
    }
    printf("\n");

  } /* end iteration */    
  printf("simeq_newton2 finished \n");
  return 0;
} /* end simeq_newton2.c */