/* equation4_nl.c  just polynomial terms        */
/*                 very fast convergence        */
/*  f1(x1, x2, x3) = x1 + 2 x2^2 + 3*x3 = 18    */
/*  f1'(x1) = 1                                 */
/*  f1'(x2) = 4 x2                              */
/*  f1'(x3) = 3                                 */
/*                                              */
/*  f2(x1, x2, x3) = 2 x1 + x2^2 + 1*x3 = 9     */
/*  f2'(x1) = 2                                 */
/*  f2'(x2) = 2 x2                              */
/*  f2'(x3) = 1                                 */
/*                                              */
/*  f3(x1, x2, x3) = 3 x1 + x2^2 + 4*x3 = 19    */
/*  f3'(x1) = 3                                 */
/*  f3'(x2) = 2 x2                              */
/*  f3'(x3) = 4                                 */
/*                                              */
/* in matrix form, the equations are: A * X = Y */
/* | 1  2  3 |   |  x1  | = | 18 |              */
/* | 2  1  1 | * | x2^2 | = |  9 |              */
/* | 3  1  4 |   |  x3  | = | 19 |              */
/*      A      *    X     =    Y                */
/*                                              */
/* in matrix form, the Jacobian is:             */
/*     |   1    |   | 1   4*x2  3 |             */
/* A * | 2*x2   | = | 2   2*x2  1 | = J         */
/*     |   1    |   | 3   2*x2  4 |             */
/* A *     D      =        J                    */
/*     deriv of X                               */
/*     wrt x1,x2,x3                             */
/*                                              */
/* Newton iteration:                            */
/* x_next = x-(A*X-Y)/J,  /J is times transpose inverse J */
/* J is, in general, dependent on all variables */

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

/* three functions representing Y */
double f1(double x1, double x2, double x3)
{
  return x1 + 2.0*x2*x2 + 3.0*x3; /*  = 18 */
}
double f2(double x1, double x2, double x3)
{
  return 2.0*x1 + x2*x2 + 1.0*x3; /* = 9 */
}
double f3(double x1, double x2, double x3)
{
  return 3.0*x1 + x2*x2 + 4.0*x3; /* = 19 */
}


int main(int argc, char * argv[])
{
  int itr, i, j;
  int n = 10;
  double A[3][3] = {{1.0, 2.0, 3.0}, {2.0, 1.0, 1.0}, {3.0, 1.0, 4.0}};
  double X[3]; /* X terms, A * X = Y  with X being nonlinear terms */
  double Y[3]; /* RHS of equations */
  double D[3]; /* derivative of X terms */
  double J[3][3]; /* Jacobian to be inverted */
  double JI[3][3]; /* Jacobian  inverted */
  double F[3];     /* A*X-Y error to be multiplied by J inverse */
  double x1, x2, x3; /* initial guess and solution */
  double fctr = 1.0; /* convergence is sensitive to initial guess  */

  printf("equation4_nl.c running \n");
  printf("solve   x1 + 2 x2^2 + 3 x3 = 18 \n");
  printf("      2 x1 +   x2^2 + 1 x3 =  9 \n");
  printf("      3 x1 +   x2^2 + 4 x3 = 19 \n");
  printf(" | 1  2  3 |   |  x1  | = | 18 | \n");
  printf(" | 2  1  1 | * | x2^2 | = |  9 | \n");
  printf(" | 3  1  4 |   |  x3  | = | 19 | \n");
  printf("      A      *    X     =     Y      \n");
  printf("guess initial x1, x2, x3 \n");
  printf("compute X = | x1  x2^2  x3 | \n");
  printf("compute derivative D = | 1  2*x2 1 | \n");
  printf("compute the Jacobian J = A * D and invert \n");
  printf("iterate X_next = X - (A*X-Y)*J^-1 \n");
  printf("no guarentee of solution or unique solution \n");
  printf("A*X-Y should go to zero \n");

  Y[0] = f1(1.0, 2.0, 3.0); /* desired solution 1, 2, 3 */
  Y[1] = f2(1.0, 2.0, 3.0); /* compute Y accurately */
  Y[2] = f3(1.0, 2.0, 3.0);
  printf("| %8.4f %8.4f %8.4f |   |  x1  |   | %8.4f | \n",
         A[0][0], A[0][1], A[0][2], Y[0]); 
  printf("| %8.4f %8.4f %8.4f | * |x2*x2 | = | %8.4f | \n",
         A[1][0], A[1][1], A[1][2], Y[1]); 
  printf("| %8.4f %8.4f %8.4f |   |  x3  |   | %8.4f | \n",
         A[2][0], A[2][1], A[2][2], Y[2]); 

  x1 = 3.0; /* variable initial guess */
  x2 = 0.5;
  x3 = 1.0;
  for(itr=0; itr<n; itr++)
  {
    printf("x1=%9.5f, x2=%9.5f, x3=%9.5f \n", x1, x2, x3);
    X[0] = x1; /* terms based on this iteration */
    X[1] = x2*x2;
    X[2] = x3;
    printf("X[0]=%9.5f  X[1]=%9.5f  X[2]=%9.5f \n", X[0], X[1], X[2]);
    D[0] = 1.0; /* derivatives based on this iteration */
    D[1] = 2.0*x2;
    D[2] = 1.0;
    printf("D[0]=%9.5f  D[1]=%9.5f  D[2]=%9.5f \n", D[0], D[1], D[2]);
    for(i=0; i<3; i++)
    { 
      F[i] = 0.0;
      for(j=0; j<3; j++)
      {
        F[i] += A[i][j]*X[j];
        J[i][j] = A[i][j]*D[j];
      }
      F[i] -= Y[i]; /* A*X-Y for row i */
    }    
    printf("zero F[0]=%9.5f  F[1]=%9.5f  F[2]=%9.5f \n", F[0], F[1], F[2]);
    printf("iteration %d, total error=%g \n\n",
           itr, abs(F[0])+abs(F[1])+abs(F[2]));
    for(i=0; i<3; i++)
      printf("J[%1d][0]=%9.5f  J[%1d][1]=%9.5f  J[%1d][2]=%9.5f \n",
             i, J[i][0], i, J[i][1], i, J[i][2]);
    inverse(3, (double *)J, (double *)JI);
    for(i=0; i<3; i++)
      printf("JI[%1d][0]=%9.5f  JI[%1d][1]=%9.5f  JI[%1d][2]=%9.5f \n",
             i, JI[i][0], i, JI[i][1], i, JI[i][2]);
    for(i=0; i<3; i++)
    {
      x1 -= fctr*F[i]*JI[0][i]; /* transpose */ 
      x2 -= fctr*F[i]*JI[1][i]; /* transpose */ 
      x3 -= fctr*F[i]*JI[2][i]; /* transpose */ 
    }
  }


  printf("final x1=%9.5f, x2=%9.5f, x3=%9.5f \n", x1, x2, x3);
  X[0] = x1; /* terms based on initial guess */
  X[1] = x2*x2;
  X[2] = x3;
  printf("terms X1=%9.5f, X2=%9.5f, X3=%9.5f \n", X[0], X[1], X[2]);
  for(i=0; i<3; i++)
  { 
    F[i] = 0.0;
    for(j=0; j<3; j++)
    {
      F[i] += A[i][j]*X[j];
    }
    F[i] -= Y[i]; /* A*X-Y for row i */
  }    
  printf("final total error=%g \n\n",
         abs(F[0])+abs(F[1])+abs(F[2]));

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