!--------------------------------------------------------------------------------------------------!
!   CP2K: A general program to perform molecular dynamics simulations                              !
!   Copyright 2000-2025 CP2K developers group <https://cp2k.org>                                   !
!                                                                                                  !
!   SPDX-License-Identifier: GPL-2.0-or-later                                                      !
!--------------------------------------------------------------------------------------------------!

! **************************************************************************************************
!> \brief  Collection of utilities for setting-up and handle velocities in MD
!>         runs
!> \author CJM
!> \author Teodoro Laino [tlaino] - University of Zurich - 10.2008
!>         reorganization of the original routines/modules
!>      Teodoro Laino [tlaino] 12.2008 - Preparing for VIRTUAL SITE constraints
!>                                       (patch by Marcel Baer)
! **************************************************************************************************
MODULE md_vel_utils
   USE atomic_kind_list_types,          ONLY: atomic_kind_list_type
   USE atomic_kind_types,               ONLY: atomic_kind_type,&
                                              get_atomic_kind,&
                                              get_atomic_kind_set
   USE bibliography,                    ONLY: West2006,&
                                              cite_reference
   USE cell_types,                      ONLY: &
        cell_type, use_perd_none, use_perd_x, use_perd_xy, use_perd_xyz, use_perd_xz, use_perd_y, &
        use_perd_yz, use_perd_z
   USE cp_linked_list_input,            ONLY: cp_sll_val_next,&
                                              cp_sll_val_type
   USE cp_log_handling,                 ONLY: cp_get_default_logger,&
                                              cp_logger_type
   USE cp_output_handling,              ONLY: cp_print_key_finished_output,&
                                              cp_print_key_unit_nr
   USE cp_subsys_types,                 ONLY: cp_subsys_get,&
                                              cp_subsys_type
   USE cp_units,                        ONLY: cp_unit_from_cp2k
   USE extended_system_types,           ONLY: npt_info_type
   USE force_env_methods,               ONLY: force_env_calc_energy_force
   USE force_env_types,                 ONLY: force_env_get,&
                                              force_env_type
   USE force_env_utils,                 ONLY: force_env_rattle,&
                                              force_env_shake
   USE global_types,                    ONLY: global_environment_type
   USE input_constants,                 ONLY: &
        md_init_default, md_init_vib, npe_f_ensemble, npe_i_ensemble, &
        nph_uniaxial_damped_ensemble, nph_uniaxial_ensemble, npt_f_ensemble, npt_i_ensemble, &
        npt_ia_ensemble, reftraj_ensemble
   USE input_cp2k_binary_restarts,      ONLY: read_binary_velocities
   USE input_restart_force_eval,        ONLY: update_subsys
   USE input_section_types,             ONLY: section_vals_get,&
                                              section_vals_get_subs_vals,&
                                              section_vals_list_get,&
                                              section_vals_type,&
                                              section_vals_val_get
   USE input_val_types,                 ONLY: val_get,&
                                              val_type
   USE kinds,                           ONLY: default_string_length,&
                                              dp
   USE mathconstants,                   ONLY: pi
   USE mathlib,                         ONLY: diamat_all
   USE md_ener_types,                   ONLY: md_ener_type
   USE md_environment_types,            ONLY: get_md_env,&
                                              md_environment_type
   USE md_util,                         ONLY: read_vib_eigs_unformatted
   USE message_passing,                 ONLY: mp_para_env_type
   USE molecule_kind_list_types,        ONLY: molecule_kind_list_type
   USE molecule_kind_types,             ONLY: fixd_constraint_type,&
                                              get_molecule_kind,&
                                              get_molecule_kind_set,&
                                              molecule_kind_type
   USE parallel_rng_types,              ONLY: UNIFORM,&
                                              rng_stream_type
   USE particle_list_types,             ONLY: particle_list_type
   USE particle_types,                  ONLY: particle_type
   USE physcon,                         ONLY: kelvin
   USE shell_opt,                       ONLY: optimize_shell_core
   USE shell_potential_types,           ONLY: shell_kind_type
   USE simpar_types,                    ONLY: simpar_type
   USE thermal_region_types,            ONLY: thermal_region_type,&
                                              thermal_regions_type
#include "../base/base_uses.f90"

   IMPLICIT NONE

   PRIVATE
   CHARACTER(len=*), PARAMETER, PRIVATE :: moduleN = 'md_vel_utils'

   PUBLIC :: temperature_control, &
             comvel_control, &
             angvel_control, &
             setup_velocities

CONTAINS

! **************************************************************************************************
!> \brief compute center of mass position
!>      *** is only used by initialize_velocities below ***
!> \param part ...
!> \param is_fixed ...
!> \param rcom ...
!> \par History
!>      2007-11-6: created
!> \author Toon Verstraelen <Toon.Verstraelen@gmail.com>
! **************************************************************************************************
   SUBROUTINE compute_rcom(part, is_fixed, rcom)
      TYPE(particle_type), DIMENSION(:), POINTER         :: part
      INTEGER, DIMENSION(:), INTENT(IN)                  :: is_fixed
      REAL(KIND=dp), DIMENSION(3), INTENT(OUT)           :: rcom

      INTEGER                                            :: i
      REAL(KIND=dp)                                      :: denom, mass
      TYPE(atomic_kind_type), POINTER                    :: atomic_kind

      rcom(:) = 0.0_dp
      denom = 0.0_dp
      DO i = 1, SIZE(part)
         atomic_kind => part(i)%atomic_kind
         CALL get_atomic_kind(atomic_kind=atomic_kind, mass=mass)
         SELECT CASE (is_fixed(i))
         CASE (use_perd_x, use_perd_y, use_perd_z, use_perd_xy, use_perd_xz, use_perd_yz, use_perd_none)
            rcom(1) = rcom(1) + part(i)%r(1)*mass
            rcom(2) = rcom(2) + part(i)%r(2)*mass
            rcom(3) = rcom(3) + part(i)%r(3)*mass
            denom = denom + mass
         END SELECT
      END DO
      rcom = rcom/denom

   END SUBROUTINE compute_rcom

! **************************************************************************************************
!> \brief compute center of mass velocity
!>      *** is only used by initialize_velocities below ***
!> \param part ...
!> \param is_fixed ...
!> \param vcom ...
!> \param ecom ...
!> \par History
!>      2007-11-6: created
!> \author Toon Verstraelen <Toon.Verstraelen@gmail.com>
! **************************************************************************************************
   SUBROUTINE compute_vcom(part, is_fixed, vcom, ecom)
      TYPE(particle_type), DIMENSION(:), POINTER         :: part
      INTEGER, DIMENSION(:), INTENT(IN)                  :: is_fixed
      REAL(KIND=dp), DIMENSION(3), INTENT(OUT)           :: vcom
      REAL(KIND=dp), INTENT(OUT), OPTIONAL               :: ecom

      INTEGER                                            :: i
      REAL(KIND=dp)                                      :: denom, mass
      TYPE(atomic_kind_type), POINTER                    :: atomic_kind

      vcom = 0.0_dp
      denom = 0.0_dp
      DO i = 1, SIZE(part)
         atomic_kind => part(i)%atomic_kind
         CALL get_atomic_kind(atomic_kind=atomic_kind, mass=mass)
         IF (mass .NE. 0.0) THEN
            SELECT CASE (is_fixed(i))
            CASE (use_perd_x, use_perd_y, use_perd_z, use_perd_xy, use_perd_xz, use_perd_yz, use_perd_none)
               vcom(1) = vcom(1) + part(i)%v(1)*mass
               vcom(2) = vcom(2) + part(i)%v(2)*mass
               vcom(3) = vcom(3) + part(i)%v(3)*mass
               denom = denom + mass
            END SELECT
         END IF
      END DO
      vcom = vcom/denom
      IF (PRESENT(ecom)) THEN
         ecom = 0.5_dp*denom*SUM(vcom*vcom)
      END IF

   END SUBROUTINE compute_vcom

! **************************************************************************************************
!> \brief Copy atom velocities into core and shell velocities
!>      *** is only used by initialize_velocities below ***
!> \param part ...
!> \param shell_part ...
!> \param core_part ...
!> \par History
!>      2007-11-6: created
!> \author Toon Verstraelen <Toon.Verstraelen@gmail.com>
! **************************************************************************************************
   SUBROUTINE clone_core_shell_vel(part, shell_part, core_part)
      TYPE(particle_type), DIMENSION(:), POINTER         :: part, shell_part, core_part

      INTEGER                                            :: i
      LOGICAL                                            :: is_shell
      TYPE(atomic_kind_type), POINTER                    :: atomic_kind

      DO i = 1, SIZE(part)
         atomic_kind => part(i)%atomic_kind
         CALL get_atomic_kind(atomic_kind=atomic_kind, shell_active=is_shell)
         IF (is_shell) THEN
            shell_part(part(i)%shell_index)%v(:) = part(i)%v(:)
            core_part(part(i)%shell_index)%v(:) = part(i)%v(:)
         END IF
      END DO

   END SUBROUTINE clone_core_shell_vel

! **************************************************************************************************
!> \brief Compute the kinetic energy. Does not subtract the center of mass kinetic
!>      energy.
!>      *** is only used by initialize_velocities below ***
!> \param part ...
!> \param ireg ...
!> \return ...
!> \par History
!>      2007-11-6: created
!> \author Toon Verstraelen <Toon.Verstraelen@gmail.com>
! **************************************************************************************************
   FUNCTION compute_ekin(part, ireg) RESULT(ekin)
      TYPE(particle_type), DIMENSION(:), POINTER         :: part
      INTEGER, INTENT(IN), OPTIONAL                      :: ireg
      REAL(KIND=dp)                                      :: ekin

      INTEGER                                            :: i
      REAL(KIND=dp)                                      :: mass
      TYPE(atomic_kind_type), POINTER                    :: atomic_kind

      NULLIFY (atomic_kind)
      ekin = 0.0_dp
      IF (PRESENT(ireg)) THEN
         DO i = 1, SIZE(part)
            IF (part(i)%t_region_index == ireg) THEN
               atomic_kind => part(i)%atomic_kind
               CALL get_atomic_kind(atomic_kind=atomic_kind, mass=mass)
               ekin = ekin + 0.5_dp*mass*SUM(part(i)%v(:)*part(i)%v(:))
            END IF
         END DO
      ELSE
         DO i = 1, SIZE(part)
            atomic_kind => part(i)%atomic_kind
            CALL get_atomic_kind(atomic_kind=atomic_kind, mass=mass)
            ekin = ekin + 0.5_dp*mass*SUM(part(i)%v(:)*part(i)%v(:))
         END DO
      END IF

   END FUNCTION compute_ekin

! **************************************************************************************************
!> \brief Rescale the velocity to mimic the given external kinetic temperature.
!>      Optionally also rescale vcom.
!>      *** is only used by initialize_velocities below ***
!> \param part ...
!> \param simpar ...
!> \param ekin ...
!> \param vcom ...
!> \param ireg ...
!> \param nfree ...
!> \param temp ...
!> \par History
!>      2007-11-6: created
!> \author Toon Verstraelen <Toon.Verstraelen@gmail.com>
! **************************************************************************************************
   SUBROUTINE rescale_vel(part, simpar, ekin, vcom, ireg, nfree, temp)
      TYPE(particle_type), DIMENSION(:), POINTER         :: part
      TYPE(simpar_type), POINTER                         :: simpar
      REAL(KIND=dp), INTENT(INOUT)                       :: ekin
      REAL(KIND=dp), DIMENSION(3), INTENT(INOUT), &
         OPTIONAL                                        :: vcom
      INTEGER, INTENT(IN), OPTIONAL                      :: ireg, nfree
      REAL(KIND=dp), INTENT(IN), OPTIONAL                :: temp

      INTEGER                                            :: i, my_ireg, my_nfree
      REAL(KIND=dp)                                      :: factor, my_temp

      IF (PRESENT(ireg) .AND. PRESENT(nfree) .AND. PRESENT(temp)) THEN
         my_ireg = ireg
         my_nfree = nfree
         my_temp = temp
      ELSEIF (PRESENT(nfree)) THEN
         my_ireg = 0
         my_nfree = nfree
         my_temp = simpar%temp_ext
      ELSE
         my_ireg = 0
         my_nfree = simpar%nfree
         my_temp = simpar%temp_ext
      END IF
      IF (my_nfree /= 0) THEN
         factor = my_temp/(2.0_dp*ekin)*REAL(my_nfree, KIND=dp)
      ELSE
         factor = 0.0_dp
      END IF
      ! Note:
      ! this rescaling is still wrong, it should take the masses into account
      ! rescaling is generally not correct, so needs fixing
      ekin = ekin*factor
      factor = SQRT(factor)
      IF (PRESENT(ireg)) THEN
         DO i = 1, SIZE(part)
            IF (part(i)%t_region_index == my_ireg) part(i)%v(:) = factor*part(i)%v(:)
         END DO
      ELSE
         DO i = 1, SIZE(part)
            part(i)%v(:) = factor*part(i)%v(:)
         END DO
         IF (PRESENT(vcom)) THEN
            vcom = factor*vcom
         END IF
      END IF

   END SUBROUTINE rescale_vel

! **************************************************************************************************
!> \brief Rescale the velocity of separated regions independently
!> \param part ...
!> \param md_env ...
!> \param simpar ...
!> \par History
!>      2008-11
!> \author  MI
! **************************************************************************************************
   SUBROUTINE rescale_vel_region(part, md_env, simpar)

      TYPE(particle_type), DIMENSION(:), POINTER         :: part
      TYPE(md_environment_type), POINTER                 :: md_env
      TYPE(simpar_type), POINTER                         :: simpar

      INTEGER                                            :: ireg, nfree, nfree0, nfree_done
      REAL(KIND=dp)                                      :: ekin, temp
      TYPE(thermal_region_type), POINTER                 :: t_region
      TYPE(thermal_regions_type), POINTER                :: thermal_regions

      NULLIFY (thermal_regions, t_region)

      CALL get_md_env(md_env, thermal_regions=thermal_regions)
      nfree_done = 0
      DO ireg = 1, thermal_regions%nregions
         NULLIFY (t_region)
         t_region => thermal_regions%thermal_region(ireg)
         nfree = t_region%npart*3
         ekin = compute_ekin(part, ireg)
         temp = t_region%temp_expected
         CALL rescale_vel(part, simpar, ekin, ireg=ireg, nfree=nfree, temp=temp)
         nfree_done = nfree_done + nfree
         ekin = compute_ekin(part, ireg)
         temp = 2.0_dp*ekin/REAL(nfree, dp)*kelvin
         t_region%temperature = temp
      END DO
      nfree0 = simpar%nfree - nfree_done
      IF (nfree0 > 0) THEN
         ekin = compute_ekin(part, 0)
         CALL rescale_vel(part, simpar, ekin, ireg=0, nfree=nfree0, temp=simpar%temp_ext)
         ekin = compute_ekin(part, 0)
         temp = 2.0_dp*ekin/REAL(nfree0, dp)*kelvin
         thermal_regions%temp_reg0 = temp
      END IF
   END SUBROUTINE rescale_vel_region

