! simeq_newton.f90  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.
!

program simeq_newton
  implicit none
  integer, parameter :: n = 6 ! number of equations
  double precision, dimension(n,n) :: A
  double precision, dimension(n,n) :: Ja
  double precision, dimension(n,n) :: JaInv
  double precision, dimension(n) :: Y  
  double precision, dimension(n) :: X_prev
  double precision, dimension(n) :: X_temp
  double precision, dimension(n) :: X_tmp2
  double precision, dimension(n) :: X_next
  double precision, dimension(n) :: X_soln      ! usually not known. test case
  double precision, dimension(n,n) :: Ja_test  ! just for debug
  double precision, dimension(n,n) :: Ja_ident ! just for debug
  double precision, parameter :: eps = 1.0D-8
  double precision, parameter :: b = 1.00D0     ! stability factor
  double precision :: resid         ! residual from last iteration
  double precision :: presid        ! residual from prior to last iteration
  double precision :: err           ! error from test solution
  integer :: i, j, k, itr 

  interface
    subroutine inverse(n, sz, A, AI)
      integer, intent(in) :: n  ! number of equations
      integer, intent(in) :: sz ! dimension of arrays
      double precision, dimension(sz,sz), intent(in) :: A
      double precision, dimension(sz,sz), intent(inout) :: AI
    end subroutine inverse 
    double precision function udrnrt()
    end function udrnrt
  end interface

  ! derivatives hand coded to load Ja

  print *, 'simeq_newton.f90 running'
  ! build test case
  do i=1,n
    do j=1,n
      A(i,j) = udrnrt()
    end do
  end do
  ! debug print
  do i=1,n
    do j=1,n
      print "('A(',I2,',',I2,')=',F10.6)", i, j, A(i,j)
    end do
  end do
  print *, ' '
  ! 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
  do i=1,n 
    print "('X_soln(',I2,')=',F10.6)", i, X_soln(i)
  end do
  print *, ' '

  ! set up Y
  do i=1,n
    Y(i) = 0.0
    do j=1,n
      Y(i) = Y(i) + A(i,j)*X_soln(j)
    end do
  end do
  ! debug print
  do i=1,n
    print "('Y(',I2,')=',F10.6)", i, Y(i)
  end do
  print *, ' '

  ! 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
  do i=1,n
    print "('X_prev(',I2,')=',F10.6)", i, X_prev(i)
  end do
  print *, ' '

  presid = 1.0D12
  ! iterate to find solution
  do itr=1,4
    ! compute residual
    resid = 0.0 
    do i=1,n
      X_temp(i) = 0.0
      do j=1,n
	X_temp(i) = X_temp(i) + A(i,j)*X_prev(j)
      end do ! j
      X_temp(i) = X_temp(i) - Y(i) ! X_temp used later if not solved
      resid = resid + abs(X_temp(i))
    end do ! i
    ! debug print
    do i=1,n
      print "('residual X_temp(',I2,')=',F10.6)", i, X_temp(i)
    end do ! i
    print *, ' '
    ! for testing, compute error with solution
    err = 0.0D0
    do i=1,n
      err = err + abs(X_prev(i) - X_soln(i))
    end do ! i
    print "('itr ',I2,', prev=',E15.6,' residual=',E15.6,', err=',E15.6)" &
           , itr, presid, resid, err
    ! check for convergence
    if(resid<eps) exit

    ! compute Jacobian
    do i=1,n
      do j=1,n
        Ja(i,j) = A(i,j)
        if(j==1) Ja(i,j) = Ja(i,j) + A(i,5)*X_prev(2)
        if(j==2) Ja(i,j) = Ja(i,j) + A(i,5)*X_prev(1)
        if(j==3) Ja(i,j) = Ja(i,j) + A(i,6)*X_prev(4)
        if(j==4) Ja(i,j) = Ja(i,j) + A(i,6)*X_prev(3)
        Ja_test(i,j) = Ja(i,j) ! just for debug
      end do ! j
    end do ! i
    ! debug print
    do i=1,n
      do j=1,n
	print "('Ja(',I2,',',I2,')=',F10.6)", i, j, Ja(i,j)
      end do ! j
    end do ! i
    print *, ' '
    ! invert Ja
    call inverse(n, n, Ja, JaInv)
    do i=1,n
      do j=1,n
        Ja(i,j) = JaInv(i,j)
        print "('Ja_inv(',I2,',',I2,')=',F10.6)", i, j, Ja(i,j)
      end do ! j
    end do ! i
    print *, ' '
    ! check invert
    do i=1,n
      do j=1,n
	Ja_ident(i,j)=0.0
	do k=1,n
	  Ja_ident(i,j) = Ja_ident(i,j) + Ja_test(i,k)*Ja(k,j)
	end do ! k
      end do ! j
    end do ! i
    print *, ' '
    do i=1,n
      do j=1,n
	print "('Ja_ident(',I2,',',I2,')=',E15.6)", i, j, Ja_ident(i,j)
      end do ! j
    end do ! i
    print *, ' '

    ! 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

    do i=1,n
      X_tmp2(i) = 0.0
      do j=1,n
	X_tmp2(i) = X_tmp2(i) + Ja(i,j)*X_temp(j) ! transpose J_inv
      end do ! j
    end do ! i
    ! debug print
    do i=1,n
      print "('change X_tmp2(',I2,')=',F10.6)", i, X_tmp2(i)
    end do ! i
    print *, ' '

    do i=1,n
      X_next(i) = X_prev(i) - b*X_tmp2(i)
    end do ! i
    do i=1,n
      print "('X_next(',I2,')=',F10.6)", i, X_next(i)
    end do ! i
    print *, ' '

    ! iterate
    presid = resid 
    do i=1,n
      X_prev(i) = X_next(i)
    end do ! i
    print *, ' '
  end do ! itr    end iteration    
  print *, 'simeq_newton finished'
end program simeq_newton