-- simeq_newton.adb  solve nonlinear system of equations
--                    method: newton iteration using Jacobian
--
--  Given problem  A X = Y  where x may have terms x1, x2, x3, x4, x1*x2, x3*x4
--                 A is 6 by 6 matrix of reals (could be complex)
--                 Y is vector of reals (could be complex)
--                 independent unknowns are x1, x2, x3, x4
--
--  for testing, generate A using pseudo random numbers
--               choose x1=1.1 x2=1.2 x3=1.4 x4 =1.5, compute x1*x2, x3*x4
--               compute terms of Y using Y = A X
--
--  Solve by initial guess at values of x1, x2, x3, x4 computing x1*x2, x3*x4
--    X_next = X_initial - J_initial^-1 * (A *  X_initial - Y)
--    in general X_next = X_prev - (J_prev^-1 * (A * X_prev - Y))*b
--                                 where 0 < b < 1, often 0.5, for stability
--
--  solved when  abs sum each row A * X_next -Y < epsilon
--
--  It may stall, stop if abs(X_next-X_prev)<epsilon
--  It may diverge, stop, indicate no solution
--                        (or try a different initial guess)
--  It may oscillate, stop, indicate no solution
--                        (or try a different initial guess)
--
--
--  The matrix equation for this case is:
--
--   x1     x2     x3     x4     x1*x2  x3*x4
--
-- | A1,1   A1,2   A1,3   A1,4   A1,5   A1,6 | | x1    | |y1|
-- | A2,1   A2,2   A2,3   A2,4   A2,5   A2,6 | | x2    | |y2|
-- | A3,1   A3,2   A3,3   A3,4   A3,5   A3,6 | | x3    | |y3|
-- | A4,1   A4,2   A4,3   A4,4   A4,5   A4,6 |*| x4    |=|y4|
-- | A5,1   A5,2   A5,3   A5,4   A5,5   A5,6 | | x1*x2 | |y5|
-- | A6,1   A6,2   A6,3   A6,4   A6,5   A6,6 | | x3*x4 | |y6|

--  The Jacobian, J is the numerically computed derivatives based on A, X_prev
--  The derivative coefficients may be computed once based on A and the
--  unknown variables. The numeric value plugs in the X_prev values.
--
--  J1,1 = A1,1 + A1,5*x2   deriv Row 1 wrt x1
--  J1,2 = A1,2 + A1,5*x1   deriv Row 1 wrt x2
--  J1,3 = A1,3 + A1,6*x4   deriv Row 1 wrt x3
--  J1,4 = A1,4 + A1,6*x3   deriv Row 1 wrt x4
--  J1,5 = A1,5             deriv Row 1 wrt x1, wrt x2
--  J1,6 = A1,6             deriv Row 1 wrt x3, wrt x4
--
--  J2,1 = A2,1 + A2,5*x2   deriv Row 2 wrt x1
--  J2,2 = A2,2 + A2,5*x1   deriv Row 2 wrt x2
--  J2,3 = A2,3 + A2,6*x4   deriv Row 2 wrt x3
--  J2,4 = A2,4 + A2,6*x3   deriv Row 2 wrt x4
--  J2,5 = A2,5             deriv Row 2 wrt x1, wrt x2
--  J2,6 = A2,6             deriv Row 2 wrt x3, wrt x4
--
--  J3,1 = A3,1 + A3,5*x2   deriv Row 3 wrt x1
--  J3,2 = A3,2 + A3,5*x1   deriv Row 3 wrt x2
--  J3,3 = A3,3 + A3,6*x4   deriv Row 3 wrt x3
--  J3,4 = A3,4 + A3,6*x3   deriv Row 3 wrt x4
--  J3,5 = A3,5             deriv Row 3 wrt x1, wrt x2
--  J3,6 = A3,6             deriv Row 3 wrt x3, wrt x4
--
--  etc.
--
--  J6,1 = A6,1 + A6,5*x2   deriv Row 6 wrt x1
--  J6,2 = A6,2 + A6,5*x1   deriv Row 6 wrt x2
--  J6,3 = A6,3 + A6,6*x4   deriv Row 6 wrt x3
--  J6,4 = A6,4 + A6,6*x3   deriv Row 6 wrt x4
--  J6,5 = A6,5             deriv Row 6 wrt x1, wrt x2
--  J6,6 = A6,6             deriv Row 6 wrt x3, wrt x4
--
--  Code or a data structure must be available to know the equation
--  of the entries in the Y vector. Symbolic computation of
--  derivatives is assumed to be available, when needed.

with Ada.text_io;
use  Ada.text_io;
with real_arrays;
use  real_arrays;
with Inverse;

procedure Simeq_newton is
  n : integer := 6;
  A : real_matrix(1..n,1..n);
  Ja : real_matrix(1..n,1..n);
  JaInv : real_matrix(1..n,1..n);
  Y : real_vector(1..n);
  X_prev : real_vector(1..n);
  X_temp : real_vector(1..n);
  X_tmp2 : real_vector(1..n);
  X_next : real_vector(1..n);
  X_soln : real_vector(1..n);       -- usually not known. test case
  eps : real := 1.0;
  b : real := 1.00;     -- stability factor
  resid : real;         -- residual from last iteration
  presid : real;        -- residual from prior to last iteration
  err : real;           -- error from test solution
  state : real := 30000000.0; -- arbitrary initial value

  -- udrnrt.adb uniformly disributed random numbers 0.0 .. 1.0
  function udrnrt return real is
    modulus : real := 2147483647.0; -- 2**31-1
    multiplier : real := 16807.0; -- prime to prime power
    t : long_float;
    i : integer;
  begin
    t := state * multiplier;
    i := integer(t / Modulus);
    state := t - (long_float(i) * modulus);
    if state < 0.0 then
     state := state + modulus;
    end if;
    return state / modulus;
  end udrnrt;

  package fltio is new float_io(real);
  use fltio;
  package intio is new integer_io(integer);
  use intio;