! **************************************************************************************************
!> \brief subtract center of mass velocity
!>      *** is only used by initialize_velocities below ***
!> \param part ...
!> \param is_fixed ...
!> \param vcom ...
!> \par History
!>      2007-11-6: created
!> \author Toon Verstraelen <Toon.Verstraelen@gmail.com>
! **************************************************************************************************
   SUBROUTINE subtract_vcom(part, is_fixed, vcom)
      TYPE(particle_type), DIMENSION(:), POINTER         :: part
      INTEGER, DIMENSION(:), INTENT(IN)                  :: is_fixed
      REAL(KIND=dp), DIMENSION(3), INTENT(IN)            :: vcom

      INTEGER                                            :: i

      DO i = 1, SIZE(part)
         SELECT CASE (is_fixed(i))
         CASE (use_perd_x)
            part(i)%v(2) = part(i)%v(2) - vcom(2)
            part(i)%v(3) = part(i)%v(3) - vcom(3)
         CASE (use_perd_y)
            part(i)%v(1) = part(i)%v(1) - vcom(1)
            part(i)%v(3) = part(i)%v(3) - vcom(3)
         CASE (use_perd_z)
            part(i)%v(1) = part(i)%v(1) - vcom(1)
            part(i)%v(2) = part(i)%v(2) - vcom(2)
         CASE (use_perd_xy)
            part(i)%v(3) = part(i)%v(3) - vcom(3)
         CASE (use_perd_xz)
            part(i)%v(2) = part(i)%v(2) - vcom(2)
         CASE (use_perd_yz)
            part(i)%v(1) = part(i)%v(1) - vcom(1)
         CASE (use_perd_none)
            part(i)%v(:) = part(i)%v(:) - vcom(:)
         END SELECT
      END DO
   END SUBROUTINE subtract_vcom

! **************************************************************************************************
!> \brief compute the angular velocity
!>      *** is only used by initialize_velocities below ***
!> \param part ...
!> \param is_fixed ...
!> \param rcom ...
!> \param vang ...
!> \par History
!>      2007-11-9: created
!> \author Toon Verstraelen <Toon.Verstraelen@gmail.com>
! **************************************************************************************************
   SUBROUTINE compute_vang(part, is_fixed, rcom, vang)
      TYPE(particle_type), DIMENSION(:), POINTER         :: part
      INTEGER, DIMENSION(:), INTENT(IN)                  :: is_fixed
      REAL(KIND=dp), DIMENSION(3), INTENT(IN)            :: rcom
      REAL(KIND=dp), DIMENSION(3), INTENT(OUT)           :: vang

      INTEGER                                            :: i
      REAL(KIND=dp)                                      :: mass, proj
      REAL(KIND=dp), DIMENSION(3)                        :: evals, mang, r
      REAL(KIND=dp), DIMENSION(3, 3)                     :: iner
      TYPE(atomic_kind_type), POINTER                    :: atomic_kind

      NULLIFY (atomic_kind)
      mang(:) = 0.0_dp
      iner(:, :) = 0.0_dp
      DO i = 1, SIZE(part)
         ! compute angular momentum and inertia tensor
         SELECT CASE (is_fixed(i))
         CASE (use_perd_x, use_perd_y, use_perd_z, use_perd_xy, use_perd_xz, use_perd_yz, use_perd_none)
            r(:) = part(i)%r(:) - rcom(:)
            atomic_kind => part(i)%atomic_kind
            CALL get_atomic_kind(atomic_kind=atomic_kind, mass=mass)
            mang(1) = mang(1) + mass*(r(2)*part(i)%v(3) - r(3)*part(i)%v(2))
            mang(2) = mang(2) + mass*(r(3)*part(i)%v(1) - r(1)*part(i)%v(3))
            mang(3) = mang(3) + mass*(r(1)*part(i)%v(2) - r(2)*part(i)%v(1))

            iner(1, 1) = iner(1, 1) + mass*(r(2)*r(2) + r(3)*r(3))
            iner(2, 2) = iner(2, 2) + mass*(r(3)*r(3) + r(1)*r(1))
            iner(3, 3) = iner(3, 3) + mass*(r(1)*r(1) + r(2)*r(2))

            iner(1, 2) = iner(1, 2) - mass*r(1)*r(2)
            iner(2, 3) = iner(2, 3) - mass*r(2)*r(3)
            iner(3, 1) = iner(3, 1) - mass*r(3)*r(1)
         END SELECT
      END DO
      iner(2, 1) = iner(1, 2)
      iner(3, 2) = iner(2, 3)
      iner(1, 3) = iner(3, 1)

      ! Take the safest route, i.e. diagonalize the inertia tensor and solve
      ! the angular velocity only with the non-zero eigenvalues. A plain inversion
      ! would fail for linear molecules.
      CALL diamat_all(iner, evals)

      vang(:) = 0.0_dp
      DO i = 1, 3
         IF (evals(i) > 0.0_dp) THEN
            proj = SUM(iner(:, i)*mang)/evals(i)
            vang(1) = vang(1) + proj*iner(1, i)
            vang(2) = vang(2) + proj*iner(2, i)
            vang(3) = vang(3) + proj*iner(3, i)
         END IF
      END DO

   END SUBROUTINE compute_vang

! **************************************************************************************************
!> \brief subtract the angular velocity
!>      *** is only used by initialize_velocities below ***
!> \param part ...
!> \param is_fixed ...
!> \param rcom ...
!> \param vang ...
!> \par History
!>      2007-11-9: created
!> \author Toon Verstraelen <Toon.Verstraelen@gmail.com>
! **************************************************************************************************
   SUBROUTINE subtract_vang(part, is_fixed, rcom, vang)
      TYPE(particle_type), DIMENSION(:), POINTER         :: part
      INTEGER, DIMENSION(:), INTENT(IN)                  :: is_fixed
      REAL(KIND=dp), DIMENSION(3), INTENT(IN)            :: rcom, vang

      INTEGER                                            :: i
      REAL(KIND=dp), DIMENSION(3)                        :: r

      DO i = 1, SIZE(part)
         r(:) = part(i)%r(:) - rcom(:)
         SELECT CASE (is_fixed(i))
         CASE (use_perd_x)
            part(i)%v(2) = part(i)%v(2) - (vang(3)*r(1) - vang(1)*r(3))
            part(i)%v(3) = part(i)%v(3) - (vang(1)*r(2) - vang(2)*r(1))
         CASE (use_perd_y)
            part(i)%v(1) = part(i)%v(1) - (vang(2)*r(3) - vang(3)*r(2))
            part(i)%v(3) = part(i)%v(3) - (vang(1)*r(2) - vang(2)*r(1))
         CASE (use_perd_z)
            part(i)%v(1) = part(i)%v(1) - (vang(2)*r(3) - vang(3)*r(2))
            part(i)%v(2) = part(i)%v(2) - (vang(3)*r(1) - vang(1)*r(3))
         CASE (use_perd_xy)
            part(i)%v(3) = part(i)%v(3) - (vang(1)*r(2) - vang(2)*r(1))
         CASE (use_perd_xz)
            part(i)%v(2) = part(i)%v(2) - (vang(3)*r(1) - vang(1)*r(3))
         CASE (use_perd_yz)
            part(i)%v(1) = part(i)%v(1) - (vang(2)*r(3) - vang(3)*r(2))
         CASE (use_perd_none)
            part(i)%v(1) = part(i)%v(1) - (vang(2)*r(3) - vang(3)*r(2))
            part(i)%v(2) = part(i)%v(2) - (vang(3)*r(1) - vang(1)*r(3))
            part(i)%v(3) = part(i)%v(3) - (vang(1)*r(2) - vang(2)*r(1))
         END SELECT
      END DO

   END SUBROUTINE subtract_vang

! **************************************************************************************************
!> \brief Initializes the velocities to the Maxwellian distribution
!> \param simpar ...
!> \param part ...
!> \param force_env ...
!> \param globenv ...
!> \param md_env ...
!> \param molecule_kinds ...
!> \param label ...
!> \param print_section ...
!> \param subsys_section ...
!> \param shell_present ...
!> \param shell_part ...
!> \param core_part ...
!> \param force_rescaling ...
!> \param para_env ...
!> \param write_binary_restart_file ...
!> \par History
!>      - is_fixed removed from particle_type
!>      - 2007-11-07: Cleanup (TV)
!>      - 2007-11-09: Added angvel_zero feature
!> \author CJM,MK,Toon Verstraelen <Toon.Verstraelen@gmail.com>
! **************************************************************************************************
   SUBROUTINE initialize_velocities(simpar, &
                                    part, &
                                    force_env, &
                                    globenv, &
                                    md_env, &
                                    molecule_kinds, &
                                    label, &
                                    print_section, &
                                    subsys_section, &
                                    shell_present, &
                                    shell_part, &
                                    core_part, &
                                    force_rescaling, &
                                    para_env, &
                                    write_binary_restart_file)

      TYPE(simpar_type), POINTER                         :: simpar
      TYPE(particle_type), DIMENSION(:), POINTER         :: part
      TYPE(force_env_type), POINTER                      :: force_env
      TYPE(global_environment_type), POINTER             :: globenv
      TYPE(md_environment_type), POINTER                 :: md_env
      TYPE(molecule_kind_list_type), POINTER             :: molecule_kinds
      CHARACTER(LEN=*), INTENT(IN)                       :: label
      TYPE(section_vals_type), POINTER                   :: print_section, subsys_section
      LOGICAL, INTENT(IN)                                :: shell_present
      TYPE(particle_type), DIMENSION(:), POINTER         :: shell_part, core_part
      LOGICAL, INTENT(IN)                                :: force_rescaling
      TYPE(mp_para_env_type), POINTER                    :: para_env
      LOGICAL, INTENT(IN)                                :: write_binary_restart_file

      CHARACTER(LEN=*), PARAMETER :: routineN = 'initialize_velocities'

      INTEGER                                            :: handle, i, ifixd, imolecule_kind, iw, &
                                                            natoms
      INTEGER, ALLOCATABLE, DIMENSION(:)                 :: is_fixed
      LOGICAL                                            :: success
      REAL(KIND=dp)                                      :: ecom, ekin, mass, mass_tot, temp, tmp_r1
      REAL(KIND=dp), DIMENSION(3)                        :: rcom, vang, vcom
      TYPE(atomic_kind_type), POINTER                    :: atomic_kind
      TYPE(cell_type), POINTER                           :: cell
      TYPE(cp_logger_type), POINTER                      :: logger
      TYPE(fixd_constraint_type), DIMENSION(:), POINTER  :: fixd_list
      TYPE(molecule_kind_type), DIMENSION(:), POINTER    :: molecule_kind_set
      TYPE(molecule_kind_type), POINTER                  :: molecule_kind
      TYPE(section_vals_type), POINTER                   :: md_section, root_section, vib_section

      CALL timeset(routineN, handle)

      ! Initializing parameters
      natoms = SIZE(part)
      NULLIFY (atomic_kind, fixd_list, logger, molecule_kind)
      NULLIFY (molecule_kind_set)

      ! Logging
      logger => cp_get_default_logger()
      iw = cp_print_key_unit_nr(logger, print_section, "PROGRAM_RUN_INFO", extension=".log")

      ! Build a list of all fixed atoms (if any)
      ALLOCATE (is_fixed(natoms))

      is_fixed = use_perd_none
      molecule_kind_set => molecule_kinds%els
      DO imolecule_kind = 1, molecule_kinds%n_els
         molecule_kind => molecule_kind_set(imolecule_kind)
         CALL get_molecule_kind(molecule_kind=molecule_kind, fixd_list=fixd_list)
         IF (ASSOCIATED(fixd_list)) THEN
            DO ifixd = 1, SIZE(fixd_list)
               IF (.NOT. fixd_list(ifixd)%restraint%active) is_fixed(fixd_list(ifixd)%fixd) = fixd_list(ifixd)%itype
            END DO
         END IF
      END DO

      ! Compute the total mass when needed
      IF (simpar%ensemble == nph_uniaxial_ensemble .OR. &
          simpar%ensemble == nph_uniaxial_damped_ensemble) THEN
         mass_tot = 0.0_dp
         DO i = 1, natoms
            atomic_kind => part(i)%atomic_kind
            CALL get_atomic_kind(atomic_kind=atomic_kind, mass=mass)
            mass_tot = mass_tot + mass
         END DO
         simpar%v_shock = simpar%v_shock*SQRT(mass_tot)
      END IF

      CALL read_input_velocities(simpar, part, force_env, md_env, subsys_section, &
                                 shell_present, shell_part, core_part, force_rescaling, para_env, is_fixed, success)
      IF (.NOT. success) THEN
         SELECT CASE (simpar%initialization_method)
         CASE (md_init_default)
            CALL generate_velocities(simpar, part, force_env, globenv, md_env, shell_present, &
                                     shell_part, core_part, is_fixed, iw)
         CASE (md_init_vib)
            CALL force_env_get(force_env=force_env, root_section=root_section)
            md_section => section_vals_get_subs_vals(root_section, "MOTION%MD")
            vib_section => section_vals_get_subs_vals(root_section, "VIBRATIONAL_ANALYSIS")
            CALL generate_coords_vels_vib(simpar, &
                                          part, &
                                          md_section, &
                                          vib_section, &
                                          force_env, &
                                          globenv, &
                                          shell_present, &
                                          shell_part, &
                                          core_part, &
                                          is_fixed)
            ! update restart file for the modified coordinates and velocities
            CALL update_subsys(subsys_section, force_env, .FALSE., write_binary_restart_file)
         END SELECT
      END IF

      IF (iw > 0) THEN
         WRITE (iw, '(/,T2,A)') &
            'MD_VEL| '//TRIM(ADJUSTL(label))
         ! Recompute vcom, ecom and ekin for IO
         CALL compute_vcom(part, is_fixed, vcom, ecom)
         ekin = compute_ekin(part) - ecom
         IF (simpar%nfree == 0) THEN
            CPASSERT(ekin == 0.0_dp)
            temp = 0.0_dp
         ELSE
            temp = 2.0_dp*ekin/REAL(simpar%nfree, KIND=dp)
         END IF
         tmp_r1 = cp_unit_from_cp2k(temp, "K")
         WRITE (iw, '(T2,A,T61,F20.6)') &
            'MD_VEL| Initial temperature [K]', tmp_r1
         WRITE (iw, '(T2,A,T30,3(1X,F16.10))') &
            'MD_VEL| COM velocity', vcom(1:3)

         ! compute and log rcom and vang if not periodic
         CALL force_env_get(force_env, cell=cell)
         IF (SUM(cell%perd(1:3)) == 0) THEN
            CALL compute_rcom(part, is_fixed, rcom)
            CALL compute_vang(part, is_fixed, rcom, vang)
            WRITE (iw, '(T2,A,T30,3(1X,F16.10))') &
               'MD_VEL| COM position', rcom(1:3)
            WRITE (iw, '(T2,A,T30,3(1X,F16.10))') &
               'MD_VEL| Angular velocity', vang(1:3)
         END IF
      END IF

      DEALLOCATE (is_fixed)
      CALL cp_print_key_finished_output(iw, logger, print_section, "PROGRAM_RUN_INFO")
      CALL timestop(handle)

   END SUBROUTINE initialize_velocities

