!C  HEAT is never used, not even in call of Space(...)
!C  Geom = input parameter vector (Geometry).
!C  Grad = input gradient vector.
!C  Geom_new = New Geometry.

    SUBROUTINE Step_diis_all(NGeomF,IGeom,Step,Geom,Grad,HP,HEAT,Hess,NCoord,allocation_flag,Tangent)
!      IMPLICIT DOUBLE PRECISION (A-H,O-Z)

    USE Io_module, only : IoOut 
    USE Path_module, only : Vfree
    
    IMPLICIT NONE
    INTEGER, parameter :: KINT = kind(1)
    INTEGER, parameter :: KREAL = kind(1.0d0)
    
    !      INCLUDE 'SIZES'
    
    INTEGER(KINT) :: NGeomF,IGeom
    INTEGER(KINT), INTENT(IN) :: NCoord
    
    REAL(KREAL) :: Geom_new(NCoord),Geom(NCoord),Grad(NCoord)
    REAL(KREAL) :: Hess(NCoord*NCoord),Step(NCoord)
    REAL(KREAL) :: HEAT,HP
    LOGICAL :: allocation_flag
    REAL(KREAL), INTENT(INOUT) :: Tangent(Ncoord)

!************************************************************************
!*                                                                      *
!*     DIIS PERFORMS DIRECT INVERSION IN THE ITERATIVE SUBSPACE         *
!*                                                                      *
!*     THIS INVOLVES SOLVING FOR C IN Geom(NEW) = Geom' - HG'      *
!*                                                                      *
!*  WHERE Geom' = SUM(C(I)Geom(I), THE C COEFFICIENTES COMING FROM      *
!*                                                                      *
!*                   | B   1 | . | C | = | 0 |                          *
!*                   | 1   0 |   |-L |   | 1 |                          *
!*                                                                      *
!*  WHERE B(I,J) =GRAD(I)H(T)HGRAD(J)  GRAD(I) = GRADIENT ON CYCLE I    *
!*                              Hess    = INVERSE HESSIAN        *
!*                                                                      *
!*                          REFERENCE                                   *
!*                                                                      *
!*  P. CSASZAR, P. PULAY, J. MOL. STRUCT. (THEOCHEM), 114, 31 (1984)    *
!*                                                                      *
!************************************************************************
!************************************************************************
!*                                                                      *
!*     GEOMETRY OPTIMIZATION USING THE METHOD OF DIRECT INVERSION IN    *
!*     THE ITERATIVE SUBSPACE (GDIIS), COMBINED WITH THE BFGS OPTIMIZER *
!*     (A VARIABLE METRIC METHOD)                                       *
!*                                                                      *
!*     WRITTEN BY PETER L. CUMMINS, UNIVERSITY OF SYDNEY, AUSTRALIA     *
!*                                                                      *
!*                              REFERENCE                               *
!*                                                                      *
!*      "COMPUTATIONAL STRATEGIES FOR THE OPTIMIZATION OF EQUILIBRIUM   *
!*     GEOMETRIES AND TRANSITION-STATE STRUCTURES AT THE SEMIEMPIRICAL  *
!*     LEVEL", PETER L. CUMMINS, JILL E. GREADY, J. COMP. CHEM., 10,    *
!*     939-950 (1989).                                                  *
!*                                                                      *
!*     MODIFIED BY JJPS TO CONFORM TO EXISTING MOPAC CONVENTIONS        *
!*                                                                      *
!************************************************************************

      ! MRESET = maximum number of iterations.
      INTEGER(KINT), PARAMETER :: MRESET=15, M2=(MRESET+1)*(MRESET+1) !M2 = 256
      REAL(KREAL), ALLOCATABLE, SAVE :: GeomSet(:,:),GradSet(:,:),ERR(:,:) ! MRESET*NCoord
      REAL(KREAL), SAVE :: ESET(MRESET)
    REAL(KREAL), ALLOCATABLE, SAVE :: DXTMP(:,:),GSAVE(:,:) !NCoord, why DXTMP has SAVE attribute??
      REAL(KREAL) :: B(M2),BS(M2),BST(M2)
      LOGICAL DEBUG, PRINT
      INTEGER(KINT), ALLOCATABLE, SAVE :: MSET(:)
    LOGICAL, ALLOCATABLE, SAVE :: FRST(:)
      INTEGER(KINT) :: NDIIS, MPLUS, INV, ITERA, MM, NFree, I, J, K
      INTEGER(KINT) :: JJ, KJ, JNV, II, IONE, IJ, INK,ITmp, Isch, Idx
      REAL(KREAL) :: XMax, XNorm, S, DET, THRES, Norm
    REAL(KREAL), PARAMETER :: eps=1e-12
    REAL(KREAL), PARAMETER :: crit=1e-8
    REAL(KREAL), ALLOCATABLE :: Tanf(:) ! NCoord
    REAL(KREAL), ALLOCATABLE :: HFree(:) ! NFree*NFree
    REAL(KREAL), ALLOCATABLE :: Htmp(:,:) ! NCoord,NFree
    REAL(KREAL), ALLOCATABLE :: Grad_free(:),Grad_new_free_inter(:),Step_free(:) ! NFree
    REAL(KREAL), ALLOCATABLE :: Geom_free(:),Geom_new_free_inter(:),Geom_new_free(:) ! NFree
    REAL(KREAL), ALLOCATABLE, SAVE :: GeomSet_free(:,:),GradSet_free(:,:)
    
      DEBUG=.TRUE.
      PRINT=.TRUE.

      IF (PRINT)  WRITE(*,'(/,''      BEGIN GDIIS   '')')
    
      ! Initialization
      IF (allocation_flag) THEN ! allocation_flag = .TRUE. at the begining and effective for all geometries in path.
      ! FRST(IGeom) will be set to False in Space, so no need to modify it here
         IF (ALLOCATED(GeomSet)) THEN
            IF (PRINT)  WRITE(*,'(/,''    In FRST, GDIIS Dealloc  '')')
            DEALLOCATE(GeomSet,GradSet,ERR,DXTMP,GSave,GeomSet_free,GradSet_free)
            RETURN
         ELSE
        ! these allocated arrays need to be properly deallocated.
            IF (PRINT)  WRITE(*,'(/,''     In FRST,  GDIIS Alloc  '')')
            ALLOCATE(GeomSet(NGeomF,MRESET*NCoord),GradSet(NGeomF,MRESET*NCoord),ERR(NGeomF,MRESET*NCoord))
      ALLOCATE(GeomSet_free(NGeomF,MRESET*NCoord),GradSet_free(NGeomF,MRESET*NCoord))
            ALLOCATE(DXTMP(NGeomF,NCoord),GSAVE(NGeomF,NCoord),MSET(NGeomF),FRST(NGeomF))
      DO I=1,NGeomF
         FRST(I) = .TRUE.
         GeomSet(I,:) = 0.d0
         GradSet(I,:) = 0.d0
         ERR(I,:) = 0.d0
         GeomSet_free(I,:) = 0.d0
         GradSet_free(I,:) = 0.d0
         DXTMP(I,:)=0.d0
         GSAVE(I,:)=0.d0
      END DO
      MSET(:)=0
         END IF
     allocation_flag = .FALSE.
      END IF
    
      ! Addded from here:
    Call FreeMv(NCoord,Vfree) ! VFree(Ncoord,Ncoord)
      ! we orthogonalize Vfree to the tangent vector of this geom 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 subtract 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 subtract 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
       
         Print *, 'Idx=', Idx
         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_DIIS_All, IGeom, NFree=', IGeom, NFree

      ! We now calculate the new step
      ! we project the hessian onto the free vectors
      ALLOCATE(HFree(NFree*NFree),Htmp(NCoord,NFree),Grad_free(NFree),Grad_new_free_inter(NFree))
    ALLOCATE(Geom_free(NFree),Step_free(NFree),Geom_new_free_inter(NFree),Geom_new_free(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-1)*NCoord)+K)*Vfree(K,J)
           END DO
        END DO
      END DO
      DO J=1,NFree
        DO I=1,NFree
       HFree(I+((J-1)*NFree))=0.d0
           DO K=1,NCoord
        HFree(I+((J-1)*NFree))=HFree(I+((J-1)*NFree))+Vfree(K,I)*Htmp(K,J)
           END DO
        END DO
      END DO

      DO I=1,NFree
         Grad_free(I)=dot_product(reshape(Vfree(:,I),(/NCoord/)),Grad)
     Geom_free(I)=dot_product(reshape(Vfree(:,I),(/NCoord/)),Geom)
      END DO
      !Added Ends here.***********************************************
    
