! This subroutine optimizes a geometry. It is mainly here for  debugging purposes..
! It has been designed to be almost independent of the rest of the code.

 SUBROUTINE Opt_geom(IGeom,Nat,NCoord,Coord,Geom,E,Flag_Opt_Geom,Step_method)

  use Path_module, only : maxcyc,sthresh,gthresh,Hinv,Smax,Nom,Atome,OrderInv,indzmat,&
						  XyzGeomF,IntCoordF,UMat,ScanCoord,Coordinate,NPrim,&
						  BTransInv,BTransInv_local,XyzGeomI,Xprimitive_t,ScanCoord,&
						  Coordinate,Pi,UMat_local,NCart
  use VarTypes
  IMPLICIT NONE

  INTEGER(KINT), INTENT(IN) :: Nat,NCoord,IGeom
  CHARACTER(32), INTENT(IN) :: Coord
  CHARACTER(32), INTENT(IN) :: Step_method
  REAL(KREAL), INTENT(INOUT) :: Geom(NCoord)
  REAL(KREAL), INTENT(OUT) :: E
  LOGICAL, INTENT(IN) :: Flag_Opt_Geom

! As this subroutine is here for debugging, debug=T !
  LOGICAL, PARAMETER :: debug=.TRUE.
  LOGICAL :: Fini
  LOGICAL, SAVE :: FRST=.TRUE.

