SUBROUTINE Calc_baker_allGeomF()
  !     
  ! This subroutine analyses a geometry to construct the baker
  ! delocalized internal coordinates
  ! v1.0
  ! We use only one geometry
  !     
  Use Path_module, only : Pi,a0,BMat_BakerT,Nat,NCoord,XyzGeomI,NGeomI,UMatF, &
                          NPrim,BTransInvF,IntCoordI,Coordinate,CurrentCoord, &
						  ScanCoord,BprimT,BBT,BBT_inv,XprimitiveF,Symmetry_elimination, &
						  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
  
  real(KREAL) ::  vx,vy,vz,dist, Norm
  real(KREAL) ::  vx1,vy1,vz1,norm1
  real(KREAL) ::  vx2,vy2,vz2,norm2
  real(KREAL) ::  vx3,vy3,vz3,norm3
  real(KREAL) ::  vx4,vy4,vz4,norm4
  real(KREAL) ::  vx5,vy5,vz5,norm5
  real(KREAL) ::  val,val_d, Q, T

  INTEGER(KINT) :: I,J, n1,n2,n3,n4,IAt,IL,JL,IFrag,ITmp, K, KMax
  INTEGER(KINT) :: I0, IOld, IAtTmp, Izm, JAt, Kat, Lat, L, NOUT

  REAL(KREAL) :: sAngleIatIKat, sAngleIIatLat
  REAL(KREAL) :: DiheTmp

  LOGICAL :: debug, bond, AddPrimitiveCoord, FAIL
  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)
!    ScanCoord=>Coordinate
!    I=0 ! index for the NPrim (NPrim is the number of primitive coordinates).
!    DO WHILE (Associated(ScanCoord%next))
!      I=I+1
!      SELECT CASE (ScanCoord%Type)
!       CASE ('BOND')
!	     Call vecteur(ScanCoord%At2,ScanCoord%At1,x,y,z,vx2,vy2,vz2,Norm2)
!	          XprimitiveF(IGeom,I) = Norm2  
!       CASE ('ANGLE')
!		 Call vecteur(ScanCoord%At2,ScanCoord%At3,x,y,z,vx1,vy1,vz1,Norm1)
!		 Call vecteur(ScanCoord%At2,ScanCoord%At1,x,y,z,vx2,vy2,vz2,Norm2)
!		      XprimitiveF(IGeom,I) = angle(vx1,vy1,vz1,Norm1,vx2,vy2,vz2,Norm2)*Pi/180.
!	   CASE ('DIHEDRAL')
!		 Call vecteur(ScanCoord%At3,ScanCoord%At2,x,y,z,vx2,vy2,vz2,Norm2)
!		 Call vecteur(ScanCoord%At2,ScanCoord%At1,x,y,z,vx1,vy1,vz1,Norm1)
!		 Call vecteur(ScanCoord%At3,ScanCoord%At4,x,y,z,vx3,vy3,vz3,Norm3)
!		 Call produit_vect(vx3,vy3,vz3,norm3,vx2,vy2,vz2,norm2, &
!                         vx5,vy5,vz5,norm5)
!		 Call produit_vect(vx1,vy1,vz1,norm1,vx2,vy2,vz2,norm2, &
!                         vx4,vy4,vz4,norm4)
!                 DiheTmp= angle_d(vx4,vy4,vz4,norm4,vx5,vy5,vz5,norm5, &
!                         vx2,vy2,vz2,norm2)
!                 XprimitiveF(IGeom,I) = DiheTmp*Pi/180.
!! We treat large dihedral angles differently as +180=-180 mathematically and physically
!! but this causes lots of troubles when converting baker to cart.
!! So we ensure that large dihedral angles always have the same sign
!                    if (abs(ScanCoord%SignDihedral).NE.1) THEN
!                        ScanCoord%SignDihedral=Int(Sign(1.d0,DiheTmp))
!                    ELSE
!                         If ((abs(DiheTmp).GE.170.D0).AND.(Sign(1.,DiheTmp)*ScanCoord%SignDihedral<0)) THEN
!                          XprimitiveF(IGeom,I) = DiheTmp*Pi/180.+ ScanCoord%SignDihedral*2.*Pi
!                        END IF
!                    END IF
!      END SELECT
!      ScanCoord => ScanCoord%next
!    END DO ! matches DO WHILE (Associated(ScanCoord%next))
  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