!C  SPACE SIMPLY LOADS THE CURRENT VALUES OF Geom AND GRAD INTO
!C  THE ARRAYS GeomSet AND GradSet
!C  HEAT is never used, not even in Space_all(...)

      CALL Space_all(NGeomF,IGeom,MRESET,MSET,Geom,Grad,HEAT,NCoord,GeomSet,GradSet,ESET,FRST)
    
      IF (PRINT)  WRITE(*,'(/,''       GDIIS after Space  '')')
    
    DO J=1,MSet(IGeom)
         DO K=1,NFree
            GradSet_free(IGeom,((J-1)*NFree)+K)=dot_product(reshape(Vfree(:,K),(/NCoord/)),&
               GradSet(IGeom,((J-1)*NCoord)+1:((J-1)*NCoord)+NCoord))
        GeomSet_free(IGeom,((J-1)*NFree)+K)=dot_product(reshape(Vfree(:,K),(/NCoord/)),&
                GeomSet(IGeom,((J-1)*NCoord)+1:((J-1)*NCoord)+NCoord))
         END DO
    END DO    
!C
!C     INITIALIZE SOME VARIABLES AND CONSTANTS
!C
      NDIIS = MSET(IGeom)
      MPLUS = MSET(IGeom) + 1
      MM = MPLUS * MPLUS            
