SUBROUTINE Extrapol_mixed(s,dist,x0,y0,z0,xgeom,Coef)

! This subroutine constructs the path, and if dist<>Infinity, it samples
! the path to obtain geometries.
! Basically, you call it twice: i) dist=infinity, it will calculate the 
! length of the path
! ii) dist finite, it will give you the images you want along the path.
!
! For now, it gives equidistant geometries
!!!!!!!!!!!!!!!!
!
! v2.0
! Uses Mix2cart for conversions.

!----------------------------------------------------------------------
!  Copyright 2003-2014 Ecole Normale Supérieure de Lyon, 
!  Centre National de la Recherche Scientifique,
!  Université Claude Bernard Lyon 1. All rights reserved.
!
!  This work is registered with the Agency for the Protection of Programs 
!  as IDDN.FR.001.100009.000.S.P.2014.000.30625
!
!  Authors: P. Fleurat-Lessard, P. Dayal
!  Contact: optnpath@gmail.com
!
! This file is part of "Opt'n Path".
!
!  "Opt'n Path" is free software: you can redistribute it and/or modify
!  it under the terms of the GNU Affero General Public License as
!  published by the Free Software Foundation, either version 3 of the License,
!  or (at your option) any later version.
!
!  "Opt'n Path" is distributed in the hope that it will be useful,
!  but WITHOUT ANY WARRANTY; without even the implied warranty of
!
!  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
!  GNU Affero General Public License for more details.
!
!  You should have received a copy of the GNU Affero General Public License
!  along with "Opt'n Path". If not, see <http://www.gnu.org/licenses/>.
!
! Contact The Office of Technology Licensing, valorisation@ens-lyon.fr,
! for commercial licensing opportunities.
!----------------------------------------------------------------------


  use Path_module
  use Io_module


  IMPLICIT NONE


  REAL(KREAL), INTENT(OUT) :: s
  REAL(KREAL), INTENT(IN) :: dist,X0(Nat),Y0(Nat),Z0(Nat)
  REAL(KREAL), INTENT(IN) :: Xgeom(NGeomI),Coef(NGeomI,NCoord)

  INTEGER(KINT) :: IdxGeom, I, J, K, Idx
  REAL(KREAL) :: Rmsd, MRot(3, 3), ds, u

  REAL(KREAL), ALLOCATABLE :: XyzTmp(:,:), XyzTmp2(:,:) ! (Nat,3)
  REAL(KREAL), ALLOCATABLE :: IntCoordTmp(:),DerInt(:) ! (NCoord)


  LOGICAL ::  debug, print
  LOGICAL, EXTERNAL :: valid

  !We will calculate the length of the path, in MW coordinates...
  ! this is done is a stupid way: we interpolate the zmatrix values,
  ! convert them into cartesian, weight the cartesian
  ! and calculate the evolution of the distance ! 
  ! We have to follow the same procedure for every geometry
  ! so even for the first one, we have to convert it from zmat
  ! to cartesian !

  debug=valid("pathcreate")
  print=valid("printgeom")

  if (debug) Call Header("Entering Extrapol_mixed")
  if (debug) WRITE(*,*) "NFroz,NCart",NFroz,NCart
  ALLOCATE(XyzTmp(Nat,3),XyzTmp2(Nat,3),IntCoordTmp(NCoord),DerInt(NCoord))
  IdxGeom=1
  
  IntCoordTmp=IntCoordI(1,1:NCoord)
  call Mixed2Cart(Nat,IndZmat,IntCoordTmp,XyzTmp2(1,1))

  XyzTmp=XyzTmp2

! We align this geometry with the original one
! PFL 01/feb/2007: useless when dealing with more than three cartesian atoms 
! PFL 17/July/2006: only if we have more than 4 atoms.
  IF ((NCart.LT.3).AND.(Nat.GT.4)) THEN
!  IF (Nat.GT.4) THEN
     Call  CalcRmsd(Nat, x0,y0,z0,                              &
          xyzTmp2(1,1),xyzTmp2(1,2),xyzTMP2(1,3),       &
          MRot,rmsd,FRot,FAlign)
  END IF

  XyzGeomF(IdxGeom,:,:)=Reshape(XyzTmp2(:,:),(/3,Nat/),ORDER=(/2,1/))
  IntCoordF(IdxGeom,:)=IntCoordI(1,:)

  if (print.AND.(Dist.LE.1e20)) THEN
     WRITE(IOOUT,'(1X,I5)') Nat
     WRITE(IOOUT,*) "# Cartesian Coordinates for geom",IdxGeom
     DO i=1,Nat
        If (Renum) THEN
           WRITE(IOOUT,'(1X,A2,3(1X,F15.6))') Nom(Atome(I)),      &
                (XyzTmp2(Order(I),J),J=1,3)
        ELSE
           WRITE(IOOUT,'(1X,A2,3(1X,F15.6))') Nom(Atome(OrderInv(I))),      &
                (XyzTmp2(I,J),J=1,3)
        END IF
     END DO
  END IF

! Calculate tangents for first geometry
  u=0.d0
     DO Idx=1,NCoord
           call splintder(u,IntCoordTmp(Idx),DerInt(Idx),NGeomI,xgeom(1),IntCoordI(1,Idx),Coef(1,Idx))
     END DO
     IntTangent(IdxGeom,:)=DerInt


