SUBROUTINE CalcDist_cart(iopt,s,dist,x0,y0,z0,xgeom,Coef)

! This subroutine computes the curvilinear distance of the images on the path
! It is adapted from Extrapol_cart 

!----------------------------------------------------------------------
!  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
  ! X0(Nat),Y0(Nat),Z0(Nat): reference geometry.
  REAL(KREAL), INTENT(IN) :: dist,X0(Nat),Y0(Nat),Z0(Nat)
  REAL(KREAL), INTENT(IN) :: Xgeom(NGeomI),Coef(NGeomI,NCoord)
! Iopt: Number of the cycles for the optimization
  INTEGER(KINT), INTENT(IN) :: Iopt
  INTEGER(KINT) :: IdxGeom, I, J, K, Idx,Iat, IGeom
! NSpline is the number of points along the interpolating path
  INTEGER(KINT) :: NSpline
! FileSpline: Filename to save the interpolating path coordinates
  CHARACTER(LCHARS) :: FileSpline,TmpChar
  REAL(KREAL) :: Rmsd,MRot(3,3), ds, u, v
  REAL(KREAL) :: a_val, d

  REAL(KREAL), ALLOCATABLE :: XyzTmp(:,:), XyzTmp2(:,:) ! (Nat,3)
  REAL(KREAL), ALLOCATABLE :: DerXyz(:,:) ! Nat,3


  LOGICAL ::  debug, print, printspline
  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")

  printspline=(valid("printspline").AND.(dist<=1e30))

  if (debug) Call Header("Entering CalcDist_cart")

! We want 100 points along the interpolating path
  NSpline=int(NMaxPtPath/100)

  if (printspline) THEN
     WRITE(TmpChar,'(I5)') Iopt
     FileSpline=Trim(adjustL(PathName)) // '_spline.' // AdjustL(TRIM(TmpChar))
     OPEN(IOTMP,FILE=FileSpline)

  END IF


  ALLOCATE(XyzTmp(Nat,3),XyzTmp2(Nat,3),DerXyz(Nat,3))

!  XyzTmp2=Reshape(XyzGeomI(1,:,:),(/Nat,3/),ORDER=(/2,1/))

  if (debug) THEN
     WRITE(*,*) "DBG Extrapol_cart Initial geometries"
     DO IGeom=1,NGeomI
        WRITE(*,*) 'XyzGeomI, IGeom=',IGeom
        DO I=1,Nat
           WRITE(IOOUT,'(1X,A2,3(1X,F15.6))') Nom(Atome(I)),      &
                (XyzGeomI(IGeom,J,I),J=1,3)
        END DO
     END DO
  END IF


! In order to mesure only the relevant distance between two points
! we align all geometries on the original one

   DO IGeom=1,NGeomI

      XyzTmp2=Reshape(XyzGeomI(IGeom,:,:),(/Nat,3/),ORDER=(/2,1/))
  ! We align this geometry with the original one
  ! PFL 17/July/2006: only if we have more than 4 atoms.
!  IF (Nat.GT.4) THEN
! PFL 24 Nov 2008 ->
! If we have frozen atoms we align only those ones.
! PFL 8 Feb 2011 ->
! I add a flag to see if we should align or not.
! For small systems, it might be better to let the user align himself
      IF (Align) THEN
         if (NFroz.GT.0) THEN
            Call AlignPartial(Nat,x0,y0,z0,                     &
                 xyzTmp2(1,1),xyzTmp2(1,2),xyzTMP2(1,3),       &
                 FrozAtoms,MRot)
         ELSE
            Call  CalcRmsd(Nat, x0,y0,z0,                              &
                 xyzTmp2(1,1),xyzTmp2(1,2),xyzTMP2(1,3),       &
                 MRot,rmsd,.TRUE.,.TRUE.)
         END IF
! <- PFL 24 Nov 2008
      END IF
