
      SUBROUTINE VPRD(X,Y,Z)
!     ******************************************************************
!     **  CROSS PRODUCT (VECTOR PRODUCT OF TWO VECTORS)               **
!     ******************************************************************
        Use VarTypes
      IMPLICIT NONE
      REAL(KREAL),INTENT(IN) :: X(3)
      REAL(KREAL),INTENT(IN) :: Y(3)
      REAL(KREAL),INTENT(OUT):: Z(3)
!     ******************************************************************
      Z(1)=X(2)*Y(3)-X(3)*Y(2)
      Z(2)=X(3)*Y(1)-X(1)*Y(3)
      Z(3)=X(1)*Y(2)-X(2)*Y(1)
      RETURN
      END

!     ..................................................................
      SUBROUTINE CONSTRAINTS_BONDLENGTH_DER(NAT,IAT1,IAT2,R0,B)
!     ******************************************************************
!     **                                                              **
!     **  BOND-LENGTH CONSTRAINT : SET UP COORDINATES                 **   
!     **                                                              **
!     ******************************************************************
        Use VarTypes
      IMPLICIT NONE
      INTEGER(KINT),INTENT(IN) :: NAT
      INTEGER(KINT),INTENT(IN) :: IAT1
      INTEGER(KINT),INTENT(IN) :: IAT2
      REAL(KREAL)   ,INTENT(IN) :: R0(3,NAT)
      REAL(KREAL)   ,INTENT(OUT):: B(3,NAT)

      INTEGER(KINT)            :: I,J
      REAL(KREAL)               :: DIS,DI,DJ
      REAL(KREAL)               :: DELTAK
      REAL(KREAL)               :: SVAR
      real(KREAL)               :: dr(3)
!     ******************************************************************
      DR(:)=R0(:,IAT2)-R0(:,IAT1)
!
!     ==================================================================
!     == CALCULATE VALUE AND FIRST TWO DERIVATIVES OF D=|R_2-R_2|     ==
!     ==================================================================
      DIS=DSQRT(DOT_PRODUCT(DR,DR))
      DO I=1,3
         DI=DR(I)/DIS
         B(I,IAT2)=DI
         B(I,IAT1)=-DI
      ENDDO
      RETURN
      END SUBROUTINE CONSTRAINTS_BONDLENGTH_DER

      SUBROUTINE CONSTRAINTS_GLC_DER(NAT,VEC,B)
!     ******************************************************************
!     **  CONSTRAINTS_SETGLC                                          **
!     **  SET GENERAL LINEAR CONSTRAINT (GLC) CONST+VEC*R=0           **
!     **                                                              **
!     **  APPROXIMATES CONSTRAINT FUNCTION (WITHOUT VALUE)            **
!     **  BY AN QUADRATIC POLYNOM IN A+B*R+0.5*R*C*R                  **
!     **  THE CONSTRAINT IS THE POLYNOMIAL MINUS THE VALUE FROM THE   **
!     **  REFERENCE STRUCTURE                                         **
!     ******************************************************************
        Use VarTypes
      IMPLICIT NONE
      INTEGER(KINT),INTENT(IN) :: NAT
      REAL(KREAL)   ,INTENT(IN) :: VEC(3,NAT)
      REAL(KREAL)   ,INTENT(OUT):: B(3,NAT)
      INTEGER(KINT)            :: IAT,I
!     ******************************************************************
      DO IAT=1,NAT
         DO I=1,3
            B(I,IAT)=VEC(I,IAT)
         ENDDO
      ENDDO
      RETURN
      END SUBROUTINE CONSTRAINTS_GLC_DER


      SUBROUTINE CONSTRAINTS_SUBSTITUTION_DER(NAT,IAT1,IAT2,IAT3,R0,B)
!     ******************************************************************
!     **  CONSTRAINTS_SETsubstitution                                 **
!     **  APPROXIMATES CONSTRAINT FUNCTION (WITHOUT VALUE)            **
!     **  BY AN QUADRATIC POLYNOMial IN A+B*R+0.5*R*C*R               **
!     **  THE CONSTRAINT IS THE POLYNOMIAL MINUS THE VALUE FROM THE   **
!     **  REFERENCE STRUCTURE                                         **
!     **  CONSTRAINS THE difference OF THE BOND DISTANCES BETWEEN     **
!     **  R1-R2 AND R2-R3                                             **
!     ******************************************************************
      Use Vartypes
      implicit none
      INTEGER(KINT),INTENT(IN) :: NAT
      INTEGER(KINT),INTENT(IN) :: IAT1
      INTEGER(KINT),INTENT(IN) :: IAT2
      INTEGER(KINT),INTENT(IN) :: IAT3
      REAL(KREAL)   ,INTENT(OUT):: B(3,NAT)
      REAL(KREAL)   ,INTENT(IN) :: R0(3,NAT)
      REAL(KREAL)               :: X(9),DI(9)
      real(KREAL)               :: phi
      REAL(KREAL)               :: X1X2,Y1Y2,Z1Z2,X2X3,Y2Y3,Z2Z3,D12,D22,  &
                                   D1,D2,D13,D23
      integer(KINT)            :: i,j
!     ******************************************************************
!
! --- TRANSFER R0 TO X ARRAY TO MAKE HANDLING AND PASSING EASIER
      DO I=1,3
       X(I)  =R0(I,IAT1)
       X(I+3)=R0(I,IAT2)
       X(I+6)=R0(I,IAT3)
      ENDDO
!
! --- FOR THE FOLLOWING, THE CONSTRAINT ITSELF WILL BE CALLED PHI
! --- CALCULATE PHI,D PHI/D X_I, D^2 PHI/D X_I D X_J
!
      X1X2=X(1)-X(4)
      Y1Y2=X(2)-X(5)
      Z1Z2=X(3)-X(6)
      X2X3=X(4)-X(7)
      Y2Y3=X(5)-X(8)
      Z2Z3=X(6)-X(9)
!
      D12=(X1X2)**2 + (Y1Y2)**2 + (Z1Z2)**2
      D22=(X2X3)**2 + (Y2Y3)**2 + (Z2Z3)**2
      D1= SQRT(D12)
      D2= SQRT(D22)
      D13= D1**3
      D23= D2**3
      PHI= D1 - D2
!
      B(1,IAT1)= (X1X2)/D1
      B(1,IAT2)= -((X1X2)/D1) - (X2X3)/D2
      B(1,IAT3)= (X2X3)/D2
      B(2,IAT1)= (Y1Y2)/D1
      B(2,IAT2)= -((Y1Y2)/D1) - (Y2Y3)/D2
      B(2,IAT3)= (Y2Y3)/D2
      B(3,IAT1)= (Z1Z2)/D1
      B(3,IAT2)= -((Z1Z2)/D1) - (Z2Z3)/D2
      B(3,IAT3)= (Z2Z3)/D2
!
      RETURN
      END SUBROUTINE CONSTRAINTS_SUBSTITUTION_DER

!     ..................................................................
      SUBROUTINE CONSTRAINTS_BONDANGLE_DER(NAT,IAT1,IAT2,IAT3,R0,B)
!     ******************************************************************
!     **
!     **  CONSTRAINS THE ANGLE ENCLOSED BETWEEN ATOMS IAT1--IAT2--IAT3
!     **  In the followin, IAT1 is also denoted by A, IAT2 by B and IAT3 by C
!     **
!     ******************************************************************
        Use VarTypes
      IMPLICIT none
      INTEGER(KINT),INTENT(IN) :: NAT
      INTEGER(KINT),INTENT(IN) :: IAT1
      INTEGER(KINT),INTENT(IN) :: IAT2
      INTEGER(KINT),INTENT(IN) :: IAT3
      REAL(KREAL)   ,INTENT(OUT):: B(3,NAT)
      REAL(KREAL)   ,INTENT(IN) :: R0(3,NAT)
      REAL(KREAL)               :: X(9),DI(9),DIDJ(9,9),B1(9)
      real(KREAL)               :: phi
      integer(KINT)            :: i,j,ichk, iat
      REAL(KREAL)               :: UU    ! U*U
      REAL(KREAL)               :: VV    ! V*V
      REAL(KREAL)               :: UV    ! U*V
      REAL(KREAL)               :: D    ! 1/SQRT(A*B)
      REAL(KREAL)               :: DADX(9)
      REAL(KREAL)               :: DBDX(9)
      REAL(KREAL)               :: DCDX(9)
      REAL(KREAL)               :: DDDX(9)
      REAL(KREAL)               :: DJDX(9)
      REAL(KREAL)               :: DVDX(9)
      REAL(KREAL)               :: DGDX(9)    ! D(A*B)/DX
      REAL(KREAL)               :: AB,AB2N,V,PREFAC
      REAL(KREAL)               :: COSPHI,SINPHI
      REAL(KREAL)               :: DIVDX,AI
!
! Variable used in case of a linear molecule with three atoms call A,B and C
! Phi is the ABC angle
! Threshold use to decide that sin(Phi)=0.
      REAL(KREAL), PARAMETER    :: ThreshSin=1e-7
! Sine and Cosine of the euler Phi angle between ABC and x axis
      REAL(KREAL)               :: CosP,SinP
! Sine and Cosine of the euler theta angle between ABC and z axis
      REAL(KREAL)               :: CosTheta,SinTheta