! Variables
  INTEGER(KINT) :: IOpt, I,J,Iat, IBEG
  REAL(KREAL), ALLOCATABLE :: GradTmp(:),ZeroVec(:) ! 3*Nat or 3*Nat-6
  REAL(KREAL), ALLOCATABLE :: GeomOld(:),GradOld(:) ! (3*Nat or 3*Nat-6)
  REAL(KREAL), ALLOCATABLE :: GeomRef(:) ! (3*Nat or 3*Nat-6)
  REAL(KREAL), ALLOCATABLE :: GeomTmp2(:),GradTmp2(:) ! 3*Nat or 3*Nat-6
  REAL(KREAL), ALLOCATABLE :: Hess_local(:,:)
  REAL(KREAL), ALLOCATABLE :: GeomCart(:,:) !(Nat,3)
  REAL(KREAL), ALLOCATABLE :: GeomCart_old(:,:) !(Nat,3)
  REAL(KREAL), ALLOCATABLE :: Hprim(:,:) !(NPrim,NPrim)
  REAL(KREAL), ALLOCATABLE :: HprimU(:,:) !(NPrim,3*Nat-6))
  REAL(KREAL), ALLOCATABLE :: NewGeom(:) !NCoord=3*Nat-6
  REAL(KREAL), ALLOCATABLE :: NewGradTmp(:)
  REAL(KREAL), ALLOCATABLE :: Hess_local_inv(:,:) ! used in diis
  REAL(KREAL) :: NormStep, FactStep, HP

    E=0.

    if (debug) WRITE(*,*) "=================== Geom Optimization =================="

     ALLOCATE(GradTmp2(NCoord),GeomTmp2(NCoord),GradTmp(NCoord))
     ALLOCATE(GradOld(NCoord),GeomOld(NCoord),ZeroVec(NCoord))
     ALLOCATE(GeomRef(NCoord))
     ALLOCATE(Hess_local(NCoord,NCoord))
	 ALLOCATE(GeomCart(Nat,3),GeomCart_old(Nat,3))
	 ALLOCATE(NewGeom(NCoord))
 	 ALLOCATE(NewGradTmp(NCoord))
	 ALLOCATE(Hess_local_inv(NCoord,NCoord))
	 
     IOpt=0
     ZeroVec=0.d0
     
     ! Initialize the Hessian
     Hess_local=0.

     SELECT CASE (COORD)
	 CASE ('ZMAT')
        ! We use the fact that we know that approximate good hessian values
        ! are 0.5 for bonds, 0.1 for valence angle and 0.02 for dihedral angles
        Hess_local(1,1)=0.5d0
        Hess_local(2,2)=0.5d0
        Hess_local(3,3)=0.1d0
        DO J=1,Nat-3
           Hess_local(3*J+1,3*J+1)=0.5d0
           Hess_local(3*J+2,3*J+2)=0.1d0
           Hess_local(3*J+3,3*J+3)=0.02d0
        END DO
        IF (HInv) THEN
           DO I=1,NCoord
              Hess_local(I,I)=1.d0/Hess_local(I,I)
           END DO
        END IF

        IF (Step_method .EQ. "RFO") Then
                   Hess_local=0.
           DO I=1,NCoord
              Hess_local(I,I)=0.5d0
           END DO
        END IF

	 CASE ('BAKER')
	     ! UMat(NPrim,3*Nat-6)
		 BTransInv_local = BTransInv
		 UMat_local = UMat
         ALLOCATE(Hprim(NPrim,NPrim),HprimU(NPrim,NCoord))
         Hprim=0.d0
         ScanCoord=>Coordinate
         I=0
         DO WHILE (Associated(ScanCoord%next))
            I=I+1
            SELECT CASE (ScanCoord%Type)
			CASE ('BOND')
			     Hprim(I,I) = 0.5d0
            CASE ('ANGLE')
			     Hprim(I,I) = 0.2d0
            CASE ('DIHEDRAL')
			     Hprim(I,I) = 0.1d0
	        END SELECT
            ScanCoord => ScanCoord%next
         END DO
		 
         ! Hprim U:
         HprimU=0.d0
         DO I=1, NCoord
		    DO J=1,NPrim
               HprimU(:,I) = HprimU(:,I) + Hprim(:,J)*UMat(J,I)
            END DO
         END DO

         ! Hess = U^T Hprim U:
         Hess_local=0.d0
         DO I=1, NCoord
            DO J=1,NPrim
		       ! UMat^T is needed below.
			   Hess_local(:,I) = Hess_local(:,I) + UMat(J,:)*HprimU(J,I)
            END DO
         END DO
		 
		 !Print *, 'Hprim='
		 DO I=1,NPrim
		 !   WRITE(*,'(10(1X,F6.3))') Hprim(I,:)
	     END DO
		 !Print *, 'UMat='
		 DO I=1,NPrim
		  !  WRITE(*,'(3(1X,F6.3))') UMat(I,1:3)
	     END DO
		 !Print *, 'HprimU='
		 DO I=1,NPrim
		  !  WRITE(*,'(3(1X,F6.3))') HprimU(I,1:3)
	     END DO
		 !Print *, 'Hess_local='
		 DO I=1,NCoord
		  !  WRITE(*,'(3(1X,F6.3))') Hess_local(I,1:3)
	     END DO
		 
	     DEALLOCATE(Hprim,HprimU)
		 
         IF (HInv) THEN
			Call GenInv(NCoord,Hess_local,Hess_local_inv,NCoord)
		    Hess_local = Hess_local_inv
        END IF
		
		!Print *, 'Hess_local after inversion='
		 DO I=1,NCoord
		 !   WRITE(*,'(3(1X,F6.3))') Hess_local(I,1:3)
	     END DO
				 
        IF (Step_method .EQ. "RFO") Then
		   Hess_local=0.
           DO I=1,NCoord
              Hess_local(I,I)=0.5d0
           END DO
        END IF
		   
	 CASE ('MIXED','CART','HYBRID')
        DO J=1,NCoord
           Hess_local(J,J)=1.
        END DO
	 CASE DEFAULT
        WRITE (*,*) "Coord=",TRIM(COORD)," not recognized, opt_Geom.f90, L164. Stop"
        STOP
     END SELECT	

     ! Go to optimization
	 GeomOld=0.d0 ! Internal coordinates
	 GeomCart_old=0.d0 ! Cartesian coordinates

     Fini=.FALSE.
     DO WHILE ((IOpt.LT.MaxCyc).AND.(.NOT.Fini))
        IOpt=IOpt+1

        ! Calculate the  energy and gradient
        IF (debug) THEN 
           WRITE(*,*) 'L134, DBG Opt_Geom, COORD=',TRIM(COORD),' IOpt=',IOpt
           WRITE(*,*) "Energy and Coordinates:"
           WRITE(*,'(F12.6,12(1X,F8.3))') E,Geom(1:min(NCoord,12))
           WRITE(*,*) "Geom Old:"
           WRITE(*,'(F12.6,12(1X,F8.3))') GEomOld(1:min(NCoord,13))
           WRITE(*,*) "Grad Old:"
           WRITE(*,'(F12.6,12(1X,F8.3))') GradOld(1:min(NCoord,13))
        END IF

        !Call EGrad(E,Geom,GradTmp,NCoord)
        IF (COORD.EQ.'BAKER') THEN
           ! GradTmp is gradient and calculated in Egrad.F, INTENT(OUT).
           ! GeomCart has INTENT(OUT)
           ! GeomRef is modified in Egrag_baker, so we should not use GeomOld variable
           GeomRef=GeomOld
           Call EGrad_baker(E,Geom,GradTmp,NCoord,IGeom,IOpt,GeomCart,Flag_Opt_Geom, &
                GeomRef,GeomCart_old)
           GeomCart_old=GeomCart
        ELSE
           Call EGrad(E,Geom,GradTmp,NCoord,IGeom,IOpt,GeomCart)
        END IF
		
        IF (debug) THEN 
           WRITE(*,*) 'L198, DBG Opt_Geom, COORD=',TRIM(COORD),' IOpt=',IOpt
           WRITE(*,*) "Energy and Coordinates:"
           WRITE(*,'(F12.6,12(1X,F8.3))') E,Geom(1:min(NCoord,12))
           WRITE(*,*) "Geom Old:"
           WRITE(*,'(F12.6,12(1X,F8.3))') GEomOld(1:min(NCoord,13))
	    END IF

     IF (IOpt.GE.2) THEN
        ! We have enough geometries and gradient to update the hessian (or its 
        ! inverse)
        GradTmp2=GradTmp-GradOld
        GeomTmp2=Geom-GeomOld

        if (debug) Write(*,*) "Call Hupdate_all, Geom"
        IF (debug) THEN
		   WRITE(*,*) "dx before calling",SHAPE(GeomTmp2)
		   WRITE(*,'(12(1X,F8.4))') GeomTmp2(1:NCoord)
		   WRITE(*,*) "dgrad before calling",SHAPE(GradTmp2)
		   WRITE(*,'(12(1X,F8.4))') GradTmp2(1:NCoord)
		END IF
           Call Hupdate_all(NCoord,GeomTmp2,GradTmp2,Hess_local)
        END IF ! matches IF (IOpt.GE.2) THEN
		
        GradOld=GradTmp
        GeomOld=Geom

        ! GradTmp becomes Step in Step_RFO_all.
		SELECT CASE (Step_method)
        CASE ('RFO')
          Call Step_RFO_all(NCoord,GradTmp,IGeom,Geom,GradTmp,Hess_local,ZeroVec)
		CASE ('GDIIS')
          Call Step_DIIS(NewGeom,Geom,NewGradTmp,GradTmp,HP,E,Hess_local,NCoord,FRST)
		 ! q_{m+1} = q'_{m+1} - H^{-1}g'_{m+1}
			Geom=0.d0
            DO I=1, NCoord
			   ! If Hinv=.False., then we need to invert Hess_local
			   Geom(:) = Geom(:) + Hess_local(:,I)*NewGradTmp(I)
			END DO
			Geom(:) = NewGeom(:) - Geom(:)
			! GradTmp now becomes "step" below:
		    GradTmp = Geom - GeomOld
		CASE ('GDIIST')
          Call Step_GDIIS_Simple_Err(NewGeom,Geom,NewGradTmp,GradTmp,HP,E,Hess_local,NCoord,FRST)
		 ! q_{m+1} = q'_{m+1} - H^{-1}g'_{m+1}
			Geom=0.d0
            DO I=1, NCoord
			   ! If Hinv=.False., then we need to invert Hess_local
			   Geom(:) = Geom(:) + Hess_local(:,I)*NewGradTmp(I)
			END DO
			Geom(:) = NewGeom(:) - Geom(:)
			! GradTmp now becomes "step" below:
		    GradTmp = Geom - GeomOld
		CASE ('GEDIIS')
          Call Step_GEDIIS(NewGeom,Geom,GradTmp,E,Hess_local,NCoord,FRST)
			! GradTmp is actually "step" below:
		     GradTmp = NewGeom - GeomOld
		  !Call Step_GEDIIS_All(3,1,GradTmp,Geom,GradTmp,E,Hess_local,NCoord,FRST,ZeroVec)
 	    CASE DEFAULT
          WRITE (*,*) "Step_method=",TRIM(Step_method)," not recognized, opt_Geom.f90, L225. Stop"
          STOP
        END SELECT	

        Fini=.TRUE.
        !   If (IOpt.LT.5) GradTmp=(IOpt*GradTmp+(5-IOpt)*0.01*Grad(IGeom,:))/5.d0
        NormStep=sqrt(DOT_Product(GradTmp,GradTmp))
		WRITE(*,'(A,E10.4,1X,A,E10.4)') 'DBG OptGeom L215, NormStep=', NormStep, 'Step Threshold=', SThresh
        FactStep=min(1.d0,SMax/NormStep)
        Fini=(NormStep.LE.SThresh)
        NormStep=sqrt(DOT_Product(GradOld,GradOld)) ! GradOld is the value of gradients.
		WRITE(*,'(A,E10.4,1X,A,E10.4)') 'DBG OptGeom L219, NormStep (Gradient)=', NormStep, 'Gradient Threshold=', GThresh
        Fini=(Fini.AND.(NormStep.LE.GThresh))
        if (debug) WRITE(*,*) "DBG OptGeom FactStep=",FactStep
        GradTmp=GradTmp*FactStep ! GradTmp is step size here, from Step_RFO_all.

        Call Check_step(IGeom,Coord,Nat,NCoord,GeomOld,GradTmp)
       
        Geom=GeomOld+GradTmp ! GradTmp is step size here, from Step_RFO_all.

        IF (debug) THEN
           WRITE(*,*) "L235, DBG OptGeom, IGeom=",IGeom," IOpt=", IOpt
           SELECT CASE (COORD)
           CASE ('ZMAT','BAKER')
              !WRITE(*,'(12(1X,F8.3))') Geom(1:min(12,NCoord))
           CASE('CART','HYBRID')
              DO Iat=1,Nat