! -> PFL 8 Feb 2011
!  END IF

  XyzGeomI(IGeom,:,:)=Reshape(XyzTmp2(:,:),(/3,Nat/),ORDER=(/2,1/))
 END DO

   if (print) THEN
      WRITE(*,*) "Aligned geometries"
      DO J=1, NGeomI
         WRITE(IOOUT,*) Nat
         WRITE(IOOUT,*) " Aligned geometry ",J,"/",NGeomI
           DO i=1,Nat
              WRITE(IOOUT,'(1X,A2,3(1X,F15.6))') Nom(Atome(I)),    &
                   XyzGeomI(J,1:3,I)
           END DO
        END DO
     END IF

  XyzGeomF(1,:,:)=XyzGeomI(1,:,:)


  ! We initialize the first geometry

  XyzTmp=Reshape(XyzGeomI(1,:,:),(/Nat,3/),ORDER=(/2,1/))

  ! We align this geometry with the original one
  ! PFL 17/July/2006: only if we have more than 4 atoms.
!  IF (Nat.GT.4) THEN
! PFL 24 Nov 2008 ->
! If we have frozen atoms we align only those ones.
! PFL 8 Feb 2011 ->
! I add a flag to see if we should align or not.
! For small systems, it might be better to let the user align himself
      IF (Align) THEN
         if (NFroz.GT.0) THEN
            Call AlignPartial(Nat,x0,y0,z0,                     &
                 xyzTmp(1,1),xyzTmp(1,2),xyzTMP(1,3),       &
                 FrozAtoms,MRot)
         ELSE
            Call  CalcRmsd(Nat, x0,y0,z0,                              &
                 xyzTmp(1,1),xyzTmp(1,2),xyzTMP(1,3),       &
                 MRot,rmsd,.TRUE.,.TRUE.)
         END IF
! <- PFL 24 Nov 2008
      END IF
! -> PFL 8 Feb 2011
!  END IF



  s=0.d0
  SGeom(1)=0.d0

  if (printspline) THEN
     u=0.d0
     DO Iat=1,Nat
        ! We generate the interpolated coordinates
        if (Linear) THEN
           call LinearInt(u,XyzTmp2(Iat,1),DerXyz(Iat,1),NGeomI,xgeom(1),XyzGeomI(1,1,Iat))
           call LinearInt(u,XyzTmp2(Iat,2),DerXyz(Iat,2),NGeomI,xgeom(1),XyzGeomI(1,2,Iat))
           call LinearInt(u,XyzTmp2(Iat,3),DerXyz(Iat,3),NGeomI,xgeom(1),XyzGeomI(1,3,Iat))

        ELSE
           call splintder(u,XyzTmp2(Iat,1),DerXyz(Iat,1),NGeomI,xgeom(1),XyzGeomI(1,1,Iat),Coef(1,3*Iat-2))
           call splintder(u,XyzTmp2(Iat,2),DerXyz(Iat,2),NGeomI,xgeom(1),XyzGeomI(1,2,Iat),Coef(1,3*Iat-1))
           call splintder(u,XyzTmp2(Iat,3),DerXyz(Iat,3),NGeomI,xgeom(1),XyzGeomI(1,3,Iat),Coef(1,3*Iat))
        END IF
     END DO

     WRITE(IOTMP,*) Nat
     WRITE(IOTMP,'(1X,A,1X,F15.6)') "Debug Spline - Coord=Cart,s=",s
     DO Iat=1,Nat
        WRITE(IOTMP,'(1X,A2,3(1X,F15.6))') Nom(Atome(Iat)),    &
                   (XyzTmp2(Iat,1:3))
     END DO
  END IF

 IdxGeom=1

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

     DO Iat=1,Nat
        ! We generate the interpolated coordinates
        if (Linear) THEN
           call LinearInt(u,XyzTmp2(Iat,1),DerXyz(Iat,1),NGeomI,xgeom(1),XyzGeomI(1,1,Iat))
           call LinearInt(u,XyzTmp2(Iat,2),DerXyz(Iat,2),NGeomI,xgeom(1),XyzGeomI(1,2,Iat))
           call LinearInt(u,XyzTmp2(Iat,3),DerXyz(Iat,3),NGeomI,xgeom(1),XyzGeomI(1,3,Iat))

        ELSE
           call splintder(u,XyzTmp2(Iat,1),DerXyz(Iat,1),NGeomI,xgeom(1),XyzGeomI(1,1,Iat),Coef(1,3*Iat-2))
           call splintder(u,XyzTmp2(Iat,2),DerXyz(Iat,2),NGeomI,xgeom(1),XyzGeomI(1,2,Iat),Coef(1,3*Iat-1))
           call splintder(u,XyzTmp2(Iat,3),DerXyz(Iat,3),NGeomI,xgeom(1),XyzGeomI(1,3,Iat),Coef(1,3*Iat))
        END IF
     END DO


  ! We align this geometry with the original one
  ! PFL 17/July/2006: only if we have more than 4 atoms.
