/* test_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*x3, x4*x4*x4*x4, ...
 *                 A sparse matrix may be used, coefficients given
 *                 Y is vector of reals given
 *                 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 ... 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_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
 *
 *  J2,1 = A2,1 + A2,5*x2 + A2,6*x3 + A2,7*x4   deriv Row 2 wrt x1
 *  J2,2 = A2,2 + A2,5*x1 + A2,8*x3 + A2,9*x4   deriv Row 2 wrt x2
 *
 *  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 "simeq_newton4.h"

static void case1()
{
  /* build (double A[][], double Y[], int var1[], int var2[], double X[]) */
  int n;   /* number of linear unknowns */
  int nlin; /* number of nonlinear terms, even if zero coef */
  /* x0 + x1 + x0^2 + x1^2 + x0^3 + x1^3 + x0^4 +x1^4 = Y */
  int var1[] = { 0, 1, 0, 1}; /* zero based subscript */
  int var2[] = {-1,-1, 0, 1}; /* e.g. last is x1^4 */
  int var3[] = {-1,-1, 0, 1}; /* possible cube or 3 term */
  int var4[] = {-1,-1, 0, 1}; /* possible 4th or 4 term */
  /* could have int var5[] etc for higher powers */
  double A[4][4]; /* n+nlin */
  double Y[4];    /* n+nlin */
  double X[4];    /* n+nlin */
  double X_soln[4];
  char *Xname[2] = {"X0", "X1"}; /* all linear terms */
  int i, j;

  printf("test_simeq_newton4.c case 1 running \n");
  /* build first test case */
  n = 2;   /* number of linear unknowns */
  nlin = 2; /* number of nonlinear terms, even if zero coef */
  A[0][0] = drand48(); /* get rid of zero */
  A[0][1] = drand48(); /* get rid of very small */
  for(i=0; i<n; i++) /* equation*/
  {
    for(j=0; j<n+nlin; j++) /* variables then terms */
    {
      A[i][j] = drand48();
    }
  }
  printf("first A generated \n");
  for(i=0; i<n; i++) /* equation*/
  {
    for(j=0; j<n+nlin; j++) /* variables then terms */
    {
      printf("A[%2d][%2d]=%f \n", i, j, A[i][j]);
    }
  }

  /* choose x1=1.1 x2=2.3 compute  */
  /* arbitrary test solution for random A matrix */
  X_soln[0] = 1.1;
  X_soln[1] = 2.3;
  printf("solve system of equations A X = Y for X = \n");
  printf("variables  ");
  for(i=0; i<n; i++) printf("%s, ", Xname[i]);
  printf("terms:\n");
  for(i=n; i<n+nlin; i++)
  {
    if(var1[i]<0 ||var1[i]>=n || var2[i]>=n || var3[i]>=n || var4[i]>=n)
    {
      printf("var error  %d  %d  %d \n", var1[i], var2[i], var3[i], var4[i]);
      break;
    }
    printf("%s ",Xname[var1[i]]);
    if(var2[i]>=0) printf("* %s ", Xname[var2[i]]);
    if(var3[i]>=0) printf("* %s ", Xname[var3[i]]);
    if(var4[i]>=0) printf("* %s ", Xname[var4[i]]);
    printf("\n");
  }
  printf(" \n");
  for(i=n; i<n+nlin; i++)
  {
    X_soln[i] = X_soln[var1[i]]; /* at least one required */
    if(var2[i]>=0) X_soln[i] *= X_soln[var2[i]];
    if(var3[i]>=0) X_soln[i] *= X_soln[var3[i]];
    if(var4[i]>=0) X_soln[i] *= X_soln[var4[i]];
  }
  for(i=0; i<n+nlin; i++)
  {
    printf("X_soln[%2d]=%f \n",i ,X_soln[i]);
  }
  printf(" \n");

  /* set up Y */
  for(i=0; i<n; i++)
  {
    Y[i] = 0.0;
    for(j=0; j<n+nlin; j++)
    {
      Y[i] += A[i][j]*X_soln[j];
    }
  }
  for(i=0; i<n; i++)
  {
    printf("Y[%2d]=%f \n",i ,Y[i]);
  }
  printf(" \n");
  /* initial guess */
  for(i=0; i<n; i++) X[i] = 1.0;
   
  simeq_newton4(n, nlin, A, Y, var1, var2, var3, var4, X, 16, 0.000001, 4);

  printf("Returned solution \n");
  for(i=0; i<n; i++) printf("X[%2d]=%f,   err=%g \n", i, X[i], X[i]-X_soln[i]);
  printf("case1 finished \n\n");
} /* end case1 */