! Vectors CB, CA,AB
      REAL(KREAL)               ::vCB(3),vCA(3),vAB(3)
! distance AB in the xAy plane
      REAL(KREAL)               :: dABxy
! cosine of the BAC angle
      REAL(KREAL)               :: CosBAC
! cosine of the BCA angle
      REAL(KREAL)               :: CosBCA

!
! For debugginh purposes:
      LOGICAL, PARAMETER :: Debug=.False.


      IF (Debug) WRITE(*,*) "================ Entering  CONSTRAINTS_BONDANGLE_DER =========="
!     ******************************************************************
!
!     ==================================================================
!     ==  SET UP CONSTRAINT IN COORDINATE SPACE                       ==
!     ==================================================================
!
!     ==================================================================
!     == CALCULATE VALUE AND FIRST TWO DERIVATIVES OF                 ==
!     == PHI=<X1-X2|X3-X2>/SQRT(<X1-X2|X1-X2><X3-X2|X3-X2>)           ==
!     ==================================================================
! --- TRANSFER R0 TO X ARRAY TO MAKE HANDLING AND PASSING EASIER
      DO I=1,3
       X(I)  =R0(I,IAT1)
       X(I+3)=R0(I,IAT2)
       X(I+6)=R0(I,IAT3)
      ENDDO

      if (debug) THEN
         WRITE(*,*) "X=",X
         WRITE(*,*) "A=",X(1)-X(4),X(2)-X(5),X(3)-X(6)
         WRITE(*,*) "B=",X(7)-X(4),X(8)-X(5),X(9)-X(6)
      END IF

!     ----------------------------------------------------------------
!     --- R1=X(1:3)   R2=X(4:6)  R3=X(7:9)
!     --- U=R1-R2 V=R3-R2
!     --- A=U*U=d(At1-At2)**2
!     --- B=V*V=d(At2-At3)**2
!     --- C=U*V=vec(At2-At1)*vec(At2-At3)
!     ----------------------------------------------------------------
      UU=(X(1)-X(4))**2 + (X(2)-X(5))**2 + (X(3)-X(6))**2
      VV=(X(7)-X(4))**2 + (X(8)-X(5))**2 + (X(9)-X(6))**2
      UV=(X(1)-X(4))*(X(7)-X(4))  &
           +(X(2)-X(5))*(X(8)-X(5))   &
           +(X(3)-X(6))*(X(9)-X(6))
      AB=UU*VV
      D=SQRT(AB)
      COSPHI=UV/D       !=COS(PHI)
      PHI=DACOS(COSPHI)
      SINPHI=DSQRT(1.D0-COSPHI**2)

      IF (SINPHI.GE.ThreshSin) THEN
! The following evaluation of the derivative involves
! 1/sinphi... so we test that it is not 0 :-)
!     ---  CALCULATE DADX
      DADX(1)= 2.D0*(X(1)-X(4))
      DADX(2)= 2.D0*(X(2)-X(5))
      DADX(3)= 2.D0*(X(3)-X(6))
      DADX(4)=-2.D0*(X(1)-X(4))
      DADX(5)=-2.D0*(X(2)-X(5))
      DADX(6)=-2.D0*(X(3)-X(6))
      DADX(7)= 0.D0
      DADX(8)= 0.D0
      DADX(9)= 0.D0
!     ---  CALCULATE DBDX
      DBDX(1)= 0.D0
      DBDX(2)= 0.D0
      DBDX(3)= 0.D0
      DBDX(4)=-2.D0*(X(7)-X(4))
      DBDX(5)=-2.D0*(X(8)-X(5))
      DBDX(6)=-2.D0*(X(9)-X(6))
      DBDX(7)= 2.D0*(X(7)-X(4))
      DBDX(8)= 2.D0*(X(8)-X(5))
      DBDX(9)= 2.D0*(X(9)-X(6))
! --- CALCULATE DCDX
      DCDX(1)= X(7)-X(4)
      DCDX(2)= X(8)-X(5)
      DCDX(3)= X(9)-X(6)
      DCDX(4)=-X(7)+2.D0*X(4)-X(1)
      DCDX(5)=-X(8)+2.D0*X(5)-X(2)
      DCDX(6)=-X(9)+2.D0*X(6)-X(3)
      DCDX(7)= X(1)-X(4)
      DCDX(8)= X(2)-X(5)
      DCDX(9)= X(3)-X(6)

!       WRITE(*,*) 'PFL DER ANGLE'
!       WRITE(*,'(15(1X,F10.6))') DADX
!       WRITE(*,'(15(1X,F10.6))') DBDX
!       WRITE(*,'(15(1X,F10.6))') DCDX

!     ==================================================================
!     ==  CALCULATE PHI ETC  ===========================================
!     ==================================================================
!     --- CALCULATE DERIVATIVE OF ARCCOS
      V=-1.D0/SINPHI                 !=-SIN(PHI)=D(ACOS(PHI))/D(COSPHI)
! --- CALCULATE FIRST DERIVATIVES OF G=A*B
      DO I=1,9
        DGDX(I)=DADX(I)*VV+UU*DBDX(I)
      ENDDO
! --- CALCULATE DDDX
      AB2N=0.5D0/D
      DO I=1,9
        DDDX(I)=AB2N*DGDX(I)
      ENDDO
! --- CALCULATE FIRST DERIVATIVES OF J
      PREFAC=1.D0/AB
      DO I=1,9
        DJDX(I)=(DCDX(I)*D-UV*DDDX(I))*PREFAC
      ENDDO
! --- CALCULATE FIRST DERIVATIVE
      DO I=1,9
        DI(I)=DJDX(I)*V
      ENDDO
!
! Store it back to B
      DO I=1,3
        B(I,IAT1)=DI(I)
        B(I,IAT2)=DI(I+3)
        B(I,IAT3)=DI(I+6)
      ENDDO

   ELSE
! First calculate cosP, sinP, CosTheta, SinTheta
         dABxy=sqrt((X(4)-X(1))**2+(X(5)-X(2))**2)
         cosP=abs((X(7)-X(1)))/sqrt((X(7)-X(1))**2+(X(8)-X(2))**2)
!         SinP=(X(8)-X(2))/sqrt((X(7)-X(1))**2+(X(8)-X(2))**2)
         sinP=sqrt(1.d0-cosP**2)
         cosTheta=(X(6)-X(3))/(sqrt(UU))
         SinTheta=dABxy/(sqrt(UU))
         IF (SinTheta.le.ThreshSin) THEN
! Theta=0, so that the euler Phi angle is ill-defined. We put it to 0:
            CosP=1.d0
            SinP=0.d0
         END IF
         IF (Debug)   WRITE(*,*) "CosP,SinP ",CosP,SinP
         if (debug)   WRITE(*,*) "CosTheta,SinTheta",CosTheta,SinTheta
! We calculate cos(BAC) and cos(BCA) because they are needed to calculate
! the derivatives for the centre atom (B)
         DO I=1,3
            vCB(I)=X(3+I)-X(6+I)
            vCA(I)=X(I)-X(6+I)
            vAB(I)=x(3+I)-x(I)
         END DO
         CosBCA=DOT_PRODUCT(vCB,vCA)/sqrt(DOT_PRODUCT(vCB,vCB)*DOT_PRODUCT(vCA,vCA))
         CosBAC=-DOT_PRODUCT(vAB,vCA)/sqrt(DOT_PRODUCT(vAB,vAB)*DOT_PRODUCT(vCA,vCA))
         if (debug) WRITE(*,*) "CosBAC,CosBCA:",CosBAC,CosBCA
! Sign of the derivatives are -1 for Phi=Pi and 1 for Phi=0
! ie is cos(Phi).
         DI(1)=COSPHI/sqrt(UU)
         DI(2)=DI(1)
         DI(3)=0.d0
         DI(7)=COSPHI/sqrt(VV)
         DI(8)=DI(7)
         DI(9)=0.d0
         DI(4)=CosBCA*DI(1)+cosBAC*DI(7)
         DI(5)=DI(4)
         DI(6)=0.D0
         if (debug) WRITE(*,'(A,9(1X,F12.6))') 'DI=',DI(1:9)
         if (debug) THEN
            WRITE(*,'(3(1X,F12.6))') cosTheta*CosP,sinP,-cosP*sinTheta
            WRITE(*,'(3(1X,F12.6))') -cosTheta*sinP,cosP,sinP*sinTheta
            WRITE(*,'(3(1X,F12.6))') sinTheta,0.,cosTheta
         END IF
! We now take care of the fact that the three fragment are
! not along the Z-axis
         B(1,Iat1)=cosTheta*CosP*DI(1)+sinP*DI(2)-cosP*sinTheta*DI(3)
         B(1,Iat2)=cosTheta*CosP*DI(4)+sinP*DI(5)-cosP*sinTheta*DI(6)
         B(1,Iat3)=cosTheta*CosP*DI(7)+sinP*DI(8)-cosP*sinTheta*DI(9)
         B(2,Iat1)=-cosTheta*SinP*DI(1)+cosP*DI(2)+sinP*sinTheta*DI(3)
         B(2,Iat2)=-cosTheta*SinP*DI(4)+cosP*DI(5)+sinP*sinTheta*DI(6)
         B(2,Iat3)=-cosTheta*SinP*DI(7)+cosP*DI(8)+sinP*sinTheta*DI(9)
         B(3,Iat1)=sinTheta*DI(1)+cosTheta*DI(3)
         B(3,Iat2)=sinTheta*DI(4)+cosTheta*DI(6)
         B(3,Iat3)=sinTheta*DI(7)+cosTheta*DI(9)
      END IF

      IF (Debug) WRITE(*,*) "================ Exiting  CONSTRAINTS_BONDANGLE_DER =========="
      END SUBROUTINE CONSTRAINTS_BONDANGLE_DER


