SUBROUTINE ConvertBakerInternal_cart(IntCoord_k,IntCoord,x_k,y_k,z_k,x,y,z,XPrim,XPrimRef)

!================================================================
! Converts the positions in Baker Coordinates to cartesian coordinates
!================================================================
!
! Input:
!    Incoord_k(NCoord) REAL: Starting internal coordinates
!    IntCoord(NCoord) REAL: Coordinate to convert to cartesian coordinates
!    x_k,y_k,z_k(Nat) REAL: Starting cartesian geometry
!    XPrimRef(NPrim) REAL: Reference values for Primitives coordinates (mainly for dihedral)
!
! Output:
!    x,y,z(Nat) REAL: Cartesian coordinates corresponding to IntCoord
!    XPrim(NPrim) REAL: Primitives for the cartesian coordinates. (This is a by-product)
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

! BTransInv,UMat should be passed as arguments of this subroutine.


  ! IdxGeom: geometry index.
  ! Internal coordinates IntCoord(NCoord) is to be converted
  ! into Cartesian coordinates x(Nat),y(Nat),z(Nat).

  use Path_module, only : Nat,AtName,IntCoordI,XyzGeomI,BMat_BakerT,NPrim,&
       BTransInv,BBT,BBT_inv,BprimT,a0,ScanCoord,Coordinate,&
       Xprimitive,Pi,NGeomI,IntCoordF,BTransInv_local,Xprimitive_t,&
       Symmetry_elimination,NCoord,UMat_local
  ! UMat_local is allocated in Calc_baker.f90
  ! XyzGeomI(NGeomI,3,Nat),BMat_BakerT(3*Nat,NCoord),a0=0.529177249d0

  Use Io_Module
  use VarTypes
  IMPLICIT NONE

  REAL(KREAL), INTENT(OUT) :: x(Nat),y(Nat),z(Nat)
  REAL(KREAL), INTENT(IN) :: x_k(Nat),y_k(Nat),z_k(Nat)
  REAL(KREAL), INTENT(IN) :: IntCoord(NCoord)
  REAL(KREAL), INTENT(INOUT) ::  IntCoord_k(NCoord)
  REAL(KREAL), INTENT(IN) :: XPrimRef(NPrim)
  REAL(KREAL), INTENT(OUT) :: XPrim(NPrim)


  ! IdxGeom: geometry index.
  Integer(KINT) :: n1,n2,n3,I,J,K,Counter
  REAL(KREAL) :: normDeltaX_int

  REAL(KREAL), ALLOCATABLE :: DeltaX_int(:)  !DeltaX_int(NPrim,3*Nat-6), 3*Nat-6??
  REAL(KREAL), ALLOCATABLE :: DeltaX_cart(:) !DeltaX_cart(3*Nat)
  REAL(KREAL), ALLOCATABLE :: Geom(:,:) !(3,Nat)
  REAL(KREAL), ALLOCATABLE :: X_cart_k(:,:) ! (3,Nat)
  REAL(KREAL), ALLOCATABLE :: GMat(:,:) !(NPrim,NPrim)
  REAL(KREAL), ALLOCATABLE :: EigVec(:,:), EigVal(:) ! EigVec(NPrim,NPrim)
  REAL(KREAL), ALLOCATABLE :: UMat_tmp(:,:)
  REAL(KREAL), ALLOCATABLE :: V(:,:) ! (NCoord,NCoord)

  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
  REAL(KREAL) :: Angle, angle_d, tol,Delta,DiheTmp
  REAL(KREAL) :: MRot(3,3), Rmsd
  REAL(KREAL), PARAMETER :: Crit=1e-08

  EXTERNAL Angle, angle_d
  LOGICAL :: debug, FAIL

  INTEGER(KINT), PARAMETER :: NbItMax=50


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



     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('ConvertBakerInternal_cart')
  if (debug) WRITE(*,*) '=======Entering ConvertBakerInternal_cart======='

  FAIL = .FALSE.	   