!  IF (Nat.GT.4) THEN
! PFL 24 Nov 2008 ->
! If we have frozen atoms we align only those ones.
! PFL 8 Feb 2011 ->
! I add a flag to see if we should align or not.
! For small systems, it might be better to let the user align himself
      IF (Align) THEN
         if (NFroz.GT.0) THEN
            Call AlignPartial(Nat,x0,y0,z0,                     &
                 xyzTmp2(1,1),xyzTmp2(1,2),xyzTMP2(1,3),       &
                 FrozAtoms,MRot)
         ELSE
            Call  CalcRmsd(Nat, x0,y0,z0,                              &
                 xyzTmp2(1,1),xyzTmp2(1,2),xyzTMP2(1,3),       &
                 MRot,rmsd,.TRUE.,.TRUE.)
         END IF
! <- PFL 24 Nov 2008
      END IF
! -> PFL 8 Feb 2011
!  END IF



     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 ((printspline).AND.(MOD(K,NSpline).EQ.0)) THEN
     WRITE(IOTMP,*) Nat
     WRITE(IOTMP,'(1X,A,1X,F15.6)') "Debug Spline - Coord=Cart,s=",s
     DO Iat=1,Nat
        WRITE(IOTMP,'(1X,A2,3(1X,F15.6))') Nom(Atome(Iat)),    &
                   (XyzTmp2(Iat,1:3))
     END DO
  END IF


     if (s>=dist) THEN

        if (debug) THEN
           WRITE(*,*) "DBG Interpol_cart",s
           DO i=1,Nat
              WRITE(IOOUT,'(1X,A2,3(1X,F15.6))') Nom(Atome(I)),    &
                   (XyzTmp2(I,J),J=1,3)
           END DO
        END IF

        s=s-dist
        IdxGeom=IdxGeom+1
        SGeom(IdxGeom)=s+dist*(IdxGeom-1)

  ! We align this geometry with the original one
  ! PFL 17/July/2006: only if we have more than 4 atoms.
!  IF (Nat.GT.4) THEN
! PFL 24 Nov 2008 ->
! If we have frozen atoms we align only those ones.
! PFL 8 Feb 2011 ->
! I add a flag to see if we should align or not.
! For small systems, it might be better to let the user align himself
      IF (Align) THEN
         if (NFroz.GT.0) THEN
            Call AlignPartial(Nat,x0,y0,z0,                     &
                 xyzTmp2(1,1),xyzTmp2(1,2),xyzTMP2(1,3),       &
                 FrozAtoms,MRot)
         ELSE
            Call  CalcRmsd(Nat, x0,y0,z0,                              &
                 xyzTmp2(1,1),xyzTmp2(1,2),xyzTMP2(1,3),       &
                 MRot,rmsd,.TRUE.,.TRUE.)
         END IF
! <- PFL 24 Nov 2008
      END IF