!     ..................................................................
      SUBROUTINE CONSTRAINTS_TORSION_DER(NAT,IAT1,IAT2,IAT3,IAT4,R0,B)
!     ******************************************************************
!     **
!     **  CONSTRAINS THE TORSION OF IAT1-IAT2---IAT3-IAT4
!     **
!     ******************************************************************
        Use VarTypes
      IMPLICIT none
      INTEGER(KINT),INTENT(IN) :: NAT
      INTEGER(KINT),INTENT(IN) :: IAT1
      INTEGER(KINT),INTENT(IN) :: IAT2
      INTEGER(KINT),INTENT(IN) :: IAT3
      INTEGER(KINT),INTENT(IN) :: IAT4
      REAL(KREAL)   ,INTENT(IN) :: R0(3,NAT)
      REAL(KREAL)   ,INTENT(OUT):: B(3,NAT)
      REAL(KREAL)               :: X(12),DI(12),DIDJ(12,12),B1(12)
      REAL(KREAL)               :: P(3),Q(3),T(3),U(3),R(3),W(3)
      real(KREAL)               :: phi
      integer(KINT)            :: i,j,ichk,k

      REAL(KREAL)   :: TORS, PP,QQ, PQ
      REAL(KREAL) :: DADX(12),DBDX(12),DCDX(12)
      REAL(KREAL) :: DDDX(12),DJDX(12),DVDX(12),DGDX(12),DPDX(3,12),DQDX(3,12)
      REAL(KREAL) :: D2QDX2(3,12,12),D2PDX2(3,12,12)
      REAL(KREAL) :: D2BDX2(12,12),D2ADX2(12,12),D2CDX2(12,12),D2DDX2(12,12)
      real(KREAL) :: d,coverd,abyb2n,abyb,ai,divdx,prefac,v

!     ******************************************************************
!
! --- TRANSFER R0 TO X ARRAY TO MAKE HANDLING AND PASSING EASIER
      DO I=1,3
        X(I)=R0(I,IAT1)
        X(I+3)=R0(I,IAT2)
        X(I+6)=R0(I,IAT3)
        X(I+9)=R0(I,IAT4)
      ENDDO
!
! --- CALCULATE PHI,D PHI/D X_I, D^2 PHI/D X_I D X_J
      T(:)=X(1:3)  -X(4:6)
      U(:)=X(7:9)  -X(4:6)
      R(:)=X(10:12)-X(7:9)
      W(:)=X(4:6)  -X(7:9)
      CALL VPRD(T,U,P)
      CALL VPRD(W,R,Q)
      PQ=DOT_PRODUCT(P,Q)
      PP=DOT_PRODUCT(P,P)
      QQ=DOT_PRODUCT(Q,Q)
!
      ABYB=PP*QQ
      D=ABYB**0.5D0
      COVERD=PQ/D
      ABYB2N=0.5D0/D
!
      TORS=DACOS(COVERD)
      WRITE(*,*) "Torsion derivs  Phi=", Tors*180./3.1415926,Iat1,Iat2,Iat3,Iat4
!
! ----------------------------------------------------------------
! --- CALCULATE FIRST DERIVATIVES
! ----------------------------------------------------------------
! --- CALCULATE DERIVATIVE OF ARCCOS
      IF (dabs(DABS(COVERD)-1.d0) < 1.D-9) THEN
        WRITE(*,*)   'WARNING: DERIVATIVE OF TORSION AT ZERO AND PI IS UNDEFINED!'
        COVERD=sign(0.999d0,COVERD)
      TORS=DACOS(COVERD)
      !WRITE(*,*) "Torsion derivs, Phi=", Tors

!        V=0.D0
!      ELSE
     END IF
        V=-1.D0/DSQRT(1.D0-(COVERD)**2.D0)
!      ENDIF
! --- CALCULATE DPDX
! --- ZERO OUT SPARSE MATRIX
      DO J=1,3
       DO I=1,12
        DPDX(J,I)=0.D0
        DQDX(J,I)=0.D0
       ENDDO
      ENDDO
      DPDX(1,2)=X(9)-X(6)
      DPDX(1,3)=X(5)-X(8)
      DPDX(1,5)=X(3)-X(9)
      DPDX(1,6)=X(8)-X(2)
      DPDX(1,8)=X(6)-X(3)
      DPDX(1,9)=X(2)-X(5)
      DPDX(2,1)=X(6)-X(9)
      DPDX(2,3)=X(7)-X(4)
      DPDX(2,4)=X(9)-X(3)
      DPDX(2,6)=X(1)-X(7)
      DPDX(2,7)=X(3)-X(6)
      DPDX(2,9)=X(4)-X(1)
      DPDX(3,1)=X(8)-X(5)
      DPDX(3,2)=X(4)-X(7)
      DPDX(3,4)=X(2)-X(8)
      DPDX(3,5)=X(7)-X(1)
      DPDX(3,7)=X(5)-X(2)
      DPDX(3,8)=X(1)-X(4)
! --- CALCULATE DQDX
      DQDX(1,5)=X(12)-X(9)
      DQDX(1,6)=X(8)-X(11)
      DQDX(1,8)=X(6)-X(12)
      DQDX(1,9)=X(11)-X(5)
      DQDX(1,11)=X(9)-X(6)
      DQDX(1,12)=X(5)-X(8)
      DQDX(2,4)=X(9)-X(12)
      DQDX(2,6)=X(10)-X(7)
      DQDX(2,7)=X(12)-X(6)
      DQDX(2,9)=X(4)-X(10)
      DQDX(2,10)=X(6)-X(9)
      DQDX(2,12)=X(7)-X(4)
      DQDX(3,4)=X(11)-X(8)
      DQDX(3,5)=X(7)-X(10)
      DQDX(3,7)=X(5)-X(11)
      DQDX(3,8)=X(10)-X(4)
      DQDX(3,10)=X(8)-X(5)
      DQDX(3,11)=X(4)-X(7)

!       WRITE(*,*) 'DP/DX(1) '
!       WRITE(*,'(1X,12(F9.5,1X))') DPDX(1,:)
!       WRITE(*,*) 'DP/DX(2) '
!       WRITE(*,'(1X,12(F9.5,1X))') DPDX(2,:)
!       WRITE(*,*) 'DP/DX(3) '
!       WRITE(*,'(1X,12(F9.5,1X))') DPDX(3,:)
!       WRITE(*,*) 'DQ/DX(1) '
!       WRITE(*,'(1X,12(F9.5,1X))') DQDX(1,:)
!       WRITE(*,*) 'DQ/DX(2) '
!       WRITE(*,'(1X,12(F9.5,1X))') DQDX(2,:)
!       WRITE(*,*) 'DQ/DX(3) '
!       WRITE(*,'(1X,12(F9.5,1X))') DQDX(3,:)

! ---  CALCULATE DADX
      DO I=1,12
       DADX(I)=0.D0
       DBDX(I)=0.D0
       DCDX(I)=0.D0
       DO J=1,3
        DADX(I)=DADX(I)+2.D0*P(J)*DPDX(J,I)
        DBDX(I)=DBDX(I)+2.D0*Q(J)*DQDX(J,I)
        DCDX(I)=DCDX(I)+DPDX(J,I)*Q(J)+P(J)*DQDX(J,I)
       ENDDO
      ENDDO

! --- CALCULATE FIRST DERIVATIVES OF G
      DO I=1,12
       DGDX(I)=DADX(I)*QQ+PP*DBDX(I)
      ENDDO
! --- CALCULATE DDDX
      DO I=1,12
       DDDX(I)=ABYB2N*DGDX(I)
      ENDDO
! --- CALCULATE FIRST DERIVATIVES OF J
      PREFAC=1.D0/ABYB
      DO I=1,12
       DJDX(I)=(DCDX(I)*D-PQ*DDDX(I))*PREFAC
      ENDDO
! --- CALCULATE FIRST DERIVATIVE
      DO I=1,12
       DI(I)=DJDX(I)*V
      ENDDO

!        WRITE(*,*) 'PFL 1st DER TORSION'
!        WRITE(*,'(15(1X,F10.6))') DADX(:)
!        WRITE(*,'(15(1X,F10.6))') DBDX(:)
!        WRITE(*,'(15(1X,F10.6))') DCDX(:)
!        WRITE(*,'(15(1X,F10.6))') DGDX(:)
!        WRITE(*,'(15(1X,F10.6))') DDDX(:)
!        WRITE(*,'(15(1X,F10.6))') DJDX(:)
!        WRITE(*,'(15(1X,F10.6))') DI(:)