!!!!!!
  ! Allocation phase

  ALLOCATE(DeltaX_int(NCoord),DeltaX_cart(3*Nat))
  ALLOCATE(UMat_tmp(NPrim,NCoord))
  ALLOCATE(X_cart_k(3,Nat))
  ALLOCATE(Geom(3,Nat),BprimT(3*Nat,NPrim))
  ALLOCATE(BBT(NCoord,NCoord))
  ALLOCATE(BBT_inv(NCoord,NCoord))
  ALLOCATE(Gmat(NPrim,NPrim))
  ALLOCATE(EigVal(NPrim),EigVec(NPrim,NPrim))


  if (debug) THEN
     WRITE(*,*) "Checking purposes...."
     WRITE(*,*) "Trying ot convert Xint initial (IntCoord)"
     WRITE(*,'(50(1X,F12.8))') IntCoord(:)
     WRITE(*,*) "BTransInv_local"
     DO J=1,3*Nat
        WRITE(*,'(50(1X,F12.8))') BTransInv_local(:,J)
     END DO
     WRITE(*,*) "Xint initial (IntCoord_k)"
     WRITE(*,'(50(1X,F12.8))') IntCoord_k(:)
     WRITE(*,*) "Cartesian coordinates"
     WRITE(*,*) Nat
     WRITE(*,*) 'GeomCart in ConvertBakerInternal_cart'
     DO I=1,Nat
        WRITE(*,'(1X,A,I4,3(1X,F12.4))') AtName(I),I,X_k(I),Y_k(i),Z_k(i)
     END DO
     WRITE(*,*) "Calculating actual values using x_k,y_k,z_k"

!     WRITE(*,*) "First, with Calc_XPrim"
!     WRITE(*,*) "Coordinate:",associated(Coordinate)
     Call Calc_Xprim(nat,x_k,y_k,z_k,Coordinate,NPrim,XPrimitive_t,XPrimRef)

!      I=0 ! index for the NPrim (NPrim is the number of ! primitive coordinates).
!      ScanCoord=>Coordinate
!      DO WHILE (Associated(ScanCoord%next))
!         I=I+1
!         SELECT CASE (ScanCoord%Type)
!         CASE ('BOND')
!            Print *, I, ':',  ScanCoord%At1, '-', ScanCoord%At2, '=', Xprimitive_t(I)
!         CASE ('ANGLE')		
!            Print *, I, ':', ScanCoord%At1, '-', ScanCoord%At2,'-',ScanCoord%At3, '=', Xprimitive_t(I)*180./Pi
!         CASE ('DIHEDRAL')
!            Print *, I, ':',  ScanCoord%At1, '-',ScanCoord%At2,'-',ScanCoord%At3,'-',&
!            ScanCoord%At4,'=',Xprimitive_t(I)*180./Pi
!         END SELECT
!         ScanCoord => ScanCoord%next
!      END DO ! matches DO WHILE (Associated(ScanCoord%next))

!      WRITE(*,*) "Second with normal proc"
!      I=0 ! index for the NPrim (NPrim is the number of ! primitive coordinates).
!      Xprimitive_t=0.d0
!      ScanCoord=>Coordinate
!      DO WHILE (Associated(ScanCoord%next))
!         I=I+1
!         SELECT CASE (ScanCoord%Type)
!         CASE ('BOND')
!            Call vecteur(ScanCoord%At2,ScanCoord%At1,x_k,y_k,z_k,vx2,vy2,vz2,Norm2)
!            Xprimitive_t(I) = Norm2
!            Print *, I, ':',  ScanCoord%At1, '-', ScanCoord%At2, '=', Xprimitive_t(I)
!         CASE ('ANGLE')		
!            Call vecteur(ScanCoord%At2,ScanCoord%At3,x_k,y_k,z_k,vx1,vy1,vz1,Norm1)
!            Call vecteur(ScanCoord%At2,ScanCoord%At1,x_k,y_k,z_k,vx2,vy2,vz2,Norm2)
!            Xprimitive_t(I) = angle(vx1,vy1,vz1,Norm1,vx2,vy2,vz2,Norm2)*Pi/180.
!            Print *, I, ':', ScanCoord%At1, '-', ScanCoord%At2,'-',ScanCoord%At3, '=', Xprimitive_t(I)*180./Pi
!         CASE ('DIHEDRAL')
!            Call vecteur(ScanCoord%At2,ScanCoord%At1,x_k,y_k,z_k,vx1,vy1,vz1,Norm1)
!            Call vecteur(ScanCoord%At2,ScanCoord%At3,x_k,y_k,z_k,vx2,vy2,vz2,Norm2)
!            Call vecteur(ScanCoord%At3,ScanCoord%At4,x_k,y_k,z_k,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)
! !!! PFL 28 Feb 2008 Test to be sure that angle does not change by more than Pi
!            !           Xprimitive_t(I)=angle_d(vx4,vy4,vz4,norm4,vx5,vy5,vz5,norm5,vx2,vy2,vz2,norm2)*Pi/180.
!            DiheTmp= angle_d(vx4,vy4,vz4,norm4,vx5,vy5,vz5,norm5, &
!                 vx2,vy2,vz2,norm2)
!            Xprimitive_t(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.,DiheTmp))
!            ELSE
!               If ((abs(DiheTmp).GE.170.D0).AND.(Sign(1.,DiheTmp)*ScanCoord%SignDihedral<0)) THEN
!                  Xprimitive_t(I) = DiheTmp*Pi/180.+ ScanCoord%SignDihedral*2.*Pi
!               END IF
!            END IF

