MODULE CoordinatesAndVelocities

   USE Constants

  IMPLICIT NONE 

   REAL, DIMENSION(4,3), SAVE :: Coordinates, Velocities    ! Arrays containing initial conditions for one traj
   REAL, DIMENSION(4,3)       :: EquilibriumCoordinates     ! Equilibrium Coordinates of CHD3
   REAL, DIMENSION(4,3)       :: EquilibriumCOMCoordinates  ! Equilibrium Coordinates of CHD3 in COM frame
   LOGICAL                    :: EquilibriumCOMComputed = .FALSE.  
   REAL, PARAMETER            :: TotMass = MN + 3.*MH  ! Molecular mass (amu)
   REAL, DIMENSION(4)         :: Masses  = (/ MN, MH, MH, MH/) ! Array with atomic masses (amu)
  
     CONTAINS 
!  ------------------------------------------------------------------------------------------------
   SUBROUTINE InitializeMoleculeInCOMCoordinates
!  Initialize the molecule in the COM frame, with C-H pointing towards positive  z-axis
   IMPLICIT NONE

   INTEGER              :: i,l
   REAL, DIMENSION(3)   :: COM

!  Compute COM for equilibrium geometry only once
   IF(.NOT.EquilibriumCOMComputed) THEN
     CALL FindCOM( EquilibriumCoordinates, COM)
     DO i=1,4
      DO l=1,3
       EquilibriumCOMCoordinates(i,l) = EquilibriumCoordinates(i,l)-COM(l)
      ENDDO
     ENDDO
     EquilibriumCOMComputed = .TRUE.
   ENDIF

   Coordinates = EquilibriumCOMCoordinates
   Velocities  = 0.

   END SUBROUTINE
! -------------------------------------------------------------------------------------------------
  SUBROUTINE FindCOM( Positions, COM)
! Compute Center of Mass of the molecule
  IMPLICIT NONE

  INTEGER                           :: i,l
  REAL, DIMENSION(4,3), INTENT(IN)  :: Positions
  REAL, DIMENSION(3)  , INTENT(OUT) :: COM

  COM = 0.
  DO i=1,3
   DO l=1,4
    COM( i) = COM( i) + Masses( l) * Positions( l, i) / TotMass
   ENDDO
  ENDDO

  RETURN
  END SUBROUTINE
! -------------------------------------------------------------------------------------------------
  SUBROUTINE FindVCOM( Velocity, VCOM)
! Compute velocity of Center of Mass of the molecule
  IMPLICIT NONE

  INTEGER                           :: i,l
  REAL, DIMENSION(4,3), INTENT(IN)  :: Velocity
  REAL, DIMENSION(3)  , INTENT(OUT) :: VCOM

  VCOM = 0.
  DO i=1,3
   DO l=1,4
    VCOM( i) = VCOM( i) + Masses( l) * Velocity( l, i) / TotMass
   ENDDO
  ENDDO

  RETURN
  END SUBROUTINE
  END MODULE