! This program reads an old hessian, old forces, new
! forces and updates the hessian matrix inverse
! using the BFGS scheme.
! 
! This routines has been modified so that its input is now:
SUBROUTINE Hinvup_BFGS(Nfree,ds,dgrad,Hess)

!----------------------------------------------------------------------
!  Copyright 2003-2014 Ecole Normale Supérieure de Lyon, 
!  Centre National de la Recherche Scientifique,
!  Université Claude Bernard Lyon 1. All rights reserved.
!
!  This work is registered with the Agency for the Protection of Programs 
!  as IDDN.FR.001.100009.000.S.P.2014.000.30625
!
!  Authors: P. Fleurat-Lessard, P. Dayal
!  Contact: optnpath@gmail.com
!
! This file is part of "Opt'n Path".
!
!  "Opt'n Path" is free software: you can redistribute it and/or modify
!  it under the terms of the GNU Affero General Public License as
!  published by the Free Software Foundation, either version 3 of the License,
!  or (at your option) any later version.
!
!  "Opt'n Path" is distributed in the hope that it will be useful,
!  but WITHOUT ANY WARRANTY; without even the implied warranty of
!
!  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
!  GNU Affero General Public License for more details.
!
!  You should have received a copy of the GNU Affero General Public License
!  along with "Opt'n Path". If not, see <http://www.gnu.org/licenses/>.
!
! Contact The Office of Technology Licensing, valorisation@ens-lyon.fr,
! for commercial licensing opportunities.
!----------------------------------------------------------------------

  IMPLICIT NONE

  INTEGER, PARAMETER :: KINT=KIND(1)
  INTEGER, PARAMETER :: KREAL=KIND(1.D0)

  INTEGER(KINT), INTENT(IN) :: NFREE
  REAL(KREAL), INTENT(INOUT) :: Hess(Nfree,Nfree)
  REAL(KREAL), INTENT(IN) :: ds(Nfree)
  REAL(KREAL), INTENT(IN) :: dGrad(Nfree)
  
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
! This subroutine updates the inverse of the Hessian
! using a BFGS formula.
!
! input:
!  - nfree: number of coordinates
!  - ds: GeomOld-Geom (expressed in the optimization coord.)
!  - dgrad: GradOld-Grad  (expressed in the optimization coord.)
!
!  input/output:
!  - Hess: old Hessian in input, replaced by updated Hessian on output
!
!  Notations for the formula:
! s=Geom-GeomOld  
! y=Grad-GradOld
! W= (H)-1 ie the inverse matrix that we are updating
! W+=W - [s(yT)W+Wy(sT)]/(y,s)+ {1+[(y,Wy)/(y,s)]}(s(sT))/(y,s)
! 
!!!!!!!!!!!!!!!!
!
! Due to the dual properties, this routine is also called to compute
! the DFP update for the Hessian
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!


  REAL(KREAL), ALLOCATABLE  :: HessOld(:,:) , Wy(:)
  REAL(KREAL), ALLOCATABLE  :: Hup(:,:),ssT(:,:)
  REAL(KREAL), ALLOCATABLE  :: HupInt(:)
  REAL(KREAL), ALLOCATABLE  :: Num(:,:), Num2(:,:)
  REAL(KREAL) :: Den, NormDS
  INTEGER(KINT) :: i,j,k, Idx

  LOGICAL debug,valid
  EXTERNAL valid

  debug=valid("Hinvup_BFGS")

  if (debug) WRITE(*,*) "=========================== Entering Hinvup_BFGS ===================="

  ALLOCATE(HessOld(nfree,nfree), Hup(nfree,nfree), &
       HupInt(nfree),  &
       Num(nfree,nfree), Num2(nfree,nfree),Wy(Nfree),ssT(Nfree,nFree) )



  HessOld=Hess

  if (debug) THEN
     WRITE(*,*) 'DBG Hinvup_BFGS HessOld'
     Idx=min(12,Nfree)
     DO I=1,NFree
        WRITE(*,'(12(1X,F8.4))') HessOld(I,1:Idx)
     END DO
     WRITE(*,*) 'DBG Hinvup_BFGS ds'
     WRITE(*,'(12(1X,F8.4))') ds(1:Idx)
     WRITE(*,*) 'DBG Hinvup_BFGS dGrad'
     WRITE(*,'(12(1X,F8.4))') dGrad(1:Idx)
  END IF



  NormDS=sqrt(DOT_PRODUCT(DS,DS))
  if (NormDS>=1e-6) THEN
     IF (debug) WRITE(*,*) 'NormDS=',NormDS
     IF (debug) WRITE(*,*) 'DS=',DS
     IF (debug) WRITE(*,*) 'DGrad=',DGrad
     ! We calculate the denominator (DGrad,DS)
     den=DOT_PRODUCT(DGrad,DS)

     if (debug) WRITE(*,*) "(y,s)=(yT)s",den

     ! We calculate Wy
     Wy=0
     DO I=1,nfree
        DO J=1,NFree
           Wy(I)=Wy(I)+HessOld(i,j)*DGrad(j)
        END DO
     END DO

     if (debug) WRITE(*,*) "Wy=",Wy

     ! We calculate Num=Wy(sT)
     Num=0.
     DO I=1,Nfree
        DO J=1,Nfree
           Num(I,J)=Wy(i)*Ds(j)
        END DO
     END DO

     if (debug) THEN
        WRITE(*,*) "Wy(sT)"
        DO I=1,NFree
           WRITE(*,*) Num(I,:)
        END DO
     END if

     ! We calculate Num2=s(yT)
     Num2=0
     Do I=1,NFree
        DO J=1,NFree
           Num2(I,J)=Ds(i)*DGrad(j)
        END DO
     END DO

     if (debug) THEN
        WRITE(*,*) "s(yT)"
        DO I=1,NFree
           WRITE(*,*) Num2(I,:)
        END DO
     END if


     ! we add Num2*W to Num
     Do I=1,NFree
        DO J=1,NFree
           DO K=1,NFree
              Num(I,J)=Num(I,J)+Num2(I,K)*HessOld(K,J)
           END DO
        END DO
     END DO

     if (debug) THEN
        WRITE(*,*) "Num=s(yT)W+Wy(sT)"
        DO I=1,NFree
           WRITE(*,*) Num(I,:)
        END DO
     END if


     ! we calculate ssT
     ssT=0.
     DO I=1,Nfree
        DO J=1,NFree
           ssT(I,J)=Ds(I)*Ds(J)
        END DO
     END DO

     if (debug) THEN
        WRITE(*,*) "s(sT)"
        DO I=1,NFree
           WRITE(*,*) ssT(I,:)
        END DO
     END if


     !We add everything
     Hup=(1+DOT_PRODUCT(DGRAD,Wy)/Den)*ssT/Den-Num/Den
     Hess=HessOld+Hup

     if (debug) THEN
        WRITE(*,*) 'DBG Hinvup_BFGS Hup'
        Idx=min(12,Nfree)
        DO I=1,NFree
           WRITE(*,'(12(1X,F8.4))') Hup(I,1:Idx)
        END DO
        WRITE(*,*) 'DBG Hinvup_BFGS Hess new'
        Idx=min(12,Nfree)
        DO I=1,NFree
           WRITE(*,'(12(1X,F8.4))') Hess(I,1:Idx)
        END DO
     END IF

  ELSE
     Hess=HessOld
  END IF
  !     WRITE(*,*) 'New hess'
  !     DO I=1,nfree
  !        WRITE(*,fmt) Hess(i,:)
  !     END DO

  DEALLOCATE(HessOld, Wy,ssT, Hup,HupInt, Num, Num2 )

  if (debug) WRITE(*,*) "=========================== Hinvup_BFGS Over ===================="

END SUBROUTINE Hinvup_BFGS