!            IF (abs(Xprimitive_t(I)-XPrimRef(I)).GE.Pi) THEN
!               if ((Xprimitive_t(I)-XPrimRef(I)).GE.Pi) THEN
!                  Xprimitive_t(I)=Xprimitive_t(I)-2.d0*Pi
!               ELSE
!                  Xprimitive_t(I)=Xprimitive_t(I)+2.d0*Pi
!               END IF
!            END IF

!            Print *, I, ':',  ScanCoord%At1, '-',ScanCoord%At2,'-',ScanCoord%At3,'-',&
!            ScanCoord%At4,'=',Xprimitive_t(I)*180./Pi
!         END SELECT
!         ScanCoord => ScanCoord%next
!      END DO ! matches DO WHILE (Associated(ScanCoord%next))

     IntCoord_k=0.d0
     DO I=1, NPrim
        ! Transpose of UMat is needed below, that is why UMat(I,:).
        IntCoord_k(:)=IntCoord_k(:)+UMat_local(I,:)*Xprimitive_t(I)		 
     END DO
     WRITE(*,*) "Xint (intCoord_k) cooresponding to x_k,y_k,z_k"
     WRITE(*,'(50(1X,F12.8))') IntCoord_k(:)
     WRITE(*,*) "UMat_local"
     DO I=1, NPrim
        WRITE(*,'(50(1X,F12.8))') UMat_local(I,:)
     END DO

     !     Geom(1,:)=X_k(1:Nat)/a0
     !     Geom(2,:)=y_k(1:Nat)/a0
     !     Geom(3,:)=z_k(1:Nat)/a0

     Geom(1,:)=X_k(1:Nat)
     Geom(2,:)=y_k(1:Nat)
     Geom(3,:)=z_k(1:Nat)

     ! Computing B^prim:
     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

     !     if (debug) THEN
     !        WRITE(*,*) "PFL DBG, I,NPrim,BPrimT",I,NPrim
     !        DO I=1,NPrim
     !           WRITE(*,'(50(1X,F12.6))') BPrimT(:,I)
     !        END DO
     !     END IF

     BMat_BakerT = 0.d0
     DO I=1,NCoord
        DO J=1,NPrim
           !BprimT is transpose of B^prim.
           !B = UMat^T B^prim, B^T = (B^prim)^T UMat
           BMat_BakerT(:,I)=BMat_BakerT(:,I)+BprimT(:,J)*UMat_local(J,I)
        END DO
     END DO

     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

     BBT_inv = 0.d0

     !Print *, 'NCoord=', NCoord
     !Print *, 'BBT='
     DO J=1,NCoord
        ! WRITE(*,'(50(1X,F12.6))') BBT(:,J)
     END DO
     !Print *, 'End of BBT'
     ! GenInv is in Mat_util.f90
     Call GenInv(NCoord,BBT,BBT_inv,NCoord)
