/* test_simeq_newton3.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, x1*x4, x2*x3, ...
 *                 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 ... x3*x4  (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 | | 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
 *
 *  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_newton3.h"

int main(int argc, char *argv[])
{
  /* 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 */
  int var1[] = { 0, 1, 2, 3, 0, 0, 0, 1, 1, 2}; /* zero based subscript */
  int var2[] = {-1,-1,-1,-1, 1, 2, 3, 2, 3, 3}; /* e.g. last is x3*x4 */
  int var3[] = {-1,-1,-1,-1,-1,-1,-1,-1,-1,-1}; /* possible cube or 3 term */
  /* could have int var4[] etc for higher powers */
  double A[10][10];
  double A1[2][4]; /* fourth test case */
  double Y[10];
  double X[10];
  double X_soln[10];
  char *Xname[4] = {"X0", "X1", "X2", "X3"}; /* all linear terms */
  int i, j;

  printf("test_simeq_newton3.c running \n");
  /* build first test case */
  n = 4;   /* 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=1.2 x3=1.4 x4 =1.5, compute x1*x2, x3*x4 */
  /* arbitrary test solution for random A matrix */
  X_soln[0] = 1.1;
  X_soln[1] = 1.2;
  X_soln[2] = 1.4;
  X_soln[3] = 1.5;
  printf("solve system of equations A X = Y for X = \n");
  for(i=0; i<n; i++) printf("%s, ", Xname[i]);
  for(i=n; i<n+nlin; i++)
  {
    if(var1[i]<0 ||var1[i]>=n || var2[i]>=n || var3[i]>=n)
    {
      printf("var error  %d  %d  %d \n", var1[i], var2[i], var3[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]]);
    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]];
  }
  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_newton3(n, nlin, A, Y, var1, var2, var3, X, 6, 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("first test finished \n\n");

  /* second  test case
   * int var1[] = { 0, 1, 2, 3, 0, 1 ,2, 3, 0, 2};    zero based subscript
   * int var2[] = {-1,-1,-1,-1, 0, 1, 2, 3, 1, 3};    e.g. last is x3*x4
   * int var3[] = {-1,-1,-1,-1,-1,-1,-1,-1,-1,-1};
   *                            --squared--
   */
  n = 4;   /* number of linear unknowns */
  nlin = 6; /* number of nonlinear terms, even if zero coef */
  var1[4]=0; var2[4]=0; /* var3 initialized to -1 */
  var1[5]=1; var2[5]=1;
  var1[6]=2; var2[6]=2;
  var1[7]=3; var2[7]=3;
  var1[8]=0; var2[8]=1;
  var1[9]=2; var2[9]=3;

  for(i=0; i<n; i++) /* all subscripts zero based, one less than comments */
  {
    for(j=0; j<n+nlin; j++)
    {
      A[i][j] = drand48();
    }
  }
  printf("second 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=1.2 x3=1.4 x4 =1.5, compute x1*x1, x2*x2 */
  /* arbitrary test solution for random A matrix */
  X_soln[0] = 1.1;
  X_soln[1] = 1.2;
  X_soln[2] = 1.4;
  X_soln[3] = 1.5;
  printf("solve system of equations A X = Y for X = \n");
  for(i=0; i<n; i++) printf("%s, ", Xname[i]);
  for(i=n; i<n+nlin; i++)
  {
    if(var1[i]<0 ||var1[i]>=n || var2[i]>=n || var3[i]>=n)
    {
      printf("var error  %d  %d  %d \n", var1[i], var2[i], var3[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]]);
    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]];
  }
  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_newton3(n, nlin, A, Y, var1, var2, var3, X, 6, 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("second test finished \n\n");

  /* third  test case
   * int var1[] = { 0, 1, 2, 3, 0, 1 ,2, 3, 0, 1};   zero based subscript
   * int var2[] = {-1,-1,-1,-1, 0, 1, 2, 3, 1, 2};   e.g. last is
   * int var3[] = {-1,-1,-1,-1, 0, 1, 2, 3, 2, 3};   x2*x3*x4
   *                            ---cubed---
   */
  n = 4;   /* number of linear unknowns */
  nlin = 6; /* number of nonlinear terms, even if zero coef */
  var1[4]=0; var2[4]=0; var3[4]=0;
  var1[5]=1; var2[5]=1; var3[5]=1;
  var1[6]=2; var2[6]=2; var3[6]=2;
  var1[7]=3; var2[7]=3; var3[7]=3;
  var1[8]=0; var2[8]=1; var3[8]=2;
  var1[9]=1; var2[9]=2; var3[9]=3;

  for(i=0; i<n; i++) /* equation*/
  {
    for(j=0; j<n; j++) /* variables followed by terms */
    {
      A[i][j] = 1.0; /* drand48(); */
    }
  }
  for(i=0; i<n; i++) /* equation*/
  {
    for(j=n; j<n+nlin; j++) /* variables followed by terms */
    {
      A[i][j] = 0.0; /* drand48(); */
    }
    A[i][i+n] = 1.0;
  }

  /* Bad initial guess of all X = 1.0 for non random, Ja singular */

  printf("third 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=1.2 x3=1.4 x4 =1.5, compute x1*x1*x1, ... */
  /* arbitrary test solution for random A matrix */
  X_soln[0] = 1.1;
  X_soln[1] = 1.2;
  X_soln[2] = 1.4;
  X_soln[3] = 1.5;
  printf("solve system of equations A X = Y for X = \n");
  for(i=0; i<n; i++) printf("%s, ", Xname[i]);
  for(i=n; i<n+nlin; i++)
  {
    if(var1[i]<0 ||var1[i]>=n || var2[i]>=n || var3[i]>=n)
    {
      printf("var error  %d  %d  %d \n", var1[i], var2[i], var3[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]]);
    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]];
  }
  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, simeq_newton3 fills in terms */
  for(i=0; i<n; i++) X[i] = 0.5;
   
  simeq_newton3(n, nlin, A, Y, var1, var2, var3, X, 6, 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("third test finished \n");

  printf("Fourth test case \n");
  n = 2;
  nlin = 2;
  var1[2]=0; var2[2]=0; var3[2]=-1;
  var1[3]=1; var2[3]=1; var3[5]=-1;

  A1[0][0] = 1.0;
  A1[0][1] = 1.0;
  A1[0][2] = 1.0; /* x[0]^2 */
  A1[0][3] = 0.0; /* x[1]^2 */
  A1[1][0] = 1.0;
  A1[1][1] = 1.0;
  A1[1][2] = 0.0;
  A1[1][3] = 1.0;

  printf("fourth A generated \n");
  /* choose x1=1 x2=1  compute x1*x1, ... */
  X_soln[0] = 1.0;
  X_soln[1] = 2.0;
  printf("solve system of equations A X = Y for X = \n");
  for(i=0; i<n; i++) printf("%s, ", Xname[i]);
  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]];
  }
  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] += A1[i][j]*X_soln[j];
    }
  }
  for(i=0; i<n; i++)
  {
    printf("Y[%2d]=%f \n",i ,Y[i]);
  }
  printf(" \n");
  /* initial guess */
  X[0] = 0.5;
  X[1] = 0.5;

  simeq_newton3(n, nlin, A1, Y, var1, var2, var3, X, 6, 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("fourth test finished \n");

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