! PFL 30 Apr 2008 ->
! Deleted ref to orderinv that is created only for coord in zmat, baker, hybrid.
!                 WRITE(*,'(1X,A5,3(1X,F11.6))') Nom(Atome(OrderInv(indzmat(iat,1)))), &
                 WRITE(*,'(1X,A5,3(1X,F11.6))') Nom(Atome(iat)), &
                      Geom(3*Iat-2:3*Iat)
! <- PFL 30 Apr 2008
              END DO
           CASE('MIXED')
             DO Iat=1,NCart
                 WRITE(*,'(1X,A5,3(1X,F11.6))') Nom(Atome(OrderInv(indzmat(iat,1)))), &
                      Geom(3*Iat-2:3*Iat)
             END DO
            
             SELECT CASE (NCart)
             CASE(1)
               if (NAt.GE.2) THEN
                WRITE(*,'(1X,A5,3(1X,I5,1X,F11.6))') Nom(Atome(OrderInv(indzmat(2,1)))), &
                      IndZmat(2,2),Geom(4)
                IBEG=5
               END IF
                IF (NAT.GE.3) THEN
                WRITE(*,'(1X,A5,3(1X,I5,1X,F11.6))') Nom(Atome(OrderInv(indzmat(3,1)))), &
                      IndZmat(3,2),Geom(5),IndZmat(3,3),Geom(6)
                IBeg=7
               END IF
              CASE(2)
                WRITE(*,'(1X,A5,3(1X,I5,1X,F11.6))')  Nom(Atome(OrderInv(indzmat(3,1)))), &
                      IndZmat(3,2),Geom(7),IndZmat(3,3),Geom(8)
                IBeg=9
              CASE DEFAULT
                 IBeg=3*NCart+1
              END SELECT

              DO Iat=max(4,NCart),Nat
                WRITE(*,'(1X,A5,3(1X,I5,1X,F11.6))') Nom(Atome(OrderInv(indzmat(iat,1)))), &
                      IndZmat(iat,1),Geom(IBeg),IndZmat(3,2),Geom(IBeg+1), &
                      IndZmat(3,3),Geom(IBeg+2)*180./pi
                IBeg=IBeg+3
              END DO

           CASE DEFAULT
               WRITE(*,*) "Coord=",TRIM(COORD)," not recognized in OptGeom. L294."
               STOP
           END SELECT
        END IF ! matches IF (debug) THEN
		
     END DO ! matches DO WHILE ((IOpt.LT.MaxCyc).AND.(.NOT.Fini))
	 
     DEALLOCATE(GradTmp2,GeomTmp2,GradTmp)
     DEALLOCATE(GradOld,GeomOld)
     DEALLOCATE(Hess_local)
	 DEALLOCATE(GeomCart_old)
	 DEALLOCATE(NewGeom,NewGradTmp)
     DEALLOCATE(Hess_local_inv)

     IF (Fini) THEN
        WRITE(*,'(1X,A,I5,A,I5,A,F15.8)') "Optimization for geom ",IGeom," converged in ",Iopt," cycles, Energy =",E
     ELSE
        WRITE(*,'(1X,A,I5,A,I5,A,F15.8)') "Optimization  for geom ",IGeom,"*NOT* converged in ",Iopt," cycles, Last Energy = ",E
     END IF
	 	 
	 WRITE(*,*) "Initial Geometry:"
	 DO I=1, Nat
	    WRITE(*,'(3(1X,F8.4))')  XyzGeomI(IGeom,:,I)
	 END DO
	  WRITE(*,*) "Final Geometry:"
	 DO I=1, Nat
	    WRITE(*,'(3(1X,F8.4))') GeomCart(I,:)
		!IF (I .GT. 1) Then
        !	WRITE(*,'(F6.4)') Sqrt(((GeomCart(I,1)-GeomCart(I-1,1))*(GeomCart(I,1)-GeomCart(I-1,1)))&
		!	 + ((GeomCart(I,2)-GeomCart(I-1,2))*(GeomCart(I,2)-GeomCart(I-1,2)))&
		!	 + ((GeomCart(I,3)-GeomCart(I-1,3))*(GeomCart(I,3)-GeomCart(I-1,3))))
		!END IF
	 END DO
	 
	 IF (COORD .EQ. "BAKER") Then
    	 I=0 ! index for the NPrim (NPrim is the number of primitive coordinates).
	     ScanCoord=>Coordinate
         DO WHILE (Associated(ScanCoord%next))
		    I=I+1
			SELECT CASE (ScanCoord%Type)
			CASE ('BOND')
				WRITE(*,'(1X,I3,":",I5," - ",I5," = ",F10.4)') I,ScanCoord%At1,ScanCoord%At2,Xprimitive_t(I)
			CASE ('ANGLE')
				WRITE(*,'(1X,I3,":",I5," - ",I5," - ",I5," = ",F10.4)') I,ScanCoord%At1, &
				 ScanCoord%At2, ScanCoord%At3,Xprimitive_t(I)*180./Pi		
			CASE ('DIHEDRAL')
				WRITE(*,'(1X,I3,":",I5," - ",I5," - ",I5," - ",I5," = ",F10.4)') I,ScanCoord%At1,&
				ScanCoord%At2, ScanCoord%At3,ScanCoord%At4,Xprimitive_t(I)*180./Pi
			END SELECT
			ScanCoord => ScanCoord%next
		 END DO ! matches DO WHILE (Associated(ScanCoord%next))
	 END IF
	 
     DEALLOCATE(GeomCart)
	 if (debug) WRITE(*,*) "================== Geom Optimization Over================="
   END SUBROUTINE Opt_geom