!C
!C     COMPUTE THE APPROXIMATE ERROR VECTORS
!C
      INV=-NFree
      DO 30 I=1,MSET(IGeom)
         INV = INV + NFree
         DO 30 J=1,NFree
            S = 0.D0
            KJ=(J*(J-1))/2
            DO 10 K=1,J
               KJ = KJ+1
   10       S = S - HFree(KJ) * GradSet_free(IGeom,INV+K)
            DO 20 K=J+1,NFree
               KJ = (K*(K-1))/2+J
   20       S = S - HFree(KJ) * GradSet_free(IGeom,INV+K)
   30 ERR(IGeom,INV+J) = S
   
!C
!C     CONSTRUCT THE GDIIS MATRIX
!C
      DO 40 I=1,MM
   40 B(I) = 1.D0
   
      JJ=0
      INV=-NFree
      DO 50 I=1,MSET(IGeom)
         INV=INV+NFree
         JNV=-NFree
         DO 50 J=1,MSET(IGeom)
            JNV=JNV+NFree
            JJ = JJ + 1
            B(JJ)=0.D0
            DO 50 K=1,NFree
      !Print *, 'B(',JJ,')=', B(JJ) + ERR(IGeom,INV+K) * ERR(IGeom,JNV+K)
   50 B(JJ) = B(JJ) + ERR(IGeom,INV+K) * ERR(IGeom,JNV+K)

     ! The following shifting is required to correct indices of B_ij elements in the GDIIS matrix.
   ! The correction is needed because the last coloumn of the matrix contains all 1 and one zero.
      DO 60 I=MSET(IGeom)-1,1,-1
         DO 60 J=MSET(IGeom),1,-1
   60 B(I*MSET(IGeom)+J+I) = B(I*MSET(IGeom)+J)
   
      ! For the last row and last column of GEDIIS matrix:
      DO 70 I=1,MPLUS
         B(MPLUS*I) = 1.D0
   70 B(MPLUS*MSET(IGeom)+I) = 1.D0
      B(MM) = 0.D0