begin
  put_line("simeq_newton.adb running");
  -- derivatives hand coded to load Ja

  -- build test case
  for i in 1..n loop
    for j in 1..n loop
      A(i,j) := udrnrt;
    end loop;
  end loop;
  -- debug print
  for i in 1..n loop
    for j in 1..n loop
      put("A(");
      put(i,2);
      put(",");
      put(J,2);
      put(")=");
      put(A(i,j),3,6);
      new_line;
    end loop;
  end loop;
  new_line;

  -- choose x1=1.1 x2=1.2 x3=1.4 x4 =1.5, compute x1*x2, x3*x4
  X_soln(1) := 1.1;
  X_soln(2) := 1.2;
  X_soln(3) := 1.4;
  X_soln(4) := 1.5;
  X_soln(5) := X_soln(1)*X_soln(2);
  X_soln(6) := X_soln(3)*X_soln(4);
  -- debug print
  for i in 1..n loop
    put("X_soln(");
    put(i,2);
    put(")=");
    Put(X_soln(i),3,6);
    new_line;
  end loop;
  new_line;

  -- set up Y
  for i in 1..n loop
    Y(i) := 0.0;
    for j in 1..n loop
      Y(i) := Y(i) + A(i,j)*X_soln(j);
    end loop;
  end loop;
  -- debug print
  for i in 1..n loop
    put("Y(");
    put(i,2);
    Put(")=");
    put(Y(i),3,6);
    new_line;
  end loop;
  new_line;

  -- setup

  -- initial condition
  X_prev(1) := 1.0;
  X_prev(2) := 1.0;
  X_prev(3) := 1.0;
  X_prev(4) := 1.0;
  X_prev(5) := 1.0;
  X_prev(6) := 1.0;
  -- debug print
  for i in 1..n loop
    put("X_prev(");
    put(i,2);
    Put(")=");
    put(X_prev(i),3,6);
    new_line;
  end loop;
  new_line;

  presid := 1.0e12;
  -- iterate to find solution
  for itr in 1..4 loop
    -- compute residual
    resid := 0.0;
    for i in 1..n loop
      X_temp(i) := 0.0;
      for j in 1..n loop
        X_temp(i) := X_temp(i) + A(i,j)*X_prev(j);
      end loop; -- j
      X_temp(i) := X_temp(i) - Y(i); -- X_temp used later if not solved
      resid := resid + abs(X_temp(i));
    end loop; -- i
    -- debug print
    for i in 1..n loop
      put("residual X_temp(");
      put(i,2);
      put(")= ");
      put(X_temp(i),3,6);
      new_line;
    end loop; -- i
    new_line;
    -- for testing, compute error with solution
    err := 0.0;
    for i in 1..n loop
      err := err + abs(X_prev(i) - X_soln(i));
    end loop; -- i
    put("itr ");
    put(itr,2);
    put(", prev= ");
    put(presid);
    put(", residual= ");
    put(resid);
    new_line;
    Put("                                          err= ");
    Put(Err);
    new_line;
    -- check for convergence
    if resid<eps then exit; end if;

    -- compute Jacobian
    for i in 1..n loop
      for j in 1..n loop
        Ja(i,j) := A(i,j);
        if j=1 then
          Ja(i,j) := Ja(i,j) + A(i,5)*X_prev(2);
        elsif j=2 then
          Ja(i,j) := Ja(i,j) + A(i,5)*X_prev(1);
        elsif j=3 then
          Ja(i,j) := Ja(i,j) + A(i,6)*X_prev(4);
        elsif j=4 then
          Ja(i,j) := Ja(i,j) + A(i,6)*X_prev(3);
        end if;
      end loop; -- j
    end loop; -- i
    -- debug print
    for i in 1..n loop
      for j in 1..n loop
        put("Ja(");
        put(i,2);
        put(",");
        put(J,2);
        put(")=");
        put(Ja(i,j),3,6);
        new_line;
      end loop; -- j
    end loop; -- i
    new_line;
    -- invert Ja
    JaInv := inverse(Ja);
    for i in 1..n loop
      for j in 1..n loop
        Ja(i,j) := JaInv(i,j);
        put("JaInv(");
        put(i,2);
        put(",");
        put(J,2);
        put(")=");
        put(JaInv(i,j),3,6);
        new_line;
      end loop; -- j
    end loop; -- i
    new_line;

    -- X_next = X_prev - (J_prev^-1 * (A X_prev - Y))*b
    -- X_next = X_prev - (J_prev^-1 * (X_temp))*b
    -- X_next = X_prev - (X_tmp2)*b

    for i in 1..n loop
      X_tmp2(i) := 0.0;
      for j in 1..n loop
        X_tmp2(i) := X_tmp2(i) + Ja(i,j)*X_temp(j); -- transpose J_inv
      end loop; -- j
    end loop; -- i
    -- debug print
    for i in 1..n loop
      put("change X_tmp2(");
      put(i,2);
      put(")= ");
      put(X_tmp2(i),3,6);
      new_line;
    end loop; -- i
    new_line;

    for i in 1..n loop
      X_next(i) := X_prev(i) - b*X_tmp2(i);
    end loop; -- i
    for i in 1..n loop
      put("X_next(");
      put(i,2);
      put(")= ");
      put(X_next(i),3,6);
      new_line;
    end loop; -- i
    new_line;


    -- iterate
    presid := Resid;
    for i in 1..n loop
      X_prev(i) := X_next(i);
    end loop; -- i
    new_line;
  end loop; -- itr    end iteration

  put_line("end simeq_newton");
end simeq_newton;