SUBROUTINE Step_RFO_all(NCoord,Step,IGeom,Geom,Grad,Hess,Tangent)

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
!This subroutine calculates new coordinates given a Hessian, the forces
! and the old coordinates
!
!!!!!!!!!!
! v1.0
! Uses only basic RFO step
!
!!!!!!!!!!
! v 2.0
! We add the test of ADF here, so that it should be more efficient.
! This version uses HInv to decide wether it should work with the Hessian
! or its inverse.
!
! v 3.0
! Uses DIIS. Done by P. Dayal.
!
! v3.1
! Back to origins ! We remove the ADF test here to see what we loose...
! untill of course its replacement by something more efficient... and free :)
!!!!!!!!!!!!!!!!!
!
! Input:
!   - NCoord: INTEGER(KINT), Number of degrees of freedom
!   - IGeom : INTEGER(KINT), Index of the geometry along the path
!   - Geom  : REAL(KREAL)(NCOORD), Geometry expressed in full coordinates
!   - Hess  : REAL(KREAL)(Ncoord,NCoord), Hessian at the current geometry
!  
!
! Output:
!   - Step  : REAL(KREAL)(Ncoord), Calculated step
!
! In/Out:
!   - Tangent: REAL(KREAL)(Ncoord), Tangent to the path, at the current point
!             if Tangent=0, then we are optimizing a minimum,  tangent is not considered.
!               Input: Expressed in full coordinates.
!              Output: Replaced by the tangent expressed in Free vectors
! (Changes by PFL 3 Jan 2008)
!   -  Grad: READ(KREAL) (NCoord), Gradient at the current point
!               Input: Expressed in full coordinates.
! PFL 18 Jan 2008: Grad is no longer changed on output ->
!              Output: Replaced by the gradient expressed in Free vector, in the subspace orthogonal to the tangent.
! <- PFL 18 Jan 2008: Grad is no longer changed on output
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

  use Io_module, only : IoOut
  
  use Path_module, only : NGeomF, Hinv,Coord,Vfree, NGintMax
  ! VFree(Ncoord,Ncoord)

  IMPLICIT NONE

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

  INTEGER(KINT), INTENT(IN) :: NCoord,IGeom
  REAL(KREAL), INTENT(IN) :: Hess(NCoord,NCoord)
  REAL(KREAL), INTENT(IN) :: Grad(NCoord),Geom(NCoord)
  REAL(KREAL), INTENT(INOUT) :: Tangent(Ncoord) ! Destroyed on output
  REAL(KREAL), INTENT(OUT) :: Step(NCoord)

  INTEGER(KINT) :: NFree
  REAL(KREAL), ALLOCATABLE :: eigval(:), eigvec(:,:), eigvli(:)
  REAL(KREAL), ALLOCATABLE :: Tanf(:)
  REAL(KREAL), ALLOCATABLE :: Grad_f(:),Step_f(:) ! NFree

  REAL(KREAL), PARAMETER :: tiny=1e-20, zero=0., dele3=1e-6, eps=1e-12
  REAL(KREAL), PARAMETER :: crit=1e-8
  INTEGER(KINT) :: n, i, j, isignx, im, idx,k, isch
  REAL(KREAL) :: grd, evl, eval,e1,e2, norm
  REAL(KREAL) :: dg, dx
  CHARACTER(120) :: fmt, fmt2, Line

  LOGICAL :: Debug

  REAL(KREAL), ALLOCATABLE :: HFree(:,:) ! NFree,NFree
  REAL(KREAL), ALLOCATABLE :: Htmp(:,:) ! NCoord,NFree

  LOGICAL, SAVE :: LFirst=.TRUE.

  INTERFACE
     function valid(string) result (isValid)
       CHARACTER(*), intent(in) :: string
       logical                  :: isValid
     END function VALID
  END INTERFACE

  debug=valid("step_rfo_all")
  if (debug) WRITE(*,*) "=========================== Entering Step_RFO_All ===================="
  if (debug) WRITE(*,*) "=========================== IGeom=",IGeom," ===================="

! PFL 22.Nov.2007 ->
! Nfree is equal to Ncoord-1 only when optimizing in the plane
! orthogonal to the tangent. 
! In the case of a regular opt, NFree=Ncoord. 
! Next line has been moved later
!  NFree=Ncoord-1
! <- PFL 22.Nov.2007

  IF (debug) THEN
     WRITE(*,*) 'DBG Step_RFO_All Hess'
     WRITE(*,*) 'DBG Step_RFO_All NCoord=',NCoord

     Idx=min(12,NCoord)
     DO I=1,NCoord
        WRITE(*,'(12(1X,F10.4))') Hess(I,1:Idx)
     END DO
     WRITE(*,*) 'DBG Step_RFO_All Grad'
     WRITE(*,'(12(1X,F10.4))') Grad(1:Idx)
     WRITE(*,*) 'DBG Step_RFO_All Coord'
     WRITE(*,'(12(1X,F10.4))') Geom(1:Idx)
     WRITE(*,*) 'DBG Step_RFO_All Tangent'
     WRITE(*,'(12(1X,F10.4))') tangent(1:Idx)

  END IF

  Call FreeMv(NCoord,Vfree) ! VFree(Ncoord,Ncoord)
