!Finding the root of the nonlinear systems of functions in ARR_FUNC.

program NONLIN_EQUATIONS
use MY_STUFF
use ARRAY_VALUED_FUNCTION
Implicit none

     integer, parameter :: n=2   !Dim. of problem 
     integer :: M
     real(KIND=dbl) :: delta, lambda, old_norm, new_norm
     real(KIND=dbl), dimension(n) :: x0, x0_save, f0, step, x_new, f_new
     real(KIND=dbl), dimension(n,n) :: Jac, Q, R 

!Initial guess x0:
x0(1) = -8.0 ; x0(2) = 15.0 ; x0_save = x0
f0 = ARR_FUNC(x0)                                   
call VECNORM(f0, n, old_norm)

!Initial delta x:
delta = 0.001

call NEWTON(x0, delta, n, Jac)

!NEWTON has found the Jacobian. Now solving the linearized equation
! Jac(2,2) * step(2) = -f(x0)(2) by QR decomposition and backsubstitution:

call QR_GS(Jac, n, n, Q, R)            !QR decomposition.
call QR_BACK(Q, R, n, n, -f0, step)    !QR backsubstitution.

!To avoid overshooting, it may be necessary not to take the full step:

lambda = 1.0
M = 0

do
   M = M + 1
   if (M == 100) then
      print *, "Stop. Too many iterations!"
      STOP
   endif
   x_new =  x0 + lambda*step
   f_new = ARR_FUNC(x_new)
   call VECNORM(f_new, n, new_norm)
   if (new_norm < 0.000001) then
            print *, "Convergence!"
            exit
            else
            if (new_norm > (1-lambda/2.0)*old_norm) then
               lambda = lambda/2.0
               cycle
               else
               x0 = x_new
               f0 = ARR_FUNC(x0)
               call VECNORM(f0, n, old_norm)
               call NEWTON(x0, delta, n, Jac) 
               call QR_GS(Jac, n, n, Q, R)
               call QR_BACK(Q, R, n, n, -f0, step)
               lambda = 1.0
         endif
   endif
enddo

print *, "Initial guess: x1 =:", x0_save(1), "x2 =", x0_save(2)
print *, "Root at (x,y) =", x_new(1), x_new(2)
print *, "Function value of final point:", f_new(1), f_new(2)
print *, "# of iterations:", M

end program NONLIN_EQUATIONS