! **************************************************************************************************
!> \brief Read velocities from binary restart file if available
!> \param simpar ...
!> \param part ...
!> \param force_env ...
!> \param md_env ...
!> \param subsys_section ...
!> \param shell_present ...
!> \param shell_part ...
!> \param core_part ...
!> \param force_rescaling ...
!> \param para_env ...
!> \param is_fixed ...
!> \param success ...
!> \author CJM,MK,Toon Verstraelen <Toon.Verstraelen@gmail.com>
! **************************************************************************************************
   SUBROUTINE read_input_velocities(simpar, part, force_env, md_env, subsys_section, &
                                    shell_present, shell_part, core_part, force_rescaling, para_env, is_fixed, success)
      TYPE(simpar_type), POINTER                         :: simpar
      TYPE(particle_type), DIMENSION(:), POINTER         :: part
      TYPE(force_env_type), POINTER                      :: force_env
      TYPE(md_environment_type), POINTER                 :: md_env
      TYPE(section_vals_type), POINTER                   :: subsys_section
      LOGICAL, INTENT(IN)                                :: shell_present
      TYPE(particle_type), DIMENSION(:), POINTER         :: shell_part, core_part
      LOGICAL, INTENT(IN)                                :: force_rescaling
      TYPE(mp_para_env_type), POINTER                    :: para_env
      INTEGER, DIMENSION(:), INTENT(INOUT)               :: is_fixed
      LOGICAL, INTENT(OUT)                               :: success

      INTEGER                                            :: i, natoms, nshell, shell_index
      LOGICAL :: atomvel_explicit, atomvel_read, corevel_explicit, corevel_read, is_ok, &
         rescale_regions, shellvel_explicit, shellvel_read
      REAL(KIND=dp)                                      :: ecom, ekin, fac_massc, fac_masss, mass
      REAL(KIND=dp), DIMENSION(3)                        :: vc, vcom, vs
      REAL(KIND=dp), DIMENSION(:), POINTER               :: vel
      TYPE(atomic_kind_type), POINTER                    :: atomic_kind
      TYPE(cp_sll_val_type), POINTER                     :: atom_list, core_list, shell_list
      TYPE(section_vals_type), POINTER                   :: atomvel_section, corevel_section, &
                                                            shellvel_section
      TYPE(shell_kind_type), POINTER                     :: shell
      TYPE(thermal_regions_type), POINTER                :: thermal_regions
      TYPE(val_type), POINTER                            :: val

! Initializing parameters

      success = .FALSE.
      natoms = SIZE(part)
      atomvel_read = .FALSE.
      corevel_read = .FALSE.
      shellvel_read = .FALSE.
      NULLIFY (vel, atomic_kind, atom_list, core_list, shell_list)
      NULLIFY (atomvel_section, shellvel_section, corevel_section)
      NULLIFY (shell, thermal_regions, val)

      ! Core-Shell Model
      nshell = 0
      IF (shell_present) THEN
         CPASSERT(ASSOCIATED(core_part))
         CPASSERT(ASSOCIATED(shell_part))
         nshell = SIZE(shell_part)
      END IF

      atomvel_section => section_vals_get_subs_vals(subsys_section, "VELOCITY")
      shellvel_section => section_vals_get_subs_vals(subsys_section, "SHELL_VELOCITY")
      corevel_section => section_vals_get_subs_vals(subsys_section, "CORE_VELOCITY")

      ! Read or initialize the particle velocities
      CALL section_vals_get(atomvel_section, explicit=atomvel_explicit)
      CALL section_vals_get(shellvel_section, explicit=shellvel_explicit)
      CALL section_vals_get(corevel_section, explicit=corevel_explicit)
      CPASSERT(shellvel_explicit .EQV. corevel_explicit)

      CALL read_binary_velocities("", part, force_env%root_section, para_env, &
                                  subsys_section, atomvel_read)
      CALL read_binary_velocities("SHELL", shell_part, force_env%root_section, para_env, &
                                  subsys_section, shellvel_read)
      CALL read_binary_velocities("CORE", core_part, force_env%root_section, para_env, &
                                  subsys_section, corevel_read)

      IF (.NOT. (atomvel_explicit .OR. atomvel_read)) RETURN
      success = .TRUE.

      IF (.NOT. atomvel_read) THEN
         ! Read the atom velocities if explicitly given in the input file
         CALL section_vals_list_get(atomvel_section, "_DEFAULT_KEYWORD_", list=atom_list)
         DO i = 1, natoms
            is_ok = cp_sll_val_next(atom_list, val)
            CALL val_get(val, r_vals=vel)
            part(i)%v = vel
         END DO
      END IF
      DO i = 1, natoms
         SELECT CASE (is_fixed(i))
         CASE (use_perd_x)
            part(i)%v(1) = 0.0_dp
         CASE (use_perd_y)
            part(i)%v(2) = 0.0_dp
         CASE (use_perd_z)
            part(i)%v(3) = 0.0_dp
         CASE (use_perd_xy)
            part(i)%v(1) = 0.0_dp
            part(i)%v(2) = 0.0_dp
         CASE (use_perd_xz)
            part(i)%v(1) = 0.0_dp
            part(i)%v(3) = 0.0_dp
         CASE (use_perd_yz)
            part(i)%v(2) = 0.0_dp
            part(i)%v(3) = 0.0_dp
         CASE (use_perd_xyz)
            part(i)%v = 0.0_dp
         END SELECT
      END DO
      IF (shell_present) THEN
         IF (shellvel_explicit) THEN
            ! If the atoms positions are given (?) and core and shell velocities are
            ! present in the input, read the latter.
            CALL section_vals_list_get(shellvel_section, "_DEFAULT_KEYWORD_", list=shell_list)
            CALL section_vals_list_get(corevel_section, "_DEFAULT_KEYWORD_", list=core_list)
            DO i = 1, nshell
               is_ok = cp_sll_val_next(shell_list, val)
               CALL val_get(val, r_vals=vel)
               shell_part(i)%v = vel
               is_ok = cp_sll_val_next(core_list, val)
               CALL val_get(val, r_vals=vel)
               core_part(i)%v = vel
            END DO
         ELSE
            IF (.NOT. (shellvel_read .AND. corevel_read)) THEN
               ! Otherwise, just copy atom velocties into shell and core velocities.
               CALL clone_core_shell_vel(part, shell_part, core_part)
            END IF
         END IF
      END IF

      ! compute vcom, ecom and ekin
      CALL compute_vcom(part, is_fixed, vcom, ecom)
      ekin = compute_ekin(part) - ecom

      IF (simpar%do_thermal_region) THEN
         CALL get_md_env(md_env, thermal_regions=thermal_regions)
         IF (ASSOCIATED(thermal_regions)) THEN
            rescale_regions = thermal_regions%force_rescaling
         END IF
      ELSE
         rescale_regions = .FALSE.
      END IF
      IF (simpar%nfree /= 0 .AND. (force_rescaling .OR. rescale_regions)) THEN
         IF (simpar%do_thermal_region) THEN
            CALL rescale_vel_region(part, md_env, simpar)
         ELSE
            CALL rescale_vel(part, simpar, ekin, vcom=vcom)
         END IF

         ! After rescaling, the core and shell velocities must also adapt.
         DO i = 1, natoms
            shell_index = part(i)%shell_index
            IF (shell_present .AND. shell_index /= 0) THEN
               atomic_kind => part(i)%atomic_kind
               CALL get_atomic_kind(atomic_kind=atomic_kind, mass=mass, shell=shell)
               fac_masss = shell%mass_shell/mass
               fac_massc = shell%mass_core/mass
               vs = shell_part(shell_index)%v
               vc = core_part(shell_index)%v

               shell_part(shell_index)%v(1) = part(i)%v(1) + fac_massc*(vs(1) - vc(1))
               shell_part(shell_index)%v(2) = part(i)%v(2) + fac_massc*(vs(2) - vc(2))
               shell_part(shell_index)%v(3) = part(i)%v(3) + fac_massc*(vs(3) - vc(3))
               core_part(shell_index)%v(1) = part(i)%v(1) + fac_masss*(vc(1) - vs(1))
               core_part(shell_index)%v(2) = part(i)%v(2) + fac_masss*(vc(2) - vs(2))
               core_part(shell_index)%v(3) = part(i)%v(3) + fac_masss*(vc(3) - vs(3))
            END IF
         END DO
      END IF
   END SUBROUTINE read_input_velocities

