PROGRAM Lapack_test
  IMPLICIT NONE
  INTEGER :: n, nrhs, lwork, j, info
  INTEGER, DIMENSION(:), ALLOCATABLE :: ipiv
  CHARACTER, PARAMETER :: uplo = 'u'
  REAL, DIMENSION(:), ALLOCATABLE :: x, gx, work
  REAL, DIMENSION(:,:), ALLOCATABLE :: hx
  REAL :: fx, xdiff, xtol
  ! Muuttujien alustus
  n = 2; nrhs = 1; lwork = n*n
  ALLOCATE(x(n), gx(n), hx(n,n), ipiv(n), work(lwork))
  x = (/ -1.2d0, 1.0d0 /) ! Alkuarvaus
  xdiff = 1.d0; xtol = 1.d-7
  j = 0
  ! Tehdään Newton-iteraatioita
  DO WHILE (xdiff > xtol)
    CALL derivfn(x, fx, gx, hx); WRITE(*,*) x, fx
    ! Ratkaistaan Newton-yhtälö Lapackilla
    CALL ssysv(uplo, n, nrhs, hx, n, ipiv, gx, n, &
        work, lwork, info)
    IF (info /= 0) STOP 'Hessen matriisi singulaarinen'
    x(:) = x(:) - gx(:)
    xdiff = SQRT(SUM(gx(:)**2))
    j = j + 1
  END DO
  WRITE(*,*) 'x = ', x, '; fx = ', fx, '; iter = ', j
CONTAINS
  SUBROUTINE derivfn(x, fx, gx, hx)
    IMPLICIT NONE
    REAL, DIMENSION(:), INTENT(IN) :: x
    REAL, INTENT(OUT) :: fx
    REAL, DIMENSION(SIZE(x)), INTENT(OUT) :: gx
    REAL, DIMENSION(SIZE(x),SIZE(x)), INTENT(OUT) :: hx
    ! Funktion arvo
    fx = 100.d0*(x(2) - x(1)**2)**2 + (1.d0 - x(1))**2
    ! Gradienttivektori
    gx(1) = -400.d0*x(1)*(x(2) - x(1)**2) + 2.d0*(x(1) - 1)
    gx(2) = 200.d0*(x(2) - x(1)**2)
    ! Hessen matriisin yläkolmio
    hx(1,1) = 1200.d0*x(1)**2 - 400.d0*x(2) + 2.d0
    hx(1,2) = -400.d0*x(1)
    hx(2,2) = 200.d0
  END SUBROUTINE derivfn
END PROGRAM Lapack_test