! we orthogonalize Vfree to the tangent vector of this geom
! 2007/05/29 Only if Tangent/=0.d0
  Norm=sqrt(dot_product(Tangent,Tangent))
  IF (Norm.GT.eps) THEN
     ALLOCATE(Tanf(NCoord))

     ! We normalize Tangent
     Tangent=Tangent/Norm

     ! We convert Tangent into vfree only displacements
     ! This is useless for now (2007.Apr.23) as vfree=Id matrix
     ! but it will be usefull as soon as we introduce Constraints
     DO I=1,NCoord
        Tanf(I)=dot_product(reshape(vfree(:,I),(/NCoord/)),Tangent)
     END DO
     Tangent=0.d0
     DO I=1,NCoord
        Tangent=Tangent+Tanf(i)*vfree(:,I)
     END DO

     ! first we substract Tangent from vfree
     DO I=1,NCoord
        Norm=dot_product(reshape(vfree(:,i),(/NCoord/)),Tangent)
        Vfree(:,I)=vfree(:,i)-Norm*Tangent
     END DO

     Idx=0.
     ! Schmidt orthogonalization of the vfree vectors
     DO I=1,NCoord
        ! We substract the first vectors
        ! we do it twice as the Schmidt procedure is not numerically stable
        DO isch=1,2
           DO J=1,Idx
              Norm=dot_product(reshape(vfree(:,I),(/NCoord/)),reshape(vfree(:,J),(/NCoord/)))
              vfree(:,I)=vfree(:,I)-Norm*vfree(:,J)
           END DO
        END DO
        Norm=dot_product(reshape(vfree(:,I),(/NCoord/)),reshape(vfree(:,I),(/NCoord/)))
        IF (Norm.GE.crit) THEN
           idx=idx+1
           vfree(:,idx)=vfree(:,i)/sqrt(Norm)
        END IF
     END DO
  
     If (Idx/= NCoord-1) THEN
        WRITE(*,*) "Pb in orthogonalizing vfree to tangent for geom",IGeom
        WRITE(Ioout,*) "Pb in orthogonalizing vfree to tangent for geom",IGeom
        STOP
     END IF
     
     DEALLOCATE(Tanf)

     NFree=Idx

  ELSE ! Tangent =0, matches IF (Norm.GT.eps) THEN
     if (debug) WRITE(*,*) "Tangent=0, using full displacement"
     NFree=NCoord
  END IF !IF (Norm.GT.eps) THEN
  
  if (debug) WRITE(*,*) 'DBG Step_RFO_All NFree=',NFree
  

! We now calculate the new step
! we project the hessian onto the free vectors
     ALLOCATE(hfree(NFree,NFree),htmp(NCoord,NFree),Grad_f(NFree),Step_f(NFree))
     DO J=1,NFree
        DO I=1,NCoord
           Htmp(I,J)=0.d0
           DO K=1,NCoord
              Htmp(I,J)=htmp(I,J)+Hess(I,K)*vfree(K,J)
           END DO
        END DO
     END DO
     DO J=1,NFree
        DO I=1,NFree
           HFree(i,j)=0.d0
           DO K=1,NCoord
              HFree(i,j)=hfree(i,j)+vfree(k,i)*htmp(k,j)
           END DO
        END DO
     END DO

! We diagonalize the hessian or its inverse
  ALLOCATE(eigval(NFree), eigvec(NFree,NFree), eigvli(NFree)) 

  call Jacobi(Hfree,NFree,eigvli,eigvec,NFree)

  if (debug) THEN
     WRITE(*,*) 'Eigensystem, Step_RFO_all.f90, L225'
     fmt2='(I5,":",F8.3," -",  (1X,F10.4))'
     WRITE(fmt2(19:20),'(I2)') min(NFree,99)
     DO I=1,NFree
        WRITE(*,fmt2) I,Eigvli(I),eigvec(:,I)
     END DO
  END IF

  ! We inverse the eigenvalues if Hess corresponds to Hessian inverse
  IF (Hinv) THEN
     do J=1,NFree
        if (abs(eigvli(j)) <= tiny) eigvli(j) = zero
        eigval(j) = sign (eigvli(j)**2 / (dele3 + abs(eigvli(j))**3), eigvli(j))
        if (abs(eigval(j)) <= tiny) eigval(j) = zero
     end do
  ELSE
     eigval=eigvli
  END IF

  call trie(NFree,eigval,eigvec,NFree)

  DO I=1,NFree
     Grad_f(I)=dot_product(reshape(vfree(:,I),(/NCoord/)),grad)
  END DO

  ! We now construct the step

  Step_f=0.d0
  DO I=1,NFree
     grd= dot_product(grad_f,reshape(eigvec(:,i),(/NFree/)))
     eval = 0.5 * (abs(eigval(i)) + sqrt (eigval(i)**2 + 4.*grd**2))
     dx   = (-grd) / eval
     step_f = step_f + dx * eigvec(:,i)
     if (debug) write (*,*) i,eigval(i),eval,grd,dx
  END DO

  Step=0.d0
! PFL 04 Apr 2008 -> 
! Grad is once again modified
!  Grad=0.d0
  DO I=1,NFree
     Step=Step+ step_f(i)*vfree(:,i)
!     Grad=Grad+ Grad_f(i)*vfree(:,i)
  END DO
! <- PFL 04 Apr 2008
     
  DEALLOCATE(eigval,eigvec,eigvli,hfree,htmp,grad_f,step_f) 

  if (debug) THEN
     WRITE(*,*) 'DBG Step_RFO_All Step'
     WRITE(*,'(12(1X,F8.4))') Step(1:Idx)
  END IF

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

END SUBROUTINE STEP_RFO_ALL