! -------------------------------------------------------------
! --- CALCULATE B
! -------------------------------------------------------------

      WRITE(*,*) 'DER TOR 1'
      WRITE(*,*) DI(1:3)
      WRITE(*,*) DI(4:6)
      WRITE(*,*) DI(7:9)
      WRITE(*,*) DI(10:12)
 
      DO I=1,3
         B(I,IAT1)=DI(I)
         B(I,IAT2)=DI(I+3)
         B(I,IAT3)=DI(I+6)
         B(I,IAT4)=DI(I+9)
      END DO
      END SUBROUTINE CONSTRAINTS_TORSION_DER


!     ..................................................................
      SUBROUTINE CONSTRAINTS_TORSION_DER3(NAT,IAT1,IAT2,IAT3,IAT4,R0,B)
!     ******************************************************************
!     **
!     **  CONSTRAINS THE TORSION OF IAT1-IAT2---IAT3-IAT4
!     **
!     ******************************************************************
      use VarTypes
      IMPLICIT none
      INTEGER(KINT),INTENT(IN) :: NAT
      INTEGER(KINT),INTENT(IN) :: IAT1
      INTEGER(KINT),INTENT(IN) :: IAT2
      INTEGER(KINT),INTENT(IN) :: IAT3
      INTEGER(KINT),INTENT(IN) :: IAT4
      REAL(KREAL)   ,INTENT(IN) :: R0(3,NAT)
      REAL(KREAL)   ,INTENT(OUT):: B(3,NAT)
      REAL(KREAL)               :: X(12),DI(12),DIDJ(12,12),B1(12)
      REAL(KREAL)               :: P(3),Q(3),T(3),U(3),R(3),W(3)
      REAL(KREAL)  :: Rij(3), rkj(3), rkl(3),rmj(3),rnk(3)
      REAL(KREAL)  :: Nrij, Nrkj, Nrkl, Nrmj, Nrnk
      REAL(KREAL) :: SignPhi
      real(KREAL)               :: phi
      integer(KINT)            :: i,j,ichk,k

      REAL(KREAL)   :: TORS, PP,QQ, PQ
      REAL(KREAL) :: DADX(12),DBDX(12),DCDX(12)
      REAL(KREAL) :: DDDX(12),DJDX(12),DVDX(12),DGDX(12),DPDX(3,12),DQDX(3,12)
      REAL(KREAL) :: D2QDX2(3,12,12),D2PDX2(3,12,12)
      REAL(KREAL) :: D2BDX2(12,12),D2ADX2(12,12),D2CDX2(12,12),D2DDX2(12,12)
      real(KREAL) :: d,coverd,abyb2n,abyb,ai,divdx,prefac,v

      LOGICAL :: Debug=.TRUE.

!     ******************************************************************
!
      if (debug)    WRITE(*,*) "============= Entering CONSTRAINTS_TORSION_DER3 ==============="

! --- TRANSFER R0 TO X ARRAY TO MAKE HANDLING AND PASSING EASIER
    
      DO I=1,3
        X(I)=R0(I,IAT1)
        X(I+3)=R0(I,IAT2)
        X(I+6)=R0(I,IAT3)
        X(I+9)=R0(I,IAT4)
      ENDDO

      if (debug) THEN
         WRITE(*,'(1X,I5,3(1X,F10.6))') IAt1,R0(:,IAT1)
         WRITE(*,'(1X,I5,3(1X,F10.6))') IAt2,R0(:,IAT2)
         WRITE(*,'(1X,I5,3(1X,F10.6))') IAt3,R0(:,IAT3)
         WRITE(*,'(1X,I5,3(1X,F10.6))') IAt4,R0(:,IAT4)
      END IF
!
      Rij=X(4:6)-X(1:3)
      Rkj=X(4:6)-X(7:9)
      Rkl=X(10:12)-X(7:9)
      
      Call VPRD(Rij,Rkj,Rmj)
      Call Vprd(Rkj,Rkl,Rnk)
      NRmj=DOT_PRODUCT(Rmj,Rmj)
      NRnk=DOT_PRODUCT(Rnk,Rnk)
      NRkj=sqrt(DOT_PRODUCT(Rkj,Rkj))

      Coverd=DOT_PRODUCT(Rmj,Rnk)/sqrt(NRmj*NRnk)
      CALL VPRD(Rmj,Rnk,T)
      SignPhi=Sign(1.d0,DOT_PRODUCT(Rkj,T))
!      SignPhi=Sign(1.d0,DOT_PRODUCT(Rkj,T))
!      WRITE(*,*) "DBG Tor 2 signphi, dacos(coverd), sign(1.d0,signphi)", &
!           signphi, dacos(coverd), sign(1.d0,signphi)
!
      TORS=-1.d0*DACOS(COVERD)*signPhi
      
      if (debug) WRITE(*,*) "Torsion derivs 3, Phi=", Tors*180./3.1415926,Iat1,Iat2,Iat3,Iat4
!
! ----------------------------------------------------------------
! --- CALCULATE FIRST DERIVATIVES
! ----------------------------------------------------------------
      DI(1:3)=NRkj/NRmj*Rmj
      DI(10:12)=-NRkj/NRnk*Rnk
      DI(4:6)=(DOT_PRODUCT(Rij,Rkj)/Nrkj**2-1.d0)*DI(1:3)-(DOT_PRODUCT(Rkl,Rkj)/NRkj**2)*DI(10:12)
      DI(7:9)=(DOT_PRODUCT(Rkl,Rkj)/Nrkj**2-1.d0)*DI(10:12)-(DOT_PRODUCT(Rij,Rkj)/NRkj**2)*DI(1:3)


      DI=DI*SignPhi

      if (debug) THEN
         WRITE(*,*) 'DER TOR 3'
         WRITE(*,*) DI(1:3)
         WRITE(*,*) DI(4:6)
         WRITE(*,*) DI(7:9)
         WRITE(*,*) DI(10:12)
      END IF
 
      DO I=1,3
         B(I,IAT1)=DI(I)
         B(I,IAT2)=DI(I+3)
         B(I,IAT3)=DI(I+6)
         B(I,IAT4)=DI(I+9)
      END DO

      if (debug)    WRITE(*,*) "============= CONSTRAINTS_TORSION_DER3  over ==============="

    END SUBROUTINE CONSTRAINTS_TORSION_DER3


!     ..................................................................
      SUBROUTINE CONSTRAINTS_TORSION_DER2(NAT,IAT1,IAT2,IAT3,IAT4,R0,DZDC)
!     ******************************************************************
!     **
!     **  CONSTRAINS THE TORSION OF IAT1-IAT2---IAT3-IAT4
!
!          corresponding to P - Q - R - S atoms iun the following
! This comes from ADF, and in turns comes from formulas from: "Vibrational States", S.Califano (Wiley, 1976)
    !       derivatives w.r.t P: (a*b)B/abs(a*b)**2)
    !       w.r.t.  S : (b*c)B/abs(b*c)**2
    !       w.r.t.  Q : 1/B**2  x ( C*dS + (a-b)*dP ) * b
    !       where * stands for vector product and capitals denote lengths
    !       -------------------------------------------------------------
    !       a=P-Q
    !       b=R-Q
    !       theta=angle(a,b)
    !       phi (negative of the dihedral angle)
    !       c=S-R

!     **
!     ******************************************************************
      use VarTypes
      IMPLICIT none
      INTEGER(KINT),INTENT(IN) :: NAT
      INTEGER(KINT),INTENT(IN) :: IAT1
      INTEGER(KINT),INTENT(IN) :: IAT2
      INTEGER(KINT),INTENT(IN) :: IAT3
      INTEGER(KINT),INTENT(IN) :: IAT4
      REAL(KREAL)   ,INTENT(IN) :: R0(3,NAT)
      REAL(KREAL)   ,INTENT(OUT):: DZDC(3,NAT)
      REAL(KREAL)               :: DI(12)
      REAL(KREAL)               :: A(3),B(3),C(3),Vec(3)
      REAL(KREAL)               :: U(3),V(3)
      real(KREAL)               :: phi
      integer(KINT)            :: i,j,ichk,k

      REAL(KREAL)   :: norm1,norm2,norm3
      real(KREAL) :: s,bleng

      REAL(KREAL), PARAMETER :: eps=1e-10

      LOGICAL :: Debug,Failed,valid
      EXTERNAL :: Valid

!     ******************************************************************
!
      debug=valid('TORSION_DER') 
      if (debug)    WRITE(*,*) "============= Entering CONSTRAINTS_TORSION_DER2 ==============="

      if (debug) THEN
         WRITE(*,'(1X,I5,3(1X,F10.6))') IAt1,R0(:,IAT1)
         WRITE(*,'(1X,I5,3(1X,F10.6))') IAt2,R0(:,IAT2)
         WRITE(*,'(1X,I5,3(1X,F10.6))') IAt3,R0(:,IAT3)
         WRITE(*,'(1X,I5,3(1X,F10.6))') IAt4,R0(:,IAT4)
      END IF
!
      A=R0(:,IAT1)-R0(:,IAT2)
      B=R0(:,IAT3)-R0(:,IAT2)
      C=R0(:,IAT4)-R0(:,IAT3)
      DZDC=0.
   
      