!     ALLOCATE(V(NCoord,NCoord))
!     tol = 1e-12
!     ! BBT is destroyed by GINVSE
!     Call GINVSE(BBT,NCoord,NCoord,BBT_inv,NCoord,NCoord,NCoord,tol,V,NCoord,FAIL,IOOUT)
!     DEALLOCATE(V)
!     IF(Fail) Then
!        WRITE (*,*) "Failed in generalized inverse, L220, ConvertBaker...f90"
!        STOP
!     END IF

     !Print *, 'BBT_inv='
     DO J=1,NCoord
        ! WRITE(*,'(50(1X,F10.2))') BBT_inv(:,J)
        ! Print *, BBT_inv(:,J)
     END DO
     !Print *, 'End of BBT_inv'

     ! Calculation of (B^T)^-1 = (BB^T)^-1B:
     BTransInv_local = 0.d0
     DO I=1,3*Nat
        DO J=1,NCoord
           BTransInv_local(:,I)=BTransInv_local(:,I)+BBT_inv(:,J)*BMat_BakerT(I,J)
        END DO
     END DO

     !Print *, 'BMat_BakerT='
     DO J=1,3*Nat
        !WRITE(*,'(50(1X,F12.8))') BMat_BakerT(:,J)
     END DO
     !Print *, 'End of BMat_BakerT'

     WRITE(*,*) "BTransInv_local Trans corresponding to x_k,y_k,z_k"
     DO J=1,3*Nat
        WRITE(*,'(50(1X,F12.8))') BTransInv_local(:,J)
     END DO


  END IF

  DeltaX_int(:) = IntCoord(:)-IntCoord_k(:)

  X_cart_k(1,:) = x_k(:)
  X_cart_k(2,:) = y_k(:)
  X_cart_k(3,:) = z_k(:)

  normDeltaX_int=0.d0 ! This is to be implemented.
  DO I=1, NCoord
     normDeltaX_int=normDeltaX_int+DeltaX_int(I)*DeltaX_int(I)
  END DO
  normDeltaX_int = sqrt(normDeltaX_int)
  ! I just need initial value of normDeltaX_int to be .GT. 1d-11,
  ! that is why it is initialized to 1.d0
  !normDeltaX_int = 1.d0

  if (debug) WRITE(*,*) "Entering the loop"
  Counter = 0
  DO While ((normDeltaX_int .GT. Crit) .AND. (Counter .LT. NbItMax))
     !DO While (normDeltaX_int .GT. 1d-6)
     Counter = Counter + 1 		  
     DeltaX_cart = 0.d0
!     if (normDeltaX_int.LE.1e-4) THEN
!        DeltaX_int=DeltaX_int*1e4
!     END IF 
     DO I=1,NCoord
        ! below BTransInv_local(I,:) is used because actually we need Transpose of BTransInv_local.
        DeltaX_cart(:)=DeltaX_cart(:)+BTransInv_local(I,:)*DeltaX_int(I)
     END DO
!     if (normDeltaX_int.LE.1e-4) THEN
!        DeltaX_int=DeltaX_int*1e-4
!     DeltaX_cart=DeltaX_cart*1e-4
!     END IF


           if (debug) THEN
              WRITE(*,*) "Info for iteration:",counter
              Print *, 'DeltaX_int='
              WRITE(*,'(50(1X,F12.8))') DeltaX_int
              Print *, 'DeltaX_cart,norm',sqrt(dot_product(DeltaX_cart,DeltaX_cart))
              DO J=1,Nat
              WRITE(*,'(1X,A4,3(1X,F15.11),3(1X,D25.15))') AtName(J),DeltaX_cart(3*J-2:3*J),DeltaX_cart(3*J-2:3*J)
              END DO
              Print *, 'BTransInv_local Trans='
              DO J=1,3*Nat
                 WRITE(*,'(50(1X,F12.8))') BTransInv_local(:,J)
              END DO
              Print *, 'DeltaX_int='
              WRITE(*,'(50(1X,F12.8))') DeltaX_int
           END IF

     ! Finite precision error correction step:
