/* equation_nl.c  handle reciprocal terms        */
/*  f1(x1, x2, x3) = x1 + 2 x2^2 + 3/x3 = 10     */
/*  f1'(x1) = 1                                  */
/*  f1'(x2) = 4 x2                               */
/*  f1'(x3) = -3/x3^2                            */
/*                                               */
/*  f2(x1, x2, x3) = 2 x1 + x2^2 + 1/x3 = 6.333  */
/*  f2'(x1) = 2                                  */
/*  f2'(x2) = 2 x2                               */
/*  f2'(x3) = -1/x3^2                            */
/*                                               */
/*  f3(x1, x2, x3) = 3 x1 + x2^2 + 4/x3 = 8.333  */
/*  f3'(x1) = 3                                  */
/*  f3'(x2) = 2 x2                               */
/*  f3'(x3) = -4/x3^2                            */
/*                                               */
/* in matrix form, the equations are: A * X = Y  */
/* | 1  2  3 |   |  x1  | = | 10.000 |           */
/* | 2  1  1 | * | x2^2 | = |  6.333 |           */
/* | 3  1  4 |   | 1/x3 | = |  8.333 |           */
/*      A      *    Xt    =     Y                */
/*                                               */
/* in matrix form, the Jacobian is:              */
/*     |   1    |   | 1   4*x2  -3/x3^2 |        */
/* A * | 2*x2   | = | 2   2*x2  -1/x3^2 | = Ja   */
/*     |-1/x3^2 |   | 3   2*x2  -4/x3^2 |        */
/* A *     D      =        Ja                    */
/*     deriv of X                                */
/*     wrt x1,x2,x3                              */
/*                                               */
/* Newton iteration:                             */
/* x_next = x-fctr*(A*Xt-Y)/Ja,  /Ja is times transpose inverse of Ja */
/* Ja is, in general, dependent on all variables */
/* fctr may need to be reduced for stability     */
#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; /*  = 10.0 */
}
double f2(double x1, double x2, double x3)
{
  return 2.0*x1 + x2*x2 + 1.0/x3; /* = 6.333 */
}
double f3(double x1, double x2, double x3)
{
  return 3.0*x1 + x2*x2 + 4.0/x3; /* = 8.333 */
}


int main(int argc, char * argv[])
{
  int itr, i, j;
  int nitr = 7;
  double A[3][3] = {{1.0, 2.0, 3.0}, {2.0, 1.0, 1.0}, {3.0, 1.0, 4.0}};
  double Xt[3]; /* X terms, A * Xt = Y  with X being nonlinear terms */
  double Y[3]; /* RHS of equations */
  double D[3]; /* derivative of Xt terms */
  double Ja[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; /* can be unstable, may need to be smaller */

  printf("equation_nl.c running \n");
  printf("solve   x1 + 2 x2^2 + 3/x3 = 10 \n");
  printf("      2 x1 +   x2^2 + 1/x3 = 6.333 \n");
  printf("      3 x1 +   x2^2 + 4/x3 = 8.333 \n");
  printf(" | 1  2  3 |   |  x1  | = | 10.000 | \n");
  printf(" | 2  1  1 | * | x2^2 | = |  6.333 | \n");
  printf(" | 3  1  4 |   | 1/x3 | = |  8.333 | \n");
  printf("      A      *    X     =     Y      \n");
  printf("guess initial x1, x2, x3 \n");
  printf("compute Xt = | x1  x2^2  1/x3 | \n");
  printf("compute derivative D = | 1  2*x2 -1/x3^2 | \n");
  printf("compute the Jacobian J = A * D and invert \n");
  printf("iterate x_next = x - fctr*(A*Xt-Y)*Ja^-1 \n");
  printf("no guarantee of solution or unique solution \n");
  printf("A*Xt-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 |   | 1/x3 |   | %8.4f | \n",
         A[2][0], A[2][1], A[2][2], Y[2]); 

  x1 = 1.0; /* variable initial guess */
  x2 = 1.0;
  x3 = 1.0;
  for(itr=0; itr<nitr; itr++)
  {
    printf("x1=%9.5f, x2=%9.5f, x3=%9.5f \n", x1, x2, x3);
    Xt[0] = x1; /* terms based on this iteration */
    Xt[1] = x2*x2;
    Xt[2] = 1.0/x3;
    printf("X[0]=%9.5f  X[1]=%9.5f  X[2]=%9.5f \n", Xt[0], Xt[1], Xt[2]);
    D[0] = 1.0; /* derivatives based on this iteration */
    D[1] = 2.0*x2;
    D[2] = -1.0/(x3*x3);
    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]*Xt[j];
        Ja[i][j] = A[i][j]*D[j];
      }
      F[i] -= Y[i]; /* residual  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("Ja[%1d][0]=%9.5f  Ja[%1d][1]=%9.5f  Ja[%1d][2]=%9.5f \n",
             i, Ja[i][0], i, Ja[i][1], i, Ja[i][2]);
    inverse(3, (double *)Ja, (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);
  Xt[0] = x1; /* terms based on current values */
  Xt[1] = x2*x2;
  Xt[2] = 1.0/x3;
  printf("terms Xt1=%9.5f, Xt2=%9.5f, Xt3=%9.5f \n", Xt[0], Xt[1], Xt[2]);
  for(i=0; i<3; i++)
  { 
    F[i] = 0.0;
    for(j=0; j<3; j++)
    {
      F[i] += A[i][j]*Xt[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("equation_nl.c finished \n");
  return 0;
} /* end equation_nl.c */