! First geometry already initialized

  s=0.
!  valzmat=0.d0

  DO K=1,NMaxPtPath
     u=real(K)/NMaxPtPath*(NGeomI-1.)

     XYZTmp2=0.

! We generate the interpolated  coordinates
     DO Idx=1,NCoord
           call splintder(u,IntCoordTmp(Idx),DerInt(Idx),NGeomI,xgeom(1),IntCoordI(1,Idx),Coef(1,Idx))
     END DO

! We convert it into Cartesian coordinates
  call Mixed2Cart(Nat,IndZmat,IntCoordTmp,XyzTmp2(1,1))

! We calculate ds
     ds=0.
     DO I=1,Nat
        DO J=1,3
           ds=ds+MassAt(I)*(XYZTMp2(I,J)-XYZTmp(I,J))**2
           XYZTmp(I,J)=XyzTMP2(I,J)
        ENDDO
     ENDDO
     s=s+sqrt(ds)

!         if (debug) WRITE(*,*) "Debug u,s,dist",u,s,dist
     if (s>=dist) THEN
        s=s-dist
        IdxGeom=IdxGeom+1
        SGeom(IdxGeom)=s+IdxGeom*dist
        XyzGeomF(IdxGeom,:,:)=Reshape(XyzTmp2(:,:),(/3,Nat/),ORDER=(/2,1/))

        IntCoordF(IdxGeom,:)=IntCoordTmp
        IntTangent(IdxGeom,:)=DerInt

        if (print) THEN
           WRITE(IOOUT,'(1X,A,12(1X,F10.5))') "#Internal Coord",IntCoordTmp(1:NCoord)
           WRITE(IOOUT,'(1X,A,12(1X,F10.5))') "#Internal tan",IntTangent(IdxGeom,1:NCoord)
           WRITE(IOOUT,'(1X,I5)') Nat
           WRITE(IOOUT,*) "# Cartesian coord for Geometry ",IdxGeom,K,u,Xgeom(NGeomI)

! PFL 01/feb/2007: useless when dealing with more than three cartesian atoms 
! PFL 17/July/2006: only if we have more than 4 atoms.
           IF ((NCart.LT.3).AND.(Nat.GT.4)) THEN
!           IF (Nat.GT.4) THEN
              Call  CalcRmsd(Nat, x0,y0,z0,                               &
                   xyzTmp2(1,1),xyzTmp2(1,2),xyzTMP2(1,3),   &
                   MRot,rmsd,FRot,FAlign)
           END IF

           DO i=1,Nat
              If (Renum) THEN
                 WRITE(IOOUT,'(1X,A2,3(1X,F15.6))') Nom(Atome(I)),   &
                      (XyzTmp2(Order(I),J),J=1,3)
              ELSE
                 WRITE(IOOUT,'(1X,A2,3(1X,F15.6))') Nom(Atome(OrderInv(I))),    &
                      (XyzTmp2(I,J),J=1,3)
              END IF
           END DO

        END IF
     END IF
  END DO


  if ((s>=0.1*dist).AND.(IdxGeom.EQ.NGeomF)) THEN
     WRITE(*,*) "** PathCreate ***"
     WRITE(*,*) "Distribution of points along the path is wrong."
     WRITE(*,*) "Increase value of NMaxPtPath in the input file"
     WRITE(*,*) "Present value is:", NMaxPtPath
     STOP
  END IF

  IdxGeom=NGeomF

! We have to add the last geometry. We copy the last geom of Initial geometries.
  IntCoordF(IdxGeom,:)=IntCoordI(NGeomI,:)
  IntTangent(IdxGeom,:)=DerInt

! we convert it to cartesian geom
  call Mixed2Cart(Nat,IndZmat,IntCoordF(IdxGeom,1),XyzTmp2(1,1))
  XyzGeomF(IdxGeom,:,:)=Reshape(XyzTmp2(:,:),(/3,Nat/),ORDER=(/2,1/))

     if (print) THEN
        WRITE(IOOUT,'(1X,I5)') Nat
        WRITE(IOOUT,*) "# Cartesian coord for last Geometry ",IdxGeom,K,u,Xgeom(NGeomI)
! PFL 01/feb/2007: useless when dealing with more than three cartesian atoms 
! PFL 17/July/2006: only if we have more than 4 atoms.
        IF ((NCART.LT.3).AND.(Nat.GT.4)) THEN
!       IF (Nat.GT.4) THEN
           Call  CalcRmsd(Nat, x0,y0,z0,                          &
                xyzTmp2(1,1),xyzTmp2(1,2),xyzTMP2(1,3),    &
                MRot,rmsd,FRot,FAlign)
        END IF

        DO i=1,Nat
           If (Renum) THEN
              WRITE(IOOUT,'(1X,A2,3(1X,F15.6))') Nom(Atome(I)),    &
                   (XyzTmp2(Order(I),J),J=1,3)
           ELSE
              WRITE(IOOUT,'(1X,A2,3(1X,F15.6))') Nom(Atome(OrderInv(I))),     &
                   (XyzTmp2(I,J),J=1,3)
           END IF
        END DO
     END IF


  if (debug) WRITE(*,*) 's final =',s

  DEALLOCATE(XyzTmp,XyzTmp2)

  if (debug) Call Header("Extrapol_Mixed Over")

 
END SUBROUTINE EXTRAPOL_MIXED