! **************************************************************************************************
!> \brief Initializing velocities AND positions randomly on all processors, based on vibrational
!>        modes of the system, so that the starting coordinates are already very close to
!>        canonical ensumble corresponding to temperature of a head bath.
!> \param simpar          : MD simulation parameters
!> \param particles       : global array of all particles
!> \param md_section      : MD input subsection
!> \param vib_section     : vibrational analysis input section
!> \param force_env       : force environment container
!> \param global_env      : global environment container
!> \param shell_present   : if core-shell model is used
!> \param shell_particles : global array of all shell particles in shell model
!> \param core_particles  : global array of all core particles in shell model
!> \param is_fixed        : array of size of total number of atoms, that determines if any
!>                          cartesian components are fixed
!> \author CJM,MK,Toon Verstraelen <Toon.Verstraelen@gmail.com>, Ole Schuett
! **************************************************************************************************
   SUBROUTINE generate_coords_vels_vib(simpar, &
                                       particles, &
                                       md_section, &
                                       vib_section, &
                                       force_env, &
                                       global_env, &
                                       shell_present, &
                                       shell_particles, &
                                       core_particles, &
                                       is_fixed)
      TYPE(simpar_type), POINTER                         :: simpar
      TYPE(particle_type), DIMENSION(:), POINTER         :: particles
      TYPE(section_vals_type), POINTER                   :: md_section, vib_section
      TYPE(force_env_type), POINTER                      :: force_env
      TYPE(global_environment_type), POINTER             :: global_env
      LOGICAL, INTENT(IN)                                :: shell_present
      TYPE(particle_type), DIMENSION(:), POINTER         :: shell_particles, core_particles
      INTEGER, DIMENSION(:), INTENT(IN)                  :: is_fixed

      INTEGER                                            :: dof, fixed_dof, iatom, ii, imode, &
                                                            my_dof, natoms, shell_index
      REAL(KIND=dp)                                      :: Erand, mass, my_phase, ratio, temperature
      REAL(KIND=dp), ALLOCATABLE, DIMENSION(:)           :: eigenvalues, phase, random
      REAL(KIND=dp), ALLOCATABLE, DIMENSION(:, :)        :: dr, eigenvectors
      TYPE(atomic_kind_type), POINTER                    :: atomic_kind
      TYPE(mp_para_env_type), POINTER                    :: para_env
      TYPE(rng_stream_type), ALLOCATABLE                 :: random_stream

      CALL cite_reference(West2006)
      natoms = SIZE(particles)
      temperature = simpar%temp_ext
      my_dof = 3*natoms
      ALLOCATE (eigenvalues(my_dof))
      ALLOCATE (eigenvectors(my_dof, my_dof))
      ALLOCATE (phase(my_dof))
      ALLOCATE (random(my_dof))
      ALLOCATE (dr(3, natoms))
      CALL force_env_get(force_env=force_env, para_env=para_env)
      ! read vibration modes
      CALL read_vib_eigs_unformatted(md_section, &
                                     vib_section, &
                                     para_env, &
                                     dof, &
                                     eigenvalues, &
                                     eigenvectors)
      IF (my_dof .NE. dof) THEN
         CALL cp_abort(__LOCATION__, &
                       "number of degrees of freedom in vibrational analysis data "// &
                       "do not match total number of cartesian degrees of freedom")
      END IF
      ! read phases
      CALL section_vals_val_get(md_section, "INITIAL_VIBRATION%PHASE", r_val=my_phase)
      my_phase = MIN(1.0_dp, my_phase)
      ! generate random numbers
      random_stream = rng_stream_type(name="MD_INIT_VIB", distribution_type=UNIFORM)
      CALL random_stream%fill(random)
      IF (my_phase .LT. 0.0_dp) THEN
         CALL random_stream%fill(phase)
      ELSE
         phase = my_phase
      END IF
      DEALLOCATE (random_stream)

      ! the first three modes are acoustic with zero frequencies,
      ! exclude these from considerations
      my_dof = dof - 3
      ! randomly selects energy from distribution about kT, all
      ! energies are scaled so that the sum over vibration modes gives
      ! exactly my_dof*kT. Note that k = 1.0 in atomic units
      Erand = 0.0_dp
      DO imode = 4, dof
         Erand = Erand - temperature*LOG(1.0_dp - random(imode))
      END DO
      ! need to take into account of fixed constraints too
      fixed_dof = 0
      DO iatom = 1, natoms
         SELECT CASE (is_fixed(iatom))
         CASE (use_perd_x, use_perd_y, use_perd_z)
            fixed_dof = fixed_dof + 1
         CASE (use_perd_xy, use_perd_xz, use_perd_yz)
            fixed_dof = fixed_dof + 2
         CASE (use_perd_xyz)
            fixed_dof = fixed_dof + 3
         END SELECT
      END DO
      my_dof = my_dof - fixed_dof
      ratio = REAL(my_dof, KIND=dp)*temperature/Erand
      ! update  velocities AND positions
      DO iatom = 1, natoms
         atomic_kind => particles(iatom)%atomic_kind
         CALL get_atomic_kind(atomic_kind=atomic_kind, mass=mass)
         SELECT CASE (is_fixed(iatom))
         CASE (use_perd_x)
            DO ii = 2, 3
               dr(ii, iatom) = dr_from_vib_data(iatom, ii, mass, temperature, eigenvalues, &
                                                eigenvectors, random, phase, dof, ratio)
               particles(iatom)%v(ii) = dv_from_vib_data(iatom, ii, mass, temperature, &
                                                         eigenvectors, random, phase, dof, &
                                                         ratio)
            END DO
         CASE (use_perd_y)
            DO ii = 1, 3, 2
               dr(ii, iatom) = dr_from_vib_data(iatom, ii, mass, temperature, eigenvalues, &
                                                eigenvectors, random, phase, dof, ratio)
               particles(iatom)%v(ii) = dv_from_vib_data(iatom, ii, mass, temperature, &
                                                         eigenvectors, random, phase, dof, &
                                                         ratio)
            END DO
         CASE (use_perd_z)
            DO ii = 1, 2
               dr(ii, iatom) = dr_from_vib_data(iatom, ii, mass, temperature, eigenvalues, &
                                                eigenvectors, random, phase, dof, ratio)
               particles(iatom)%v(ii) = dv_from_vib_data(iatom, ii, mass, temperature, &
                                                         eigenvectors, random, phase, dof, &
                                                         ratio)
            END DO
         CASE (use_perd_xy)
            dr(3, iatom) = dr_from_vib_data(iatom, 3, mass, temperature, eigenvalues, &
                                            eigenvectors, random, phase, dof, ratio)
            particles(iatom)%v(3) = dv_from_vib_data(iatom, 3, mass, temperature, &
                                                     eigenvectors, random, phase, dof, &
                                                     ratio)
         CASE (use_perd_xz)
            dr(2, iatom) = dr_from_vib_data(iatom, 2, mass, temperature, eigenvalues, &
                                            eigenvectors, random, phase, dof, ratio)
            particles(iatom)%v(2) = dv_from_vib_data(iatom, 2, mass, temperature, &
                                                     eigenvectors, random, phase, dof, &
                                                     ratio)
         CASE (use_perd_yz)
            dr(1, iatom) = dr_from_vib_data(iatom, 1, mass, temperature, eigenvalues, &
                                            eigenvectors, random, phase, dof, ratio)
            particles(iatom)%v(1) = dv_from_vib_data(iatom, 1, mass, temperature, &
                                                     eigenvectors, random, phase, dof, &
                                                     ratio)
         CASE (use_perd_none)
            DO ii = 1, 3
               dr(ii, iatom) = dr_from_vib_data(iatom, ii, mass, temperature, eigenvalues, &
                                                eigenvectors, random, phase, dof, ratio)
               particles(iatom)%v(ii) = dv_from_vib_data(iatom, ii, mass, temperature, &
                                                         eigenvectors, random, phase, dof, &
                                                         ratio)
            END DO
         END SELECT
      END DO ! iatom
      ! free memory
      DEALLOCATE (eigenvalues)
      DEALLOCATE (eigenvectors)
      DEALLOCATE (phase)
      DEALLOCATE (random)
      ! update particle coordinates
      DO iatom = 1, natoms
         particles(iatom)%r(:) = particles(iatom)%r(:) + dr(:, iatom)
      END DO
      ! update core-shell model coordinates
      IF (shell_present) THEN
         ! particles have moved, and for core-shell model this means
         ! the cores and shells must also move by the same amount. The
         ! shell positions will then be optimised if needed
         shell_index = particles(iatom)%shell_index
         IF (shell_index .NE. 0) THEN
            core_particles(shell_index)%r(:) = core_particles(shell_index)%r(:) + &
                                               dr(:, iatom)
            shell_particles(shell_index)%r(:) = shell_particles(shell_index)%r(:) + &
                                                dr(:, iatom)
         END IF
         CALL optimize_shell_core(force_env, &
                                  particles, &
                                  shell_particles, &
                                  core_particles, &
                                  global_env)
      END IF
      ! cleanup
      DEALLOCATE (dr)
   END SUBROUTINE generate_coords_vels_vib

! **************************************************************************************************
!> \brief calculates componbent of initial velocity of an atom from vibreational modes
!> \param iatom        : global atomic index
!> \param icart        : cartesian index (1, 2 or 3)
!> \param mass         : atomic mass
!> \param temperature  : target temperature of canonical ensemble
!> \param eigenvalues  : array containing all cartesian vibrational mode eigenvalues (frequencies)
!> \param eigenvectors : array containing all corresponding vibrational mode eigenvectors
!>                       (displacements)
!> \param random       : array containing uniform distributed random numbers, must be the size
!>                       of 3*natoms. Numbers must be between 0 and 1
!> \param phase        : array containing numbers between 0 and 1 that determines for each
!>                       vibration mode the ratio of potential energy vs kinetic energy contribution
!>                       to total energy
!> \param dof          : total number of degrees of freedom, = 3*natoms
!> \param scale        : scale to make sure the sum of vibrational modes give the correct energy
!> \return : outputs icart-th cartesian component of initial position of atom iatom
!> \author Lianheng Tong, lianheng.tong@kcl.ac.uk
! **************************************************************************************************
   PURE FUNCTION dr_from_vib_data(iatom, &
                                  icart, &
                                  mass, &
                                  temperature, &
                                  eigenvalues, &
                                  eigenvectors, &
                                  random, &
                                  phase, &
                                  dof, &
                                  scale) &
      RESULT(res)
      INTEGER, INTENT(IN)                                :: iatom, icart
      REAL(KIND=dp), INTENT(IN)                          :: mass, temperature
      REAL(KIND=dp), DIMENSION(:), INTENT(IN)            :: eigenvalues
      REAL(KIND=dp), DIMENSION(:, :), INTENT(IN)         :: eigenvectors
      REAL(KIND=dp), DIMENSION(:), INTENT(IN)            :: random, phase
      INTEGER, INTENT(IN)                                :: dof
      REAL(KIND=dp), INTENT(IN)                          :: scale
      REAL(KIND=dp)                                      :: res

      INTEGER                                            :: imode, ind

      res = 0.0_dp
      ! assuming the eigenvalues are sorted in ascending order, the
      ! first three modes are acoustic with zero frequencies. These are
      ! excluded from considerations, and should have been reflected in
      ! the calculation of scale outside this function
      IF (mass .GT. 0.0_dp) THEN
         ! eigenvector rows assumed to be grouped in atomic blocks
         ind = (iatom - 1)*3 + icart
         DO imode = 4, dof
            res = res + &
                  SQRT(-2.0_dp*scale*temperature*LOG(1 - random(imode))/mass)/ &
                  eigenvalues(imode)* &
                  eigenvectors(ind, imode)* &
                  COS(2.0_dp*pi*phase(imode))
         END DO
      END IF
   END FUNCTION dr_from_vib_data

! **************************************************************************************************
!> \brief calculates componbent of initial velocity of an atom from vibreational modes
!> \param iatom        : global atomic index
!> \param icart        : cartesian index (1, 2 or 3)
!> \param mass         : atomic mass
!> \param temperature  : target temperature of canonical ensemble
!> \param eigenvectors : array containing all corresponding vibrational mode eigenvectors
!>                       (displacements)
!> \param random       : array containing uniform distributed random numbers, must be the size
!>                       of 3*natoms. Numbers must be between 0 and 1
!> \param phase        : array containing numbers between 0 and 1 that determines for each
!>                       vibration mode the ratio of potential energy vs kinetic energy contribution
!>                       to total energy
!> \param dof          : total number of degrees of freedom, = 3*natoms
!> \param scale        : scale to make sure the sum of vibrational modes give the correct energy
!> \return : outputs icart-th cartesian component of initial velocity of atom iatom
!> \author Lianheng Tong, lianheng.tong@kcl.ac.uk
! **************************************************************************************************
   PURE FUNCTION dv_from_vib_data(iatom, &
                                  icart, &
                                  mass, &
                                  temperature, &
                                  eigenvectors, &
                                  random, &
                                  phase, &
                                  dof, &
                                  scale) &
      RESULT(res)
      INTEGER, INTENT(IN)                                :: iatom, icart
      REAL(KIND=dp), INTENT(IN)                          :: mass, temperature
      REAL(KIND=dp), DIMENSION(:, :), INTENT(IN)         :: eigenvectors
      REAL(KIND=dp), DIMENSION(:), INTENT(IN)            :: random, phase
      INTEGER, INTENT(IN)                                :: dof
      REAL(KIND=dp), INTENT(IN)                          :: scale
      REAL(KIND=dp)                                      :: res

      INTEGER                                            :: imode, ind

      res = 0.0_dp
      ! assuming the eigenvalues are sorted in ascending order, the
      ! first three modes are acoustic with zero frequencies. These are
      ! excluded from considerations, and should have been reflected in
      ! the calculation of scale outside this function
      IF (mass .GT. 0.0_dp) THEN
         ! eigenvector rows assumed to be grouped in atomic blocks
         ind = (iatom - 1)*3 + icart
         DO imode = 4, dof
            res = res - &
                  SQRT(-2.0_dp*scale*temperature*LOG(1 - random(imode))/mass)* &
                  eigenvectors(ind, imode)* &
                  SIN(2.0_dp*pi*phase(imode))
         END DO
      END IF
   END FUNCTION dv_from_vib_data

! **************************************************************************************************
!> \brief Initializing velocities deterministically on all processors, if not given in input
!> \param simpar ...
!> \param part ...
!> \param force_env ...
!> \param globenv ...
!> \param md_env ...
!> \param shell_present ...
!> \param shell_part ...
!> \param core_part ...
!> \param is_fixed ...
!> \param iw ...
!> \author CJM,MK,Toon Verstraelen <Toon.Verstraelen@gmail.com>, Ole Schuett
! **************************************************************************************************
   SUBROUTINE generate_velocities(simpar, part, force_env, globenv, md_env, &
                                  shell_present, shell_part, core_part, is_fixed, iw)
      TYPE(simpar_type), POINTER                         :: simpar
      TYPE(particle_type), DIMENSION(:), POINTER         :: part
      TYPE(force_env_type), POINTER                      :: force_env
      TYPE(global_environment_type), POINTER             :: globenv
      TYPE(md_environment_type), POINTER                 :: md_env
      LOGICAL, INTENT(IN)                                :: shell_present
      TYPE(particle_type), DIMENSION(:), POINTER         :: shell_part, core_part
      INTEGER, DIMENSION(:), INTENT(INOUT)               :: is_fixed
      INTEGER, INTENT(IN)                                :: iw

      INTEGER                                            :: i, natoms
      REAL(KIND=dp)                                      :: mass
      TYPE(atomic_kind_type), POINTER                    :: atomic_kind

      NULLIFY (atomic_kind)
      natoms = SIZE(part)

      DO i = 1, natoms
         atomic_kind => part(i)%atomic_kind
         CALL get_atomic_kind(atomic_kind=atomic_kind, mass=mass)
         part(i)%v(1) = 0.0_dp
         part(i)%v(2) = 0.0_dp
         part(i)%v(3) = 0.0_dp
         IF (mass .NE. 0.0) THEN
            SELECT CASE (is_fixed(i))
            CASE (use_perd_x)
               part(i)%v(2) = globenv%gaussian_rng_stream%next()/SQRT(mass)
               part(i)%v(3) = globenv%gaussian_rng_stream%next()/SQRT(mass)
            CASE (use_perd_y)
               part(i)%v(1) = globenv%gaussian_rng_stream%next()/SQRT(mass)
               part(i)%v(3) = globenv%gaussian_rng_stream%next()/SQRT(mass)
            CASE (use_perd_z)
               part(i)%v(1) = globenv%gaussian_rng_stream%next()/SQRT(mass)
               part(i)%v(2) = globenv%gaussian_rng_stream%next()/SQRT(mass)
            CASE (use_perd_xy)
               part(i)%v(3) = globenv%gaussian_rng_stream%next()/SQRT(mass)
            CASE (use_perd_xz)
               part(i)%v(2) = globenv%gaussian_rng_stream%next()/SQRT(mass)
            CASE (use_perd_yz)
               part(i)%v(1) = globenv%gaussian_rng_stream%next()/SQRT(mass)
            CASE (use_perd_none)
               part(i)%v(1) = globenv%gaussian_rng_stream%next()/SQRT(mass)
               part(i)%v(2) = globenv%gaussian_rng_stream%next()/SQRT(mass)
               part(i)%v(3) = globenv%gaussian_rng_stream%next()/SQRT(mass)
            END SELECT
         END IF
      END DO

      CALL normalize_velocities(simpar, part, force_env, md_env, is_fixed)
      CALL soften_velocities(simpar, part, force_env, md_env, is_fixed, iw)

      ! Initialize the core and the shell velocity. Atom velocities are just
      ! copied so that the initial relative core-shell velocity is zero.
      IF (shell_present) THEN
         CALL optimize_shell_core(force_env, part, shell_part, core_part, globenv)
      END IF
   END SUBROUTINE generate_velocities

