SUBROUTINE Extrapol_baker(s,dist,x0,y0,z0,xgeom,Coef,XgeomF)

  ! This subroutine constructs the path, andabscissa 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.
  !
  ! A reference geometry for the alignment: X0(Nat),Y0(Nat),Z0(Nat)

!----------------------------------------------------------------------
!  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, only : IntCoordI, NMaxPtPath, XyzGeomF, IntCoordF, &
       IntTangent, Renum, Nom, Order, MassAt, SGeom, Nat, NGeomI, &
       NGeomF, Atome, NCoord, OrderInv, XyzGeomI,BTransInvF, &
       XPrimitive,XPrimitiveF, NPrim,                                      &
       BTransInv_local,UMatF,UMat_local,FirstTimePathCreate,Pi
  ! IntCoordI(NGeomI,3*Nat-6), Coef(NGeomI,NCoord), NMaxPtPath=1000 (default value)
  ! More appropriate: IntCoordI(NGeomI,NCoord)
  use Io_module
  IMPLICIT NONE

  REAL(KREAL), INTENT(OUT) :: s
  ! A reference geometry for the alignment:
  REAL(KREAL), INTENT(IN) :: dist,X0(Nat),Y0(Nat),Z0(Nat)
  ! Xgeom(NGeomI): abscissa of all initial geometries.
  ! Coef(NGeomI,NCoord): spline coefficients.
  REAL(KREAL), INTENT(IN) :: Xgeom(NGeomI),Coef(NGeomI,NCoord)
  ! Number of the cycles for the optimization:
  ! XGeomF(NGeomF): Final geometries.
  REAL(KREAL), INTENT(OUT) :: XGeomF(NGeomF)

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

  REAL(KREAL), ALLOCATABLE :: XyzTmp(:,:), XyzTmp2(:,:), DerInt(:) ! (Nat,3)
  REAL(KREAL), ALLOCATABLE :: Xyz_k(:,:) ! (Nat,3)
  REAL(KREAL), ALLOCATABLE :: IntCoord_interpol(:) ! (3*Nat-6)
  REAL(KREAL), ALLOCATABLE :: IntCoord_k(:) ! (3*Nat-6)
  REAL(KREAL), ALLOCATABLE :: XPrimRef(:),XPrim(:) ! NPrim

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

  INTEGER(KINT) :: NSpline


  ! We will calculate the length of the path, in MW coordinates...
  ! this is done in a stupid way: we interpolate the Baker coordinates 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 internal Baker 
  ! coordinates to cartesian !

  debug=valid("Extrapol_baker")
  print=valid("printgeom")
  printspline=(valid("printspline").AND.(dist<=1e30))

  if (debug) WRITE(*,*) "================= Entering Extrapol_baker ===================="
  if (debug) WRITE(*,*) "DBG Extrapol_baker dist=",Dist
  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),IntCoord_interpol(NCoord),DerInt(NCoord))
  ALLOCATE(IntCoord_k(NCoord),Xyz_k(Nat,3))
  ALLOCATE(XPrimRef(NPrim),XPrim(NPrim))

  ! XyzGeomI(:,:,:) ! (NGeomI,3,Nat)
  ! IntCoordI(:,:) ! (NGeomI,3*Nat-6)

  !XyzGeomF(1,:,:)=Reshape(XyzTmp2(:,:),(/3,Nat/),ORDER=(/2,1/))
  XyzGeomF(1,:,:)=XyzGeomI(1,:,:) ! 1st index is geometry-index.
  IntCoordF(1,:)=IntCoordI(1,:)

  ! We calculate the first derivatives
  u=0.d0
  DO I=1,NCoord
     ! Given the arrays xgeom(1:NGeomI) and IntCoordI(1:NGeomI,Idx) of length 
     ! NGeomI, which tabulate a function
     ! (with the xgeom's in order), and given the array Coef(1:NGeomI,Idx), 
     ! which is the output from spline, and given a value of u,
     ! this routine returns a cubic-spline interpolated value v.
     ! and the derivative DerInt(Idx).
     call splintder(u,v,DerInt(I),NGeomI,xgeom(1),IntCoordI(1,I),Coef(1,I))
  END DO
  IntTangent(1,:)=DerInt

  IF (print.AND.(Dist.LE.1e20)) THEN
     WRITE(IOOUT,'(1X,I5)') Nat
     WRITE(IOOUT,*) "# Cartesian Coordinates for geom",1
     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 ! matches IF (print.AND.(Dist.LE.1e20)) THEN

  XyzTmp(:,1) = XyzGeomI(1,1,:) ! 1st index is geometry-index.
  XyzTmp(:,2) = XyzGeomI(1,2,:)
  XyzTmp(:,3) = XyzGeomI(1,3,:)

  s=0.d0
  IntCoord_k=IntCoordF(1,:)
  Xyz_k(:,1) = XyzGeomI(1,1,:) ! 1st index is geometry-index.
  Xyz_k(:,2) = XyzGeomI(1,2,:)
  Xyz_k(:,3) = XyzGeomI(1,3,:)
  IdxGeom=1
  XPrimRef=XPrimitive(1,:)
  XPrimitiveF(1,:)=XPrimitive(1,:)
  DO K=1,NMaxPtPath
     u=real(K)/NMaxPtPath*(NGeomI-1.)

     ! We generate the interpolated internal coordinates in v.
     ! Given the arrays Xgeom(1:NGeomI) (Xgeom(NGeomI): abscissa of all initial geometries)
     ! and IntCoordI(1:NGeomI,I) of length NGeomI, which tabulate a function (with the 
     ! Xgeom's in order), and given the array Coef(1:NGeomI,Idx), which is the output from 
     ! spline, and given a value of u, this routine returns a cubic-spline interpolated 
     ! value v and the derivative DerInt(I).

     ! this loop is to be confirmed:
     ! IntCoordI(NGeomI,3*Nat-6)
     DO I=1,NCoord
        call splintder(u,v,DerInt(I),NGeomI,Xgeom(1),IntCoordI(1,I),Coef(1,I))
        IntCoord_interpol(I)=v
     END DO
     IF(.NOT.FirstTimePathCreate) Then
       WRITE(*,*) "DBG Extrapol_baker Umat_local=UMatF"
        DO I=1,NCoord ! these variables are used in ConvertBakerInternal_cart()
           BTransInv_local(I,:) = BTransInvF(IdxGeom,I,:)
           UMat_local(:,I) = UMatF(IdxGeom,:,I)
        END DO
     END IF
     ! We convert it into Cartesian coordinates:
     if (debug) WRITE(*,*) "DBG Extrapol_baker, call ConvertBakerInt_car for k=",k
     Call ConvertBakerInternal_cart(IntCoord_k,IntCoord_interpol,Xyz_k(1,1), &
          Xyz_k(1,2),Xyz_k(1,3),XyzTMP2(1,1),XyzTMP2(1,2),XyzTMP2(1,3),XPrim,XPrimRef)
     XPrimRef=Xprim
     IF(.NOT.FirstTimePathCreate) Then
        DO I=1,NCoord ! these variables are used in ConvertBakerInternal_cart()
           BTransInvF(IdxGeom,I,:) = BTransInv_local(I,:)
        END DO
     END IF

     if (debug) THEN
        WRITE(*,*) "DBG Extrapol_baker, XyzTmp2 before RMSD"
           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
     

     call CalcRmsd(Nat,XyzTmp(1:Nat,1),XyzTmp(1:Nat,2),XyzTmp(1:Nat,3), &
    XyzTmp2(1:Nat,1),XyzTmp2(1:Nat,2),XyzTmp2(1:Nat,3),           &
          MRot,rmsd,.TRUE.,.TRUE.)


     if (debug) THEN
        WRITE(*,*) "DBG Extrapol_baker, XyzTmp2 after RMSD"
           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


     IntCoord_k=IntCoord_interpol
     Xyz_k(:,1)=XyzTMP2(:,1)
     Xyz_k(:,2)=XyzTMP2(:,2)
     Xyz_k(:,3)=XyzTMP2(:,3)

     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)
        END DO
     END DO

     s=s+sqrt(ds)

     IF (s>=dist) THEN
        if (debug) THEN
           WRITE(*,*) "DBG Extrapol_baker s,IdxGeom,dist",s,IdxGeom,dist
           WRITE(*,'(50(1X,F12.8))') IntCoord_interpol
           WRITE(*,*) "DBG Extrapol_baker Angles in deg ?"
           WRITE(*,'(50(1X,F12.8))') IntCoord_interpol*180./pi
        END IF
        s=s-dist
        IdxGeom=IdxGeom+1
        XprimitiveF(IdxGeom,:)=Xprim(:)
        UMatF(IdxGeom,:,:)=UMat_local(:,:)
        SGeom(IdxGeom)=s+IdxGeom*dist !SGeom(NGeomF)
        XgeomF(IdxGeom)=u
        XyzGeomF(IdxGeom,:,:)=Reshape(XyzTmp2(:,:),(/3,Nat/),ORDER=(/2,1/))

        ! IntCoordF(NGeomF,NCoord): Final Internal coordinates for number of final 
        ! geometries. NCoord is the number of coordinates (NCoord) of each geometry.
        IntCoordF(IdxGeom,:)=IntCoord_interpol(:)
        IntTangent(IdxGeom,:)=DerInt

        IF (print) THEN
           WRITE(IOOUT,'(1X,I5)') Nat
           WRITE(IOOUT,*) "# Cartesian coord for Geometry ",IdxGeom,K
           ! PFL 17/July/2006: only if we have more than 4 atoms.
           IF (Nat.GE.4) THEN
              Call  CalcRmsd(Nat,x0,y0,z0, &
                   xyzTmp2(1,1),xyzTmp2(1,2),xyzTMP2(1,3), &
                   MRot,rmsd,.TRUE.,.TRUE.)
           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 !matches IF (print) THEN
     END IF ! matches IF (s>=dist) THEN
  END DO ! matches DO K=1,NMaxPtPath


  if (s>=0.9*dist) THEN
     s=s-dist
     IdxGeom=IdxGeom+1
     SGeom(IdxGeom)=s+IdxGeom*dist
     XgeomF(IdxGeom)=min(u,NGeomI-1.d0)
     XyzGeomF(IdxGeom,:,:)=Reshape(XyzTmp2(:,:),(/3,Nat/),ORDER=(/2,1/))
     !  XyzGeomF(IdxGeom,:,:)=XyzTmp2(:,:)

     IntCoordF(IdxGeom,:)=IntCoord_interpol(:)
     XprimitiveF(IdxGeom,:)=Xprim(:)
     UMatF(IdxGeom,:,:)=UMat_local(:,:)
     IntTangent(IdxGeom,:)=DerInt     

     if (print) THEN
        WRITE(IOOUT,'(1X,I5)') Nat
        WRITE(IOOUT,*) "# Cartesian coord for Geometry ",IdxGeom,K
        ! PFL 17/July/2006: only if we have more than 4 atoms.
        IF (Nat.GE.4) THEN
           Call  CalcRmsd(Nat,x0,y0,z0, &
                xyzTmp2(1,1),xyzTmp2(1,2),xyzTMP2(1,3), &
                MRot,rmsd,.TRUE.,.TRUE.)
        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 ! matches if (print) THEN
  END IF ! matches if (s>=0.9*dist) THEN

  if (debug) WRITE(*,*) 's final =',s
   if (debug) THEN
      WRITE(*,*) "XPrimitiveF"
      DO I=1,NGeomF
        WRITE(*,'(1X,I5," : ",50(1X,F10.6))') I,XPrimitiveF(I,:)
     END DO
     END IF

  DEALLOCATE(XyzTmp,XyzTmp2,IntCoord_interpol,IntCoord_k,Xyz_k)

  if (printspline) CLOSE(IOTMP)
  if (debug) WRITE(*,*) "================= Extrapol_baker Over ====================="

END SUBROUTINE EXTRAPOL_BAKER