! -> PFL 8 Feb 2011
!  END IF

        XyzGeomF(IdxGeom,:,:)=Reshape(XyzTmp2(:,:),(/3,Nat/),ORDER=(/2,1/))
        XyzTangent(IdxGeom,:)=Reshape(DerXyz(:,:),(/3*Nat/))

        if (print) THEN
           WRITE(IOOUT,'(1X,I5)') Nat
           WRITE(IOOUT,*) "# Cartesian coord for Geometry ",IdxGeom,K

           DO i=1,Nat
              WRITE(IOOUT,'(1X,A2,3(1X,F15.6))') Nom(Atome(I)),    &
                   (XyzTmp2(I,J),J=1,3)
           END DO

        END IF
     END IF  ! s>= dist
  ENDDO  ! K


  if (s>=0.9*dist) THEN
     s=s-dist
     XyzTmp2=Reshape(XyzGeomI(NGeomI,:,:),(/Nat,3/),ORDER=(/2,1/))

  if (printspline) THEN
     WRITE(IOTMP,*) Nat
     WRITE(IOTMP,*) "Debug Spline - Coord=Cart,s=",s
     DO Iat=1,Nat
        WRITE(IOTMP,'(1X,A2,3(1X,F15.6))') Nom(Atome(Iat)),    &
                   (XyzTmp2(Iat,1:3))
     END DO
  END IF


  ! We align this geometry with the original one
  ! PFL 17/July/2006: only if we have more than 4 atoms.
!  IF (Nat.GT.4) THEN
! PFL 24 Nov 2008 ->
! If we have frozen atoms we align only those ones.
! PFL 8 Feb 2011 ->
! I add a flag to see if we should align or not.
! For small systems, it might be better to let the user align himself
      IF (Align) THEN
         if (NFroz.GT.0) THEN
            Call AlignPartial(Nat,x0,y0,z0,                     &
                 xyzTmp2(1,1),xyzTmp2(1,2),xyzTMP2(1,3),       &
                 FrozAtoms,MRot)
         ELSE
            Call  CalcRmsd(Nat, x0,y0,z0,                              &
                 xyzTmp2(1,1),xyzTmp2(1,2),xyzTMP2(1,3),       &
                 MRot,rmsd,.TRUE.,.TRUE.)
         END IF
! <- PFL 24 Nov 2008
      END IF
! -> PFL 8 Feb 2011
!  END IF

     IdxGeom=IdxGeom+1
     SGeom(IdxGeom)=s+dist*(IdxGeom-1)

    IF (IdxGeom.GT.NGeomF) THEN
        WRITE(IOOUT,*) "!!! ERROR in Extrapol_cart !!!!"
        WRITE(IOOUT,*) "Too many structures. Increase NMaxPath"
        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


  ! We align this geometry with the original one
  ! PFL 17/July/2006: only if we have more than 4 atoms.
!  IF (Nat.GT.4) THEN
! PFL 24 Nov 2008 ->
! If we have frozen atoms we align only those ones.
! PFL 8 Feb 2011 ->
! I add a flag to see if we should align or not.
! For small systems, it might be better to let the user align himself
      IF (Align) THEN
         if (NFroz.GT.0) THEN
            Call AlignPartial(Nat,x0,y0,z0,                     &
                 xyzTmp2(1,1),xyzTmp2(1,2),xyzTMP2(1,3),       &
                 FrozAtoms,MRot)
         ELSE
            Call  CalcRmsd(Nat, x0,y0,z0,                              &
                 xyzTmp2(1,1),xyzTmp2(1,2),xyzTMP2(1,3),       &
                 MRot,rmsd,.TRUE.,.TRUE.)
         END IF
! <- PFL 24 Nov 2008
      END IF
! -> PFL 8 Feb 2011
!  END IF

     XyzGeomF(IdxGeom,:,:)=Reshape(XyzTmp2(:,:),(/3,Nat/),ORDER=(/2,1/))
     XyzTangent(IdxGeom,:)=Reshape(DerXyz(:,:),(/3*Nat/))

     if (print) THEN
        WRITE(IOOUT,'(1X,I5)') Nat
        WRITE(IOOUT,*) "# Cartesian coord for Geometry ",IdxGeom,K

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

  DEALLOCATE(XyzTmp,XyzTmp2)

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

  if (printspline) CLOSE(IOTMP)

  if (debug) Call Header("Extrapol_cart over")

END SUBROUTINE EXTRAPOL_CART