! ----------------------------------------------------------------
! --- We check that PQR and QRS are not aligned.
! If they are, we return 0. as derivatives instead of just crashing.
! ----------------------------------------------------------------
    call produit_vect (b(1),b(2),b(3),Norm1,c(1),c(2),c(3),Norm2, vec(1),vec(2),vec(3),Norm3)
     norm1=sqrt(dot_product(b,b))
     norm2=sqrt(dot_product(c,c))
    Failed=.FALSE.
     
     if (Norm3 <= eps) then
           WRITE(*,*) '*** WARNING: ILL-DEFINED DIHEDRAL ANGLE(S)'
           write (*,*) 'QRS aligned',iat2,iat3,iat4
           Failed=.TRUE.
     end if
     call produit_vect (b(1),b(2),b(3),Norm1,a(1),a(2),a(3),Norm2, vec(1),vec(2),vec(3),Norm3)
     if (norm3 <= eps) then
           WRITE(*,*) '*** WARNING: ILL-DEFINED DIHEDRAL ANGLE(S)'
           write (*,*) 'PQR aligned',iat1,iat2,iat3
           Failed=.TRUE.
     end if

! ----------------------------------------------------------------
! --- CALCULATE FIRST DERIVATIVES
! ----------------------------------------------------------------

     bleng = sqrt (b(1)**2 + b(2)**2 + b(3)**2)
     call produit_vect (a(1),a(2),a(3),Norm1,b(1),b(2),b(3),Norm2, vec(1),vec(2),vec(3),Norm3)
     s = bleng / (vec(1)**2 + vec(2)**2 + vec(3)**2)
!     dzdc(:,ip,3,ip) = s * vec
      dzdc(:,iat1) = s * vec

     call produit_vect (b(1),b(2),b(3),Norm1,c(1),c(2),c(3),Norm2, vec(1),vec(2),vec(3),Norm3)
     s = bleng / (vec(1)**2 + vec(2)**2 + vec(3)**2)
!    dzdc(:,is,3,ip) = s * vec
     dzdc(:,iat4) = s * vec


     call produit_vect (c(1),c(2),c(3),Norm1,    &
          dzdc(1,iat4),  dzdc(2,iat4),  dzdc(3,iat4), Norm2, &
          u(1),u(2),u(3),Norm3)
     vec = a - b
     call produit_vect (vec(1),vec(2),vec(3),Norm1,    &
          dzdc(1,iat1),  dzdc(2,iat1),  dzdc(3,iat1), Norm2, &
          v(1),v(2),v(3),Norm3)
     vec = u + v
     call produit_vect (vec(1),vec(2),vec(3),Norm1,    &
          b(1), b(2), b(3), Norm2, &
          u(1),u(2),u(3),Norm3)

!     dzdc(:,iq,3,ip) = u / bleng**2
!     dzdc(:,ir,3,ip) = - (dzdc(:,ip,3,ip) + dzdc(:,iq,3,ip) + dzdc(:,is,3,ip))
     dzdc(:,iat2) = u / bleng**2
     dzdc(:,iat3) = - (dzdc(:,iat1) + dzdc(:,iat2) + dzdc(:,iat4))

      if (debug) THEN
         WRITE(*,*) 'DER TOR 2'
         WRITE(*,*) DZDC(:,IAT1)
         WRITE(*,*) DZDC(:,IAT2)
         WRITE(*,*) DZDC(:,IAT3)
         WRITE(*,*) DZDC(:,IAT4)
      END IF
 
      if (Failed) DZDC=0.

      if (debug)    WRITE(*,*) "============= CONSTRAINTS_TORSION_DER2  over ==============="

    END SUBROUTINE CONSTRAINTS_TORSION_DER2

! Not yet implemented


! !     ..................................................................
!       SUBROUTINE CONSTRAINTS_COGSEP_DER(NAT,GROUP1,GROUP2,R0,RMass,B)
! !     ******************************************************************
! !     **  SET UP CONSTRAINT IN COORDINATE SPACE                       **
! !     **  CONSTRAINT IS THE DISTANCE BETWEEN THE CENTER OF GRAVITIES  **
! !     **  OF TWO NAMED GROUPS                                         **
! !     **                                                              **
! !     **  THE CONSTRAINT IS EXPRESSED AS A+B*R+0.5*R*C*R=0            **
! !     **                                                              **
! !     ******************************************************************
!        use Geometry_module

!        Use VarTypes
!       IMPLICIT NONE
!       INTEGER(KINT)  ,INTENT(IN) :: NAT
!       CHARACTER(*),INTENT(IN) :: GROUP1  ! NAME OF FIRST GROUP
!       CHARACTER(*),INTENT(IN) :: GROUP2  ! NAME OF SECOND GROUP
!       REAL(KREAL)     ,INTENT(OUT):: B(3,NAT)
!       REAL(KREAL)     ,INTENT(IN) :: R0(3,NAT)
!       REAL(KREAL)                 :: RMASS(NAT),A
!       LOGICAL(4)              :: TMEMBER1(NAT)
!       LOGICAL(4)              :: TMEMBER2(NAT)
!       REAL(KREAL)                 :: SUMMASS1,SUMMASS2
!       REAL(KREAL)                 :: COG1(3)   ! COG OF FIRST GROUP
!       REAL(KREAL)                 :: COG2(3)   ! COG OF SECOND GROUP
!       REAL(KREAL)                 :: X(3)      ! COG1-COG2
!       REAL(KREAL)                 :: COGSEP    
!       REAL(KREAL)                 :: DADX(3)   
!       INTEGER(KINT)              :: I,J,IAT,IAT1,IAT2
! !     ******************************************************************

!       CALL GROUPLIST_MEMBERS(GROUP1,TMEMBER1)
!       CALL GROUPLIST_MEMBERS(GROUP2,TMEMBER2)
! !
! !     ==================================================================
! !     ==  CALCULATE CENTERS OF GRAVITY                                ==
! !     ==================================================================
!       SUMMASS1=0.D0
!       SUMMASS2=0.D0
!       COG1(:)=0.D0
!       COG2(:)=0.D0
!       DO IAT=1,NAT
!         IF(TMEMBER1(IAT)) THEN
!           SUMMASS1=SUMMASS1+RMASS(IAT)
!           COG1(:)=COG1(:)+RMASS(IAT)*R0(:,IAT)
!         ENDIF
!         IF(TMEMBER2(IAT)) THEN
!           SUMMASS2=SUMMASS2+RMASS(IAT)
!           COG2(:)=COG2(:)+RMASS(IAT)*R0(:,IAT)
!         ENDIF
!       ENDDO
!       COG1(:)=COG1(:)/SUMMASS1
!       COG2(:)=COG2(:)/SUMMASS2
! !
! !     =============================================================
! !     ==  SEPARATION BETWEEN THE CENTERS OF GRAVITY              ==
! !     =============================================================
!       X(:)=COG1(:)-COG2(:)
! !
! !     =============================================================
! !     ==  CALCULATE A(X) AND ITS DERIVATIVES                     ==
! !     =============================================================
!       A=SQRT(DOT_PRODUCT(X(:),X(:)))
!       DADX(:)=X(:)/A

!       DO IAT1=1,NAT
!          IF(TMEMBER1(IAT1)) THEN
!             B(:,IAT1)=B(:,IAT1)+DADX(:)*RMASS(IAT1)/SUMMASS1
!          END IF
!          IF(TMEMBER2(IAT1)) THEN
!             B(:,IAT1)=B(:,IAT1)-DADX(:)*RMASS(IAT1)/SUMMASS2
!          END IF
!       END DO

!       END SUBROUTINE CONSTRAINTS_COGSEP_DER


! !     ..................................................................
!       SUBROUTINE CONSTRAINTS_COGANG_DER(NAT,GROUP1,GROUP2,GROUP3,R0,RMass,B)
! !     ******************************************************************
! !     **  SET UP CONSTRAINT IN COORDINATE SPACE                       **
! !     **  CONSTRAINT IS THE ANGLE    BETWEEN THE CENTER OF GRAVITIES  **
! !     **  OF THREE NAMED GROUPS                                       **
! !     **                                                              **
! !     **  THE CONSTRAINT IS EXPRESSED AS A+B*R+0.5*R*C*R=0            **
! ! This makes heavy use of BONDANGLEDERIVS
! !     **                                                              **
! !     ******************************************************************
!       IMPLICIT NONE
!       INTEGER(KINT)  ,INTENT(IN) :: NAT
!       CHARACTER(*),INTENT(IN) :: GROUP1  ! NAME OF FIRST GROUP
!       CHARACTER(*),INTENT(IN) :: GROUP2  ! NAME OF SECOND GROUP
!       CHARACTER(*),INTENT(IN) :: GROUP3  ! NAME OF THIRD GROUP
!       REAL(KREAL)     ,INTENT(OUT):: B(3,NAT)
!       REAL(KREAL)     ,INTENT(IN) :: R0(3,NAT)
!       REAL(KREAL)                 :: RMASS(NAT)
!       LOGICAL(4)              :: TMEMBER1(NAT)
!       LOGICAL(4)              :: TMEMBER2(NAT)
!       LOGICAL(4)              :: TMEMBER3(NAT)
!       REAL(KREAL)                 :: SUMMASS1,SUMMASS2, SUMMASS3
!       REAL(KREAL)                 :: COG1(3)   ! COG OF FIRST GROUP
!       REAL(KREAL)                 :: COG2(3)   ! COG OF SECOND GROUP
!       REAL(KREAL)                 :: COG3(3)   ! COG OF THIRD GROUP
!       REAL(KREAL)            :: DCOG1(3,NAT), DCOG2(3,NAT), DCOG3(3,NAT)
!       REAL(KREAL)                 :: vecU(3), vecV(3)
!       REAL(KREAL)                 :: UU, VV, UV
!       INTEGER(KINT)              :: I,J,IAT,IAT1,IAT2,JAT
!       REAL(KREAL)           :: Pi
! ! From BONDANGLEDERIVS
!       REAL(KREAL)               :: D    ! 1/SQRT(A*B)
!       REAL(KREAL)               :: DUUDX(3,NAT)
!       REAL(KREAL)               :: DVVDX(3,NAT)
!       REAL(KREAL)               :: DUVDX(3,NAT)
!       REAL(KREAL)               :: DDDX(3,NAT)
!       REAL(KREAL)               :: DJDX(3,NAT)
!       REAL(KREAL)               :: DVDX(3,NAT)
!       REAL(KREAL)               :: DGDX(3,NAT)    ! D(A*B)/DX
!       REAL(KREAL)               :: AB,AB2N,V,PREFAC
!       REAL(KREAL)               :: PHI,COSPHI,SINPHI
!       REAL(KREAL)               :: DIVDX,AI


