/* equation2_nl.c  handle reciprocal terms
 *  f1(x1, x2, x3) = a1*x1 + a2*x2^2 + a3*x3^3 + a4/x4 + a5/x5^2  = Y
 *  f1'(x1) = a1
 *  f1'(x2) = a2*2*x2
 *  f1'(x3) = a3*3*x3^2
 *  f1'(x4) = -a4/x4^2
 *  f1'(x5) = -a5*2/x5^3
 *
 *  the other coefficients are shown in matrix form
 *
 * in matrix form, the equations are: A * X = Y
 * | 5  4  3  2  1 |   |  x1  |   | 205.1488 |
 * | 4  5  3  2  1 |   | x2^2 |   | 217.6888 |
 * | 3  3  3  2  1 | * | x3^3 | = | 177.3088 |
 * | 1  2  2  2  1 |   | 1/x4 |   | 113.5318 |
 * | 2  3  1  1  2 |   |1/x5^2|   | 100.3626 |
 *         A         *    Xt    =      Y
 *
 * The Y values are for specific unknown values of x1, ... ,x5
 *
 * in matrix form, the analytic Jacobian is:
 *     |    1    |
 *     |  2*x2   |
 * A * |  3*x3^2 | =  Ja
 *     | -1/x4^2 |
 *     | -2/x5^3 |
 * A *     D      =  Ja
 *     deriv of X
 *     wrt x1,x2,x3,x4,x4
 *
 * A numeric Jacobian can be approximated if the analytic is unknown.
 *
 * Newton iteration:
 * x_next = x-fctr*(A*Xt-Y)/Ja,  /Ja is times transpose inverse of Ja
 * x is individual variables, unknowns. Xt is equation terms
 * fctr may need to be less than 1 for stability
 * fctr may be adjusted by heuristics.
 * Ja is, in general, dependent on all variables
 *
 * The goal is to zero the sum of the absolute values of
 * all of the equations. This is the computed total error.
 * Thus, solving a system of nonlinear equations has become
 * a problem of finding roots.
 *
 * Because: if anything can go wrong then it will go wrong -
 * Automatic control of the change factor, fctr, needs to be added:
 * x_next = x - fctr*(A*Xt-Y)/Ja,  /Ja is times transpose inverse of Ja
 * Start with fctr = 1.0 and reduce fctr by half if there is no
 * improvement. Then, increase fctr by sqrt[2] if two iterations
 * show improvement. (Other heuristics may be used)
 *
 * Due to the possibility of multiple solutions and just plain wild
 * oscillations of x values, bound the minimum and maximum of
 * all variables when any information is known.
 * This equation has all unique 'terms' and thus all are included
 * in the iterative computation.
 * An equation with an  x1 term,  an  x2 term  and any terms involving
 * these two, would have nx=2.  x1^2 term , x2^3 term , x1*x2 term
 * are completely defined by x1 and x2.
 * The Jacobian is based on each variable and all the terms in
 * the equations to be solved.
 */

#include <stdio.h>
#include "inverse.h"
#include <math.h>
#undef  abs
#define abs(a) ((a)<0.0?(-(a)):(a))

/* functions representing Y, given a set of x's and a's */
static double f1(double A[], double X[]) /* A is Arow */
{
  return A[0]*X[0] + A[1]*X[1]*X[1] + A[2]*X[2]*X[2]*X[2] + 
    A[3]/X[3] + A[4]/(X[4]*X[4]);
} /* end f1 */