!     DO I=1,3*Nat
!        IF (DeltaX_cart(I) .NE. 0.d0) Then
!           IF(abs(DeltaX_cart(I)) .LT. 1d-10) Then
!              !Print *, 'Correcting DeltaX_cart(',I,')=', DeltaX_cart(I)
!              DeltaX_cart(I) = 0.d0
!           END IF
!        END IF
!     END DO

     ! new cartesian coordinates:
     DO I=1, Nat
        X_cart_k(1,I) = X_cart_k(1,I) + DeltaX_cart(3*I-2)
        X_cart_k(2,I) = X_cart_k(2,I) + DeltaX_cart(3*I-1)
        X_cart_k(3,I) = X_cart_k(3,I) + DeltaX_cart(3*I)
     END DO

     !     Geom(1,:)=X_cart_k(1,1:Nat)/a0
     !     Geom(2,:)=X_cart_k(2,1:Nat)/a0
     !     Geom(3,:)=X_cart_k(3,1:Nat)/a0
     Geom(1,:)=X_cart_k(1,1:Nat)
     Geom(2,:)=X_cart_k(2,1:Nat)
     Geom(3,:)=X_cart_k(3,1:Nat)

     if (debug) THEN
     WRITE(*,*) 'GeomCart used to compute BPrim'
     DO I=1,Nat
        WRITE(*,'(1X,A,I4,3(1X,F12.4))') AtName(I),I,Geom(1,I),Geom(2,i),Geom(3,i)
     END DO
     END IF

     ! Computing B^prim:
     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

     if (debug) THEN
        WRITE(*,*) "Bprim "
        DO J=1,3*Nat
           WRITE(*,'(50(1X,F12.6))') BprimT(J,:)
        END DO
     END IF


     BMat_BakerT = 0.d0
     DO I=1,NCoord
        DO J=1,NPrim
           !BprimT is transpose of B^prim.
           !B = UMat^T B^prim, B^T = (B^prim)^T UMat
           BMat_BakerT(:,I)=BMat_BakerT(:,I)+BprimT(:,J)*UMat_local(J,I)
        END DO
     END DO

     ! ADDED FROM HERE:
     ! We now compute G=B(BT) matrix
     !IF (IOptt .EQ. 5000) Then
     !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

     ! We diagonalize G
     !Call Jacobi(GMat,NPrim,EigVal,EigVec,NPrim)
     !Call Trie(NPrim,EigVal,EigVec,NPrim)

     ! We construct the new DzDc(b=baker) matrix
     ! UMat is nonredundant vector set, i.e. set of eigenvectors of
     !  BB^T corresponding to eigenvalues > zero.

     !UMat=0.d0
     !UMat_tmp=0.d0
     !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 = UMat^T B^prim, B^T = (B^prim)^T UMat
     !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 (=UMat) (=' ,J,') exceeded 3*Nat-6=',3*Nat-6, &
     !    'Stopping the calculation.'
     !	 STOP
     !  END IF
     ! UMat_tmp(:,J) =  Eigvec(:,I)
     ! END IF
     !END DO

     ! THIS IS TO BE CORRECTED, A special case for debugging C2H3F case:
     !J=0
     !DO I=1, 3*Nat-6
     ! IF ((I .NE. 6) .AND. (I .NE. 7) .AND. (I .NE. 9)) Then
     !  J=J+1
     ! Print *, 'J=', J, ' I=', I
     !UMat(:,J) = UMat_tmp(:,I)
     !     END IF
     ! END DO
     !BMat_BakerT=0.d0
     !    DO I=1,NCoord
     !      DO J=1,NPrim
     ! B = UMat^T B^prim, B^T = (B^prim)^T UMat
     !       BMat_BakerT(:,I)=BMat_BakerT(:,I)+BprimT(:,J)*UMat(J,I)
     !    END DO
     !END DO
     ! TO BE CORRECTED, ENDS HERE.

     !END IF
     ! END of the new changes.

     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

     BBT_inv = 0.d0

     !Print *, 'NCoord=', NCoord
     !Print *, 'BBT='
     DO J=1,NCoord
        ! WRITE(*,'(50(1X,F12.6))') BBT(:,J)
     END DO
     !Print *, 'End of BBT'
     ! GenInv is in Mat_util.f90
     Call GenInv(NCoord,BBT,BBT_inv,NCoord)
     !     if (debug) THEN
     !        WRITE(*,*) 'BBT_inv by GenInv'
     !        DO J=1,NCoord
     !           WRITE(*,'(50(1X,F12.6))') BBT_inv(:,J)
     !        END DO
     !     END IF