! !     ******************************************************************
!       CALL GROUPLIST_MEMBERS(GROUP1,TMEMBER1)
!       CALL GROUPLIST_MEMBERS(GROUP2,TMEMBER2)
!       CALL GROUPLIST_MEMBERS(GROUP3,TMEMBER3)
! !
! !     ==================================================================
! !     ==  CALCULATE CENTERS OF GRAVITY                                ==
! !     ==================================================================
!       SUMMASS1=0.D0
!       SUMMASS2=0.D0
!       SUMMASS3=0.D0
!       COG1(:)=0.D0
!       COG2(:)=0.D0
!       COG3(:)=0.D0
!       DO IAT=1,NAT
!         IF(TMEMBER1(IAT)) THEN
!           SUMMASS1=SUMMASS1+RMASS(IAT)
!           COG1(:)=COG1(:)+RMASS(IAT)*R0(:,IAT)
!         ENDIF
!         IF(TMEMBER2(IAT)) THEN
!           SUMMASS2=SUMMASS2+RMASS(IAT)
!           COG2(:)=COG2(:)+RMASS(IAT)*R0(:,IAT)
!         ENDIF
!         IF(TMEMBER3(IAT)) THEN
!           SUMMASS3=SUMMASS3+RMASS(IAT)
!           COG3(:)=COG3(:)+RMASS(IAT)*R0(:,IAT)
!         ENDIF

!       ENDDO
!       COG1(:)=COG1(:)/SUMMASS1
!       COG2(:)=COG2(:)/SUMMASS2
!       COG3(:)=COG3(:)/SUMMASS3
! !
! !
!       vecU(:)=COG1(:)-COG2(:)
!       vecV(:)=COG3(:)-COG2(:)

!       UV=DOT_PRODUCT(vecU,vecV)
!       UU=DOT_PRODUCT(vecU,vecU)
!       VV=DOT_PRODUCT(vecV,vecV)
! ! We now calculate the first derivatives of the COG vs the atomic
! ! Coords... 
!       DCOG1=0.
!       DCOG2=0.
!       DCOG3=0.
!       DO IAT=1,NAT
!         IF (TMEMBER1(IAT))  DCOG1(:,IAT)=RMASS(IAT)/SUMMASS1
!         IF (TMEMBER2(IAT))  DCOG2(:,IAT)=RMASS(IAT)/SUMMASS2
!         IF (TMEMBER3(IAT))  DCOG3(:,IAT)=RMASS(IAT)/SUMMASS3
!       END DO
! ! We now calculate the first derivatives of UU, VV, UV
!       DUUDX=0.
!       DVVDX=0.
!       DUVDX=0.
!       DO IAT=1,NAT
!         DUUDX(:,IAT)=2.D0*(COG1(:)-COG2(:))*(DCOG1(:,IAT)-DCOG2(:,IAT))
!         DVVDX(:,IAT)=2.D0*(COG3(:)-COG2(:))*(DCOG3(:,IAT)-DCOG2(:,IAT))
!         DUVDX(:,IAT)=(COG3(:)-COG2(:))*(DCOG1(:,IAT)-DCOG2(:,IAT)) +    &
!         (DCOG3(:,IAT)-DCOG2(:,IAT))*(COG1(:)-COG2(:))
!       END DO

! !       WRITE(*,*) 'PFL DER COGANGLE'
! !       WRITE(*,'(15(1X,F10.6))') DUUDX(:,:)
! !       WRITE(*,'(15(1X,F10.6))') DVVDX(:,:)
! !       WRITE(*,'(15(1X,F10.6))') DUVDX(:,:)


! ! We now go for the hard part: calculate Phi, dPhi/dx, d2Phi/dxidxj
!       Pi=DACOS(-1.D0)
!       AB=UU*VV
!       D=SQRT(AB)
!       COSPHI=UV/D       !=COS(PHI)
!       PHI=DACOS(COSPHI)
! !      WRITE(*,*) 'COGANGLE Phi=',Phi,Phi*180/Pi
! !     --- CALCULATE DERIVATIVE OF ARCCOS
!       SINPHI=DSQRT(1.D0-COSPHI**2)
!       V=-1.D0/SINPHI                 !=-SIN(PHI)=D(ACOS(PHI))/D(COSPHI)
! ! --- CALCULATE FIRST DERIVATIVES OF G=A*B
!       DGDX(:,:)=DUUDX(:,:)*VV+DVVDX(:,:)*UU
! ! --- CALCULATE DDDX
!       AB2N=0.5D0/D
!       DDDX(:,:)=AB2N*DGDX(:,:)
! ! --- CALCULATE FIRST DERIVATIVES OF J = COS(PHI)
!       PREFAC=1.D0/AB
!       DJDX(:,:)=(DUVDX(:,:)*D-UV*DDDX(:,:))*PREFAC
! ! --- CALCULATE FIRST DERIVATIVE
!       B(:,:)=DJDX(:,:)*V

!       END SUBROUTINE CONSTRAINTS_COGANG_DER

! !     ..................................................................
!       SUBROUTINE CONSTRAINTS_COGTORSION_DER(NAT,GROUP1,GROUP2, GROUP3,GROUP4, R0,RMAss,B)
! !     ******************************************************************
! !     **  SET UP CONSTRAINT IN COORDINATE SPACE                       **
! !     **  CONSTRAINT IS THE ANGLE    BETWEEN THE CENTER OF GRAVITIES  **
! !     **  OF THREE NAMED GROUPS                                       **
! !     **                                                              **
! !     **  THE CONSTRAINT IS EXPRESSED AS A+B*R+0.5*R*C*R=0            **
! ! This makes heavy use of BONDANGLEDERIVS
! !     **                                                              **
! !     ******************************************************************
!       IMPLICIT NONE
!       INTEGER(KINT)  ,INTENT(IN) :: NAT
!       CHARACTER(*),INTENT(IN) :: GROUP1  ! NAME OF FIRST GROUP
!       CHARACTER(*),INTENT(IN) :: GROUP2  ! NAME OF SECOND GROUP
!       CHARACTER(*),INTENT(IN) :: GROUP3  ! NAME OF THIRD GROUP
!       CHARACTER(*),INTENT(IN) :: GROUP4  ! NAME OF FOURTH GROUP
!       REAL(KREAL)     ,INTENT(OUT):: B(3,NAT)
!       REAL(KREAL)     ,INTENT(IN) :: R0(3,NAT)
!       REAL(KREAL)                 :: RMASS(NAT)
!       LOGICAL(4)              :: TMEMBER1(NAT)
!       LOGICAL(4)              :: TMEMBER2(NAT)
!       LOGICAL(4)              :: TMEMBER3(NAT)
!       LOGICAL(4)              :: TMEMBER4(NAT)
!       REAL(KREAL)                 :: SUMMASS1,SUMMASS2, SUMMASS3, SUMMASS4
!       REAL(KREAL)                 :: COG1(3)   ! COG OF FIRST GROUP
!       REAL(KREAL)                 :: COG2(3)   ! COG OF SECOND GROUP
!       REAL(KREAL)                 :: COG3(3)   ! COG OF THIRD GROUP
!       REAL(KREAL)                 :: COG4(3)   ! COG OF FOURTH GROUP
! ! Even thought D COG(i)/DXj =0 (ie COG_x depends only on x-coordinates
! ! of the atoms, not on y or z), we store a 3,3*NAT matrix to make 
! ! handling easier below
!       REAL(KREAL)                 :: DCOG1(3,3,NAT), DCOG2(3,3,NAT),              &
!                                 DCOG3(3,3,NAT), DCOG4(3,3,NAT)
!       REAL(KREAL)                 :: T(3), U(3), R(3), W(3), P(3), Q(3)
!       REAL(KREAL)                 :: PP, QQ, PQ
!       INTEGER(KINT)              :: I,J,IAT,IAT1,IAT2,JAT
!       REAL(KREAL)                 :: Pi, Phi
! ! From BONDANGLEDERIVS
!       REAL(KREAL)               :: D    ! SQRT(PP*QQ)
!       REAL(KREAL)               :: DPDX(3,3,NAT)
!       REAL(KREAL)               :: DQDX(3,3,NAT)
!       REAL(KREAL)               :: DPPDX(3,NAT)
!       REAL(KREAL)               :: DQQDX(3,NAT)
!       REAL(KREAL)               :: DPQDX(3,NAT)
!       REAL(KREAL)               :: DDDX(3,NAT)
!       REAL(KREAL)               :: DJDX(3,NAT) ! D(C/D)/DX*D2
!       REAL(KREAL)               :: DVDX(3,NAT)
!       REAL(KREAL)               :: DGDX(3,NAT)    ! D(A*B)/DX
!       REAL(KREAL)               :: AB,AB2N,V,PREFAC,ABYB,ABYB2N
!       REAL(KREAL)               :: COSPHI,SINPHI


