!C  HEAT is never used, not even in call of Space(...)
!C  XPARAM = input parameter vector (Geometry).
!C  Grad = input gradient vector.
      SUBROUTINE Step_DIIS(XP,XPARAM,GP,GRAD,HP,HEAT,Hess,NVAR,FRST)
	  !SUBROUTINE DIIS(XPARAM,STEP,GRAD,HP,HEAT,Hess,NVAR,FRST)
!      IMPLICIT DOUBLE PRECISION (A-H,O-Z)
      IMPLICIT NONE
      integer, parameter :: KINT = kind(1)
      integer, parameter :: KREAL = kind(1.0d0)

!      INCLUDE 'SIZES'

      INTEGER(KINT) :: NVAR
      REAL(KREAL) :: XP(NVAR), XPARAM(NVAR), GP(NVAR), GRAD(NVAR), Hess(NVAR*NVAR)
	  !REAL(KREAL) :: Hess_inv(NVAR,NVAR),XPARAM_old(NVAR),STEP(NVAR)
      REAL(KREAL) :: HEAT, HP
      LOGICAL :: FRST
	  
!************************************************************************
!*                                                                      *
!*     DIIS PERFORMS DIRECT INVERSION IN THE ITERATIVE SUBSPACE         *
!*                                                                      *
!*     THIS INVOLVES SOLVING FOR C IN XPARAM(NEW) = XPARAM' - HG'       *
!*                                                                      *
!*  WHERE XPARAM' = SUM(C(I)XPARAM(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 = number of iterations.
      INTEGER(KINT), PARAMETER :: MRESET=15, M2=(MRESET+1)*(MRESET+1) !M2 = 256
      REAL(KREAL), ALLOCATABLE, SAVE :: XSET(:),GSET(:),ERR(:) ! MRESET*NVAR
      REAL(KREAL) :: ESET(MRESET)
      REAL(KREAL), ALLOCATABLE, SAVE :: DX(:),GSAVE(:) !NVAR
      REAL(KREAL) :: B(M2),BS(M2),BST(M2)
      LOGICAL DEBUG, PRINT
      INTEGER(KINT), SAVE :: MSET
      INTEGER(KINT) :: NDIIS, MPLUS, INV, ITERA, MM
      INTEGER(KINT) :: I,J,K, JJ, KJ, JNV, II, IONE, IJ, INK,ITmp
      REAL(KREAL) :: XMax, XNorm, S, DET, THRES

      DEBUG=.TRUE.
      PRINT=.TRUE.

      IF (PRINT)  WRITE(*,'(/,''      BEGIN GDIIS   '')')
	  	 	  
	  !XPARAM_old = XPARAM
	  
! Initialization
      IF (FRST) THEN
! FRST will be set to False in Space, so no need to modify it here
         IF (ALLOCATED(XSET)) THEN
            IF (PRINT)  WRITE(*,'(/,''    In FRST, GDIIS Dealloc  '')')
            DEALLOCATE(XSet,GSET,ERR,DX,GSave)
            RETURN
         ELSE
            IF (PRINT)  WRITE(*,'(/,''     In FRST,  GDIIS alloc  '')')
            ALLOCATE(XSet(MRESET*NVAR), GSet(MRESET*NVAR), ERR(MRESET*NVAR))
            ALLOCATE(DX(NVAR),GSAVE(NVAR))
         END IF
      END IF
