     ! Geom = input parameter vector (Geometry), Grad = input gradient vector.
     ! HEAT is Energy(Geom)
      SUBROUTINE Step_GEDIIS(Geom_new,Geom,Grad,HEAT,Hess,NCoord,FRST)

	  use Io_module
	  use Path_module, only : Nom, Atome, OrderInv, indzmat, Pi, Nat
	  
      IMPLICIT NONE

      INTEGER(KINT) :: NCoord
      REAL(KREAL) :: Geom_new(NCoord), Grad(NCoord), Hess(NCoord*NCoord)
	  REAL(KREAL), INTENT(IN) :: Geom(NCoord)
	  REAL(KREAL) :: HEAT ! HEAT= Energy
      LOGICAL :: FRST

      ! MRESET = maximum number of iterations.
      INTEGER(KINT), PARAMETER :: MRESET=15, M2=(MRESET+1)*(MRESET+1) !M2 = 256
      REAL(KREAL), ALLOCATABLE, SAVE :: GeomSet(:), GradSet(:) ! MRESET*NCoord
      REAL(KREAL), ALLOCATABLE, SAVE :: DX(:), GSAVE(:) !NCoord
	  REAL(KREAL), SAVE :: ESET(MRESET)
	  REAL(KREAL) :: ESET_tmp(MRESET), B(M2),BS(M2),BST(M2), B_tmp(M2) ! M2=256
      LOGICAL DEBUG, PRINT, ci_lt_zero
      INTEGER(KINT), SAVE :: MSET ! mth Iteration
	  REAL(KREAL) :: ci(MRESET), ci_tmp(MRESET) ! MRESET = maximum number of iterations.
      INTEGER(KINT) :: NGEDIIS, MPLUS, INV, ITERA, MM, cis_zero, IXX, JXX, MSET_minus_cis_zero
      INTEGER(KINT) :: I,J,K, JJ, KJ, JNV, II, IONE, IJ, INK,ITmp, IX, JX, KX, MSET_C_cis_zero
	  INTEGER(KINT) :: current_size_B_mat, MyPointer, Iat
      REAL(KREAL) :: XMax, XNorm, S, DET, THRES, tmp, ER_star, ER_star_tmp

      DEBUG=.TRUE.
      PRINT=.FALSE.
	  
      IF (PRINT)  WRITE(*,'(/,''      BEGIN GEDIIS   '')')
	  
      ! Initialization
      IF (FRST) THEN
      ! FRST will be set to False in SPACE_GEDIIS, so no need to modify it here
         IF (ALLOCATED(GeomSet)) THEN
            IF (PRINT)  WRITE(*,'(/,''    In FRST, GEDIIS Dealloc  '')')
            DEALLOCATE(GeomSet,GradSet,DX,GSave)
            RETURN
         ELSE
            IF (PRINT)  WRITE(*,'(/,''     In FRST,  GEDIIS Alloc  '')')
            ALLOCATE(GeomSet(MRESET*NCoord),GradSet(MRESET*NCoord),DX(NCoord),GSAVE(NCoord))
         END IF
      END IF ! IF (FRST) THEN

      ! SPACE_GEDIIS SIMPLY LOADS THE CURRENT VALUES OF Geom AND Grad INTO THE ARRAYS GeomSet
      ! AND GradSet, MSET is set to zero and then 1 in SPACE_GEDIIS at first iteration.
      CALL SPACE_GEDIIS(MRESET,MSET,Geom,Grad,HEAT,NCoord,GeomSet,GradSet,ESET,FRST)

      IF (PRINT)  WRITE(*,'(/,''       GEDIIS after SPACE_GEDIIS  '')')

      ! INITIALIZE SOME VARIABLES AND CONSTANTS:
      NGEDIIS = MSET !MSET=mth iteration
      MPLUS = MSET + 1
      MM = MPLUS * MPLUS

      ! CONSTRUCT THE GEDIIS 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 GEDIIS matrix.
	 ! The correction is needed because the last coloumn of the matrix contains all 1 and one zero.
      DO I=MSET-1,1,-1
         DO J=MSET,1,-1
            B(I*MSET+J+I) = B(I*MSET+J) 
         END DO
	  END DO
   
      ! for last row and last column of GEDIIS matrix 
      DO I=1,MPLUS
         B(MPLUS*I) = 1.D0
         B(MPLUS*MSET+I) = 1.D0
      END DO
      B(MM) = 0.D0
	  
	  DO I=1, MPLUS
	     !WRITE(*,'(10(1X,F20.4))') B((I-1)*MPLUS+1:I*(MPLUS))
	  END DO
	  	  
      ! ELIMINATE ERROR VECTORS WITH THE LARGEST NORM:
   80 CONTINUE
      DO I=1,MM !MM = (MSET+1) * (MSET+1)
         BS(I) = B(I) !just a copy of the original B (GEDIIS) matrix
      END DO
	  
      IF (NGEDIIS .NE. MSET) THEN
        DO II=1,MSET-NGEDIIS
           XMAX = -1.D10
           ITERA = 0
           DO I=1,MSET
              XNORM = 0.D0
              INV = (I-1) * MPLUS
              DO J=1,MSET
                 XNORM = XNORM + ABS(B(INV + J))
              END DO
              IF (XMAX.LT.XNORM .AND. XNORM.NE.1.0D0) THEN
                 XMAX = XNORM
                 ITERA = I
                 IONE = INV + I
              ENDIF
           END DO
		 
           DO I=1,MPLUS
              INV = (I-1) * MPLUS
              DO J=1,MPLUS
                 JNV = (J-1) * MPLUS
                 IF (J.EQ.ITERA) B(INV + J) = 0.D0
                 B(JNV + I) = B(INV + J)
              END DO
		   END DO
           B(IONE) = 1.0D0
        END DO
	  END IF ! matches IF (NGEDIIS .NE. MSET) THEN
	  
      ! SCALE GEDIIS MATRIX BEFORE INVERSION:
      DO I=1,MPLUS
         II = MPLUS * (I-1) + I ! B(II)=diagonal elements of B matrix
         GSAVE(I) = 1.D0 / DSQRT(1.D-20+DABS(B(II)))
		 !Print *, 'GSAVE(',I,')=', GSAVE(I) 
      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
  
     ! INVERT THE GEDIIS MATRIX B:
	  DO I=1, MPLUS
	     !WRITE(*,'(10(1X,F20.4))') B((I-1)*MPLUS+1:I*(MPLUS))
	  END DO
	  	  
      CALL MINV(B,MPLUS,DET) ! matrix inversion.
	  
	  DO I=1, MPLUS
	     !WRITE(*,'(10(1X,F20.16))') B((I-1)*MPLUS+1:I*(MPLUS))
	  END DO
	  
      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 NEW INTERPOLATED PARAMETER VECTOR (Geometry):
	  ci=0.d0
	  ci_tmp=0.d0
	
	  ci_lt_zero= .FALSE.
	  DO I=1, MSET
		 DO J=1, MSET ! B matrix is read column-wise
		    ci(I)=ci(I)+B((J-1)*(MPLUS)+I)*ESET(J) !ESET is energy set, yet to be fixed.
		 END DO
		 ci(I)=ci(I)+B((MPLUS-1)*(MPLUS)+I)
		 !Print *, 'NO ci < 0 yet, c(',I,')=', ci(I)
		 IF((ci(I) .LT. 0.0D0) .OR. (ci(I) .GT. 1.0D0)) THEN
		   ci_lt_zero=.TRUE.
		   EXIT
		 END IF
      END DO !matches DO I=1, MSET
	  
	  IF (ci_lt_zero) Then
	  	 cis_zero = 0
         ER_star = 0.D0
		 ER_star_tmp = 1e32
		 
		 ! B_ij calculations from <B_ij=(g_i-g_j)(R_i-R_j)>, Full B matrix created first and then rows and columns are removed.
         JJ=0
         INV=-NCoord
         DO IX=1,MSET
            INV=INV+NCoord
            JNV=-NCoord
            DO JX=1,MSET
               JNV=JNV+NCoord
               JJ = JJ + 1
               BST(JJ)=0.D0
		       DO KX=1, NCoord
		          BST(JJ) = BST(JJ) + (((GradSet(INV+KX)-GradSet(JNV+KX))*(GeomSet(INV+KX)-GeomSet(JNV+KX)))/2.D0)
		       END DO
            END DO
	     END DO  
		
		 DO I=1, (2**MSET)-2 ! all (2**MSET)-2 combinations of cis, except the one where all cis are .GT. 0 and .LT. 1
		     ci(:)=1.D0
		     ! find out which cis are zero in I:
			 DO IX=1, MSET
			    JJ=IAND(I, 2**(IX-1))
				IF(JJ .EQ. 0) Then
				  ci(IX)=0.D0
			    END IF
			 END DO
			 
			 ci_lt_zero = .FALSE.
			 ! B_ij calculations from <B_ij=(g_i-g_j)(R_i-R_j)>, Full B matrix created first and then rows and columns are removed.
			 DO IX=1, MSET*MSET
                B(IX) = BST(IX) !just a copy of the original B (GEDIIS) matrix
             END DO
			 
             ! Removal of KXth row and KXth column in order to accomodate cI to be zero:
			 current_size_B_mat=MSET
			 cis_zero = 0
			 ! The bits of I (index of the upper loop 'DO I=1, (2**MSET)-2'), gives which cis are zero.
			 DO KX=1, MSET ! searching for each bit of I (index of the upper loop 'DO I=1, (2**MSET)-2')
			    IF (ci(KX) .EQ. 0.D0) Then !remove KXth row and KXth column
				   cis_zero = cis_zero + 1
				   
			       ! First row removal: (B matrix is read column-wise)
			       JJ=0
                   DO IX=1,current_size_B_mat ! columns reading
                      DO JX=1,current_size_B_mat ! rows reading
				         IF (JX .NE. KX) Then
				             JJ = JJ + 1
				             B_tmp(JJ) = B((IX-1)*current_size_B_mat+JX)
				         END IF
				      END DO   
			       END DO
			 
			       DO IX=1,current_size_B_mat*(current_size_B_mat-1)
			          B(IX) = B_tmp(IX)
			       END DO
			 
			       ! Now column removal:
			       JJ=0	 
                   DO IX=1,KX-1 ! columns reading
                      DO JX=1,current_size_B_mat-1 ! rows reading
				         JJ = JJ + 1
				         B_tmp(JJ) = B(JJ)
				      END DO
			       END DO				
			  
                   DO IX=KX+1,current_size_B_mat
                      DO JX=1,current_size_B_mat-1
				         JJ = JJ + 1
			   	         B_tmp(JJ) = B(JJ+current_size_B_mat-1)
				      END DO
			       END DO
			 			 
			       DO IX=1,(current_size_B_mat-1)*(current_size_B_mat-1)
			          B(IX) = B_tmp(IX)
			       END DO
				   current_size_B_mat = current_size_B_mat - 1
				END IF ! matches IF (ci(KX) .EQ. 0.D0) Then !remove
	         END DO ! matches DO KX=1, MSET

			 ! The following shifting is required to correct indices of B_ij elements in the GEDIIS matrix.
			 ! The correction is needed because the last coloumn and row of the matrix contains all 1 and one zero.
			 DO IX=MSET-cis_zero-1,1,-1
				DO JX=MSET-cis_zero,1,-1
				   B(IX*(MSET-cis_zero)+JX+IX) = B(IX*(MSET-cis_zero)+JX) 
				END DO
			 END DO
			 
			 ! for last row and last column of GEDIIS matrix 
			 DO IX=1,MPLUS-cis_zero
				B((MPLUS-cis_zero)*IX) = 1.D0
				B((MPLUS-cis_zero)*(MSET-cis_zero)+IX) = 1.D0
			 END DO
			 B((MPLUS-cis_zero) * (MPLUS-cis_zero)) = 0.D0
	       
	         DO IX=1, MPLUS
	            !WRITE(*,'(10(1X,F20.4))') B((IX-1)*MPLUS+1:IX*(MPLUS))
	         END DO
		   		   
			 ! ELIMINATE ERROR VECTORS WITH THE LARGEST NORM:
             IF (NGEDIIS .NE. MSET) THEN
			    JX = min(MSET-NGEDIIS,MSET-cis_zero-1)
                DO II=1,JX
                   XMAX = -1.D10
                   ITERA = 0
                   DO IX=1,MSET-cis_zero
                      XNORM = 0.D0
                      INV = (IX-1) * (MPLUS-cis_zero)
                      DO J=1,MSET-cis_zero
                         XNORM = XNORM + ABS(B(INV + J))
                      END DO
                      IF (XMAX.LT.XNORM .AND. XNORM.NE.1.0D0) THEN
                         XMAX = XNORM
                         ITERA = IX
                         IONE = INV + IX
                      ENDIF
				   END DO
		 
                   DO IX=1,MPLUS-cis_zero
                      INV = (IX-1) * (MPLUS-cis_zero)
                      DO J=1,MPLUS-cis_zero
                         JNV = (J-1) * (MPLUS-cis_zero)
                         IF (J.EQ.ITERA) B(INV + J) = 0.D0
                         B(JNV + IX) = B(INV + J)
					  END DO
		           END DO
                   B(IONE) = 1.0D0
			    END DO
	         END IF ! matches IF (NGEDIIS .NE. MSET) THEN

			 ! SCALE GEDIIS MATRIX BEFORE INVERSION:
			 DO IX=1,MPLUS-cis_zero
				II = (MPLUS-cis_zero) * (IX-1) + IX ! B(II)=diagonal elements of B matrix
				GSAVE(IX) = 1.D0 / DSQRT(1.D-20+DABS(B(II)))
			 END DO
			 GSAVE(MPLUS-cis_zero) = 1.D0
			 DO IX=1,MPLUS-cis_zero
				DO JX=1,MPLUS-cis_zero
				   IJ = (MPLUS-cis_zero) * (IX-1) + JX
				   B(IJ) = B(IJ) * GSAVE(IX) * GSAVE(JX)
				END DO
			 END DO

			 ! INVERT THE GEDIIS MATRIX B:	  
			 CALL MINV(B,MPLUS-cis_zero,DET) ! matrix inversion.
	  
			 DO IX=1,MPLUS-cis_zero
				DO JX=1,MPLUS-cis_zero
				   IJ = (MPLUS-cis_zero) * (IX-1) + JX
				   B(IJ) = B(IJ) * GSAVE(IX) * GSAVE(JX)
				END DO
			 END DO
			 
	         DO IX=1, MPLUS
	            !WRITE(*,'(10(1X,F20.4))') B((IX-1)*MPLUS+1:IX*(MPLUS))
	         END DO
			 			 
             ! ESET is rearranged to handle zero cis and stored in ESET_tmp:
			 JJ=0
			 DO IX=1, MSET
				IF (ci(IX) .NE. 0) Then
				   JJ=JJ+1
				   ESET_tmp(JJ) = ESET(IX)
				END IF
			 END DO
			 
			 ! DETERMINATION OF nonzero cis:
			 MyPointer=1
     	     DO IX=1, MSET-cis_zero
			    tmp = 0.D0
		        DO J=1, MSET-cis_zero ! B matrix is read column-wise
				   tmp=tmp+B((J-1)*(MPLUS-cis_zero)+IX)*ESET_tmp(J)
				END DO
		        tmp=tmp+B((MPLUS-cis_zero-1)*(MPLUS-cis_zero)+IX)
		        IF((tmp .LT. 0.0D0) .OR. (tmp .GT. 1.0D0)) THEN
		           ci_lt_zero=.TRUE.
		           EXIT
				ELSE
				   DO JX=MyPointer,MSET
				      IF (ci(JX) .NE. 0) Then
				         ci(JX) = tmp
					     MyPointer=JX+1
					     EXIT
					  END IF
				   END DO
		        END IF
             END DO !matches DO I=1, MSET-cis_zero
	         !Print *, 'Local set of cis, first 10:, MSET=', MSET, ', I of (2**MSET)-2=', I
			 !WRITE(*,'(10(1X,F20.4))') ci(1:MSET)
	         !Print *, 'Local set of cis ends:****************************************'

             ! new set of cis determined based on the lower energy (ER_star):
			 IF (.NOT. ci_lt_zero) Then
                Call Energy_GEDIIS(MRESET,MSET,ci,GeomSet,GradSet,ESET,NCoord,ER_star)
				IF (ER_star .LT. ER_star_tmp) Then			
				   ci_tmp=ci	
				   ER_star_tmp = ER_star
				END IF
             END IF ! matches IF (.NOT. ci_lt_zero) Then
          END DO !matches DO I=1, (2**K)-2 ! all (2**K)-2 combinations of cis, except the one where all cis are .GT. 0 and .LT. 1 
		  ci = ci_tmp
	  END IF! matches IF (ci_lt_zero) Then
	  
	  Print *, 'Final set of cis, first 10:***********************************'
	  WRITE(*,'(10(1X,F20.4))') ci(1:MSET)
	  Print *, 'Final set of cis ends:****************************************'
	  Geom_new(:) = 0.D0
	  DO I=1, MSET	  
         Geom_new(:) = Geom_new(:) + (ci(I)*GeomSet((I-1)*NCoord+1:I*NCoord)) !MPLUS=MSET+1
		 ! R_(N+1)=R*+DeltaR:
		 DO J=1, NCoord
			tmp=0.D0
			DO K=1,NCoord
			   !tmp=tmp+Hess((J-1)*NCoord+K)*GradSet((I-1)*NCoord+K) ! If Hinv=.False., then we need to invert Hess
			END DO
			Geom_new(J) = Geom_new(J) - (ci(I)*tmp)
		 END DO
	  END DO
	  
	  DX(:) = Geom(:) - Geom_new(:)
	  
      XNORM = SQRT(DOT_PRODUCT(DX,DX))
      IF (PRINT) THEN
         WRITE (6,'(/10X,''DEVIATION IN X '',F10.4,8X,''DETERMINANT '',G9.3)') XNORM, DET
         !WRITE(*,'(10X,''GEDIIS 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 GEDIIS MATRIX IS ILL CONDITIONED"     
            WRITE(*,*) " - PROBABLY, VECTORS ARE LINEARLY DEPENDENT - "
            WRITE(*,*) "THE GEDIIS STEP WILL BE REPEATED WITH A SMALLER SPACE"
         END IF
         DO K=1,MM
			B(K) = BS(K) ! why this is reverted? Because "IF (NGEDIIS .GT. 0) GO TO 80", see below
         END DO
         NGEDIIS = NGEDIIS - 1
         IF (NGEDIIS .GT. 0) GO TO 80
         IF (PRINT) WRITE(*,'(10X,''NEWTON-RAPHSON STEP TAKEN'')')
         Geom_new(:) = Geom(:) ! Geom_new is set to original Geom, thus DX = Geom(:) - Geom_new(:)=zero
      END IF ! matches IF (XNORM.GT.2.D0 .OR. DABS(DET).LT. THRES) THEN
	  
	 !*******************************************************************************************************************
	  Geom_new(:) = 0.D0
	  DO I=1, MSET	  
         Geom_new(:) = Geom_new(:) + (ci(I)*GeomSet((I-1)*NCoord+1:I*NCoord)) !MPLUS=MSET+1
		 ! R_(N+1)=R*+DeltaR:
		 DO J=1, NCoord
			tmp=0.D0
			DO K=1,NCoord
			   tmp=tmp+Hess((J-1)*NCoord+K)*GradSet((I-1)*NCoord+K) ! If Hinv=.False., then we need to invert Hess
			END DO
			Geom_new(J) = Geom_new(J) - (ci(I)*tmp)
		 END DO
	  END DO
	 !*******************************************************************************************************************
	  
      IF (PRINT)  WRITE(*,'(/,''       END GEDIIS  '',/)')

      END SUBROUTINE Step_GEDIIS