int main(int argc, char *argv[])
{
  int nitr = 20;       /* maximum number of times to try */
  int Improve = 0;
  int Improve_Count = 0;
  int fctr_Count = 0;
  int nx = 5;          /* number of unique unknowns, x's */
  double A[5][5] = {{5.0, 4.0, 3.0, 2.0, 1.0},
      	            {4.0, 5.0, 3.0, 2.0, 1.0},
	            {3.0, 3.0, 3.0, 2.0, 1.0},
	            {1.0, 2.0, 2.0, 2.0, 1.0},
	            {2.0, 3.0, 1.0, 1.0, 2.0}};
  double Arow[5];      /* temporary row of A */
  double Xt[5];        /* X terms, A * Xt = Y */
                       /* with X being nonlinear terms */
  double Y[5];         /* RHS of equations */
  double D[5];         /* derivative of X terms */
  double Ja[5][5];     /* Jacobian to be inverted */
  double JI[5][5];     /* Jacobian  inverted */
  double F[5];         /* A*Xt-Y error to be multiplied */
                       /* by Ja inverse transpose */
  double X[5];         /* initial guess and solution */
  double fctr = 0.5;   /* can be unstable */
  double Perror;       /* previous, total error */
  double Terror;       /* total error, sum of absolute values */
  double S[5] = {5.1, 4.2, 3.3, 2.4, 1.5}; /* desired solution */
  char * Xts[5] = {"  x1  ", " x2^2 ", " x3^3 ", " 1/x4 ", "1/x5^2"};
  int i, j, k, itr;

  printf("equation2_nl.c running \n");
  printf("solve system of nonlinear equations \n");
  for(i=0; i<nx; i++) /* compute Y accurately, given A and solution S */
  {
    for(j=0; j<nx; j++) Arow[j] = A[i][j];
    Y[i] = f1(Arow,S);
  }
  for(i=0; i<nx; i++)
  {
    printf("|%7.4f,%7.4f,%7.4f,%7.4f,%7.4f | |%s| |%8.4f |\n",
	   A[i][0], A[i][1], A[i][2], A[i][3], A[i][4], Xts[i], Y[i]);
  }
  printf("                     A                    *    Xt    =     Y \n");
  printf("guess initial X(1,5) \n");
  printf("compute Xt = | x1  x2^2  x3^3  1/x4  1/x5^2 | \n");
  printf("compute derivative D = | 1  2*x2 3*x3^2 -1/x4^2 -2/x5^3  | \n");
  printf("compute the Jacobian Ja = A * D and invert into JI \n");
  printf("iterate X_next = X - fctr*(A*Xt-Y)*Ja^-1  inverse transpose \n");
  printf("no guarantee of solution or unique solution \n");
  printf("sum absolute value A*Xt-Y should go to zero \n");
  printf("initial fctr, for stability = %g \n", fctr);
  printf("\n ");

  for(i=0; i<nx; i++)
  {
    X[i] = 1.0; /* initial guess */
  }
  printf("initial guess \n");
  for(itr=1; itr<nitr; itr++)
  {
    for(i=0; i<nx; i++) printf("X(%1d)=%11.7f \n", i, X[i]);
    printf("\n");
    Xt[0] = X[0]; /* terms based on this iteration */
    Xt[1] = X[1]*X[1];
    Xt[2] = X[2]*X[2]*X[2];
    Xt[3] = 1.0/X[3];
    Xt[4] = 1.0/(X[4]*X[4]);

    D[0] = 1.0; /* derivatives based on this iteration */
    D[1] = 2.0*X[1];
    D[2] = 3.0*X[2]*X[2];
    D[3] = -1.0/(X[3]*X[3]);
    D[4] = -2.0/(X[4]*X[4]*X[4]);

    for(i=0; i<nx; i++)
    {
      F[i] = 0.0;
      for(j=0; j<nx; j++)
      {
        F[i] = F[i] + A[i][j]*Xt[j];
        Ja[i][j] = A[i][j]*D[j]; /* build Jacobian analytically */
      }
      F[i] = F[i] - Y[i]; /* residual  A*Xt-Y for row i */
    }
    Terror = 0.0;
    for(i=0; i<nx; i++) Terror = Terror + abs(F[i]);

    if(itr==1)
    {
      Perror = Terror;
    }
    else
    {
      if(Terror<Perror)
      {
        Improve++;
        Perror = Terror;
        if(Improve>0)
	{
          Improve_Count++;
          fctr = fctr*1.1;
          if(fctr > 1.0) fctr = 1.0; 
        }
      }
      else
      {
        fctr_Count++;
        fctr = fctr*0.9;
        Improve = 0;
      }
    }
    printf("iteration %3d, total error= %g \n",itr, Terror);
    printf("\n");

    inverse(nx, (double *)Ja, (double *)JI);

    /* compute X for next iteration */
    for(j=0; j<nx; j++)    /* loop through variables */
      for(i=0; i<nx; i++)  /* loop through equations */
        X[j] = X[j] - fctr*F[i]*JI[j][i]; /* in place update */
  } /* end iteration */

  printf("final results             should be \n");
  for(i=0; i<nx; i++)
    printf("X(%1d)=%11.7f        %11.7f \n", i, X[i], S[i]);

  Terror = 0.0;
  for(i=0; i<nx; i++)
  {
    F[i] = 0.0;
    for(j=0; j<nx; j++)
    {
      F[i] = F[i] + A[i][j]*Xt[j];
    }
    F[i] = F[i] - Y[i]; /* A*X-Y for row i */
    Terror = Terror + abs(F[i]);
  }
  if(fctr_Count>0)
    printf("increased error reduced fctr %3d times \n", fctr_Count);
  if(Improve_Count>0)
    printf("fctr increased due to reduced error %3d times \n",Improve_Count);
  printf("final fctr=%g \n", fctr);
  printf("final total error=%g \n", Terror);

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