-- equation_nl.adb  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

with Ada.Text_IO;
use  Ada.Text_IO;
with Real_Arrays;
use  Real_Arrays;
with Inverse;

procedure equation_nl is

  -- three functions representing Y
  function f1(x1 : Real; x2 : Real; x3 : Real) return Real is
  begin
    return x1 + 2.0*x2*x2 + 3.0/x3; --  = 10.0
  end f1;

  function f2(x1 : Real; x2 : Real; x3 : Real) return Real is
  begin
    return 2.0*x1 + x2*x2 + 1.0/x3; -- = 6.333
  end f2;

  function f3(x1 : Real; x2 : Real; x3 : Real) return Real is
  begin
    return 3.0*x1 + x2*x2 + 4.0/x3; -- = 8.333
  end f3;

  package Real_IO is new Float_IO(Real);
  use Real_IO;

  Nitr : Integer := 7;
  A : Real_Matrix(1..3,1..3) := ((1.0, 2.0, 3.0),
                                 (2.0, 1.0, 1.0),
                                 (3.0, 1.0, 4.0));
  X : Real_Vector(1..3); -- X terms, A * X = Y  with X being nonlinear terms
  Y : Real_Vector(1..3); -- RHS of equations
  D : Real_Vector(1..3); -- derivative of X terms
  Ja : Real_Matrix(1..3,1..3); -- Jacobian to be inverted
  JI : Real_Matrix(1..3,1..3); -- Jacobian  inverted
  F : Real_Vector(1..3);     -- A*X-Y error to be multiplied by J inverse
  x1, x2, X3 : Real; -- initial guess and solution
  fctr : Real := 1.0; -- can be unstable

begin

  Put_Line("equation_nl.adb running ");
  Put_Line("solve   x1 + 2 x2^2 + 3/x3 = 10 ");
  Put_Line("      2 x1 +   x2^2 + 1/x3 = 6.333 ");
  Put_Line("      3 x1 +   x2^2 + 4/x3 = 8.333 ");
  Put_Line(" | 1  2  3 |   |  x1  | = | 10.000 | ");
  Put_Line(" | 2  1  1 | * | x2^2 | = |  6.333 | ");
  Put_Line(" | 3  1  4 |   | 1/x3 | = |  8.333 | ");
  Put_Line("      A      *    X     =     Y      ");
  Put_Line("guess initial x1, x2, x3 ");
  Put_Line("compute X = | x1  x2^2  1/x3 | ");
  Put_Line("compute derivative D = | 1  2*x2 -1/x3^2 | ");
  Put_Line("compute the Jacobian Ja = A * D and invert ");
  Put_Line("iterate X_next = X - fctr*(A*Xt-Y)*Ja^-1 ");
  Put_Line("no guarantee of solution or unique solution ");
  Put_Line("A*X-Y should go to zero ");

  Y(1) := f1(1.0, 2.0, 3.0); -- desired solution 1, 2, 3
  Y(2) := f2(1.0, 2.0, 3.0); -- compute Y accurately
  Y(3) := f3(1.0, 2.0, 3.0);

  Put("| "); Put(A(1,1),3,4,0); Put(A(1,2),3,4,0); Put(A(1,3),3,4,0);
  Put("|   |  x1  |   | "); Put(Y(1),4,4,0); Put_Line("|");
  Put("| "); Put(A(2,1),3,4,0); Put(A(2,2),3,4,0); Put(A(2,3),3,4,0);
  Put("|   |x2*x2 |   | "); Put(Y(2),4,4,0); Put_Line("|");
  Put("| "); Put(A(3,1),3,4,0); Put(A(3,2),3,4,0); Put(A(3,3),3,4,0);
  Put("|   | 1/x3 |   | "); Put(Y(3),4,4,0); Put_Line("|");
  New_Line;
  x1 := 1.0; -- variable initial guess
  x2 := 1.0;
  x3 := 1.0;
  for Itr in 1..Nitr loop
    Put("x1=");   Put(x1,3,5,0);
    Put(", x2="); Put(x2,3,5,0);
    Put(", x3="); Put(x3,3,5,0); New_Line;
    X(1) := x1; -- terms based on this iteration
    X(2) := x2*x2;
    X(3) := 1.0/x3;
    Put("X(1)=");   Put(X(1),3,5,0);
    Put(", X(2)="); Put(X(2),3,5,0);
    Put(", X(3)="); Put(X(3),3,5,0); New_Line;
    D(1) := 1.0; -- derivatives based on this iteration values
    D(2) := 2.0*x2;
    D(3) := -1.0/(x3*x3);
    Put("D(1)=");   Put(D(1),3,5,0);
    Put(", D(2)="); Put(D(2),3,5,0);
    Put(", D(3)="); Put(D(3),3,5,0); New_Line;
    for i in 1..3 loop
      F(i) := 0.0;
      for j in 1..3 loop
        F(i) := F(i) + A(i,j)*X(j);
        Ja(i,j) := A(i,j)*D(j);
      end loop;
      F(i) := F(i) - Y(i); -- residual  A*X-Y for row i
    end loop;
    Put("F(1)=");   Put(F(1),3,5,0);
    Put(", F(2)="); Put(F(2),3,5,0);
    Put(", F(3)="); Put(F(3),3,5,0); New_Line;
    Put_Line("iteration "&integer'image(itr)&", total error="&
             Real'Image(abs(F(1))+abs(F(2))+abs(F(3))));
    New_Line;
    for I in 1..3 loop
      Put("Ja("&Integer'Image(i)&",1)=");   Put(Ja(i,1),3,5,0);
      Put(", Ja("&Integer'Image(i)&",2)="); Put(Ja(i,2),3,5,0);
      Put(", Ja("&Integer'Image(i)&",3)="); Put(Ja(i,3),3,5,0); New_Line;
    end loop;
    JI := inverse(Ja);
    for i in 1..3 loop
      Put("JI("&Integer'Image(i)&",1)=");   Put(JI(i,1),3,5,0);
      Put(", JI("&Integer'Image(i)&",2)="); Put(JI(i,2),3,5,0);
      Put(", JI("&Integer'Image(i)&",3)="); Put(JI(i,3),3,5,0); New_Line;
    end loop;
    for I in 1..3 loop
      x1 := x1 - fctr*F(i)*JI(1,i); -- transpose
      x2 := x2 - fctr*F(i)*JI(2,i); -- transpose
      x3 := x3 - fctr*F(i)*JI(3,i); -- transpose
    end loop;
  end loop;

  New_Line;
  Put("final x1=");   Put(x1,3,5,0);
  Put(", x2="); Put(x2,3,5,0);
  Put(", x3="); Put(x3,3,5,0); New_Line;
  X(1) := x1; -- terms based on initial guess
  X(2) := x2*x2;
  X(3) := 1.0/x3;
  Put("terms X(1)=");   Put(X(1),3,5,0);
  Put(", X(2)="); Put(X(2),3,5,0);
  Put(", X(3)="); Put(X(3),3,5,0); New_Line;
  for i in 1..3 loop
    F(i) := 0.0;
    for j in 1..3 loop
      F(i) := F(i) + A(i,j)*X(j);
    end loop;
    F(i) := F(i) - Y(i); -- A*X-Y for row i
  end loop;
  Put_Line("final total error="&Real'Image(
         abs(F(1))+abs(F(2))+abs(F(3))));

  Put_Line("equation_nl.adb finished ");
end Equation_Nl;