! **************************************************************************************************
!> \brief Direct velocities along a low-curvature direction in order to
!>        favors MD trajectories to cross rapidly over small energy barriers
!>        into neighboring basins.
!> \param simpar ...
!> \param part ...
!> \param force_env ...
!> \param md_env ...
!> \param is_fixed ...
!> \param iw ...
!> \author Ole Schuett
! **************************************************************************************************
   SUBROUTINE soften_velocities(simpar, part, force_env, md_env, is_fixed, iw)
      TYPE(simpar_type), POINTER                         :: simpar
      TYPE(particle_type), DIMENSION(:), POINTER         :: part
      TYPE(force_env_type), POINTER                      :: force_env
      TYPE(md_environment_type), POINTER                 :: md_env
      INTEGER, DIMENSION(:), INTENT(INOUT)               :: is_fixed
      INTEGER, INTENT(IN)                                :: iw

      INTEGER                                            :: i, k
      REAL(KIND=dp), DIMENSION(SIZE(part), 3)            :: F, F_t, N, x0

      IF (simpar%soften_nsteps <= 0) RETURN !nothing todo

      IF (ANY(is_fixed /= use_perd_none)) &
         CPABORT("Velocitiy softening with constraints is not supported.")

      !backup positions
      DO i = 1, SIZE(part)
         x0(i, :) = part(i)%r
      END DO

      DO k = 1, simpar%soften_nsteps

         !use normalized velocities as displace direction
         DO i = 1, SIZE(part)
            N(i, :) = part(i)%v
         END DO
         N = N/SQRT(SUM(N**2))

         ! displace system temporarly to calculate forces
         DO i = 1, SIZE(part)
            part(i)%r = part(i)%r + simpar%soften_delta*N(i, :)
         END DO
         CALL force_env_calc_energy_force(force_env)

         ! calculate velocity update direction F_t
         DO i = 1, SIZE(part)
            F(i, :) = part(i)%f
         END DO
         F_t = F - N*SUM(N*F)

         ! restore positions and update velocities
         DO i = 1, SIZE(part)
            part(i)%r = x0(i, :)
            part(i)%v = part(i)%v + simpar%soften_alpha*F_t(i, :)
         END DO

         CALL normalize_velocities(simpar, part, force_env, md_env, is_fixed)
      END DO

      IF (iw > 0) THEN
         WRITE (iw, "(A,T71, I10)") " Velocities softening Steps: ", simpar%soften_nsteps
         WRITE (iw, "(A,T71, E10.3)") " Velocities softening NORM(F_t): ", SQRT(SUM(F_t**2))
      END IF
   END SUBROUTINE soften_velocities

! **************************************************************************************************
!> \brief Scale velocities according to temperature and remove rigid body motion.
!> \param simpar ...
!> \param part ...
!> \param force_env ...
!> \param md_env ...
!> \param is_fixed ...
!> \author CJM,MK,Toon Verstraelen <Toon.Verstraelen@gmail.com>, Ole Schuett
! **************************************************************************************************
   SUBROUTINE normalize_velocities(simpar, part, force_env, md_env, is_fixed)
      TYPE(simpar_type), POINTER                         :: simpar
      TYPE(particle_type), DIMENSION(:), POINTER         :: part
      TYPE(force_env_type), POINTER                      :: force_env
      TYPE(md_environment_type), POINTER                 :: md_env
      INTEGER, DIMENSION(:), INTENT(INOUT)               :: is_fixed

      REAL(KIND=dp)                                      :: ekin
      REAL(KIND=dp), DIMENSION(3)                        :: rcom, vang, vcom
      TYPE(cell_type), POINTER                           :: cell

      NULLIFY (cell)

      ! Subtract the vcom
      CALL compute_vcom(part, is_fixed, vcom)
      CALL subtract_vcom(part, is_fixed, vcom)
      ! If requested and the system is not periodic, subtract the angular velocity
      CALL force_env_get(force_env, cell=cell)
      IF (SUM(cell%perd(1:3)) == 0 .AND. simpar%angvel_zero) THEN
         CALL compute_rcom(part, is_fixed, rcom)
         CALL compute_vang(part, is_fixed, rcom, vang)
         CALL subtract_vang(part, is_fixed, rcom, vang)
      END IF
      ! Rescale the velocities
      IF (simpar%do_thermal_region) THEN
         CALL rescale_vel_region(part, md_env, simpar)
      ELSE
         ekin = compute_ekin(part)
         CALL rescale_vel(part, simpar, ekin)
      END IF
   END SUBROUTINE normalize_velocities

! **************************************************************************************************
!> \brief Computes Ekin, VCOM and Temp for particles
!> \param subsys ...
!> \param md_ener ...
!> \param vsubtract ...
!> \par History
!>     Teodoro Laino - University of Zurich - 09.2007 [tlaino]
! **************************************************************************************************
   SUBROUTINE reset_vcom(subsys, md_ener, vsubtract)
      TYPE(cp_subsys_type), POINTER                      :: subsys
      TYPE(md_ener_type), POINTER                        :: md_ener
      REAL(KIND=dp), DIMENSION(3), INTENT(IN)            :: vsubtract

      CHARACTER(LEN=*), PARAMETER                        :: routineN = 'reset_vcom'

      INTEGER                                            :: atom, handle, iatom, ikind, natom, &
                                                            shell_index
      INTEGER, DIMENSION(:), POINTER                     :: atom_list
      LOGICAL                                            :: is_shell
      REAL(KIND=dp)                                      :: ekin_old, imass_c, imass_s, mass, v2
      REAL(KIND=dp), DIMENSION(3)                        :: tmp, v
      TYPE(atomic_kind_list_type), POINTER               :: atomic_kinds
      TYPE(atomic_kind_type), POINTER                    :: atomic_kind
      TYPE(particle_list_type), POINTER                  :: core_particles, particles, &
                                                            shell_particles
      TYPE(shell_kind_type), POINTER                     :: shell

      NULLIFY (particles, atomic_kind, atomic_kinds, atom_list, shell)
      CALL timeset(routineN, handle)

      CALL cp_subsys_get(subsys, &
                         atomic_kinds=atomic_kinds, &
                         particles=particles, &
                         shell_particles=shell_particles, &
                         core_particles=core_particles)

      ekin_old = md_ener%ekin
      ! Possibly subtract a quantity from all velocities
      DO ikind = 1, atomic_kinds%n_els
         atomic_kind => atomic_kinds%els(ikind)
         CALL get_atomic_kind(atomic_kind=atomic_kind, atom_list=atom_list, &
                              natom=natom, mass=mass, shell_active=is_shell, shell=shell)
         IF (is_shell) THEN
            tmp = 0.5_dp*vsubtract*mass
            imass_s = 1.0_dp/shell%mass_shell
            imass_c = 1.0_dp/shell%mass_core
            DO iatom = 1, natom
               atom = atom_list(iatom)
               shell_index = particles%els(atom)%shell_index
               shell_particles%els(shell_index)%v = shell_particles%els(shell_index)%v - tmp*imass_s
               core_particles%els(shell_index)%v = core_particles%els(shell_index)%v - tmp*imass_c
               particles%els(atom)%v = particles%els(atom)%v - vsubtract
            END DO
         ELSE
            DO iatom = 1, natom
               atom = atom_list(iatom)
               particles%els(atom)%v = particles%els(atom)%v - vsubtract
            END DO
         END IF
      END DO
      ! Compute Kinetic Energy and COM Velocity
      md_ener%vcom = 0.0_dp
      md_ener%total_mass = 0.0_dp
      md_ener%ekin = 0.0_dp
      DO ikind = 1, atomic_kinds%n_els
         atomic_kind => atomic_kinds%els(ikind)
         CALL get_atomic_kind(atomic_kind=atomic_kind, atom_list=atom_list, mass=mass, natom=natom)
         v2 = 0.0_dp
         v = 0.0_dp
         DO iatom = 1, natom
            atom = atom_list(iatom)
            v2 = v2 + SUM(particles%els(atom)%v**2)
            v(1) = v(1) + particles%els(atom)%v(1)
            v(2) = v(2) + particles%els(atom)%v(2)
            v(3) = v(3) + particles%els(atom)%v(3)
         END DO
         md_ener%ekin = md_ener%ekin + 0.5_dp*mass*v2
         md_ener%vcom(1) = md_ener%vcom(1) + mass*v(1)
         md_ener%vcom(2) = md_ener%vcom(2) + mass*v(2)
         md_ener%vcom(3) = md_ener%vcom(3) + mass*v(3)
         md_ener%total_mass = md_ener%total_mass + REAL(natom, KIND=dp)*mass
      END DO
      md_ener%vcom = md_ener%vcom/md_ener%total_mass
      md_ener%constant = md_ener%constant - ekin_old + md_ener%ekin
      IF (md_ener%nfree /= 0) THEN
         md_ener%temp_part = 2.0_dp*md_ener%ekin/REAL(md_ener%nfree, KIND=dp)*kelvin
      END IF
      CALL timestop(handle)

   END SUBROUTINE reset_vcom

! **************************************************************************************************
!> \brief Scale velocities to get the correct temperature
!> \param subsys ...
!> \param md_ener ...
!> \param temp_expected ...
!> \param temp_tol ...
!> \param iw ...
!> \par History
!>     Teodoro Laino - University of Zurich - 09.2007 [tlaino]
! **************************************************************************************************
   SUBROUTINE scale_velocity(subsys, md_ener, temp_expected, temp_tol, iw)
      TYPE(cp_subsys_type), POINTER                      :: subsys
      TYPE(md_ener_type), POINTER                        :: md_ener
      REAL(KIND=dp), INTENT(IN)                          :: temp_expected, temp_tol
      INTEGER, INTENT(IN)                                :: iw

      REAL(KIND=dp)                                      :: ekin_old, scale, temp_old

      IF (ABS(temp_expected - md_ener%temp_part/kelvin) > temp_tol) THEN
         scale = 0.0_dp
         IF (md_ener%temp_part > 0.0_dp) scale = SQRT((temp_expected/md_ener%temp_part)*kelvin)
         ekin_old = md_ener%ekin
         temp_old = md_ener%temp_part
         md_ener%ekin = 0.0_dp
         md_ener%temp_part = 0.0_dp
         md_ener%vcom = 0.0_dp
         md_ener%total_mass = 0.0_dp

         CALL scale_velocity_low(subsys, scale, ireg=0, ekin=md_ener%ekin, vcom=md_ener%vcom)
         IF (md_ener%nfree /= 0) THEN
            md_ener%temp_part = 2.0_dp*md_ener%ekin/REAL(md_ener%nfree, KIND=dp)*kelvin
         END IF
         md_ener%constant = md_ener%constant - ekin_old + md_ener%ekin
         IF (iw > 0) THEN
            WRITE (UNIT=iw, FMT='(/,T2,A)') &
               'MD_VEL| Temperature scaled to requested temperature'
            WRITE (UNIT=iw, FMT='(T2,A,T61,F20.6)') &
               'MD_VEL| Old temperature [K]', temp_old, &
               'MD_VEL| New temperature [K]', md_ener%temp_part
         END IF
      END IF

   END SUBROUTINE scale_velocity

! **************************************************************************************************
!> \brief Scale velocities of set of regions
!> \param md_env ...
!> \param subsys ...
!> \param md_ener ...
!> \param simpar ...
!> \param iw ...
!> \par author MI
! **************************************************************************************************
   SUBROUTINE scale_velocity_region(md_env, subsys, md_ener, simpar, iw)

      TYPE(md_environment_type), POINTER                 :: md_env
      TYPE(cp_subsys_type), POINTER                      :: subsys
      TYPE(md_ener_type), POINTER                        :: md_ener
      TYPE(simpar_type), POINTER                         :: simpar
      INTEGER, INTENT(IN)                                :: iw

      INTEGER                                            :: ireg, nfree, nfree_done, nregions
      REAL(KIND=dp)                                      :: ekin, ekin_old, ekin_total_new, fscale, &
                                                            vcom(3), vcom_total(3)
      REAL(KIND=dp), ALLOCATABLE, DIMENSION(:)           :: temp_new, temp_old
      TYPE(particle_list_type), POINTER                  :: particles
      TYPE(particle_type), DIMENSION(:), POINTER         :: part
      TYPE(thermal_region_type), POINTER                 :: t_region
      TYPE(thermal_regions_type), POINTER                :: thermal_regions

      NULLIFY (particles, part, thermal_regions, t_region)
      CALL cp_subsys_get(subsys, particles=particles)
      part => particles%els
      CALL get_md_env(md_env, thermal_regions=thermal_regions)

      nregions = thermal_regions%nregions
      nfree_done = 0
      ekin_total_new = 0.0_dp
      ekin_old = md_ener%ekin
      vcom_total = 0.0_dp
      ALLOCATE (temp_new(0:nregions), temp_old(0:nregions))
      temp_new = 0.0_dp
      temp_old = 0.0_dp
      !loop regions
      DO ireg = 1, nregions
         NULLIFY (t_region)
         t_region => thermal_regions%thermal_region(ireg)
         nfree = 3*t_region%npart
         ekin = compute_ekin(part, ireg)
         IF (nfree > 0) t_region%temperature = 2.0_dp*ekin/REAL(nfree, KIND=dp)*kelvin
         temp_old(ireg) = t_region%temperature
         IF (t_region%temp_tol > 0.0_dp .AND. &
             ABS(t_region%temp_expected - t_region%temperature/kelvin) > t_region%temp_tol) THEN
            fscale = SQRT((t_region%temp_expected/t_region%temperature)*kelvin)
            CALL scale_velocity_low(subsys, fscale, ireg, ekin, vcom)
            t_region%temperature = 2.0_dp*ekin/REAL(nfree, KIND=dp)*kelvin
            temp_new(ireg) = t_region%temperature
         END IF
         nfree_done = nfree_done + nfree
         ekin_total_new = ekin_total_new + ekin
      END DO
      nfree = simpar%nfree - nfree_done
      ekin = compute_ekin(part, ireg=0)
      IF (nfree > 0) thermal_regions%temp_reg0 = 2.0_dp*ekin/REAL(nfree, KIND=dp)*kelvin
      temp_old(0) = thermal_regions%temp_reg0
      IF (simpar%temp_tol > 0.0_dp .AND. nfree > 0) THEN
         IF (ABS(simpar%temp_ext - thermal_regions%temp_reg0/kelvin) > simpar%temp_tol) THEN
            fscale = SQRT((simpar%temp_ext/thermal_regions%temp_reg0)*kelvin)
            CALL scale_velocity_low(subsys, fscale, 0, ekin, vcom)
            thermal_regions%temp_reg0 = 2.0_dp*ekin/REAL(nfree, KIND=dp)*kelvin
            temp_new(0) = thermal_regions%temp_reg0
         END IF
      END IF
      ekin_total_new = ekin_total_new + ekin

      md_ener%ekin = ekin_total_new
      IF (md_ener%nfree /= 0) THEN
         md_ener%temp_part = 2.0_dp*md_ener%ekin/REAL(md_ener%nfree, KIND=dp)*kelvin
      END IF
      md_ener%constant = md_ener%constant - ekin_old + md_ener%ekin
      IF (iw > 0) THEN
         DO ireg = 0, nregions
            IF (temp_new(ireg) > 0.0_dp) THEN
               WRITE (UNIT=iw, FMT='(/,T2,A,I0,A)') &
                  'MD_VEL| Temperature region ', ireg, ' scaled to requested temperature'
               WRITE (UNIT=iw, FMT='(T2,A,T61,F20.6)') &
                  'MD_VEL| Old temperature [K]', temp_old(ireg), &
                  'MD_VEL| New temperature [K]', temp_new(ireg)
            END IF
         END DO
      END IF
      DEALLOCATE (temp_new, temp_old)

   END SUBROUTINE scale_velocity_region