!C
!C  SPACE SIMPLY LOADS THE CURRENT VALUES OF XPARAM AND GRAD INTO
!C  THE ARRAYS XSET AND GSET
!C
!C  HEAT is never used, not even in Space(...)
      CALL SPACE(MRESET,MSET,XPARAM,GRAD,HEAT,NVAR,XSET,GSET,ESET,FRST)

      IF (PRINT)  WRITE(*,'(/,''       GDIIS after Space  '')')

   ! INITIALIZE SOME VARIABLES AND CONSTANTS:
      NDIIS = MSET
      MPLUS = MSET + 1
      MM = MPLUS * MPLUS

   ! COMPUTE THE APPROXIMATE ERROR VECTORS:
      INV=-NVAR
      DO 30 I=1,MSET
         INV = INV + NVAR
         DO 30 J=1,NVAR
            S = 0.D0
            KJ=(J*(J-1))/2
            DO 10 K=1,J
               KJ = KJ+1
   10       S = S - Hess(KJ) * GSET(INV+K)
            DO 20 K=J+1,NVAR
               KJ = (K*(K-1))/2+J
   20       S = S - Hess(KJ) * GSET(INV+K)
   30 ERR(INV+J) = S
   
   

    ! CONSTRUCT THE GDIIS MATRIX:
      ! initialization (not really needed)
      DO I=1,MM
         B(I) = 1.D0
      END DO
	     
      ! B_ij calculations from <e_i|e_j>  
      JJ=0
      INV=-NVAR
      DO 50 I=1,MSET
         INV=INV+NVAR
         JNV=-NVAR
         DO 50 J=1,MSET
            JNV=JNV+NVAR
            JJ = JJ + 1
            B(JJ)=0.D0
            DO 50 K=1,NVAR
   50 B(JJ) = B(JJ) + ERR(INV+K) * ERR(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-1,1,-1
         DO 60 J=MSET,1,-1
   60 B(I*MSET+J+I) = B(I*MSET+J)
   
      ! For last row and last column of GDIIS matrix 
      DO 70 I=1,MPLUS
         B(MPLUS*I) = 1.D0
   70 B(MPLUS*MSET+I) = 1.D0
      B(MM) = 0.D0

   ! ELIMINATE ERROR VECTORS WITH THE LARGEST NORM:
   80 CONTINUE
      DO 90 I=1,MM
   90 BS(I) = B(I)
      IF (NDIIS .EQ. MSET) GO TO 140
      DO 130 II=1,MSET-NDIIS
         XMAX = -1.D10
         ITERA = 0
         DO 110 I=1,MSET
            XNORM = 0.D0
            INV = (I-1) * MPLUS
            DO 100 J=1,MSET
  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)
  120    CONTINUE
         B(IONE) = 1.0D0
  130 CONTINUE
  140 CONTINUE

      IF (DEBUG) THEN

      ! OUTPUT THE GDIIS MATRIX:
         WRITE(*,'(/5X,'' GDIIS MATRIX'')')
         ITmp=min(12,MPLUS)
         DO IJ=1,MPLUS
            WRITE(*,'(12(F10.4,1X))') B((IJ-1)*MPLUS+1:(IJ-1)*MPLUS+ITmp)
         END DO
      ENDIF

     ! SCALE DIIS MATRIX BEFORE INVERSION:
 
      DO I=1,MPLUS
         II = MPLUS * (I-1) + I
         GSAVE(I) = 1.D0 / DSQRT(1.D-20+DABS(B(II)))
      END DO
	  
      GSAVE(MPLUS) = 1.D0
      DO I=1,MPLUS
         DO J=1,MPLUS
            IJ = MPLUS * (I-1) + J
            B(IJ) = B(IJ) * GSAVE(I) * GSAVE(J)
         END DO
      END DO
	  
      IF (DEBUG) THEN
     ! OUTPUT SCALED GDIIS MATRIX:
         WRITE(*,'(/5X,'' GDIIS MATRIX (SCALED)'')')
         ITmp=min(12,MPLUS)
         DO IJ=1,MPLUS
            WRITE(*,'(12(F10.4,1X))') B((IJ-1)*MPLUS+1:(IJ-1)*MPLUS+ITmp)
         END DO
            
      ENDIF ! matches IF (DEBUG) THEN

     ! INVERT THE GDIIS MATRIX B:
      CALL MINV(B,MPLUS,DET) ! matrix inversion.

      DO I=1,MPLUS
         DO J=1,MPLUS
            IJ = MPLUS * (I-1) + J
            B(IJ) = B(IJ) * GSAVE(I) * GSAVE(J)
         END DO
      END DO

      ! COMPUTE THE INTERMEDIATE INTERPOLATED PARAMETER AND GRADIENT VECTORS:
      DO K=1,NVAR
         XP(K) = 0.D0
         GP(K) = 0.D0
         DO I=1,MSET
            INK = (I-1) * NVAR + K
			!Print *, 'B(',MPLUS*MSET+I,')=', B(MPLUS*MSET+I)
            XP(K) = XP(K) + B(MPLUS*MSET+I) * XSET(INK)
            GP(K) = GP(K) + B(MPLUS*MSET+I) * GSET(INK)
         END DO
	  END DO
	  
      HP=0.D0
      DO I=1,MSET
         HP=HP+B(MPLUS*MSET+I)*ESET(I)
      END DO
	      
      DO K=1,NVAR
         DX(K) = XPARAM(K) - XP(K)
      END DO
      XNORM = SQRT(DOT_PRODUCT(DX,DX))
      IF (PRINT) THEN
         WRITE (6,'(/10X,''DEVIATION IN X '',F7.4,8X,''DETERMINANT '',G9.3)') XNORM,DET
         WRITE(*,'(10X,''GDIIS COEFFICIENTS'')')
         WRITE(*,'(10X,5F12.5)') (B(MPLUS*MSET+I),I=1,MSET)
      ENDIF

      ! THE FOLLOWING TOLERENCES FOR XNORM AND DET ARE SOMEWHAT ARBITRARY!
      THRES = MAX(10.D0**(-NVAR), 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 K=1,MM
	        B(K) = BS(K)
         END DO
         NDIIS = NDIIS - 1
         IF (NDIIS .GT. 0) GO TO 80
         IF (PRINT) WRITE(*,'(10X,''NEWTON-RAPHSON STEP TAKEN'')')
         DO K=1,NVAR
            XP(K) = XPARAM(K)
            GP(K) = GRAD(K)  
         END DO
      ENDIF ! matches IF (XNORM.GT.2.D0 .OR. DABS(DET).LT. THRES) THEN
	  
         ! q_{m+1} = q'_{m+1} - H^{-1}g'_{m+1}
		 ! Hess is a symmetric matrix.
		 !Hess_inv = 1.d0 ! to be deleted.
		 !Call GenInv(NVAR,Reshape(Hess,(/NVAR,NVAR/)),Hess_inv,NVAR) ! Implemented in Mat_util.f90
		 ! H^{-1}g'_{m+1}
		 !Print *, 'Hess_inv='
		 !Print *, Hess_inv
		 !XPARAM=0.d0
		 !DO I=1, NVAR
		 !XPARAM(:) = XPARAM(:) + Hess_inv(:,I)*GP(I)
		 !END DO
		 !XPARAM(:) = XP(:) - XPARAM(:) ! now XPARAM is a new geometry.
		 
		 !STEP is the difference between the new and old geometry and thus "step":
         !STEP = XPARAM - XPARAM_old
         
      IF (PRINT)  WRITE(*,'(/,''       END GDIIS  '',/)')

      END SUBROUTINE Step_DIIS