static void case2()
{
  /* build (double A[][], double Y[], int var1[], int var2[], double X[]) */
  int n;   /* number of linear unknowns */
  int nlin; /* number of nonlinear terms, even if zero coef */
  /* x0 + x1 + x0^2 + x1^2 + x0^3 + x1^3 + x0^4 +x1^4 = Y */
  int var1[] = { 0, 1, 0, 1, 0, 1, 0, 1}; /* zero based subscript */
  int var2[] = {-1,-1, 0, 1, 0, 1, 0, 1}; /* e.g. last is x1^4 */
  int var3[] = {-1,-1,-1,-1, 0, 1, 0, 1}; /* possible cube or 3 term */
  int var4[] = {-1,-1,-1,-1,-1,-1, 0, 1}; /* possible 4th or 4 term */
  /* could have int var5[] etc for higher powers */
  double A[8][8]; /* n+nlin */
  double Y[8];    /* n+nlin */
  double X[8];    /* n+nlin */
  double X_soln[8];
  char *Xname[2] = {"X0", "X1"}; /* all linear terms */
  int i, j;

  printf("test_simeq_newton4.c case 2 running \n");
  /* build second test case */
  n = 2;   /* number of linear unknowns */
  nlin = 6; /* number of nonlinear terms, even if zero coef */
  for(i=0; i<n; i++) /* equation*/
  {
    for(j=0; j<n+nlin; j++) /* variables then terms */
    {
      A[i][j] = drand48();
    }
  }
  printf("first A generated \n");
  for(i=0; i<n; i++) /* equation*/
  {
    for(j=0; j<n+nlin; j++) /* variables then terms */
    {
      printf("A[%2d][%2d]=%f \n", i, j, A[i][j]);
    }
  }

  /* choose x1=1.1 x2=2.3 compute  */
  /* arbitrary test solution for random A matrix */
  X_soln[0] = 1.1;
  X_soln[1] = 2.3;
  printf("solve system of equations A X = Y for X = \n");
  printf("variables  ");
  for(i=0; i<n; i++) printf("%s, ", Xname[i]);
  printf("terms:\n");
  for(i=n; i<n+nlin; i++)
  {
    if(var1[i]<0 ||var1[i]>=n || var2[i]>=n || var3[i]>=n || var4[i]>=n)
    {
      printf("var error  %d  %d  %d \n", var1[i], var2[i], var3[i], var4[i]);
      break;
    }
    printf("%s ",Xname[var1[i]]);
    if(var2[i]>=0) printf("* %s ", Xname[var2[i]]);
    if(var3[i]>=0) printf("* %s ", Xname[var3[i]]);
    if(var4[i]>=0) printf("* %s ", Xname[var4[i]]);
    printf("\n");
  }
  printf(" \n");
  for(i=n; i<n+nlin; i++)
  {
    X_soln[i] = X_soln[var1[i]]; /* at least one required */
    if(var2[i]>=0) X_soln[i] *= X_soln[var2[i]];
    if(var3[i]>=0) X_soln[i] *= X_soln[var3[i]];
    if(var4[i]>=0) X_soln[i] *= X_soln[var4[i]];
  }
  for(i=0; i<n+nlin; i++)
  {
    printf("X_soln[%2d]=%f \n",i ,X_soln[i]);
  }
  printf(" \n");

  /* set up Y */
  for(i=0; i<n; i++)
  {
    Y[i] = 0.0;
    for(j=0; j<n+nlin; j++)
    {
      Y[i] += A[i][j]*X_soln[j];
    }
  }
  for(i=0; i<n; i++)
  {
    printf("Y[%2d]=%f \n",i ,Y[i]);
  }
  printf(" \n");
  /* initial guess */
  for(i=0; i<n; i++) X[i] = 1.0;
   
  simeq_newton4(n, nlin, A, Y, var1, var2, var3, var4, X, 16, 0.000001, 4);

  printf("Returned solution \n");
  for(i=0; i<n; i++) printf("X[%2d]=%f,   err=%g \n", i, X[i], X[i]-X_soln[i]);
  printf("case2 finished \n\n");
} /* end case2 */


int main(int argc, char *argv[])
{
  case1();
  case2();
  return 0;
} /* end test_simeq_newton4.c */