! **************************************************************************************************
!> \brief Scale velocities  for a specific region
!> \param subsys ...
!> \param fscale ...
!> \param ireg ...
!> \param ekin ...
!> \param vcom ...
!> \par author MI
! **************************************************************************************************
   SUBROUTINE scale_velocity_low(subsys, fscale, ireg, ekin, vcom)

      TYPE(cp_subsys_type), POINTER                      :: subsys
      REAL(KIND=dp), INTENT(IN)                          :: fscale
      INTEGER, INTENT(IN)                                :: ireg
      REAL(KIND=dp), INTENT(OUT)                         :: ekin, vcom(3)

      INTEGER                                            :: atom, iatom, ikind, my_ireg, natom, &
                                                            shell_index
      INTEGER, DIMENSION(:), POINTER                     :: atom_list
      LOGICAL                                            :: is_shell
      REAL(KIND=dp)                                      :: imass, mass, tmass, v2
      REAL(KIND=dp), DIMENSION(3)                        :: tmp, v, vc, vs
      TYPE(atomic_kind_list_type), POINTER               :: atomic_kinds
      TYPE(atomic_kind_type), POINTER                    :: atomic_kind
      TYPE(particle_list_type), POINTER                  :: core_particles, particles, &
                                                            shell_particles
      TYPE(shell_kind_type), POINTER                     :: shell

      NULLIFY (atomic_kinds, particles, shell_particles, core_particles, shell, atom_list)

      my_ireg = ireg
      ekin = 0.0_dp
      tmass = 0.0_dp
      vcom = 0.0_dp

      CALL cp_subsys_get(subsys, atomic_kinds=atomic_kinds, particles=particles, &
                         shell_particles=shell_particles, core_particles=core_particles)

      DO ikind = 1, atomic_kinds%n_els
         atomic_kind => atomic_kinds%els(ikind)
         CALL get_atomic_kind(atomic_kind=atomic_kind, atom_list=atom_list, mass=mass, &
                              natom=natom, shell_active=is_shell, shell=shell)
         IF (is_shell) THEN
            imass = 1.0_dp/mass
            v2 = 0.0_dp
            v = 0.0_dp
            DO iatom = 1, natom
               atom = atom_list(iatom)
               !check region
               IF (particles%els(atom)%t_region_index /= my_ireg) CYCLE

               particles%els(atom)%v(:) = fscale*particles%els(atom)%v
               shell_index = particles%els(atom)%shell_index
               vs = shell_particles%els(shell_index)%v
               vc = core_particles%els(shell_index)%v
               tmp(1) = imass*(vs(1) - vc(1))
               tmp(2) = imass*(vs(2) - vc(2))
               tmp(3) = imass*(vs(3) - vc(3))

               shell_particles%els(shell_index)%v(1) = particles%els(atom)%v(1) + tmp(1)*shell%mass_core
               shell_particles%els(shell_index)%v(2) = particles%els(atom)%v(2) + tmp(2)*shell%mass_core
               shell_particles%els(shell_index)%v(3) = particles%els(atom)%v(3) + tmp(3)*shell%mass_core

               core_particles%els(shell_index)%v(1) = particles%els(atom)%v(1) - tmp(1)*shell%mass_shell
               core_particles%els(shell_index)%v(2) = particles%els(atom)%v(2) - tmp(2)*shell%mass_shell
               core_particles%els(shell_index)%v(3) = particles%els(atom)%v(3) - tmp(3)*shell%mass_shell

               ! kinetic energy and velocity of COM
               v2 = v2 + SUM(particles%els(atom)%v**2)
               v(1) = v(1) + particles%els(atom)%v(1)
               v(2) = v(2) + particles%els(atom)%v(2)
               v(3) = v(3) + particles%els(atom)%v(3)
               tmass = tmass + mass
            END DO
         ELSE
            v2 = 0.0_dp
            v = 0.0_dp
            DO iatom = 1, natom
               atom = atom_list(iatom)
               !check region
               IF (particles%els(atom)%t_region_index /= my_ireg) CYCLE

               particles%els(atom)%v(:) = fscale*particles%els(atom)%v
               ! kinetic energy and velocity of COM
               v2 = v2 + SUM(particles%els(atom)%v**2)
               v(1) = v(1) + particles%els(atom)%v(1)
               v(2) = v(2) + particles%els(atom)%v(2)
               v(3) = v(3) + particles%els(atom)%v(3)
               tmass = tmass + mass
            END DO
         END IF
         ekin = ekin + 0.5_dp*mass*v2
         vcom(1) = vcom(1) + mass*v(1)
         vcom(2) = vcom(2) + mass*v(2)
         vcom(3) = vcom(3) + mass*v(3)

      END DO
      vcom = vcom/tmass

   END SUBROUTINE scale_velocity_low

! **************************************************************************************************
!> \brief Scale internal motion of CORE-SHELL model to the correct temperature
!> \param subsys ...
!> \param md_ener ...
!> \param temp_expected ...
!> \param temp_tol ...
!> \param iw ...
!> \par History
!>     Teodoro Laino - University of Zurich - 09.2007 [tlaino]
! **************************************************************************************************
   SUBROUTINE scale_velocity_internal(subsys, md_ener, temp_expected, temp_tol, iw)
      TYPE(cp_subsys_type), POINTER                      :: subsys
      TYPE(md_ener_type), POINTER                        :: md_ener
      REAL(KIND=dp), INTENT(IN)                          :: temp_expected, temp_tol
      INTEGER, INTENT(IN)                                :: iw

      INTEGER                                            :: atom, iatom, ikind, natom, shell_index
      INTEGER, DIMENSION(:), POINTER                     :: atom_list
      LOGICAL                                            :: is_shell
      REAL(KIND=dp)                                      :: ekin_shell_old, fac_mass, mass, scale, &
                                                            temp_shell_old, v2
      REAL(KIND=dp), DIMENSION(3)                        :: tmp, v, vc, vs
      TYPE(atomic_kind_list_type), POINTER               :: atomic_kinds
      TYPE(atomic_kind_type), POINTER                    :: atomic_kind
      TYPE(particle_list_type), POINTER                  :: core_particles, particles, &
                                                            shell_particles
      TYPE(shell_kind_type), POINTER                     :: shell

      NULLIFY (atom_list, atomic_kinds, atomic_kind, core_particles, particles, shell_particles, shell)
      IF (ABS(temp_expected - md_ener%temp_shell/kelvin) > temp_tol) THEN
         scale = 0.0_dp
         IF (md_ener%temp_shell > EPSILON(0.0_dp)) scale = SQRT((temp_expected/md_ener%temp_shell)*kelvin)
         ekin_shell_old = md_ener%ekin_shell
         temp_shell_old = md_ener%temp_shell
         md_ener%ekin_shell = 0.0_dp
         md_ener%temp_shell = 0.0_dp

         CALL cp_subsys_get(subsys, atomic_kinds=atomic_kinds, particles=particles, shell_particles=shell_particles, &
                            core_particles=core_particles)

         DO ikind = 1, atomic_kinds%n_els
            atomic_kind => atomic_kinds%els(ikind)
            CALL get_atomic_kind(atomic_kind=atomic_kind, atom_list=atom_list, mass=mass, natom=natom, &
                                 shell_active=is_shell, shell=shell)
            IF (is_shell) THEN
               fac_mass = 1.0_dp/mass
               v2 = 0.0_dp
               DO iatom = 1, natom
                  atom = atom_list(iatom)
                  shell_index = particles%els(atom)%shell_index
                  vs = shell_particles%els(shell_index)%v
                  vc = core_particles%els(shell_index)%v
                  v = particles%els(atom)%v
                  tmp(1) = fac_mass*(vc(1) - vs(1))
                  tmp(2) = fac_mass*(vc(2) - vs(2))
                  tmp(3) = fac_mass*(vc(3) - vs(3))

                  shell_particles%els(shell_index)%v(1) = v(1) - shell%mass_core*scale*tmp(1)
                  shell_particles%els(shell_index)%v(2) = v(2) - shell%mass_core*scale*tmp(2)
                  shell_particles%els(shell_index)%v(3) = v(3) - shell%mass_core*scale*tmp(3)

                  core_particles%els(shell_index)%v(1) = v(1) + shell%mass_shell*scale*tmp(1)
                  core_particles%els(shell_index)%v(2) = v(2) + shell%mass_shell*scale*tmp(2)
                  core_particles%els(shell_index)%v(3) = v(3) + shell%mass_shell*scale*tmp(3)

                  vs = shell_particles%els(shell_index)%v
                  vc = core_particles%els(shell_index)%v
                  tmp(1) = vc(1) - vs(1)
                  tmp(2) = vc(2) - vs(2)
                  tmp(3) = vc(3) - vs(3)
                  v2 = v2 + SUM(tmp**2)
               END DO
               md_ener%ekin_shell = md_ener%ekin_shell + 0.5_dp*shell%mass_core*shell%mass_shell*fac_mass*v2
            END IF
         END DO
         IF (md_ener%nfree_shell > 0) THEN
            md_ener%temp_shell = 2.0_dp*md_ener%ekin_shell/REAL(md_ener%nfree_shell, KIND=dp)*kelvin
         END IF
         md_ener%constant = md_ener%constant - ekin_shell_old + md_ener%ekin_shell
         IF (iw > 0) THEN
            WRITE (UNIT=iw, FMT='(/,T2,A)') &
               'MD_VEL| Temperature of shell internal motion scaled to requested temperature'
            WRITE (UNIT=iw, FMT='(T2,A,T61,F20.6)') &
               'MD_VEL| Old temperature [K]', temp_shell_old, &
               'MD_VEL| New temperature [K]', md_ener%temp_shell
         END IF
      END IF

   END SUBROUTINE scale_velocity_internal

! **************************************************************************************************
!> \brief Scale barostat velocities to get the desired temperature
!> \param md_env ...
!> \param md_ener ...
!> \param temp_expected ...
!> \param temp_tol ...
!> \param iw ...
!> \par History
!>     MI 02.2008
! **************************************************************************************************
   SUBROUTINE scale_velocity_baro(md_env, md_ener, temp_expected, temp_tol, iw)
      TYPE(md_environment_type), POINTER                 :: md_env
      TYPE(md_ener_type), POINTER                        :: md_ener
      REAL(KIND=dp), INTENT(IN)                          :: temp_expected, temp_tol
      INTEGER, INTENT(IN)                                :: iw

      INTEGER                                            :: i, j, nfree
      REAL(KIND=dp)                                      :: ekin_old, scale, temp_old
      TYPE(npt_info_type), POINTER                       :: npt(:, :)
      TYPE(simpar_type), POINTER                         :: simpar

      NULLIFY (npt, simpar)
      CALL get_md_env(md_env, simpar=simpar, npt=npt)
      IF (ABS(temp_expected - md_ener%temp_baro/kelvin) > temp_tol) THEN
         scale = 0.0_dp
         IF (md_ener%temp_baro > 0.0_dp) scale = SQRT((temp_expected/md_ener%temp_baro)*kelvin)
         ekin_old = md_ener%baro_kin
         temp_old = md_ener%temp_baro
         md_ener%baro_kin = 0.0_dp
         md_ener%temp_baro = 0.0_dp
         IF (simpar%ensemble == npt_i_ensemble .OR. simpar%ensemble == npe_i_ensemble &
             .OR. simpar%ensemble == npt_ia_ensemble) THEN
            npt(1, 1)%v = npt(1, 1)%v*scale
            md_ener%baro_kin = 0.5_dp*npt(1, 1)%v**2*npt(1, 1)%mass
         ELSE IF (simpar%ensemble == npt_f_ensemble .OR. simpar%ensemble == npe_f_ensemble) THEN
            md_ener%baro_kin = 0.0_dp
            DO i = 1, 3
               DO j = 1, 3
                  npt(i, j)%v = npt(i, j)%v*scale
                  md_ener%baro_kin = md_ener%baro_kin + 0.5_dp*npt(i, j)%v**2*npt(i, j)%mass
               END DO
            END DO
         END IF
         nfree = SIZE(npt, 1)*SIZE(npt, 2)
         md_ener%temp_baro = 2.0_dp*md_ener%baro_kin/REAL(nfree, dp)*kelvin
         IF (iw > 0) THEN
            WRITE (UNIT=iw, FMT='(/,T2,A)') &
               'MD_VEL| Temperature of barostat motion scaled to requested temperature'
            WRITE (UNIT=iw, FMT='(T2,A,T61,F20.6)') &
               'MD_VEL| Old temperature [K]', temp_old, &
               'MD_VEL| New temperature [K]', md_ener%temp_baro
         END IF
      END IF

   END SUBROUTINE scale_velocity_baro