!    ALLOCATE(V(NCoord,NCoord))
!    tol = 1e-12
!    ! BBT is destroyed by GINVSE
!    Call GINVSE(BBT,NCoord,NCoord,BBT_inv,NCoord,NCoord,NCoord,tol,V,NCoord,FAIL,IOOUT)
!    DEALLOCATE(V)
!    IF(Fail) Then
!       WRITE (*,*) "Failed in generalized inverse, L220, ConvertBaker...f90"
!       STOP
!    END IF

     !     if (debug) THEN
     !        WRITE(*,*) 'BBT_inv by Genvse'
     !        DO J=1,NCoord
     !           WRITE(*,'(50(1X,F12.6))') BBT_inv(:,J)
     !        END DO
     !     END IF

     !Print *, 'End of BBT_inv'

     ! Calculation of (B^T)^-1 = (BB^T)^-1B:
     BTransInv_local = 0.d0
     DO I=1,3*Nat
        DO J=1,NCoord
           BTransInv_local(:,I)=BTransInv_local(:,I)+BBT_inv(:,J)*BMat_BakerT(I,J)
        END DO
     END DO

     !     if (debug) THEN
     !        Print *, 'BMat_BakerT='
     !        DO J=1,3*Nat
     !           WRITE(*,'(50(1X,F12.6))') BMat_BakerT(:,J)
     !        END DO
     !        Print *, 'End of BMat_BakerT'
     !     END IF

     !      if (debug) THEN
     !         Print *, 'BTransInv_local Trans='
     !         DO J=1,3*Nat
     !            WRITE(*,'(50(1X,F16.6))') BTransInv_local(:,J)
     !         END DO
     !         Print *, 'End of BTransInv_local Trans'
     !      END IF

     ! New cartesian cooordinates:
     x(:) = X_cart_k(1,:)
     y(:) = X_cart_k(2,:)
     z(:) = X_cart_k(3,:)


     Call Calc_Xprim(nat,x,y,z,Coordinate,NPrim,XPrimitive_t,XPrimRef)

