/* simeq_newton.c  solve a system of non-linear equations
 *  Newton's method in one variariable for finding root of F(x)=0 is:
 *    guess an initial solution x_0
 *    compute until x_n+1 close to x_n   x_n+1 = x_n - F(x_n)/F'(x_n)
 *       solution: may not exists, may oscillate, may not converge
 *                 may be complex
 *  for a known solution of x1 = 2, x2 = 3, given two equations
 *      2 x1 + 3 x1^2 +   x2 + 2 x1 x2 = 31
 *       -x1 +   x1^2 + 2 x2 +   x1 x2 = 14
 *  then
 *    F1(x1,x2)    = 2 x1 + 3 x1^2 +   x2 + 2 x1 x2 - 31
 *    F2(x1,x2)    =  -x1 +   x1^2 + 2 x2 +   x1 x2 - 14
 *    F1'1(x1,x2)) = 2    + 2*3*x1        + 2 x2
 *    F1'2(x1,x2)) =                 1    + 2 x1
 *    F2'1(x1,x2)) =  -1  + 2 x1          + x2
 *    F2'2(x1,x2)) =                 2    + x1
 *
 *    Jacobian = | F1'1  F2'1 |     F = | F1 |   X = | x1 |
 *               | F1'2  F2'2 |         | F2 |       | x2 |
 *
 *     X_n+1 = X_n - J(Xn)^-1 F(Xn)   J^-1 is transpose of the inverse Jacobian
 */
 
#include "simeq_plus.h"
#include <stdio.h>

double F1(double x1, double x2)
{
  return 2.0*x1+3.0*x1*x1+x2+2.0*x1*x2-31.0;
}

double F2(double x1, double x2)
{
  return -x1+x1*x1+2.0*x2+x1*x2-14.0;
}

double F1P1(double x1, double x2)
{
  return 2.0+2.0*3.0*x1+2.0*x2;
}

double F1P2(double x1, double x2)
{
  return  1.0+2.0*x1;
}

double F2P1(double x1, double x2)
{
  return (-1.0)+2.0*x1+x2;
}

double F2P2(double x1, double x2)
{
  return 2.0+x1;
}

int main(int argc, char *argv[])
{
  double J[4];  /* jacobian */
  double JI[4]; /* Jacobian inverse */
  double X[2];  /* x1, x2 */
  double Xn[2]; /* next x1, x2 */
  double F[2];  /* F1(X) F2(X) */
  double T[2];  /* temporary JI*F */
  double M[4];  /* temporary for checking */
  double a;     /* temp for transpose */
  int i;
  
  printf("simeq_newton.c running \n");
  printf("check residual F1(2,3)=0  %g \n", F1(2.0, 3.0));
  printf("check residual F2(2,3)=0  %g \n", F2(2.0, 3.0));
  J[0] = F1P1(2.0,3.0);
  J[1] = F2P1(2.0,3.0);
  J[2] = F1P2(2.0,3.0);
  J[3] = F2P2(2.0,3.0);
  printf("J at 2,3 = \n");
  mat_put(2,J);
  printf("F1P1 numeric = %g \n", (F1(2.1,3.0)-F1(2.0,3.0))/0.1);
  printf("F2P1 numeric = %g \n", (F2(2.1,3.0)-F2(2.0,3.0))/0.1);
  printf("F1P2 numeric = %g \n", (F1(2.0,3.1)-F1(2.0,3.0))/0.1);
  printf("F2P2 numeric = %g \n", (F2(2.0,3.1)-F2(2.0,3.0))/0.1);

  inverse(2,J,JI);
  printf("JI at 2,3 = \n");
  mat_put(2,JI);
  mat_mul(2,J,JI,M);
  printf("M = identity \n");
  mat_put(2,M);
  
  /* start test solution at 2.1,2.9 */
  X[0] = 1.0;
  X[1] = 1.0;
  printf("X = \n");
  vec_put(2,X);
  F[0] = F1(X[0],X[1]);
  F[1] = F2(X[0],X[1]);
  printf("residuals = \n");
  vec_put(2,F);
  J[0] = F1P1(X[0],X[1]);
  J[1] = F2P1(X[0],X[1]);
  J[2] = F1P2(X[0],X[1]);
  J[3] = F2P2(X[0],X[1]);
  printf("J = \n");
  mat_put(2,J);
  inverse(2,J,JI);
  printf("JI transpose = \n");
  a = JI[1];  /* transpose for pre multiply */
  JI[1] = JI[2];
  JI[2] = a;
  mat_put(2,JI);
  mat_vec_mul(2,JI,F,T);
  printf("T = J^-1 * F = \n");
  vec_put(2,T);
  vec_sub(2,X,T,Xn);
  printf("Xn = \n");
  vec_put(2,Xn);
  X[0] = Xn[0];
  X[1] = Xn[1];
  for(i=0; i<10; i++)
  {
    F[0] = F1(X[0],X[1]);
    F[1] = F2(X[0],X[1]);
    printf("residuals = \n");
    vec_put(2,F);
    J[0] = F2P1(X[0],X[1]);
    J[1] = F1P1(X[0],X[1]);
    J[2] = F2P2(X[0],X[1]);
    J[3] = F1P2(X[0],X[1]);
    inverse(2,J,JI);
    a = JI[1];  /* transpose for pre multiply */
    JI[1] = JI[2];
    JI[2] = a;
    mat_vec_mul(2,JI,F,T);
    printf("T = \n");
    vec_put(2,T);
    vec_sub(2,X,T,Xn);
    printf("Xn = \n");
    vec_put(2,Xn);
    X[0] = Xn[0];
    X[1] = Xn[1];
  }
  printf("simeq_newton finished \n");
  return 0;
} /* end simeq_newton.c */