!C  HEAT is never used, not even in call of Space(...)
!C  Geom = input parameter vector (Geometry).
!C  Grad = input gradient vector.
      SUBROUTINE Step_GDIIS_Simple_Err(NewGeom,Geom,NewGrad,GRAD,HP,HEAT,Hess,NCoord,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) :: NCoord
      REAL(KREAL) :: NewGeom(NCoord), Geom(NCoord), NewGrad(NCoord), GRAD(NCoord), Hess(NCoord*NCoord)
      REAL(KREAL) :: HEAT, HP
      LOGICAL :: FRST
    
!************************************************************************
!*                                                                      *
!*     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 = 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) :: ESET(MRESET)
      REAL(KREAL), ALLOCATABLE, SAVE :: DX(:), GSAVE(:) !NCoord
      REAL(KREAL) :: B(M2), BS(M2), BST(M2)
      LOGICAL DEBUG, PRINT
      INTEGER(KINT), SAVE :: MSET
      INTEGER(KINT) :: NDIIS, MPLUS, INV, ITERA, MM, I, J, K
      INTEGER(KINT) :: JJ, KJ, JNV, II, IONE, IJ, INK,ITmp
      REAL(KREAL) :: XMax, XNorm, S, DET, THRES

      DEBUG=.TRUE.
      PRINT=.TRUE.

      IF (PRINT)  WRITE(*,'(/,''      BEGIN Step_GDIIS_Simple_Err   '')')

      ! Initialization
      IF (FRST) THEN
       ! FRST will be set to False in Space, so no need to modify it here
         IF (ALLOCATED(GeomSet)) THEN
            IF (PRINT)  WRITE(*,'(/,''    In FRST, Step_GDIIS_Simple_Err Dealloc  '')')
            DEALLOCATE(GeomSet,GradSet,ERR,DX,GSave)
            RETURN
         ELSE
            IF (PRINT)  WRITE(*,'(/,''     In FRST,  Step_GDIIS_Simple_Err alloc  '')')
            ALLOCATE(GeomSet(MRESET*NCoord), GradSet(MRESET*NCoord), ERR(MRESET*NCoord))
            ALLOCATE(DX(NCoord),GSAVE(NCoord))
         END IF
      END IF

      ! SPACE SIMPLY LOADS THE CURRENT VALUES OF Geom AND GRAD INTO THE ARRAYS GeomSet AND GradSet
      ! HEAT is never used, not even in Space(...)
      CALL SPACE(MRESET,MSET,Geom,Grad,HEAT,NCoord,GeomSet,GradSet,ESET,FRST)

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

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

      ! CONSTRUCT THE GDIIS MATRIX:
      ! B_ij calculations from <B_ij=(g_i-g_j)(R_i-R_j)>
      JJ=0
      INV=-NCoord
      DO I=1,MSET
         INV=INV+NCoord
         JNV=-NCoord
         DO J=1,MSET
            JNV=JNV+NCoord
            JJ = JJ + 1
            B(JJ)=0.D0
      DO K=1, NCoord
         B(JJ) = B(JJ) + (((GradSet(INV+K)-GradSet(JNV+K))*(GeomSet(INV+K)-GeomSet(JNV+K)))/2.D0)
      END DO
         END DO
      END DO   

     ! 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,'' Step_GDIIS_Simple_Err 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 160 I=1,MPLUS
         II = MPLUS * (I-1) + I
  160 GSAVE(I) = 1.D0 / DSQRT(1.D-20+DABS(B(II)))
      GSAVE(MPLUS) = 1.D0
      DO 170 I=1,MPLUS
         DO 170 J=1,MPLUS
            IJ = MPLUS * (I-1) + J
  170 B(IJ) = B(IJ) * GSAVE(I) * GSAVE(J)
  
      IF (DEBUG) THEN

      ! OUTPUT SCALED GDIIS MATRIX:
         WRITE(*,'(/5X,'' Step_GDIIS_Simple_Err 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 190 I=1,MPLUS
         DO 190 J=1,MPLUS
            IJ = MPLUS * (I-1) + J
  190 B(IJ) = B(IJ) * GSAVE(I) * GSAVE(J)
  
      ! COMPUTE THE INTERMEDIATE INTERPOLATED PARAMETER AND GRADIENT VECTORS:
      DO 200 K=1,NCoord
         NewGeom(K) = 0.D0
         NewGrad(K) = 0.D0
         DO 200 I=1,MSET
            INK = (I-1) * NCoord + K
      !Print *, 'B(',MPLUS*MSET+I,')=', B(MPLUS*MSET+I)
            NewGeom(K) = NewGeom(K) + B(MPLUS*MSET+I) * GeomSet(INK)
  200 NewGrad(K) = NewGrad(K) + B(MPLUS*MSET+I) * GradSet(INK)
      HP=0.D0
      DO 210 I=1,MSET
  210 HP=HP+B(MPLUS*MSET+I)*ESET(I)
      
      DO 220 K=1,NCoord
  220 DX(K) = Geom(K) - NewGeom(K)
      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,''Step_GDIIS_Simple_Err 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**(-NCoord), 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,NCoord
            NewGeom(K) = Geom(K)
  240       NewGrad(K) = GRAD(K)  
      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(NCoord,Reshape(Hess,(/NCoord,NCoord/)),Hess_inv,NCoord) ! Implemented in Mat_util.f90
     ! H^{-1}g'_{m+1}
     !Print *, 'Hess_inv='
    ! Print *, Hess_inv
     !Geom=0.d0
     !DO I=1, NCoord
      !  Geom(:) = Geom(:) + Hess_inv(:,I)*NewGrad(I)
     !END DO
     !Geom(:) = NewGeom(:) - Geom(:) ! now Geom is a new geometry.
     
     ! STEP is the difference between the new and old geometry and thus "step":
         !STEP = Geom - Geom_old
         
      IF (PRINT)  WRITE(*,'(/,''       END Step_GDIIS_Simple_Err  '',/)')

      END SUBROUTINE Step_GDIIS_Simple_Err