!C
!C     ELIMINATE ERROR VECTORS WITH THE LARGEST NORM
!C
   80 CONTINUE
      DO 90 I=1,MM
   90 BS(I) = B(I)
   
      IF (NDIIS .EQ. MSET(IGeom)) GO TO 140
      DO 130 II=1,MSET(IGeom)-NDIIS
         XMAX = -1.D10
         ITERA = 0
         DO 110 I=1,MSET(IGeom)
            XNORM = 0.D0
            INV = (I-1) * MPLUS
            DO 100 J=1,MSET(IGeom)
  100       XNORM = XNORM + ABS(B(INV + J))
            IF (XMAX.LT.XNORM .AND. XNORM.NE.1.0D0) THEN
               XMAX = XNORM
               ITERA = I
               IONE = INV + I
            ENDIF
  110    CONTINUE
  
         DO 120 I=1,MPLUS
            INV = (I-1) * MPLUS
            DO 120 J=1,MPLUS
               JNV = (J-1) * MPLUS
               IF (J.EQ.ITERA) B(INV + J) = 0.D0
               B(JNV + I) = B(INV + J)
         !Print *,'B(JNV + I)=',B(JNV + I)
  120    CONTINUE
         B(IONE) = 1.0D0
  130 CONTINUE
  140 CONTINUE
!C
!C     OUTPUT THE GDIIS MATRIX
!C
      IF (DEBUG) THEN
         WRITE(*,'(/5X,'' GDIIS MATRIX'')')
         ITmp=min(12,MPLUS)
         DO IJ=1,MPLUS
            WRITE(*,'(12(F12.4,1X))') B((IJ-1)*MPLUS+1:(IJ-1)*MPLUS+ITmp)
         END DO
      ENDIF
!C
!C     SCALE DIIS MATRIX BEFORE INVERSION
!C
      DO 160 I=1,MPLUS
         II = MPLUS * (I-1) + I
     !Print *, 'B(',II,')=', B(II)
     !Print *, 'GSave(',IGeom,',',I,')=', 1.D0 / DSQRT(1.D-20+DABS(B(II)))
  160 GSAVE(IGeom,I) = 1.D0 / DSQRT(1.D-20+DABS(B(II)))
  
      GSAVE(IGeom,MPLUS) = 1.D0
      !Print *, 'GSave(',IGeom,',',MPlus,')=1.D0'  
        
      DO 170 I=1,MPLUS
         DO 170 J=1,MPLUS
            IJ = MPLUS * (I-1) + J
  170 B(IJ) = B(IJ) * GSAVE(IGeom,I) * GSAVE(IGeom,J)  
!C
!C     OUTPUT SCALED GDIIS MATRIX
!C
      IF (DEBUG) THEN
         WRITE(*,'(/5X,'' GDIIS MATRIX (SCALED)'')')
         ITmp=min(12,MPLUS)
         DO IJ=1,MPLUS
            WRITE(*,'(12(F12.4,1X))') B((IJ-1)*MPLUS+1:(IJ-1)*MPLUS+ITmp)
         END DO            
      ENDIF
!C
!C     INVERT THE GDIIS MATRIX B
!C
      CALL MINV(B,MPLUS,DET) ! matrix inversion.

      DO 190 I=1,MPLUS
         DO 190 J=1,MPLUS
            IJ = MPLUS * (I-1) + J
      !Print *, 'B(',IJ,')=', B(IJ)
      !Print *, 'GSAVE(',IGeom,',',I,')=', GSAVE(IGeom,I)
      !Print *, 'GSAVE(',IGeom,',',J,')=', GSAVE(IGeom,J)
      !Print *, 'B(',IJ,')=', B(IJ) * GSAVE(I) * GSAVE(J)
  190 B(IJ) = B(IJ) * GSAVE(IGeom,I) * GSAVE(IGeom,J)