! !     ******************************************************************

! !      WRITE(*,*) 'ENTERING SET_COGTORSION'
! !      WRITE(*,*) 'G1:',TRIM(GROUP1),' G2:',TRIM(GROUP2),' G3:',TRIM(GROUP3),' G4:',TRIM(GROUP4)

! !     ==================================================================
! !     == DETERMINE GROUPS AND ATOM-MASSES
! !     ==================================================================

!       CALL GROUPLIST_MEMBERS(GROUP1,TMEMBER1)
!       CALL GROUPLIST_MEMBERS(GROUP2,TMEMBER2)
!       CALL GROUPLIST_MEMBERS(GROUP3,TMEMBER3)
!       CALL GROUPLIST_MEMBERS(GROUP4,TMEMBER4)

! !
! !     ==================================================================
! !     == CALCULATE CENTER OF GRAVITY FOR GROUPS
! !     ==================================================================
!       SUMMASS1=0.D0
!       SUMMASS2=0.D0
!       SUMMASS3=0.D0
!       SUMMASS4=0.D0

!       COG1=0.D0
!       COG2=0.D0
!       COG3=0.D0
!       COG4=0.D0

!       DO IAT=1,NAT
! ! One atom can belong to many groups as we are looking at
! ! center of gravity of the groups
!         IF (TMEMBER1(IAT)) THEN
!           SUMMASS1=SUMMASS1+RMASS(IAT)
!           COG1(:)=COG1(:)+RMASS(IAT)*R0(:,IAT)
!         END IF
!         IF (TMEMBER2(IAT)) THEN
!           SUMMASS2=SUMMASS2+RMASS(IAT)
!           COG2(:)=COG2(:)+RMASS(IAT)*R0(:,IAT)
!         END IF
!         IF (TMEMBER3(IAT)) THEN
!           SUMMASS3=SUMMASS3+RMASS(IAT)
!           COG3(:)=COG3(:)+RMASS(IAT)*R0(:,IAT)
!         END IF
!         IF (TMEMBER4(IAT)) THEN
!           SUMMASS4=SUMMASS4+RMASS(IAT)
!           COG4(:)=COG4(:)+RMASS(IAT)*R0(:,IAT)
!         END IF
!       ENDDO

!       COG1(:)=COG1(:)/SUMMASS1
!       COG2(:)=COG2(:)/SUMMASS2
!       COG3(:)=COG3(:)/SUMMASS3
!       COG4(:)=COG4(:)/SUMMASS4
!       T(:)=COG1(:)-COG2(:)
!       U(:)=COG3(:)-COG2(:)
!       R(:)=COG4(:)-COG3(:)
!       W(:)=COG2(:)-COG3(:)

!       CALL VPRD(T,U,P)                     ! P=CROSS-PRODUCT(T,U)
!       CALL VPRD(W,R,Q)
!       PQ=DOT_PRODUCT(P,Q)
!       PP=DOT_PRODUCT(P,P)
!       QQ=DOT_PRODUCT(Q,Q)
!       ABYB=PP*QQ
!       D=SQRT(ABYB)

!       ABYB2N=0.5D0/D
!       COSPHI=PQ/D
!       Phi=DACOS(COSPHI)
!       Pi=DACOS(-1.0D0)
!       SINPHI=DSQRT(1.D0-(COSPHI)**2.D0)

! ! --- CALCULATE DERIVATIVE OF ARCCOS
!       IF (dabs(DABS(COSPHI)-1.d0) < 1.D-9) THEN
!         WRITE(*,*)   'WARNING: DERIVATIVE OF TORSION AT ZERO AND PI IS 
!      &UNDEFINED!'
!         V=0.D0
!       ELSE
!         V=-1.D0/SINPHI
!       ENDIF

! !      WRITE(*,*)  'Phi et al',Phi,Phi*180./Pi,cosphi,sinphi,V

! ! We now calculate the first derivatives of the COG vs the atomic
! ! Coords... 
!       DCOG1=0.
!       DCOG2=0.
!       DCOG3=0.
!       DCOG4=0.
!       DO IAT=1,NAT
!         IF (TMEMBER1(IAT))  THEN
!            DO J=1,3
!               DCOG1(J,J,IAT)=RMASS(IAT)/SUMMASS1
!            END DO
!         END IF
!         IF (TMEMBER2(IAT))  THEN
!            DO J=1,3
!               DCOG2(J,J,IAT)=RMASS(IAT)/SUMMASS2
!            END DO
!         END IF
!         IF (TMEMBER3(IAT))  THEN
!            DO J=1,3
!               DCOG3(J,J,IAT)=RMASS(IAT)/SUMMASS3
!            END DO
!         END IF
!         IF (TMEMBER4(IAT))  THEN
!            DO J=1,3
!               DCOG4(J,J,IAT)=RMASS(IAT)/SUMMASS4
!            END DO
!         END IF
!       END DO

! ! We now calculate the first derivatives of P, Q
!       DPDX=0.
!       DQDX=0.
!       DO I=1,NAT
!          DO J=1,3
!             DPDX(1,J,I)=(DCOG1(2,J,I)-DCOG2(2,J,I))*(COG3(3)-COG2(3))     &
!      &                 +(COG1(2)-COG2(2))*(DCOG3(3,J,I)-DCOG2(3,J,I))     &
!      &                 -(DCOG1(3,J,I)-DCOG2(3,J,I))*(COG3(2)-COG2(2))     &
!      &                 -(COG1(3)-COG2(3))*(DCOG3(2,J,I)-DCOG2(2,J,I)) 
!             DPDX(2,J,I)=(DCOG1(3,J,I)-DCOG2(3,J,I))*(COG3(1)-COG2(1))     &
!      &                 +(COG1(3)-COG2(3))*(DCOG3(1,J,I)-DCOG2(1,J,I))     &
!      &                 -(DCOG1(1,J,I)-DCOG2(1,J,I))*(COG3(3)-COG2(3))     &
!      &                 -( COG1(1)-COG2(1))*(DCOG3(3,J,I)-DCOG2(3,J,I)) 
!             DPDX(3,J,I)=(DCOG1(1,J,I)-DCOG2(1,J,I))*(COG3(2)-COG2(2))     &
!      &                 +(COG1(1)-COG2(1))*(DCOG3(2,J,I)-DCOG2(2,J,I))     &
!      &                 -(DCOG1(2,J,I)-DCOG2(2,J,I))*(COG3(1)-COG2(1))     &
!      &                 -(COG1(2)-COG2(2))*(DCOG3(1,J,I)-DCOG2(1,J,I)) 
!             DQDX(1,J,I)=(DCOG2(2,J,I)-DCOG3(2,J,I))*(COG4(3)-COG3(3))     &
!      &                 +(COG2(2)-COG3(2))*(DCOG4(3,J,I)-DCOG3(3,J,I))     &
!      &                 -(DCOG2(3,J,I)-DCOG3(3,J,I))*(COG4(2)-COG3(2))     &
!      &                 -(COG2(3)-COG3(3))*(DCOG4(2,J,I)-DCOG3(2,J,I)) 
!             DQDX(2,J,I)=(DCOG2(3,J,I)-DCOG3(3,J,I))*(COG4(1)-COG3(1))     &
!      &                 +(COG2(3)-COG3(3))*(DCOG4(1,J,I)-DCOG3(1,J,I))     &
!      &                 -(DCOG2(1,J,I)-DCOG3(1,J,I))*(COG4(3)-COG3(3))     &
!      &                 -( COG2(1)-COG3(1))*(DCOG4(3,J,I)-DCOG3(3,J,I)) 
!             DQDX(3,J,I)=(DCOG2(1,J,I)-DCOG3(1,J,I))*(COG4(2)-COG3(2))     &
!      &                 +(COG2(1)-COG3(1))*(DCOG4(2,J,I)-DCOG3(2,J,I))     &
!      &                 -(DCOG2(2,J,I)-DCOG3(2,J,I))*(COG4(1)-COG3(1))     &
!      &                 -(COG2(2)-COG3(2))*(DCOG4(1,J,I)-DCOG3(1,J,I)) 

!          END DO
!       END DO


