SUBROUTINE Calc_baker_allGeomF()
  !     
  ! This subroutine analyses a geometry to construct the baker
  ! delocalized internal coordinates
  ! v1.0
  ! We use only one geometry
  !     

!----------------------------------------------------------------------
!  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 : BMat_BakerT,Nat,NCoord,UMatF, &
                          NPrim,BTransInvF,Coordinate, &
              ScanCoord,BprimT,BBT,BBT_inv,XprimitiveF, &
              NgeomF,XyzGeomF
  ! BMat_BakerT(3*Nat,NCoord), NCoord=3*Nat or NFree=3*Nat-6-Symmetry_elimination
  ! depending upon the coordinate choice. IntCoordI(NGeomI,NCoord) where 
  ! UMatF(NGeomI,NPrim,NCoord), NCoord number of vectors in UMat matrix, i.e. NCoord
  ! Baker coordinates. NPrim is the number of primitive internal coordinates.
  
  Use Io_module
  IMPLICIT NONE

  REAL(KREAL), ALLOCATABLE :: Geom(:,:) !(3,Nat)
  ! NPrim is the number of primitive coordinates and NCoord is the number
  ! of internal coordinates. BMat is actually (NPrim,3*Nat).
  REAL(KREAL), ALLOCATABLE :: GMat(:,:) !(NPrim,NPrim)
  ! EigVec(..) contains ALL eigevectors of BMat times BprimT, NOT only Baker Coordinate vectors.
  REAL(KREAL), ALLOCATABLE :: EigVec(:,:), EigVal(:) ! EigVec(NPrim,NPrim)
  REAL(KREAL), ALLOCATABLE :: x(:), y(:), z(:)
  REAL(KREAL), ALLOCATABLE :: XPrimRef(:) ! NPrim
  INTEGER(KINT) :: IGeom
  

  INTEGER(KINT) :: I, J, K


  LOGICAL :: debug
  LOGICAL :: DebugPFL

  INTERFACE
     function valid(string) result (isValid)
       CHARACTER(*), intent(in) :: string
       logical                  :: isValid
     END function VALID

     FUNCTION angle(v1x,v1y,v1z,norm1,v2x,v2y,v2z,norm2)
       use Path_module, only :  Pi,KINT, KREAL
       real(KREAL) ::  v1x,v1y,v1z,norm1
       real(KREAL) ::  v2x,v2y,v2z,norm2
       real(KREAL) ::  angle
     END FUNCTION ANGLE

     FUNCTION angle_d(v1x,v1y,v1z,norm1,v2x,v2y,v2z,norm2,v3x,v3y,v3z,norm3)
       use Path_module, only :  Pi,KINT, KREAL
       real(KREAL) ::  v1x,v1y,v1z,norm1
       real(KREAL) ::  v2x,v2y,v2z,norm2
       real(KREAL) ::  v3x,v3y,v3z,norm3
       real(KREAL) ::  angle_d,ca,sa
     END FUNCTION ANGLE_D



     SUBROUTINE Calc_Xprim(nat,x,y,z,Coordinate,NPrim,XPrimitive,XPrimRef)
       !     
       ! This subroutine uses the description of a list of Coordinates
       ! to compute the values of the coordinates for a given geometry.
       !
!!!!!!!!!!
       ! Input:
       !   Na: INTEGER, Number of atoms
       !  x,y,z(Na): REAL, cartesian coordinates of the considered geometry
       ! Coordinate (Pointer(ListCoord)): description of the wanted coordiantes
       ! NPrim, INTEGER: Number of coordinates to compute
       !
       ! Optional: XPrimRef(NPrim) REAL: array that contains coordinates values for
       ! a former geometry. Useful for Dihedral angles evolution...

!!!!!!!!!!!
       ! Output:
       ! XPrimimite(NPrim) REAL: array that will contain the values of the coordinates
       !