!      I=0 ! index for the NPrim (NPrim is the number of 
!      ! primitive coordinates).
!      Xprimitive_t=0.d0
!      ScanCoord=>Coordinate
!      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)
!            Xprimitive_t(I) = Norm2
!            !Print *, I, ':',  ScanCoord%At1, '-', ScanCoord%At2, '=', Xprimitive_t(I)
!         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)
!            Xprimitive_t(I) = angle(vx1,vy1,vz1,Norm1,vx2,vy2,vz2,Norm2)*Pi/180.
!            !Print *, I, ':', ScanCoord%At1, '-', ScanCoord%At2,'-',ScanCoord%At3, '=', Xprimitive_t(I)*180./Pi
!         CASE ('DIHEDRAL')
!            Call vecteur(ScanCoord%At2,ScanCoord%At3,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)
! !!! PFL 28 Feb 2008 Test to be sure that angle does not change by more than Pi
!            !           Xprimitive_t(I)=angle_d(vx4,vy4,vz4,norm4,vx5,vy5,vz5,norm5,vx2,vy2,vz2,norm2)*Pi/180.
!            DiheTmp=angle_d(vx4,vy4,vz4,norm4,vx5,vy5,vz5,norm5,vx2,vy2,vz2,norm2)
!            Xprimitive_t(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
!                  Xprimitive_t(I) = DiheTmp*Pi/180.+ ScanCoord%SignDihedral*2.*Pi
!               END IF
!            END IF



!            !Print *, I, ':',  ScanCoord%At1, '-',ScanCoord%At2,'-',ScanCoord%At3,'-',&
!            !ScanCoord%At4,'=',Xprimitive_t(I)*180./Pi
!         END SELECT
!         ScanCoord => ScanCoord%next
!      END DO ! matches DO WHILE (Associated(ScanCoord%next))

     !     if (debug) THEN
     !        WRITE(*,*) "Xprimitive_t"
     !        WRITE(*,'(50(1X,F12.6))') Xprimitive_t
     !     END IF

     IntCoord_k=0.d0
     DO I=1, NPrim
        ! Transpose of UMat is needed below, that is why UMat(I,:).
        IntCoord_k(:)=IntCoord_k(:)+UMat_local(I,:)*Xprimitive_t(I)		 
     END DO

     if (debug) THEN
        WRITE(*,*) "New Xint (IntCoord_k)"
        WRITE(*,'(50(1X,F12.8))') IntCoord_k
        WRITE(*,*) "Target (IntCoord)"
        WRITE(*,'(50(1X,F12.8))') IntCoord

     END IF


     ! Computing DeltaX_int

     DeltaX_int(:) = IntCoord(:)-IntCoord_k(:)




     ! norm2 of DeltaX_int is being calculated here:
     normDeltaX_int=0.d0
     DO I=1, NCoord
        normDeltaX_int=normDeltaX_int+DeltaX_int(I)*DeltaX_int(I)
     END DO
     normDeltaX_int = sqrt(normDeltaX_int)

          if (debug) THEN
             WRITE(*,*) "New Delta_Xint (deltaX_int)"
             WRITE(*,'(50(1X,F12.6))') DeltaX_int
             WRITE(*,*) "Norm:",normDeltaX_int
          END IF

     !IF (Counter .GE. 400) THEN
     !Print *, 'Counter=', Counter, 'normDeltaX_int=', normDeltaX_int
     !Print *, 'DeltaX_int='
     !WRITE(*,'(50(1X,F8.2))') DeltaX_int
     !END IF

     IF (Mod(Counter,1).EQ.0) THEN
        !WRITE(*,*) Nat
        !WRITE(*,*) 'GeomCart in ConvertBakerInternal_cart'
        DO I=1,Nat
           !	WRITE(*,'(1X,A,I4,3(1X,F12.4))') AtName(I),I,X_Cart_k(1:3,I)
        END DO
     END IF

     !call two_norm(DeltaX_int,normDeltaX_int,3*Nat)
  END DO !matches DO While (normDeltaX_int .GT. 1d-11)

  IF (debug) THEN
  WRITE(*,*) 'GeomCart in ConvertBakerInternal_cart'
  DO I=1,Nat
     WRITE(*,'(1X,A,I4,3(1X,F12.4))') AtName(I),I,X_Cart_k(1:3,I)
  END DO
  END IF

  if (debug) THEN
     WRITE(*,*) "Bmat_bakerT"
     DO J=1,NCoord
        WRITE(*,'(50(1X,F12.6))') BMat_BakerT(:,J)
     END DO
  END IF



  !IF (IOptt .GE. 2) THEN
  !   Print  *, 'Counter=', Counter
  !END IF
  IF (Counter .GE. NbItMax) Then
     Print *, 'Counter GE 500, Stopping, normDeltaX_int=', normDeltaX_int
     STOP
  END IF


     x(:) = X_cart_k(1,:)
     y(:) = X_cart_k(2,:)
     z(:) = X_cart_k(3,:)


!  call CalcRmsd(Nat,x_k,y_k,z_k,x,y,z,MRot,rmsd,.TRUE.,.TRUE.)


  if (debug) WRITE(*,*) "COnverted in ",counter," iterations"

  DEALLOCATE(Geom,DeltaX_int,DeltaX_cart)
  DEALLOCATE(BprimT,BBT,BBT_inv,UMat_tmp)
  DEALLOCATE(X_cart_k)
  DEALLOCATE(GMat,EigVal,EigVec)

  XPrim=Xprimitive_t

  if (debug) WRITE(*,*) '=====ConvertBakerInternal_cart Over====='

END SUBROUTINE ConvertBakerInternal_cart