!C
!C     COMPUTE THE INTERMEDIATE INTERPOLATED PARAMETER AND GRADIENT VECTORS
!C
      !Print *, 'MSET(',IGeom,')=', MSET(IGeom), ' MPLUS=', MPLUS
      DO 200 K=1,NFree
         Geom_new_free_inter(K) = 0.D0
         Grad_new_free_inter(K) = 0.D0
         DO 200 I=1,MSET(IGeom)
            INK = (I-1) * NFree + K
            Geom_new_free_inter(K) = Geom_new_free_inter(K) + B(MPLUS*MSET(IGeom)+I) * GeomSet_free(IGeom,INK)
      !Print *, 'Geom_new_free_inter(',K,')=', Geom_new_free_inter(K)
      !Print *, 'B(MPLUS*MSET(',IGeom,')+',I,')=', B(MPLUS*MSET(IGeom)+I)
      !Print *, 'GeomSet_free(',IGeom,',',INK,')=', GeomSet_free(IGeom,INK)
  200 Grad_new_free_inter(K) = Grad_new_free_inter(K) + B(MPLUS*MSET(IGeom)+I) * GradSet_free(IGeom,INK)
      HP=0.D0
      DO 210 I=1,MSET(IGeom)
  210 HP=HP+B(MPLUS*MSET(IGeom)+I)*ESET(I)
      DO 220 K=1,NFree
  220 DXTMP(IGeom,K) = Geom_free(K) - Geom_new_free_inter(K)
      XNORM = SQRT(DOT_PRODUCT(DXTMP(IGeom,1:NFree),DXTMP(IGeom,1:NFree)))
      IF (PRINT) THEN
         WRITE (6,'(/10X,''DEVIATION IN X '',F10.6, 8X,''DETERMINANT '',G9.3)') XNORM,DET
         WRITE(*,'(10X,''GDIIS COEFFICIENTS'')')
         WRITE(*,'(10X,5F12.5)') (B(MPLUS*MSET(IGeom)+I),I=1,MSET(IGeom))
      ENDIF

!C     THE FOLLOWING TOLERENCES FOR XNORM AND DET ARE SOMEWHAT ARBITRARY!
      THRES = MAX(10.D0**(-NFree), 1.D-25)
      IF (XNORM.GT.2.D0 .OR. DABS(DET).LT. THRES) THEN
         IF (PRINT)THEN
            WRITE(*,*) "THE DIIS MATRIX IS ILL CONDITIONED"     
            WRITE(*,*) " - PROBABLY, VECTORS ARE LINEARLY DEPENDENT - "
            WRITE(*,*) "THE DIIS STEP WILL BE REPEATED WITH A SMALLER SPACE"
         END IF
         DO 230 K=1,MM
  230    B(K) = BS(K)
         NDIIS = NDIIS - 1
         IF (NDIIS .GT. 0) GO TO 80
         IF (PRINT) WRITE(*,'(10X,''NEWTON-RAPHSON STEP TAKEN'')')
         DO 240 K=1,NFree
            Geom_new_free_inter(K) = Geom_free(K)
  240       Grad_new_free_inter(K) = Grad_free(K)
      ENDIF ! matches IF (XNORM.GT.2.D0 .OR. DABS(DET).LT. THRES) THEN, L378
    
    ! q_{m+1} = q'_{m+1} - H^{-1}g'_{m+1}
    Geom_new_free=0.d0
    DO I = 1, NFree
       DO J = 1, NFree
          ! If Hinv=.False., then we need to invert Hess
        !Geom_new_free(:) = Geom_new_free(:) + HFree(:,I)*Grad_new_free_inter(I)
       Geom_new_free(J) = Geom_new_free(J) + HFree(I+((J-1)*NFree))*Grad_new_free_inter(I)
     END DO
    END DO
    Geom_new_free(:) = Geom_new_free_inter(:) - Geom_new_free(:)
    
    Step_free = Geom_new_free - Geom_free
      
    Step = 0.d0
      DO I=1,NFree
     Step = Step + Step_free(I)*Vfree(:,I)
      END DO

      DEALLOCATE(Hfree,Htmp,Grad_free,Grad_new_free_inter,Step_free,Geom_free)
      DEALLOCATE(Geom_new_free_inter,Geom_new_free)
    
    IF (PRINT)  WRITE(*,'(/,''       END GDIIS  '',/)')
    
      END SUBROUTINE Step_diis_all