!!!!!!!!!

       Use VarTypes
       Use Io_module
       Use Path_module, only : pi

       IMPLICIT NONE

       Type (ListCoord), POINTER :: Coordinate
       INTEGER(KINT), INTENT(IN) :: Nat,NPrim
       REAL(KREAL), INTENT(IN) :: x(Nat), y(Nat), z(Nat)
       REAL(KREAL), INTENT(IN), OPTIONAL :: XPrimRef(NPrim) 
       REAL(KREAL), INTENT(OUT) :: XPrimitive(NPrim)

     END SUBROUTINE CALC_XPRIM
  END INTERFACE




  debug=valid("Calc_baker_allGeomF")
  debugPFL=valid("bakerPFL")
  if (debug) WRITE(*,*) '============ Entering Calc_baker_allGeomF ============='

  ALLOCATE(Geom(3,Nat),x(Nat),y(Nat),z(Nat))
  ALLOCATE(XPrimRef(NPrim))
 
  ! Now calculating values of all primitive bonds for all final geometries:
  DO IGeom=1, NGeomF
     x(1:Nat) = XyzGeomF(IGeom,1,1:Nat)
     y(1:Nat) = XyzGeomF(IGeom,2,1:Nat)
     z(1:Nat) = XyzGeomF(IGeom,3,1:Nat)
     XPrimREf=XPrimitiveF(IGeom,:)
     Call Calc_XPrim(nat,x,y,z,Coordinate,NPrim,XPrimitiveF(IGeom,:),XPrimRef)
  END DO ! matches DO IGeom=1, NGeomF
   
  ALLOCATE(BprimT(3*Nat,NPrim))
  ALLOCATE(Gmat(NPrim,NPrim))
  ALLOCATE(EigVal(NPrim),EigVec(NPrim,NPrim))
  ALLOCATE(BBT(NCoord,NCoord))
  ALLOCATE(BBT_inv(NCoord,NCoord))
  BTransInvF = 0.d0

  DO IGeom=1, NGeomF 
     Geom(1,:)=XyzGeomF(IGeom,1,1:Nat) ! XyzGeomI(NGeomI,3,Nat)
     Geom(2,:)=XyzGeomF(IGeom,2,1:Nat)
     Geom(3,:)=XyzGeomF(IGeom,3,1:Nat)

   BprimT=0.d0
     ScanCoord=>Coordinate
     I=0
     DO WHILE (Associated(ScanCoord%next))
        I=I+1
        SELECT CASE (ScanCoord%Type)
        CASE ('BOND') 
          CALL CONSTRAINTS_BONDLENGTH_DER(Nat,ScanCoord%at1,ScanCoord%AT2, &
               Geom,BprimT(1,I))
        CASE ('ANGLE')
          CALL CONSTRAINTS_BONDANGLE_DER(Nat,ScanCoord%At1,ScanCoord%AT2, &
               ScanCoord%At3,Geom,BprimT(1,I))
        CASE ('DIHEDRAL')
          CALL CONSTRAINTS_TORSION_DER2(Nat,ScanCoord%At1,ScanCoord%AT2, &
               ScanCoord%At3,ScanCoord%At4,Geom,BprimT(1,I))
      END SELECT
        ScanCoord => ScanCoord%next
     END DO

   ! BprimT(3*Nat,NPrim)
   ! We now compute G=B(BT) matrix
   GMat=0.d0
   DO I=1,NPrim
    DO J=1,3*Nat
           GMat(:,I)=Gmat(:,I)+BprimT(J,:)*BprimT(J,I) !*1.d0/mass(atome(int(K/3.d0)))
    END DO
   END DO
  
   ! Diagonalize G
   EigVal=0.d0
   EigVec=0.d0
   Call Jacobi(GMat,NPrim,EigVal,EigVec,NPrim)
   Call Trie(NPrim,EigVal,EigVec,NPrim)
   DO I=1,NPrim
    !WRITE(*,'(1X,"Vector ",I3,": e=",F8.3)') I,EigVal(i)
    !WRITE(*,'(20(1X,F8.4))') EigVec(1:min(20,NPrim),I)
   END DO

   ! UMatF is nonredundant vector set, i.e. set of eigenvectors of BB^T
   !  corresponding to eigenvalues > zero.
   ! BMat_BakerT(3*Nat,NCoord), allocated in Path.f90, 
   ! NCoord=3*Nat-6
   BMat_BakerT = 0.d0
   J=0
   DO I=1,NPrim
    IF (abs(eigval(I))>=1e-6) THEN
       J=J+1
       DO K=1,NPrim
        ! BprimT is transpose of B^prim.
        ! B = UMatF^T B^prim, B^T = (B^prim)^T UMatF
        BMat_BakerT(:,J)=BMat_BakerT(:,J)+BprimT(:,K)*Eigvec(K,I)
       END DO
         IF(J .GT. 3*Nat-6) THEN
           WRITE(*,*) 'Number of vectors in Eigvec with eigval .GT. 1e-6(=UMatF) (=' &
                      ,J,') exceeded 3*Nat-6=',3*Nat-6, &
            'Stopping the calculation.'
           STOP
         END IF
         UMatF(IGeom,:,J) =  Eigvec(:,I)
    END IF
   END DO

!!!!!!!!!!!!!!!!!!!!
!
! Debug purposes
!
         if (debugPFL) THEN
        UMatF(IGeom,:,:)=0.
        DO J=1,3*Nat-6
           UMatF(IGeom,J,J)=1.
        END DO
        END IF

 
   !DO I=1, NPrim ! This loop is not needed because we already have IntCoordF
             ! from interpolation. 
   ! Transpose of UMatF is needed below, that is why UMatF(IGeom,I,:).
   ! IntCoordF(IGeom,:) = IntCoordF(IGeom,:) + UMat(IGeom,I,:)*XprimitiveF(IGeom,I)
   !END DO

   ! Calculation of BTransInvF starts here:
   ! Calculation of BBT(3*Nat-6,3*Nat-6)=BB^T:
   ! BMat_BakerT(3*Nat,NCoord) is Transpose of B = UMatF^TB^prim
    
   BBT = 0.d0
   DO I=1, NCoord
      DO J=1, 3*Nat
       ! BBT(:,I) forms BB^T
       BBT(:,I) = BBT(:,I) + BMat_BakerT(J,:)*BMat_BakerT(J,I)
    END DO
   END DO
    
   Call GenInv(NCoord,BBT,BBT_inv,NCoord) ! GenInv is in Mat_util.f90

   ! Calculation of (B^T)^-1 = (BB^T)^-1B:
   DO I=1, 3*Nat
      DO J=1, NCoord
        BTransInvF(IGeom,:,I) = BTransInvF(IGeom,:,I) + BBT_inv(:,J)*BMat_BakerT(I,J)
      END DO
   END DO
   
  END DO !matches DO IGeom=1, NGeomF
  
  DEALLOCATE(BBT,BBT_inv,BprimT,GMat,EigVal,EigVec)
  DEALLOCATE(Geom,x,y,z,XprimRef)
  
  IF (debug) WRITE(*,*) "DBG Calc_baker_allGeomF over."
END SUBROUTINE Calc_baker_allGeomF