! **************************************************************************************************
!> \brief Perform all temperature manipulations during a QS MD run.
!> \param simpar ...
!> \param md_env ...
!> \param md_ener ...
!> \param force_env ...
!> \param logger ...
!> \par History
!>     Creation (15.09.2003,MK)
!>     adapted to force_env (05.10.2003,fawzi)
!>     Cleaned (09.2007) Teodoro Laino [tlaino] - University of Zurich
! **************************************************************************************************
   SUBROUTINE temperature_control(simpar, md_env, md_ener, force_env, logger)

      TYPE(simpar_type), POINTER                         :: simpar
      TYPE(md_environment_type), POINTER                 :: md_env
      TYPE(md_ener_type), POINTER                        :: md_ener
      TYPE(force_env_type), POINTER                      :: force_env
      TYPE(cp_logger_type), POINTER                      :: logger

      CHARACTER(LEN=*), PARAMETER :: routineN = 'temperature_control'

      INTEGER                                            :: handle, iw
      TYPE(cp_subsys_type), POINTER                      :: subsys
      TYPE(mp_para_env_type), POINTER                    :: para_env

      CALL timeset(routineN, handle)
      NULLIFY (subsys, para_env)
      CPASSERT(ASSOCIATED(simpar))
      CPASSERT(ASSOCIATED(md_ener))
      CPASSERT(ASSOCIATED(force_env))
      CALL force_env_get(force_env, subsys=subsys, para_env=para_env)
      iw = cp_print_key_unit_nr(logger, force_env%root_section, "MOTION%MD%PRINT%PROGRAM_RUN_INFO", &
                                extension=".mdLog")

      ! Control the particle motion
      IF (simpar%do_thermal_region) THEN
         CALL scale_velocity_region(md_env, subsys, md_ener, simpar, iw)
      ELSE
         IF (simpar%temp_tol > 0.0_dp) THEN
            CALL scale_velocity(subsys, md_ener, simpar%temp_ext, simpar%temp_tol, iw)
         END IF
      END IF
      ! Control the internal core-shell motion
      IF (simpar%temp_sh_tol > 0.0_dp) THEN
         CALL scale_velocity_internal(subsys, md_ener, simpar%temp_sh_ext, simpar%temp_sh_tol, iw)
      END IF
      ! Control cell motion
      SELECT CASE (simpar%ensemble)
      CASE (nph_uniaxial_damped_ensemble, nph_uniaxial_ensemble, &
            npt_f_ensemble, npt_i_ensemble, npe_f_ensemble, npe_i_ensemble, npt_ia_ensemble)
         IF (simpar%temp_baro_tol > 0.0_dp) THEN
            CALL scale_velocity_baro(md_env, md_ener, simpar%temp_baro_ext, simpar%temp_baro_tol, iw)
         END IF
      END SELECT

      CALL cp_print_key_finished_output(iw, logger, force_env%root_section, &
                                        "MOTION%MD%PRINT%PROGRAM_RUN_INFO")
      CALL timestop(handle)
   END SUBROUTINE temperature_control

! **************************************************************************************************
!> \brief Set to 0 the velocity of the COM along MD runs, if required.
!> \param md_ener ...
!> \param force_env ...
!> \param md_section ...
!> \param logger ...
!> \par History
!>      Creation (29.04.2007,MI)
!>      Cleaned (09.2007) Teodoro Laino [tlaino] - University of Zurich
! **************************************************************************************************
   SUBROUTINE comvel_control(md_ener, force_env, md_section, logger)

      TYPE(md_ener_type), POINTER                        :: md_ener
      TYPE(force_env_type), POINTER                      :: force_env
      TYPE(section_vals_type), POINTER                   :: md_section
      TYPE(cp_logger_type), POINTER                      :: logger

      CHARACTER(LEN=*), PARAMETER                        :: routineN = 'comvel_control'

      INTEGER                                            :: handle, iw
      LOGICAL                                            :: explicit
      REAL(KIND=dp)                                      :: comvel_tol, temp_old, vel_com
      REAL(KIND=dp), DIMENSION(3)                        :: vcom_old
      TYPE(cp_subsys_type), POINTER                      :: subsys

      CALL timeset(routineN, handle)
      NULLIFY (subsys)
      CPASSERT(ASSOCIATED(force_env))
      CALL force_env_get(force_env, subsys=subsys)

      ! Print COMVEL and COM Position
      iw = cp_print_key_unit_nr(logger, md_section, "PRINT%CENTER_OF_MASS", extension=".mdLog")
      IF (iw > 0) THEN
         WRITE (UNIT=iw, FMT="(/,T2,A)") &
            "MD_VEL| Centre of mass motion (COM)"
         WRITE (UNIT=iw, FMT="(T2,A,T30,3(1X,F16.10))") &
            "MD_VEL| VCOM [a.u.]", md_ener%vcom(1:3)
      END IF
      CALL cp_print_key_finished_output(iw, logger, md_section, "PRINT%CENTER_OF_MASS")

      ! If requested rescale COMVEL
      CALL section_vals_val_get(md_section, "COMVEL_TOL", explicit=explicit)
      IF (explicit) THEN
         CALL section_vals_val_get(md_section, "COMVEL_TOL", r_val=comvel_tol)
         iw = cp_print_key_unit_nr(logger, md_section, "PRINT%PROGRAM_RUN_INFO", &
                                   extension=".mdLog")
         vel_com = SQRT(md_ener%vcom(1)**2 + md_ener%vcom(2)**2 + md_ener%vcom(3)**2)

         ! Subtract the velocity of the COM, if requested
         IF (vel_com > comvel_tol) THEN
            temp_old = md_ener%temp_part/kelvin
            vcom_old = md_ener%vcom
            CALL reset_vcom(subsys, md_ener, vsubtract=vcom_old)
            CALL scale_velocity(subsys, md_ener, temp_old, 0.0_dp, iw)
            IF (iw > 0) THEN
               WRITE (UNIT=iw, FMT="(T2,A,T30,3(1X,F16.10))") &
                  "MD_VEL| Old VCOM [a.u.]", vcom_old(1:3)
               WRITE (UNIT=iw, FMT="(T2,A,T30,3(1X,F16.10))") &
                  "MD_VEL| New VCOM [a.u.]", md_ener%vcom(1:3)
            END IF
         END IF
         CALL cp_print_key_finished_output(iw, logger, md_section, &
                                           "PRINT%PROGRAM_RUN_INFO")
      END IF

      CALL timestop(handle)
   END SUBROUTINE comvel_control

! **************************************************************************************************
!> \brief Set to 0 the angular velocity along MD runs, if required.
!> \param md_ener ...
!> \param force_env ...
!> \param md_section ...
!> \param logger ...
!> \par History
!>      Creation (10.2009) Teodoro Laino [tlaino]
! **************************************************************************************************
   SUBROUTINE angvel_control(md_ener, force_env, md_section, logger)

      TYPE(md_ener_type), POINTER                        :: md_ener
      TYPE(force_env_type), POINTER                      :: force_env
      TYPE(section_vals_type), POINTER                   :: md_section
      TYPE(cp_logger_type), POINTER                      :: logger

      CHARACTER(LEN=*), PARAMETER                        :: routineN = 'angvel_control'

      INTEGER                                            :: handle, ifixd, imolecule_kind, iw, natoms
      INTEGER, ALLOCATABLE, DIMENSION(:)                 :: is_fixed
      LOGICAL                                            :: explicit
      REAL(KIND=dp)                                      :: angvel_tol, rcom(3), temp_old, vang(3), &
                                                            vang_new(3)
      TYPE(cell_type), POINTER                           :: cell
      TYPE(cp_subsys_type), POINTER                      :: subsys
      TYPE(fixd_constraint_type), DIMENSION(:), POINTER  :: fixd_list
      TYPE(molecule_kind_list_type), POINTER             :: molecule_kinds
      TYPE(molecule_kind_type), DIMENSION(:), POINTER    :: molecule_kind_set
      TYPE(molecule_kind_type), POINTER                  :: molecule_kind
      TYPE(particle_list_type), POINTER                  :: particles

      CALL timeset(routineN, handle)
      ! If requested rescale ANGVEL
      CALL section_vals_val_get(md_section, "ANGVEL_TOL", explicit=explicit)
      IF (explicit) THEN
         NULLIFY (subsys, cell)
         CPASSERT(ASSOCIATED(force_env))
         CALL force_env_get(force_env, subsys=subsys, cell=cell)

         IF (SUM(cell%perd(1:3)) == 0) THEN
            CALL section_vals_val_get(md_section, "ANGVEL_TOL", r_val=angvel_tol)
            iw = cp_print_key_unit_nr(logger, md_section, "PRINT%PROGRAM_RUN_INFO", &
                                      extension=".mdLog")

            CALL cp_subsys_get(subsys, molecule_kinds=molecule_kinds, &
                               particles=particles)

            natoms = SIZE(particles%els)
            ! Build a list of all fixed atoms (if any)
            ALLOCATE (is_fixed(natoms))

            is_fixed = use_perd_none
            molecule_kind_set => molecule_kinds%els
            DO imolecule_kind = 1, molecule_kinds%n_els
               molecule_kind => molecule_kind_set(imolecule_kind)
               CALL get_molecule_kind(molecule_kind=molecule_kind, fixd_list=fixd_list)
               IF (ASSOCIATED(fixd_list)) THEN
                  DO ifixd = 1, SIZE(fixd_list)
                     IF (.NOT. fixd_list(ifixd)%restraint%active) &
                        is_fixed(fixd_list(ifixd)%fixd) = fixd_list(ifixd)%itype
                  END DO
               END IF
            END DO

            ! If requested and the system is not periodic, subtract the angular velocity
            CALL compute_rcom(particles%els, is_fixed, rcom)
            CALL compute_vang(particles%els, is_fixed, rcom, vang)
            ! SQRT(DOT_PRODUCT(vang,vang))>angvel_tol
            IF (DOT_PRODUCT(vang, vang) > (angvel_tol*angvel_tol)) THEN
               CALL subtract_vang(particles%els, is_fixed, rcom, vang)

               ! Rescale velocities after removal
               temp_old = md_ener%temp_part/kelvin
               CALL scale_velocity(subsys, md_ener, temp_old, 0.0_dp, iw)
               CALL compute_vang(particles%els, is_fixed, rcom, vang_new)
               IF (iw > 0) THEN
                  WRITE (UNIT=iw, FMT="(T2,A,T30,3(1X,F16.10))") &
                     'MD_VEL| Old VANG [a.u.]', vang(1:3)
                  WRITE (UNIT=iw, FMT="(T2,A,T30,3(1X,F16.10))") &
                     'MD_VEL| New VANG [a.u.]', vang_new(1:3)
               END IF
            END IF

            DEALLOCATE (is_fixed)

            CALL cp_print_key_finished_output(iw, logger, md_section, &
                                              "PRINT%PROGRAM_RUN_INFO")
         END IF
      END IF

      CALL timestop(handle)
   END SUBROUTINE angvel_control

! **************************************************************************************************
!> \brief Initialize Velocities for MD runs
!> \param force_env ...
!> \param simpar ...
!> \param globenv ...
!> \param md_env ...
!> \param md_section ...
!> \param constraint_section ...
!> \param write_binary_restart_file ...
!> \par History
!>     Teodoro Laino - University of Zurich - 09.2007 [tlaino]
! **************************************************************************************************
   SUBROUTINE setup_velocities(force_env, simpar, globenv, md_env, md_section, &
                               constraint_section, write_binary_restart_file)

      TYPE(force_env_type), POINTER                      :: force_env
      TYPE(simpar_type), POINTER                         :: simpar
      TYPE(global_environment_type), POINTER             :: globenv
      TYPE(md_environment_type), POINTER                 :: md_env
      TYPE(section_vals_type), POINTER                   :: md_section, constraint_section
      LOGICAL, INTENT(IN)                                :: write_binary_restart_file

      CHARACTER(LEN=*), PARAMETER                        :: routineN = 'setup_velocities'

      INTEGER                                            :: handle, nconstraint, nconstraint_fixd
      LOGICAL                                            :: apply_cns0, shell_adiabatic, &
                                                            shell_present
      TYPE(atomic_kind_list_type), POINTER               :: atomic_kinds
      TYPE(cell_type), POINTER                           :: cell
      TYPE(cp_subsys_type), POINTER                      :: subsys
      TYPE(molecule_kind_list_type), POINTER             :: molecule_kinds
      TYPE(mp_para_env_type), POINTER                    :: para_env
      TYPE(particle_list_type), POINTER                  :: core_particles, particles, &
                                                            shell_particles
      TYPE(particle_type), DIMENSION(:), POINTER         :: core_particle_set, particle_set, &
                                                            shell_particle_set
      TYPE(section_vals_type), POINTER                   :: force_env_section, print_section, &
                                                            subsys_section

      CALL timeset(routineN, handle)

      NULLIFY (atomic_kinds, cell, para_env, subsys, molecule_kinds, core_particles, particles)
      NULLIFY (shell_particles, core_particle_set, particle_set, shell_particle_set)
      NULLIFY (force_env_section, print_section, subsys_section)

      print_section => section_vals_get_subs_vals(md_section, "PRINT")
      apply_cns0 = .FALSE.
      IF (simpar%constraint) THEN
         CALL section_vals_val_get(constraint_section, "CONSTRAINT_INIT", l_val=apply_cns0)
      END IF
      ! Always initialize velocities and possibly restart them
      CALL force_env_get(force_env, subsys=subsys, cell=cell, para_env=para_env, &
                         force_env_section=force_env_section)
      subsys_section => section_vals_get_subs_vals(force_env_section, "SUBSYS")

      CALL cp_subsys_get(subsys, &
                         atomic_kinds=atomic_kinds, &
                         core_particles=core_particles, &
                         molecule_kinds=molecule_kinds, &
                         particles=particles, &
                         shell_particles=shell_particles)

      CALL get_atomic_kind_set(atomic_kind_set=atomic_kinds%els, &
                               shell_present=shell_present, &
                               shell_adiabatic=shell_adiabatic)

      NULLIFY (core_particle_set)
      NULLIFY (particle_set)
      NULLIFY (shell_particle_set)
      particle_set => particles%els

      IF (shell_present .AND. shell_adiabatic) THEN
         ! Constraints are not yet implemented for core-shell models generally
         CALL get_molecule_kind_set(molecule_kind_set=molecule_kinds%els, &
                                    nconstraint=nconstraint, &
                                    nconstraint_fixd=nconstraint_fixd)
         IF (nconstraint - nconstraint_fixd /= 0) &
            CPABORT("Only the fixed atom constraint is implemented for core-shell models")
