/* simeq_newton4.c     solve nonlinear system of equations
 *                     method: newton iteration using Jacobian
 *                     use list for higher order terms
 *
 *  Given problem  A X = Y  where X may have terms x1, x2, x3, x4,
 *                 and higher order such as: x1*x2, x1*x1*x3*x3, x4*x4*x4*x4,
 *                  Y is vector of reals given
 *                  independent unknowns are x1, x2, x3, x4
 *
 *   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 - (J_prev^-1 * (A * X - 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)<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 is something like:
 *
 *    x1     x2     x3     x4     x1*x2 ... x4^4   (may be subset of nl terms) 
 *
 *  | 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 | | x4^4  | |y10|
 *
 *   The Jacobian, J is the numerically computed derivatives based on A, X
 *   The derivative coefficients may be computed once based on A and the
 *   unknown variables. The numeric value plugs in the X 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.
 *
 *   j4,1 ... j4,10  should be last ones needed? others not independent
 *
 *   Code or a data structure must be available to know the equation
 *   of the entries in the Y vector. Numeric computation of
 *   derivatives is used, based on var1, var2, var3, var4.
 */

#include <stdio.h>
#include "simeq_newton4.h"
#include "invert.h"

#undef abs
#define abs(x) ((x)<0?(-(x)):(x))

