! Steepest Descent Methods with a Hessian Modification from Pounds

! This program solves 10.4 2d in BandF 10th ed.

program nsystems

integer, parameter :: N = 3

real (kind=8), dimension(N) :: F, X, Xold, Y
real (kind=8), dimension(N,N) :: H

real (kind=8), parameter :: TOL = 1.0E-12;
real (kind=8) :: tolcheck

! Needed for Lapack
integer, dimension(N) :: IPIV  
integer :: INFO 

! Set the initial solution vector

Xold(1) = 0.0 
Xold(2) = 0.0
Xold(3) = 0.0 

do i=1, 100

    do k=1,N
       X(k) = Xold(k)
    enddo

    ! Build F vector

    call buildf( F, X)

    ! Build Hessian 

    call buildh(H, X, N )

    ! Solve Linear System using LAPACK

    call dgesv(N,1,H,N,IPIV,F,N,INFO)

    ! Solve for New X

    do k=1,N
       X(k) = X(k) - F(k)
    enddo

    !Check for Tolerance
    tolcheck = 0.0D0
    do k=1,N
      tolcheck = tolcheck + abs(X(k) - Xold(k))
    enddo

    print *, X(1), X(2), X(3),  tolcheck

    if ( tolcheck > TOL ) then 
    do k=1,N
      Xold(k) = X(k) 
    enddo
    endif

enddo

end program nsystems

subroutine buildf ( F, X )

    real (kind=8), dimension(*) :: F, X
    real (kind=8) :: x1, x2, x3

    x1 = X(1)
    x2 = X(2)
    x3 = X(3)

! Create the proper F functions by combining the three equations like in
! example 1 on page 669 of BandF and then taking partial derivatives with respect
! to each variable.  This forms the gradient vector.

! I let Mathematica generate these for me

    F(1) = 4.*x1*(1. + x1**2 - 0.01*x2 + 0.1*x2**2 - 1.*x3) -  &
      (0.5*(-1 + (1 - x1)**0.25 + x2 - 0.15*x3 +               &
           0.05*x3**2))/(1 - x1)**0.75 -                       &
      2*(-1 + x1 + Cos(x1*x2*x3))*                             &
       (-1 + x2*x3*Sin(x1*x2*x3))

    F(2) = 2*(-1 + (1 - x1)**0.25 + x2 - 0.15*x3 + 0.05*x3**2 + & 
      (0.01 - 0.2*x2)*                                          &
      (-1 - x1**2 + 0.01*x2 - 0.1*x2**2 + x3) -                 &
      x1*x3*(-1 + x1 + Cos(x1*x2*x3))*Sin(x1*x2*x3))

    F(3) = 2*(-1 - x1**2 + 0.01*x2 - 0.1*x2**2 + x3 +           &
       (-0.15 + 0.1*x3)*                                        &
       (-1 + (1 - x1)**0.25 + x2 - 0.15*x3 + 0.05*x3**2)       &
       - x1*x2*(-1 + x1 + Cos(x1*x2*x3))*Sin(x1*x2*x3)) 


end subroutine buildf


subroutine buildh ( H, X, N )

    integer :: N
    real (kind=8), dimension(*) ::  X
    real (kind=8), dimension(N,*) :: H 

    real (kind=8) :: x1, x2, x3

    x1 = X(1)
    x2 = X(2)
    x3 = X(3)

! Using the same function you built by combining the three equations, and that you used 
! to build the gradient vector, now take partial second derivatives to construct
! the Hessian matrix.  Note -- this is NOT found in your text but it is in your
! class notes.  You can also save time by using rules related to the equivalence
! of mixed partial derivatives.  


! I let Mathematica generate all these for me...

    H(1,1) = 4 + 0.125/(1 - x1)**1.5 + 12*x1**2 - 0.04*x2 + 0.4*x2**2 - 4*x3 - & 
       (0.375*(-1 + (1 - x1)**0.25 + x2 - 0.15*x3 + 0.05*x3**2))/              &
       (1 - x1)**1.75 - 2*x2**2*x3**2*Cos(x1*x2*x3)*                           &
       (-1 + x1 + Cos(x1*x2*x3)) + 2*(-1 + x2*x3*Sin(x1*x2*x3))**2    

    H(2,2) = 2.4002 + 0.4*x1**2 - 0.011999999999999999*x2 +  &
       0.12000000000000002*x2**2 - 0.4*x3 +                  &
       (2. - 2.*x1)*x1**2*x3**2*Cos(x1*x2*x3) -              &
       2.*x1**2*x3**2*Cos(2*x1*x2*x3)

    H(3,3) = 2 + 2*(0.15 - 0.1*x3)**2 +                           &
       0.2*(-1 + (1 - x1)**0.25 + x2 - 0.15*x3 + 0.05*x3**2) -    &
       2*x1**2*x2**2*Cos(x1*x2*x3)*(-1 + x1 + Cos(x1*x2*x3)) +    &
       2*x1**2*x2**2*Sin(x1*x2*x3)**2

    H(1,2) = -0.5/(1 - x1)**0.75 + x1*(-0.04 + 0.8*x2) -   &
       2*x3*(-1 + x1 + Cos(x1*x2*x3))*                     &
       (x1*x2*x3*Cos(x1*x2*x3) + Sin(x1*x2*x3)) +          &
       2*x1*x3*Sin(x1*x2*x3)*(-1 + x2*x3*Sin(x1*x2*x3))

    H(2,1) = H(1,2)

    H(1,3) = -4*x1 + (0.075 - 0.05*x3)/(1 - x1)**0.75 -  &
        2*x2*(-1 + x1 + Cos(x1*x2*x3))*                  &
         (x1*x2*x3*Cos(x1*x2*x3) + Sin(x1*x2*x3)) +      &
        2*x1*x2*Sin(x1*x2*x3)*(-1 + x2*x3*Sin(x1*x2*x3))

    H(3,1) = H(1,3)

    H(2,3) = 2*(-0.13999999999999999 - 0.2*x2 + 0.1*x3 -        &
        x1**2*x2*x3*Cos(x1*x2*x3)*(-1 + x1 + Cos(x1*x2*x3)) - &
        x1*(-1 + x1 + Cos(x1*x2*x3))*Sin(x1*x2*x3) +          &
        x1**2*x2*x3*Sin(x1*x2*x3)**2)

    H(3,2) = H(2,3)

end subroutine buildh