!MK    CPPostcondition(.NOT.simpar%constraint,cp_failure_level,routineP,failure)
         CPASSERT(ASSOCIATED(shell_particles))
         CPASSERT(ASSOCIATED(core_particles))
         shell_particle_set => shell_particles%els
         core_particle_set => core_particles%els
      END IF

      CALL initialize_velocities(simpar, &
                                 particle_set, &
                                 molecule_kinds=molecule_kinds, &
                                 force_env=force_env, &
                                 globenv=globenv, &
                                 md_env=md_env, &
                                 label="Velocities initialization", &
                                 print_section=print_section, &
                                 subsys_section=subsys_section, &
                                 shell_present=(shell_present .AND. shell_adiabatic), &
                                 shell_part=shell_particle_set, &
                                 core_part=core_particle_set, &
                                 force_rescaling=.FALSE., &
                                 para_env=para_env, &
                                 write_binary_restart_file=write_binary_restart_file)

      ! Apply constraints if required and rescale velocities..
      IF (simpar%ensemble /= reftraj_ensemble) THEN
         IF (apply_cns0) THEN
            CALL force_env_calc_energy_force(force_env, calc_force=.TRUE.)
            CALL force_env_shake(force_env, &
                                 shake_tol=simpar%shake_tol, &
                                 log_unit=simpar%info_constraint, &
                                 lagrange_mult=simpar%lagrange_multipliers, &
                                 dump_lm=simpar%dump_lm, &
                                 compold=.TRUE.)
            CALL force_env_rattle(force_env, shake_tol=simpar%shake_tol, &
                                  log_unit=simpar%info_constraint, lagrange_mult=simpar%lagrange_multipliers, &
                                  dump_lm=simpar%dump_lm, reset=.TRUE.)
            IF (simpar%do_respa) THEN
               CALL force_env_calc_energy_force(force_env%sub_force_env(1)%force_env, &
                                                calc_force=.TRUE.)
               CALL force_env_shake(force_env%sub_force_env(1)%force_env, &
                                    shake_tol=simpar%shake_tol, log_unit=simpar%info_constraint, &
                                    lagrange_mult=simpar%lagrange_multipliers, dump_lm=simpar%dump_lm, compold=.TRUE.)
               CALL force_env_rattle(force_env%sub_force_env(1)%force_env, &
                                     shake_tol=simpar%shake_tol, log_unit=simpar%info_constraint, &
                                     lagrange_mult=simpar%lagrange_multipliers, dump_lm=simpar%dump_lm, reset=.TRUE.)
            END IF
            ! Reinitialize velocities rescaling properly after rattle
            subsys_section => section_vals_get_subs_vals(force_env_section, "SUBSYS")
            CALL update_subsys(subsys_section, force_env, .FALSE., write_binary_restart_file)
            CALL initialize_velocities(simpar, &
                                       particle_set, &
                                       molecule_kinds=molecule_kinds, &
                                       force_env=force_env, &
                                       globenv=globenv, &
                                       md_env=md_env, &
                                       label="Re-Initializing velocities after applying constraints", &
                                       print_section=print_section, &
                                       subsys_section=subsys_section, &
                                       shell_present=(shell_present .AND. shell_adiabatic), &
                                       shell_part=shell_particle_set, &
                                       core_part=core_particle_set, &
                                       force_rescaling=.TRUE., &
                                       para_env=para_env, &
                                       write_binary_restart_file=write_binary_restart_file)
         END IF
      END IF

      ! Perform setup for a cascade run
      CALL initialize_cascade(simpar, particle_set, molecule_kinds, md_section)

      CALL timestop(handle)

   END SUBROUTINE setup_velocities

! **************************************************************************************************
!> \brief   Perform the initialization for a cascade run
!> \param simpar ...
!> \param particle_set ...
!> \param molecule_kinds ...
!> \param md_section ...
!> \date    05.02.2012
!> \author  Matthias Krack (MK)
!> \version 1.0
! **************************************************************************************************
   SUBROUTINE initialize_cascade(simpar, particle_set, molecule_kinds, md_section)

      TYPE(simpar_type), POINTER                         :: simpar
      TYPE(particle_type), DIMENSION(:), POINTER         :: particle_set
      TYPE(molecule_kind_list_type), POINTER             :: molecule_kinds
      TYPE(section_vals_type), POINTER                   :: md_section

      CHARACTER(LEN=*), PARAMETER :: routineN = 'initialize_cascade'

      CHARACTER(len=2*default_string_length)             :: line
      INTEGER                                            :: handle, iatom, ifixd, imolecule_kind, &
                                                            iparticle, iw, natom, nparticle
      INTEGER, ALLOCATABLE, DIMENSION(:)                 :: atom_index, is_fixed
      LOGICAL                                            :: init_cascade, is_ok, no_read_error
      REAL(KIND=dp)                                      :: ecom, ekin, energy, norm, temp, &
                                                            temperature
      REAL(KIND=dp), ALLOCATABLE, DIMENSION(:)           :: matom, weight
      REAL(KIND=dp), ALLOCATABLE, DIMENSION(:, :)        :: vatom
      REAL(KIND=dp), DIMENSION(3)                        :: vcom
      TYPE(atomic_kind_type), POINTER                    :: atomic_kind
      TYPE(cp_logger_type), POINTER                      :: logger
      TYPE(cp_sll_val_type), POINTER                     :: atom_list
      TYPE(fixd_constraint_type), DIMENSION(:), POINTER  :: fixd_list
      TYPE(molecule_kind_type), DIMENSION(:), POINTER    :: molecule_kind_set
      TYPE(molecule_kind_type), POINTER                  :: molecule_kind
      TYPE(section_vals_type), POINTER                   :: atom_list_section, cascade_section, &
                                                            print_section
      TYPE(val_type), POINTER                            :: val

      CALL timeset(routineN, handle)

      NULLIFY (atom_list)
      NULLIFY (atom_list_section)
      NULLIFY (atomic_kind)
      NULLIFY (cascade_section)
      NULLIFY (fixd_list)
      NULLIFY (molecule_kind)
      NULLIFY (molecule_kind_set)
      NULLIFY (logger)
      NULLIFY (val)

      logger => cp_get_default_logger()
      print_section => section_vals_get_subs_vals(md_section, "PRINT")
      iw = cp_print_key_unit_nr(logger, print_section, "PROGRAM_RUN_INFO", extension=".log")

      cascade_section => section_vals_get_subs_vals(md_section, "CASCADE")
      CALL section_vals_val_get(cascade_section, "_SECTION_PARAMETERS_", l_val=init_cascade)

      nparticle = SIZE(particle_set)

      IF (init_cascade) THEN

         CALL section_vals_val_get(cascade_section, "ENERGY", r_val=energy)
         IF (energy < 0.0_dp) &
            CPABORT("Error occurred reading &CASCADE section: Negative energy found")

         IF (iw > 0) THEN
            ekin = cp_unit_from_cp2k(energy, "keV")
            WRITE (UNIT=iw, FMT="(/,T2,A,T61,F20.6)") &
               "CASCADE| Energy [keV]", ekin
            WRITE (UNIT=iw, FMT="(T2,A)") &
               "CASCADE|"
         END IF

         ! Read the atomic velocities given in the input file
         atom_list_section => section_vals_get_subs_vals(cascade_section, "ATOM_LIST")
         CALL section_vals_val_get(atom_list_section, "_DEFAULT_KEYWORD_", n_rep_val=natom)
         CALL section_vals_list_get(atom_list_section, "_DEFAULT_KEYWORD_", list=atom_list)
         IF (natom <= 0) &
            CPABORT("Error occurred reading &CASCADE section: No atom list found")

         IF (iw > 0) THEN
            WRITE (UNIT=iw, FMT="(T2,A,T11,A,3(11X,A),9X,A)") &
               "CASCADE| ", "Atom index", "v(x)", "v(y)", "v(z)", "weight"
         END IF

         ALLOCATE (atom_index(natom))
         ALLOCATE (matom(natom))
         ALLOCATE (vatom(3, natom))
         ALLOCATE (weight(natom))

         DO iatom = 1, natom
            is_ok = cp_sll_val_next(atom_list, val)
            CALL val_get(val, c_val=line)
            ! Read atomic index, velocity vector, and weight
            no_read_error = .FALSE.
            READ (UNIT=line, FMT=*, ERR=999) atom_index(iatom), vatom(1:3, iatom), weight(iatom)
            no_read_error = .TRUE.
999         IF (.NOT. no_read_error) &
               CPABORT("Error occurred reading &CASCADE section. Last line read <"//TRIM(line)//">")
            IF ((atom_index(iatom) <= 0) .OR. ((atom_index(iatom) > nparticle))) &
               CPABORT("Error occurred reading &CASCADE section: Invalid atom index found")
            IF (weight(iatom) < 0.0_dp) &
               CPABORT("Error occurred reading &CASCADE section: Negative weight found")
            IF (iw > 0) THEN
               WRITE (UNIT=iw, FMT="(T2,A,I10,4(1X,F14.6))") &
                  "CASCADE| ", atom_index(iatom), vatom(1:3, iatom), weight(iatom)
            END IF
         END DO

         ! Normalise velocities and weights
         norm = 0.0_dp
         DO iatom = 1, natom
            iparticle = atom_index(iatom)
            IF (particle_set(iparticle)%shell_index /= 0) THEN
               CPWARN("Warning: The primary knock-on atom is a core-shell atom")
            END IF
            atomic_kind => particle_set(iparticle)%atomic_kind
            CALL get_atomic_kind(atomic_kind=atomic_kind, mass=matom(iatom))
            norm = norm + matom(iatom)*weight(iatom)
         END DO
         weight(:) = matom(:)*weight(:)*energy/norm
         DO iatom = 1, natom
            norm = SQRT(DOT_PRODUCT(vatom(1:3, iatom), vatom(1:3, iatom)))
            vatom(1:3, iatom) = vatom(1:3, iatom)/norm
         END DO

         IF (iw > 0) THEN
            WRITE (UNIT=iw, FMT="(T2,A)") &
               "CASCADE|", &
               "CASCADE| Normalised velocities and additional kinetic energy [keV]", &
               "CASCADE|"
            WRITE (UNIT=iw, FMT="(T2,A,T11,A,3(11X,A),9X,A)") &
               "CASCADE| ", "Atom index", "v(x)", "v(y)", "v(z)", "E(kin)"
            DO iatom = 1, natom
               ekin = cp_unit_from_cp2k(weight(iatom), "keV")
               WRITE (UNIT=iw, FMT="(T2,A,I10,4(1X,F14.6))") &
                  "CASCADE| ", atom_index(iatom), vatom(1:3, iatom), ekin
            END DO
         END IF

         ! Apply velocity modifications
         DO iatom = 1, natom
            iparticle = atom_index(iatom)
            particle_set(iparticle)%v(:) = particle_set(iparticle)%v(:) + &
                                           SQRT(2.0_dp*weight(iatom)/matom(iatom))*vatom(1:3, iatom)
         END DO

         DEALLOCATE (atom_index)
         DEALLOCATE (matom)
         DEALLOCATE (vatom)
         DEALLOCATE (weight)

         IF (iw > 0) THEN
            ! Build a list of all fixed atoms (if any)
            ALLOCATE (is_fixed(nparticle))
            is_fixed = use_perd_none
            molecule_kind_set => molecule_kinds%els
            DO imolecule_kind = 1, molecule_kinds%n_els
               molecule_kind => molecule_kind_set(imolecule_kind)
               CALL get_molecule_kind(molecule_kind=molecule_kind, fixd_list=fixd_list)
               IF (ASSOCIATED(fixd_list)) THEN
                  DO ifixd = 1, SIZE(fixd_list)
                     IF (.NOT. fixd_list(ifixd)%restraint%active) is_fixed(fixd_list(ifixd)%fixd) = fixd_list(ifixd)%itype
                  END DO
               END IF
            END DO
            ! Compute vcom, ecom and ekin for printout
            CALL compute_vcom(particle_set, is_fixed, vcom, ecom)
            ekin = compute_ekin(particle_set) - ecom
            IF (simpar%nfree == 0) THEN
               CPASSERT(ekin == 0.0_dp)
               temp = 0.0_dp
            ELSE
               temp = 2.0_dp*ekin/REAL(simpar%nfree, KIND=dp)
            END IF
            temperature = cp_unit_from_cp2k(temp, "K")
            WRITE (UNIT=iw, FMT="(T2,A)") &
               "CASCADE|"
            WRITE (UNIT=iw, FMT="(T2,A,T61,F20.6)") &
               "CASCADE| Temperature after cascade initialization [K]", temperature
            WRITE (UNIT=iw, FMT="(T2,A,T30,3(1X,ES16.8))") &
               "CASCADE| COM velocity", vcom(1:3)
!MK          ! compute and log rcom and vang if not periodic
!MK          CALL force_env_get(force_env,cell=cell)
!MK          IF (SUM(cell%perd(1:3)) == 0) THEN
!MK             CALL compute_rcom(particle_set,is_fixed,rcom)
!MK             CALL compute_vang(particle_set,is_fixed,rcom,vang)
!MK             WRITE (iw, '( A, T21, F20.12 , F20.12 , F20.12 )' ) ' COM position:',rcom(1:3)
!MK             WRITE (iw, '( A, T21, F20.12 , F20.12 , F20.12 )' ) ' Angular velocity:',vang(1:3)
!MK          END IF
            DEALLOCATE (is_fixed)
         END IF

      END IF

      CALL cp_print_key_finished_output(iw, logger, print_section, "PROGRAM_RUN_INFO")

      CALL timestop(handle)

   END SUBROUTINE initialize_cascade

END MODULE md_vel_utils