void simeq_newton4(int n, int nlin, double A[n][n+nlin], double Y[],
                   int var1[], int var2[], int var3[], int var4[],
                   double X[], int maxiter, double eps, int monitor)
{
  /* derivatives computed from var1 and var2 and var3 and var4 to */
  /* load Ja, initial guess in X[] */

  double b = 1.00; /* stability factor, could be parameter */
  double resid;  /* residual from last iteration */
  double presid; /* residual from prior to last iteration */
  double Ja[n][n]; /* inverted in place */
  double X_resid[n];
  double X_tmp2[n];
  double X_next[n];
  int i, j, k, itr;

  if(monitor>0) printf("simeq_newton4 running \n");

  /* setup, assume var's correct */
  /* nonlinear terms X initialized */
  for(i=n; i<n+nlin; i++)
  {
    X[i] = X[var1[i]]; /* can not be empty */
    if(var2[i]>=0) X[i] *= X[var2[i]];
    if(var3[i]>=0) X[i] *= X[var3[i]];
    if(var4[i]>=0) X[i] *= X[var4[i]];
  }
  /* debug print */
  if(monitor>1)
  {
    printf("%d equations, %d variables, with %d nonlinear terms\n",n, n, nlin);
    for(i=0; i<n+nlin; i++)
    {
      printf("X[%2d]=%f \n", i, X[i]);
    }
    printf(" \n");
  }
  presid = 1.0e12;
  /* iterate to find solution */
  for(itr=0; itr<maxiter; itr++)
  {
    /* compute residual */
    resid = 0.0; 
    for(i=0; i<n; i++)
    {
      X_resid[i] = 0.0;
      for(j=0; j<n+nlin; j++)
      {
	X_resid[i] += A[i][j]*X[j];
      }
      X_resid[i] -= Y[i]; /* X_resid used later if not solved */
      resid += abs(X_resid[i]);
    }
    if(monitor>1)
    {
      for(i=0; i<n; i++)
      {
	printf("residual X_resid[%2d]=%g \n", i, X_resid[i]);
      }
      printf(" \n");
    }

    /* check for convergence */
    printf("simeq_newton4 itr=%d, prev=%g, residual=%g \n",
           itr, presid, resid);
    if(resid<eps) break;

    /* compute Jacobian */
    for(i=0; i<n; i++) /* equation */
    {
      for(j=0; j<n; j++) /* variable */
      {
	Ja[i][j] = A[i][j]; /* each variable in each equation */
        for(k=n; k<n+nlin; k++)
        {
          /* add derivative of nonlinear term, unindented to look better */
  /* fourth power terms, one case */
  if(var1[k]==j && var2[k]==j && var3[k]==j && var4[k]==j)      /* X^4 */
    Ja[i][j] += A[i][k]*4.0*X[var2[k]]*X[var3[k]]*X[var4[k]];   /* 4X^3 */

  /* third power terms, four cases with sub case */
  else if(var1[k]==j && var2[k]==j && var3[k]==j && var4[k]>=0) /* X^3 x  123*/
    Ja[i][j] += A[i][k]*3.0*X[var2[k]]*X[var3[k]]*X[var4[k]];   /* 3X^2 x */
  else if(var1[k]==j && var2[k]==j && var3[k]==j && var4[k]<0)  /* X^3  */
    Ja[i][j] += A[i][k]*3.0*X[var2[k]]*X[var3[k]];              /* 3X^2 */
  else if(var1[k]==j && var2[k]==j && var4[k]==j && var3[k]>=0) /* X^3 x  124*/
    Ja[i][j] += A[i][k]*3.0*X[var2[k]]*X[var4[k]]*X[var3[k]];   /* 3X^2 x */
  else if(var1[k]==j && var2[k]==j && var4[k]==j && var3[k]<0)  /* X^3  */
    Ja[i][j] += A[i][k]*3.0*X[var2[k]]*X[var4[k]];              /* 3X^2 */
  else if(var1[k]==j && var3[k]==j && var4[k]==j && var2[k]>=0) /* X^3 x  134*/
    Ja[i][j] += A[i][k]*3.0*X[var3[k]]*X[var4[k]]*X[var2[k]];   /* 3X^2 x */
  else if(var1[k]==j && var3[k]==j && var4[k]==j && var2[k]<0)  /* X^3  */
    Ja[i][j] += A[i][k]*3.0*X[var3[k]]*X[var4[k]];              /* 3X^2 */
  else if(var2[k]==j && var3[k]==j && var4[k]==j && var1[k]>=0) /* X^3 x  234*/
    Ja[i][j] += A[i][k]*3.0*X[var3[k]]*X[var4[k]]*X[var1[k]];   /* 3X^2 x */
  else if(var2[k]==j && var3[k]==j && var4[k]==j && var1[k]<0)  /* X^3  */
    Ja[i][j] += A[i][k]*3.0*X[var3[k]]*X[var4[k]];              /* 3X^2 */

  /* second power terms, six cases with three sub cases */       /* 12 ?? */
  else if(var1[k]==j && var2[k]==j && var3[k]>=0 && var4[k]>=0)  /* X^2 x x */
    Ja[i][j] += A[i][k]*2.0*X[var2[k]]*X[var3[k]]*X[var4[k]];    /* 2X x x */
  else if(var1[k]==j && var2[k]==j && var3[k]>=0 && var4[k]<0)   /* X^2 x */
    Ja[i][j] += A[i][k]*2.0*X[var2[k]]*X[var3[k]];               /* 2X x */
  else if(var1[k]==j && var2[k]==j && var3[k]<0 && var4[k]>=0)   /* X^2 x */
    Ja[i][j] += A[i][k]*2.0*X[var2[k]]*X[var4[k]];               /* 2X x */
  else if(var1[k]==j && var2[k]==j && var3[k]<0 && var4[k]<0 )   /* X^2 */
    Ja[i][j] += A[i][k]*2.0*X[var2[k]];                          /* 2X */
                                                                 /* 13 ?? */
  else if(var1[k]==j && var3[k]==j && var2[k]>=0 && var4[k]>=0)  /* X^2 x x */
    Ja[i][j] += A[i][k]*2.0*X[var3[k]]*X[var2[k]]*X[var4[k]];    /* 2X x x */
  else if(var1[k]==j && var3[k]==j && var2[k]>=0 && var4[k]<0)   /* X^2 x */
    Ja[i][j] += A[i][k]*2.0*X[var3[k]]*X[var2[k]];               /* 2X x */
  else if(var1[k]==j && var3[k]==j && var2[k]<0 && var4[k]>=0)   /* X^2 x */
    Ja[i][j] += A[i][k]*2.0*X[var3[k]]*X[var4[k]];               /* 2X x */
  else if(var1[k]==j && var3[k]==j && var2[k]>=0 && var4[k]<0)   /* X^2 */
    Ja[i][j] += A[i][k]*2.0*X[var3[k]];                          /* 2X */
                                                                 /* 14 ?? */
  else if(var1[k]==j && var4[k]==j && var2[k]>=0 && var3[k]>=0)  /* X^2 x x */
    Ja[i][j] += A[i][k]*2.0*X[var4[k]]*X[var2[k]]*X[var3[k]];    /* 2X x x */
  else if(var1[k]==j && var4[k]==j && var2[k]>=0 && var3[k]<0)   /* X^2 x */
    Ja[i][j] += A[i][k]*2.0*X[var4[k]]*X[var2[k]];               /* 2X x */
  else if(var1[k]==j && var4[k]==j && var2[k]<0 && var3[k]>=0)   /* X^2 x */
    Ja[i][j] += A[i][k]*2.0*X[var4[k]]*X[var3[k]];               /* 2X x */
  else if(var1[k]==j && var4[k]==j && var2[k]<0 && var3[k]<0 )   /* X^2 */
    Ja[i][j] += A[i][k]*2.0*X[var4[k]];                          /* 2X */
                                                                 /* 23 ?? */
  else if(var2[k]==j && var3[k]==j && var1[k]>=0 && var4[k]>=0)  /* X^2 x x */
    Ja[i][j] += A[i][k]*2.0*X[var3[k]]*X[var1[k]]*X[var4[k]];    /* 2X x x */
  else if(var2[k]==j && var3[k]==j && var1[k]>=0 && var4[k]<0)   /* X^2 x */
    Ja[i][j] += A[i][k]*2.0*X[var3[k]]*X[var1[k]];               /* 2X x */
  else if(var2[k]==j && var3[k]==j && var1[k]<0 && var4[k]>=0)   /* X^2 x */
    Ja[i][j] += A[i][k]*2.0*X[var3[k]]*X[var4[k]];               /* 2X x */
  else if(var2[k]==j && var3[k]==j && var1[k]<0 && var4[k]<0)    /* X^2 */
    Ja[i][j] += A[i][k]*2.0*X[var3[k]];                          /* 2X */
                                                                 /* 24 ?? */
  else if(var2[k]==j && var4[k]==j && var1[k]>=0 && var3[k]>=0)  /* X^2 x x */
    Ja[i][j] += A[i][k]*2.0*X[var4[k]]*X[var1[k]]*X[var3[k]];    /* 2X x x */
  else if(var2[k]==j && var4[k]==j && var1[k]>=0 && var3[k]<0)   /* X^2 x */
    Ja[i][j] += A[i][k]*2.0*X[var4[k]]*X[var1[k]];               /* 2X x */
  else if(var2[k]==j && var4[k]==j && var1[k]<0 && var3[k]>=0)   /* X^2 x */
    Ja[i][j] += A[i][k]*2.0*X[var4[k]]*X[var3[k]];               /* 2X x */
  else if(var2[k]==j && var4[k]==j && var1[k]<0 && var3[k]<0)    /* X^2 */
    Ja[i][j] += A[i][k]*2.0*X[var4[k]];                          /* 2X */
                                                                 /* 34 ?? */
  else if(var3[k]==j && var4[k]==j && var1[k]>=0 && var2[k]>=0)  /* X^2 x x */
    Ja[i][j] += A[i][k]*2.0*X[var4[k]]*X[var1[k]]*X[var2[k]];    /* 2X x x */
  else if(var3[k]==j && var4[k]==j && var1[k]>=0 && var2[k]<0)   /* X^2 x */
    Ja[i][j] += A[i][k]*2.0*X[var4[k]]*X[var1[k]];               /* 2X x */
  else if(var3[k]==j && var4[k]==j && var1[k]<0 && var2[k]>=0)   /* X^2 x */
    Ja[i][j] += A[i][k]*2.0*X[var4[k]]*X[var2[k]];               /* 2X x */
  else if(var3[k]==j && var4[k]==j && var1[k]<0 && var2[k]<0)    /* X^2 */
    Ja[i][j] += A[i][k]*2.0*X[var4[k]];                          /* 2X */

  /* first power, four cases with three sub cases */
  else if(var1[k]==j) /* higher powers eliminated above */
  {
    if(var2[k]>=0) Ja[i][j] += A[i][k]*X[var2[k]];
    if(var3[k]>=0) Ja[i][j] += A[i][k]*X[var3[k]];
    if(var4[k]>=0) Ja[i][j] += A[i][k]*X[var4[k]];
  }
  else if(var2[k]==j)
  {
    if(var1[k]>=0) Ja[i][j] += A[i][k]*X[var1[k]];
    if(var3[k]>=0) Ja[i][j] += A[i][k]*X[var3[k]];
    if(var4[k]>=0) Ja[i][j] += A[i][k]*X[var4[k]];
  }
  else if(var3[k]==j)
  {
    if(var1[k]>=0) Ja[i][j] += A[i][k]*X[var1[k]];
    if(var2[k]>=0) Ja[i][j] += A[i][k]*X[var2[k]];
    if(var4[k]>=0) Ja[i][j] += A[i][k]*X[var4[k]];
  }
  else if(var4[k]==j)
  {
    if(var1[k]>=0) Ja[i][j] += A[i][k]*X[var1[k]];
    if(var2[k]>=0) Ja[i][j] += A[i][k]*X[var2[k]];
    if(var3[k]>=0) Ja[i][j] += A[i][k]*X[var3[k]];
  }
        } /* end k    end of unindenting*/
      } /* end j */
    } /* end i */
    if(monitor>2)
    {
      printf("Ja computed \n");
      for(i=0; i<n; i++) /* equation */
      {
        for(j=0; j<n; j++) /* variable */
        {
          printf("Ja[%2d][%2d]=%f \n", i, j, Ja[i][j]);
        }
      }
    } /* end debug */
    /* invert Ja */
    invert(n, (double *)Ja); /* Ja must be n by n packed */
    if(monitor>3)
    {
      printf("Ja inverted \n");
      for(i=0; i<n; i++) /* equation */
      {
        for(j=0; j<n; j++) /* variable */
        {
          printf("invJa[%2d][%2d]=%f \n", i, j, Ja[i][j]);
        }
      }
    } /* end debug */

    /* X_resid = A X - Y        error vector, computed above */
    /* X_tmp2 = J^-1 * X_resid  raw correction vector */
    /* X_next = X - (X_tmp2)*b weighted correction vector */
    /* X      = X_next         next X with computed higher terms */

    for(i=0; i<n; i++)
    {
      X_tmp2[i] = 0.0;
      for(j=0; j<n; j++)
      {
	X_tmp2[i] += Ja[i][j]*X_resid[j]; 
      }
    }
    /* debug print */
    if(monitor>3)
    {
      for(i=0; i<n; i++)
      {
	printf("change X_tmp2[%2d]=%f \n", i, X_tmp2[i]);
      }
      printf(" \n");
    } /* end debug */
    for(i=0; i<n; i++)
    {
      X_next[i] = X[i] - b*X_tmp2[i];
    }
    for(i=n; i<n+nlin; i++)  
    {
      X_next[i] = X_next[var1[i]]; /* can not be empty */
      if(var2[i]>=0) X_next[i] *= X_next[var2[i]];
      if(var3[i]>=0) X_next[i] *= X_next[var3[i]];
      if(var4[i]>=0) X_next[i] *= X_next[var4[i]];
    }
    if(monitor>2)
    {
      for(i=0; i<n+nlin; i++)
      {
        printf("X_next[%2d]=%f \n", i, X_next[i]);
      }
      printf(" \n");
    } /* end debug */

    /* iterate */
    presid = resid; 
    for(i=0; i<n+nlin; i++)
    {
      X[i] = X_next[i];
    }
    if(monitor>3) printf(" \n");
  } /* end iteration */    

  if(monitor>0) printf("simeq_newton4 finished \n");
} /* end simeq_newton4.c */