! !       WRITE(*,*) 'DP/DX(1) '
! !       WRITE(*,'(1X,12(F9.5,1X))') DPDX(1,1:3,5:8)
! !       WRITE(*,*) 'DP/DX(2) '
! !       WRITE(*,'(1X,12(F9.5,1X))') DPDX(2,1:3,5:8)
! !       WRITE(*,*) 'DP/DX(3) '
! !       WRITE(*,'(1X,12(F9.5,1X))') DPDX(3,1:3,5:8)
! !       WRITE(*,*) 'DQ/DX(1) '
! !       WRITE(*,'(1X,12(F9.5,1X))') DQDX(1,1:3,5:8)
! !       WRITE(*,*) 'DQ/DX(2) '
! !       WRITE(*,'(1X,12(F9.5,1X))') DQDX(2,1:3,5:8)
! !       WRITE(*,*) 'DQ/DX(3) '
! !       WRITE(*,'(1X,12(F9.5,1X))') DQDX(3,1:3,5:8)

! ! We now calculate the first derivatives of PP, QQ and PQ and DD/DX*2D
      
!       DPPDX=0.
!       DQQDX=0.
!       DPQDX=0.
!       PREFAC=1.d0/ABYB
!       DO IAT=1,NAT
!          DO J=1,3
!             DPPDX(J,IAT)=2.D0*(P(1)*DPDX(1,J,IAT)+P(2)*DPDX(2,J,IAT)+
!      &                P(3)*DPDX(3,J,IAT))
!             DQQDX(J,IAT)=2.D0*(Q(1)*DQDX(1,J,IAT)+Q(2)*DQDX(2,J,IAT)+ 
!      &                Q(3)*DQDX(3,J,IAT))
!             DPQDX(J,IAT)=P(1)*DQDX(1,J,IAT)+DPDX(1,J,IAT)*Q(1)  
!      &               +P(2)*DQDX(2,J,IAT)+DPDX(2,J,IAT)*Q(2)
!      &               +P(3)*DQDX(3,J,IAT)+DPDX(3,J,IAT)*Q(3) 
!             DGDX(J,IAT)=DPPDX(J,IAT)*QQ+PP*DQQDX(J,IAT)
!             DDDX(J,IAT)=DGDX(J,IAT)*ABYB2N
!             DJDX(J,IAT)=(DPQDX(J,IAT)*D-PQ*DDDX(J,IAT))*PREFAC
!             B(J,IAT)=DJDX(J,IAT)*V
!          END DO
!       END DO
      
!       END SUBROUTINE CONSTRAINTS_COGTORSION_DER


! !     ..................................................................
!       SUBROUTINE CONSTRAINTS_COGDIFF_DER(NAT,GROUP1,GROUP2,   
!      &    GROUP3,R0,RMAss,B)
! !     ******************************************************************
! !     **  SET UP CONSTRAINT IN COORDINATE SPACE                       **
! !     **  CONSTRAINT IS THE DIFFERENCE BETWEEN THE DISTANCES BETWEEN  **
! !     **  THE CENTER OF GRAVITIES  OF THREE NAMED GROUPS              **
! !     **                                                              **
! !     **  THE CONSTRAINT IS EXPRESSED AS A+B*R+0.5*R*C*R=0            **
! !     **                                                              **
! !     ******************************************************************
!       IMPLICIT NONE
!       INTEGER(KINT)  ,INTENT(IN) :: NAT
!       CHARACTER(*),INTENT(IN) :: GROUP1  ! NAME OF FIRST GROUP
!       CHARACTER(*),INTENT(IN) :: GROUP2  ! NAME OF SECOND GROUP
!       CHARACTER(*),INTENT(IN) :: GROUP3  ! NAME OF THIRD GROUP
!       REAL(KREAL)     ,INTENT(OUT):: B(3,NAT)
!       REAL(KREAL)     ,INTENT(IN) :: R0(3,NAT)
!       REAL(KREAL)     ,INTENT(IN) :: RMASS(NAT)
!       LOGICAL(4)              :: TMEMBER1(NAT)
!       LOGICAL(4)              :: TMEMBER2(NAT)
!       LOGICAL(4)              :: TMEMBER3(NAT)
!       REAL(KREAL)                 :: SUMMASS1,SUMMASS2, SUMMASS3
!       REAL(KREAL)                 :: COG1(3)   ! COG OF FIRST GROUP
!       REAL(KREAL)                 :: COG2(3)   ! COG OF SECOND GROUP
!       REAL(KREAL)                 :: COG3(3)   ! COG OF THIRD GROUP
!       REAL(KREAL)            :: DCOG1(3,NAT), DCOG2(3,NAT), DCOG3(3,NAT)
!       REAL(KREAL)                 :: vecU(3), vecV(3)
!       REAL(KREAL)                 :: UU, VV, UV
!       INTEGER(KINT)              :: I,J,IAT,IAT1,IAT2,JAT
!       REAL(KREAL)           :: Pi
! ! From BONDANGLEDERIVS
!       REAL(KREAL)               :: D1,D2 
!       REAL(KREAL)               :: DUUDX(3,NAT)
!       REAL(KREAL)               :: DVVDX(3,NAT)
!       REAL(KREAL)               :: DUVDX(3,NAT)
!       REAL(KREAL)               :: DDDX(3,NAT)
!       REAL(KREAL)               :: DJDX(3,NAT)
!       REAL(KREAL)               :: DVDX(3,NAT)
!       REAL(KREAL)               :: DGDX(3,NAT)    ! D(A*B)/DX
!       REAL(KREAL)               :: DIDJ(3,NAT,3,NAT), DI(3,NAT)
!       REAL(KREAL)               :: AB,AB2N,V,PREFAC
!       REAL(KREAL)               :: PHI,COSPHI,SINPHI
!       REAL(KREAL)               :: DIVDX,AI


! !     ******************************************************************

!       CALL GROUPLIST_MEMBERS(GROUP1,TMEMBER1)
!       CALL GROUPLIST_MEMBERS(GROUP2,TMEMBER2)
!       CALL GROUPLIST_MEMBERS(GROUP3,TMEMBER3)
! !
! !     ==================================================================
! !     ==  CALCULATE CENTERS OF GRAVITY                                ==
! !     ==================================================================
!       SUMMASS1=0.D0
!       SUMMASS2=0.D0
!       SUMMASS3=0.D0
!       COG1(:)=0.D0
!       COG2(:)=0.D0
!       COG3(:)=0.D0
!       DO IAT=1,NAT
!         IF(TMEMBER1(IAT)) THEN
!           SUMMASS1=SUMMASS1+RMASS(IAT)
!           COG1(:)=COG1(:)+RMASS(IAT)*R0(:,IAT)
!         ENDIF
!         IF(TMEMBER2(IAT)) THEN
!           SUMMASS2=SUMMASS2+RMASS(IAT)
!           COG2(:)=COG2(:)+RMASS(IAT)*R0(:,IAT)
!         ENDIF
!         IF(TMEMBER3(IAT)) THEN
!           SUMMASS3=SUMMASS3+RMASS(IAT)
!           COG3(:)=COG3(:)+RMASS(IAT)*R0(:,IAT)
!         ENDIF

!       ENDDO
!       COG1(:)=COG1(:)/SUMMASS1
!       COG2(:)=COG2(:)/SUMMASS2
!       COG3(:)=COG3(:)/SUMMASS3
! !
! !
!       vecU(:)=COG1(:)-COG2(:)
!       vecV(:)=COG3(:)-COG2(:)

!       UU=DOT_PRODUCT(vecU,vecU)
!       VV=DOT_PRODUCT(vecV,vecV)
! ! We now calculate the first derivatives of the COG vs the atomic
! ! Coords... 
!       DCOG1=0.
!       DCOG2=0.
!       DCOG3=0.
!       DO IAT=1,NAT
!         IF (TMEMBER1(IAT))  DCOG1(:,IAT)=RMASS(IAT)/SUMMASS1
!         IF (TMEMBER2(IAT))  DCOG2(:,IAT)=RMASS(IAT)/SUMMASS2
!         IF (TMEMBER3(IAT))  DCOG3(:,IAT)=RMASS(IAT)/SUMMASS3
!       END DO
! ! We now calculate the first derivatives of UU, VV
!       DUUDX=0.
!       DVVDX=0.
!       DO IAT=1,NAT
!         DUUDX(:,IAT)=2.D0*(COG1(:)-COG2(:))*(DCOG1(:,IAT)-DCOG2(:,IAT))
!         DVVDX(:,IAT)=2.D0*(COG3(:)-COG2(:))*(DCOG3(:,IAT)-DCOG2(:,IAT))
!       END DO
! !       WRITE(*,*) 'PFL DER COGDIFF'
! !       WRITE(*,'(15(1X,F10.6))') DUUDX(:,:)
! !       WRITE(*,'(15(1X,F10.6))') DVVDX(:,:)

! ! We now use the fact that Sigma=sqrt(UU) - sqrt(VV) to calculate the 
! ! first  derivatives of Sigma along R

! ! Calculate the first derivatives and second derivatives.
!       D1=dsqrt(UU)
!       D2=dsqrt(VV)
!       B(:,:)=0.5d0*( DUUDX(:,:)/D1 - DVVDX(:,:)/D2 )

!       END SUBROUTINE CONSTRAINTS_COGDIFF_DER

