!--------------------------------------------------------------------------------------------------!
!   CP2K: A general program to perform molecular dynamics simulations                              !
!   Copyright (C) 2000 - 2018  CP2K developers group                                               !
!--------------------------------------------------------------------------------------------------!

! **************************************************************************************************
!> \brief Module that handles the COLLECTIVE constraints
!> \par History
!>      none
!> \author Teodoro Laino [tlaino]
! **************************************************************************************************
MODULE constraint_clv
   USE cell_types,                      ONLY: cell_type
   USE colvar_methods,                  ONLY: colvar_eval_mol_f
   USE colvar_types,                    ONLY: colvar_type,&
                                              diff_colvar
   USE input_section_types,             ONLY: section_vals_get,&
                                              section_vals_get_subs_vals,&
                                              section_vals_type,&
                                              section_vals_val_get,&
                                              section_vals_val_set
   USE kinds,                           ONLY: dp
   USE mathlib,                         ONLY: matvec_3x3
   USE molecule_kind_types,             ONLY: colvar_constraint_type,&
                                              fixd_constraint_type,&
                                              get_molecule_kind,&
                                              molecule_kind_type
   USE molecule_types,                  ONLY: get_molecule,&
                                              global_constraint_type,&
                                              local_colvar_constraint_type,&
                                              molecule_type
   USE particle_types,                  ONLY: particle_type
#include "./base/base_uses.f90"

   IMPLICIT NONE

   PRIVATE
   PUBLIC :: shake_roll_colv_int, &
             rattle_roll_colv_int, &
             shake_colv_int, &
             rattle_colv_int, &
             shake_roll_colv_ext, &
             rattle_roll_colv_ext, &
             shake_colv_ext, &
             rattle_colv_ext, &
             shake_update_colv_int, &
             shake_update_colv_ext

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

CONTAINS

! **************************************************************************************************
!> \brief Intramolecular subroutine
!>      shake_colv algorithm for collective variables constraints
!>      updates the multiplier one molecule type at a time
!> \param molecule ...
!> \param particle_set ...
!> \param pos ...
!> \param vel ...
!> \param dt ...
!> \param ishake ...
!> \param cell ...
!> \param imass ...
!> \param max_sigma ...
!> \par History
!>      none
!> \author Teodoro Laino [tlaino]
! **************************************************************************************************
   SUBROUTINE shake_colv_int(molecule, particle_set, pos, vel, dt, ishake, &
                             cell, imass, max_sigma)

      TYPE(molecule_type), POINTER                       :: molecule
      TYPE(particle_type), POINTER                       :: particle_set(:)
      REAL(KIND=dp), INTENT(INOUT)                       :: pos(:, :), vel(:, :)
      REAL(kind=dp), INTENT(in)                          :: dt
      INTEGER, INTENT(IN)                                :: ishake
      TYPE(cell_type), POINTER                           :: cell
      REAL(KIND=dp), DIMENSION(:)                        :: imass
      REAL(KIND=dp), INTENT(INOUT)                       :: max_sigma

      CHARACTER(len=*), PARAMETER :: routineN = 'shake_colv_int', routineP = moduleN//':'//routineN

      TYPE(colvar_constraint_type), POINTER              :: colv_list(:)
      TYPE(fixd_constraint_type), DIMENSION(:), POINTER  :: fixd_list
      TYPE(local_colvar_constraint_type), POINTER        :: lcolv(:)
      TYPE(molecule_kind_type), POINTER                  :: molecule_kind

      NULLIFY (fixd_list)
      molecule_kind => molecule%molecule_kind
      CALL get_molecule_kind(molecule_kind, colv_list=colv_list, fixd_list=fixd_list)
      CALL get_molecule(molecule, lcolv=lcolv)
      ! Real Shake
      CALL shake_colv_low(fixd_list, colv_list, lcolv, &
                          particle_set, pos, vel, dt, ishake, cell, imass, max_sigma)

   END SUBROUTINE shake_colv_int

! **************************************************************************************************
!> \brief Intramolecular subroutine for updating the TARGET value of collective
!>        constraints
!> \param molecule ...
!> \param dt ...
!> \param motion_section ...
!> \author Teodoro Laino [tlaino] - University of Zurich
! **************************************************************************************************
   SUBROUTINE shake_update_colv_int(molecule, dt, motion_section)

      TYPE(molecule_type), POINTER                       :: molecule
      REAL(kind=dp), INTENT(in)                          :: dt
      TYPE(section_vals_type), POINTER                   :: motion_section

      CHARACTER(len=*), PARAMETER :: routineN = 'shake_update_colv_int', &
         routineP = moduleN//':'//routineN

      TYPE(colvar_constraint_type), POINTER              :: colv_list(:)
      TYPE(molecule_kind_type), POINTER                  :: molecule_kind

      molecule_kind => molecule%molecule_kind
      CALL get_molecule_kind(molecule_kind, colv_list=colv_list)
      ! Real update of the Shake target
      CALL shake_update_colv_low(colv_list, dt, motion_section)

   END SUBROUTINE shake_update_colv_int

! **************************************************************************************************
!> \brief Intramolecular subroutine
!>      rattle algorithm for collective variables constraints
!>      updates the multiplier one molecule type at a time
!> \param molecule ...
!> \param particle_set ...
!> \param vel ...
!> \param dt ...
!> \param irattle ...
!> \param cell ...
!> \param imass ...
!> \param max_sigma ...
!> \par History
!>      none
!> \author Teodoro Laino [tlaino]
! **************************************************************************************************
   SUBROUTINE rattle_colv_int(molecule, particle_set, vel, dt, irattle, &
                              cell, imass, max_sigma)

      TYPE(molecule_type), POINTER                       :: molecule
      TYPE(particle_type), POINTER                       :: particle_set(:)
      REAL(KIND=dp), INTENT(INOUT)                       :: vel(:, :)
      REAL(kind=dp), INTENT(in)                          :: dt
      INTEGER, INTENT(IN)                                :: irattle
      TYPE(cell_type), POINTER                           :: cell
      REAL(KIND=dp), DIMENSION(:)                        :: imass
      REAL(KIND=dp), INTENT(INOUT)                       :: max_sigma

      TYPE(colvar_constraint_type), POINTER              :: colv_list(:)
      TYPE(fixd_constraint_type), DIMENSION(:), POINTER  :: fixd_list
      TYPE(local_colvar_constraint_type), POINTER        :: lcolv(:)
      TYPE(molecule_kind_type), POINTER                  :: molecule_kind

      NULLIFY (fixd_list)
      molecule_kind => molecule%molecule_kind
      CALL get_molecule_kind(molecule_kind, colv_list=colv_list, fixd_list=fixd_list)
      CALL get_molecule(molecule, lcolv=lcolv)
      ! Real Rattle
      CALL rattle_colv_low(fixd_list, colv_list, lcolv, &
                           particle_set, vel, dt, irattle, cell, imass, max_sigma)

   END SUBROUTINE rattle_colv_int

! **************************************************************************************************
!> \brief Intramolecular subroutine
!>      shake algorithm (box allowed to change) for collective variables constraints
!>      updates the multiplier one molecule type at a time
!> \param molecule ...
!> \param particle_set ...
!> \param pos ...
!> \param vel ...
!> \param r_shake ...
!> \param v_shake ...
!> \param dt ...
!> \param ishake ...
!> \param cell ...
!> \param imass ...
!> \param max_sigma ...
!> \par History
!>      none
!> \author Teodoro Laino [tlaino]
! **************************************************************************************************
   SUBROUTINE shake_roll_colv_int(molecule, particle_set, pos, vel, r_shake, v_shake, &
                                  dt, ishake, cell, imass, max_sigma)

      TYPE(molecule_type), POINTER                       :: molecule
      TYPE(particle_type), POINTER                       :: particle_set(:)
      REAL(KIND=dp), INTENT(INOUT)                       :: pos(:, :), vel(:, :)
      REAL(KIND=dp), DIMENSION(:, :), INTENT(IN)         :: r_shake, v_shake
      REAL(kind=dp), INTENT(in)                          :: dt
      INTEGER, INTENT(in)                                :: ishake
      TYPE(cell_type), POINTER                           :: cell
      REAL(KIND=dp), DIMENSION(:)                        :: imass
      REAL(KIND=dp), INTENT(INOUT)                       :: max_sigma

      TYPE(colvar_constraint_type), POINTER              :: colv_list(:)
      TYPE(fixd_constraint_type), DIMENSION(:), POINTER  :: fixd_list
      TYPE(local_colvar_constraint_type), POINTER        :: lcolv(:)
      TYPE(molecule_kind_type), POINTER                  :: molecule_kind

      NULLIFY (fixd_list)
      molecule_kind => molecule%molecule_kind
      CALL get_molecule_kind(molecule_kind, colv_list=colv_list, fixd_list=fixd_list)
      CALL get_molecule(molecule, lcolv=lcolv)
      ! Real Shake
      CALL shake_roll_colv_low(fixd_list, colv_list, lcolv, &
                               particle_set, pos, vel, r_shake, v_shake, dt, ishake, cell, &
                               imass, max_sigma)

   END SUBROUTINE shake_roll_colv_int

! **************************************************************************************************
!> \brief Intramolecular subroutine
!>      rattle algorithm (box allowed to change) for collective variables constraints
!>      updates the multiplier one molecule type at a time
!> \param molecule ...
!> \param particle_set ...
!> \param vel ...
!> \param r_rattle ...
!> \param dt ...
!> \param irattle ...
!> \param veps ...
!> \param cell ...
!> \param imass ...
!> \param max_sigma ...
!> \par History
!>      none
!> \author Teodoro Laino [tlaino]
! **************************************************************************************************
   SUBROUTINE rattle_roll_colv_int(molecule, particle_set, vel, r_rattle, &
                                   dt, irattle, veps, cell, imass, max_sigma)

      TYPE(molecule_type), POINTER                       :: molecule
      TYPE(particle_type), POINTER                       :: particle_set(:)
      REAL(KIND=dp), INTENT(INOUT)                       :: vel(:, :)
      REAL(KIND=dp), INTENT(IN)                          :: r_rattle(:, :), dt
      INTEGER, INTENT(in)                                :: irattle
      REAL(KIND=dp), INTENT(IN)                          :: veps(:, :)
      TYPE(cell_type), POINTER                           :: cell
      REAL(KIND=dp), DIMENSION(:)                        :: imass
      REAL(KIND=dp), INTENT(INOUT)                       :: max_sigma

      TYPE(colvar_constraint_type), POINTER              :: colv_list(:)
      TYPE(fixd_constraint_type), DIMENSION(:), POINTER  :: fixd_list
      TYPE(local_colvar_constraint_type), POINTER        :: lcolv(:)
      TYPE(molecule_kind_type), POINTER                  :: molecule_kind

      NULLIFY (fixd_list)
      molecule_kind => molecule%molecule_kind
      CALL get_molecule_kind(molecule_kind, colv_list=colv_list, fixd_list=fixd_list)
      CALL get_molecule(molecule, lcolv=lcolv)
      ! Real Rattle
      CALL rattle_roll_colv_low(fixd_list, colv_list, lcolv, &
                                particle_set, vel, r_rattle, dt, irattle, veps, cell, &
                                imass, max_sigma)

   END SUBROUTINE rattle_roll_colv_int

! **************************************************************************************************
!> \brief Intermolecular subroutine
!>      shake_colv algorithm for collective variables constraints
!>      updates the multiplier one molecule type at a time
!> \param gci ...
!> \param particle_set ...
!> \param pos ...
!> \param vel ...
!> \param dt ...
!> \param ishake ...
!> \param cell ...
!> \param imass ...
!> \param max_sigma ...
!> \par History
!>      none
!> \author Teodoro Laino [tlaino]
! **************************************************************************************************
   SUBROUTINE shake_colv_ext(gci, particle_set, pos, vel, dt, ishake, &
                             cell, imass, max_sigma)

      TYPE(global_constraint_type), POINTER              :: gci
      TYPE(particle_type), POINTER                       :: particle_set(:)
      REAL(KIND=dp), INTENT(INOUT)                       :: pos(:, :), vel(:, :)
      REAL(kind=dp), INTENT(in)                          :: dt
      INTEGER, INTENT(IN)                                :: ishake
      TYPE(cell_type), POINTER                           :: cell
      REAL(KIND=dp), DIMENSION(:)                        :: imass
      REAL(KIND=dp), INTENT(INOUT)                       :: max_sigma

      CHARACTER(len=*), PARAMETER :: routineN = 'shake_colv_ext', routineP = moduleN//':'//routineN

      TYPE(colvar_constraint_type), POINTER              :: colv_list(:)
      TYPE(fixd_constraint_type), DIMENSION(:), POINTER  :: fixd_list
      TYPE(local_colvar_constraint_type), POINTER        :: lcolv(:)

      colv_list => gci%colv_list
      fixd_list => gci%fixd_list
      lcolv => gci%lcolv
      ! Real Shake
      CALL shake_colv_low(fixd_list, colv_list, lcolv, &
                          particle_set, pos, vel, dt, ishake, cell, imass, max_sigma)

   END SUBROUTINE shake_colv_ext

! **************************************************************************************************
!> \brief Intermolecular subroutine for updating the TARGET value for collective
!>        constraints
!> \param gci ...
!> \param dt ...
!> \param motion_section ...
!> \author Teodoro Laino [tlaino]
! **************************************************************************************************
   SUBROUTINE shake_update_colv_ext(gci, dt, motion_section)

      TYPE(global_constraint_type), POINTER              :: gci
      REAL(kind=dp), INTENT(in)                          :: dt
      TYPE(section_vals_type), POINTER                   :: motion_section

      CHARACTER(len=*), PARAMETER :: routineN = 'shake_update_colv_ext', &
         routineP = moduleN//':'//routineN

      TYPE(colvar_constraint_type), POINTER              :: colv_list(:)

      colv_list => gci%colv_list
      ! Real update of the Shake target
      CALL shake_update_colv_low(colv_list, dt, motion_section)

   END SUBROUTINE shake_update_colv_ext

! **************************************************************************************************
!> \brief Intermolecular subroutine
!>      rattle algorithm for collective variables constraints
!>      updates the multiplier one molecule type at a time
!> \param gci ...
!> \param particle_set ...
!> \param vel ...
!> \param dt ...
!> \param irattle ...
!> \param cell ...
!> \param imass ...
!> \param max_sigma ...
!> \par History
!>      none
!> \author Teodoro Laino [tlaino]
! **************************************************************************************************
   SUBROUTINE rattle_colv_ext(gci, particle_set, vel, dt, irattle, &
                              cell, imass, max_sigma)

      TYPE(global_constraint_type), POINTER              :: gci
      TYPE(particle_type), POINTER                       :: particle_set(:)
      REAL(KIND=dp), INTENT(INOUT)                       :: vel(:, :)
      REAL(kind=dp), INTENT(in)                          :: dt
      INTEGER, INTENT(IN)                                :: irattle
      TYPE(cell_type), POINTER                           :: cell
      REAL(KIND=dp), DIMENSION(:)                        :: imass
      REAL(KIND=dp), INTENT(INOUT)                       :: max_sigma

      TYPE(colvar_constraint_type), POINTER              :: colv_list(:)
      TYPE(fixd_constraint_type), DIMENSION(:), POINTER  :: fixd_list
      TYPE(local_colvar_constraint_type), POINTER        :: lcolv(:)

      colv_list => gci%colv_list
      fixd_list => gci%fixd_list
      lcolv => gci%lcolv
      ! Real Rattle
      CALL rattle_colv_low(fixd_list, colv_list, lcolv, &
                           particle_set, vel, dt, irattle, cell, imass, max_sigma)

   END SUBROUTINE rattle_colv_ext

! **************************************************************************************************
!> \brief Intermolecular subroutine
!>      shake algorithm (box allowed to change) for collective variables constraints
!>      updates the multiplier one molecule type at a time
!> \param gci ...
!> \param particle_set ...
!> \param pos ...
!> \param vel ...
!> \param r_shake ...
!> \param v_shake ...
!> \param dt ...
!> \param ishake ...
!> \param cell ...
!> \param imass ...
!> \param max_sigma ...
!> \par History
!>      none
!> \author Teodoro Laino [tlaino]
! **************************************************************************************************
   SUBROUTINE shake_roll_colv_ext(gci, particle_set, pos, vel, r_shake, v_shake, &
                                  dt, ishake, cell, imass, max_sigma)

      TYPE(global_constraint_type), POINTER              :: gci
      TYPE(particle_type), POINTER                       :: particle_set(:)
      REAL(KIND=dp), INTENT(INOUT)                       :: pos(:, :), vel(:, :)
      REAL(KIND=dp), DIMENSION(:, :), INTENT(IN)         :: r_shake, v_shake
      REAL(kind=dp), INTENT(in)                          :: dt
      INTEGER, INTENT(in)                                :: ishake
      TYPE(cell_type), POINTER                           :: cell
      REAL(KIND=dp), DIMENSION(:)                        :: imass
      REAL(KIND=dp), INTENT(INOUT)                       :: max_sigma

      TYPE(colvar_constraint_type), POINTER              :: colv_list(:)
      TYPE(fixd_constraint_type), DIMENSION(:), POINTER  :: fixd_list
      TYPE(local_colvar_constraint_type), POINTER        :: lcolv(:)

      colv_list => gci%colv_list
      fixd_list => gci%fixd_list
      lcolv => gci%lcolv
      ! Real Shake
      CALL shake_roll_colv_low(fixd_list, colv_list, lcolv, &
                               particle_set, pos, vel, r_shake, v_shake, dt, ishake, cell, &
                               imass, max_sigma)

   END SUBROUTINE shake_roll_colv_ext

! **************************************************************************************************
!> \brief Intermolecular subroutine
!>      rattle algorithm (box allowed to change) for collective variables constraints
!>      updates the multiplier one molecule type at a time
!> \param gci ...
!> \param particle_set ...
!> \param vel ...
!> \param r_rattle ...
!> \param dt ...
!> \param irattle ...
!> \param veps ...
!> \param cell ...
!> \param imass ...
!> \param max_sigma ...
!> \par History
!>      none
!> \author Teodoro Laino [tlaino]
! **************************************************************************************************
   SUBROUTINE rattle_roll_colv_ext(gci, particle_set, vel, r_rattle, &
                                   dt, irattle, veps, cell, imass, max_sigma)

      TYPE(global_constraint_type), POINTER              :: gci
      TYPE(particle_type), POINTER                       :: particle_set(:)
      REAL(KIND=dp), INTENT(INOUT)                       :: vel(:, :)
      REAL(KIND=dp), INTENT(IN)                          :: r_rattle(:, :), dt
      INTEGER, INTENT(in)                                :: irattle
      REAL(KIND=dp), INTENT(IN)                          :: veps(:, :)
      TYPE(cell_type), POINTER                           :: cell
      REAL(KIND=dp), DIMENSION(:)                        :: imass
      REAL(KIND=dp), INTENT(INOUT)                       :: max_sigma

      TYPE(colvar_constraint_type), POINTER              :: colv_list(:)
      TYPE(fixd_constraint_type), DIMENSION(:), POINTER  :: fixd_list
      TYPE(local_colvar_constraint_type), POINTER        :: lcolv(:)

      colv_list => gci%colv_list
      fixd_list => gci%fixd_list
      lcolv => gci%lcolv
      ! Real Rattle
      CALL rattle_roll_colv_low(fixd_list, colv_list, lcolv, &
                                particle_set, vel, r_rattle, dt, irattle, veps, cell, &
                                imass, max_sigma)

   END SUBROUTINE rattle_roll_colv_ext

! **************************************************************************************************
!> \brief Real Shake subroutine - Low Level
!>      shake_colv algorithm for collective variables constraints
!>      updates the multiplier one molecule type at a time
!> \param fixd_list ...
!> \param colv_list ...
!> \param lcolv ...
!> \param particle_set ...
!> \param pos ...
!> \param vel ...
!> \param dt ...
!> \param ishake ...
!> \param cell ...
!> \param imass ...
!> \param max_sigma ...
!> \par History
!>      none
!> \author Teodoro Laino [tlaino]
! **************************************************************************************************
   SUBROUTINE shake_colv_low(fixd_list, colv_list, lcolv, &
                             particle_set, pos, vel, dt, ishake, cell, imass, max_sigma)
      TYPE(fixd_constraint_type), DIMENSION(:), POINTER  :: fixd_list
      TYPE(colvar_constraint_type), POINTER              :: colv_list(:)
      TYPE(local_colvar_constraint_type), POINTER        :: lcolv(:)
      TYPE(particle_type), POINTER                       :: particle_set(:)
      REAL(KIND=dp), INTENT(INOUT)                       :: pos(:, :), vel(:, :)
      REAL(kind=dp), INTENT(in)                          :: dt
      INTEGER, INTENT(IN)                                :: ishake
      TYPE(cell_type), POINTER                           :: cell
      REAL(KIND=dp), DIMENSION(:)                        :: imass
      REAL(KIND=dp), INTENT(INOUT)                       :: max_sigma

      CHARACTER(len=*), PARAMETER :: routineN = 'shake_colv_low', routineP = moduleN//':'//routineN

      INTEGER                                            :: iconst
      REAL(KIND=dp)                                      :: del_lam, dtby2, dtsqby2, fdotf_sum

      dtsqby2 = dt*dt*.5_dp
      dtby2 = dt*.5_dp
      IF (ishake == 1) THEN
         DO iconst = 1, SIZE(colv_list)
            IF (colv_list(iconst)%restraint%active) CYCLE
            ! Update positions
            CALL update_con_colv(pos, dtsqby2, lcolv(iconst), &
                                 lambda=lcolv(iconst)%lambda, &
                                 imass=imass)
            ! Update velocities
            CALL update_con_colv(vel, dtby2, lcolv(iconst), &
                                 lambda=lcolv(iconst)%lambda, &
                                 imass=imass)
         END DO
      ELSE
         DO iconst = 1, SIZE(colv_list)
            IF (colv_list(iconst)%restraint%active) CYCLE
            ! Update colvar
            CALL colvar_eval_mol_f(lcolv(iconst)%colvar, cell, particles=particle_set, &
                                   pos=pos, fixd_list=fixd_list)
            lcolv(iconst)%sigma = diff_colvar(lcolv(iconst)%colvar, &
                                              colv_list(iconst)%expected_value)
            fdotf_sum = eval_Jac_colvar(lcolv(iconst)%colvar, &
                                        lcolv(iconst)%colvar_old, imass=imass)
            del_lam = 2.0_dp*lcolv(iconst)%sigma/(dt*dt*fdotf_sum)
            lcolv(iconst)%lambda = lcolv(iconst)%lambda+del_lam

            ! Update positions
            CALL update_con_colv(pos, dtsqby2, lcolv(iconst), &
                                 lambda=del_lam, &
                                 imass=imass)
            ! Update velocities
            CALL update_con_colv(vel, dtby2, lcolv(iconst), &
                                 lambda=del_lam, &
                                 imass=imass)
         END DO
      END IF
      ! computing the constraint and value of tolerance
      DO iconst = 1, SIZE(colv_list)
         IF (colv_list(iconst)%restraint%active) CYCLE
         CALL colvar_eval_mol_f(lcolv(iconst)%colvar, cell, particles=particle_set, &
                                pos=pos, fixd_list=fixd_list)
         lcolv(iconst)%sigma = diff_colvar(lcolv(iconst)%colvar, &
                                           colv_list(iconst)%expected_value)
         max_sigma = MAX(ABS(lcolv(iconst)%sigma), max_sigma)
      END DO
   END SUBROUTINE shake_colv_low

! **************************************************************************************************
!> \brief Real Shake subroutine - Low Level - for updating the TARGET value
!> \param colv_list ...
!> \param dt ...
!> \param motion_section ...
!> \date 02.2008
!> \author Teodoro Laino [tlaino] - University of Zurich
! **************************************************************************************************
   SUBROUTINE shake_update_colv_low(colv_list, dt, motion_section)
      TYPE(colvar_constraint_type), POINTER              :: colv_list(:)
      REAL(kind=dp), INTENT(in)                          :: dt
      TYPE(section_vals_type), POINTER                   :: motion_section

      CHARACTER(len=*), PARAMETER :: routineN = 'shake_update_colv_low', &
         routineP = moduleN//':'//routineN

      INTEGER                                            :: iconst, irep, n_rep
      LOGICAL                                            :: do_update_colvar, explicit
      REAL(KIND=dp)                                      :: clv_target, limit, new_clv_target, value
      TYPE(section_vals_type), POINTER                   :: collective_sections

! Update globally for restart

      collective_sections => section_vals_get_subs_vals(motion_section, "CONSTRAINT%COLLECTIVE")
      CALL section_vals_get(collective_sections, n_repetition=n_rep)
      IF (n_rep /= 0) THEN
         DO irep = 1, n_rep
            CALL section_vals_val_get(collective_sections, "TARGET_GROWTH", r_val=value, &
                                      i_rep_section=irep)
            IF (value /= 0.0_dp) THEN
               CALL section_vals_val_get(collective_sections, "TARGET", r_val=clv_target, &
                                         i_rep_section=irep)
               new_clv_target = clv_target+value*dt
               ! Check limits..
               CALL section_vals_val_get(collective_sections, "TARGET_LIMIT", explicit=explicit, &
                                         i_rep_section=irep)
               do_update_colvar = .TRUE.
               IF (explicit) THEN
                  CALL section_vals_val_get(collective_sections, "TARGET_LIMIT", r_val=limit, &
                                            i_rep_section=irep)
                  IF (value > 0.0_dp) THEN
                     IF (clv_target == limit) THEN
                        do_update_colvar = .FALSE.
                     ELSE IF (new_clv_target >= limit) THEN
                        new_clv_target = limit
                     END IF
                  ELSE
                     IF (clv_target == limit) THEN
                        do_update_colvar = .FALSE.
                     ELSE IF (new_clv_target <= limit) THEN
                        new_clv_target = limit
                     END IF
                  ENDIF
               END IF
               IF (do_update_colvar) THEN
                  CALL section_vals_val_set(collective_sections, "TARGET", r_val=new_clv_target, &
                                            i_rep_section=irep)
               END IF
            END IF
         END DO
      END IF

      ! Update locally the value to each processor
      DO iconst = 1, SIZE(colv_list)
         ! Update local to each processor
         IF (colv_list(iconst)%expected_value_growth_speed == 0.0_dp) CYCLE
         CALL section_vals_val_get(collective_sections, "TARGET", &
                                   r_val=colv_list(iconst)%expected_value, &
                                   i_rep_section=colv_list(iconst)%inp_seq_num)
      END DO

   END SUBROUTINE shake_update_colv_low

! **************************************************************************************************
!> \brief Real Rattle - Low Level
!>      rattle algorithm for collective variables constraints
!>      updates the multiplier one molecule type at a time
!> \param fixd_list ...
!> \param colv_list ...
!> \param lcolv ...
!> \param particle_set ...
!> \param vel ...
!> \param dt ...
!> \param irattle ...
!> \param cell ...
!> \param imass ...
!> \param max_sigma ...
!> \par History
!>      none
!> \author Teodoro Laino [tlaino]
! **************************************************************************************************
   SUBROUTINE rattle_colv_low(fixd_list, colv_list, lcolv, &
                              particle_set, vel, dt, irattle, cell, imass, max_sigma)

      TYPE(fixd_constraint_type), DIMENSION(:), POINTER  :: fixd_list
      TYPE(colvar_constraint_type), POINTER              :: colv_list(:)
      TYPE(local_colvar_constraint_type), POINTER        :: lcolv(:)
      TYPE(particle_type), POINTER                       :: particle_set(:)
      REAL(KIND=dp), INTENT(INOUT)                       :: vel(:, :)
      REAL(kind=dp), INTENT(in)                          :: dt
      INTEGER, INTENT(IN)                                :: irattle
      TYPE(cell_type), POINTER                           :: cell
      REAL(KIND=dp), DIMENSION(:)                        :: imass
      REAL(KIND=dp), INTENT(INOUT)                       :: max_sigma

      INTEGER                                            :: iconst
      REAL(KIND=dp)                                      :: del_lam, dtby2, fdotf_sum

      dtby2 = dt*.5_dp
      IF (irattle == 1) THEN
         DO iconst = 1, SIZE(colv_list)
            IF (colv_list(iconst)%restraint%active) CYCLE
            ! Update colvar_old
            CALL colvar_eval_mol_f(lcolv(iconst)%colvar_old, cell, &
                                   particles=particle_set, fixd_list=fixd_list)
            ! Update velocities
            CALL update_con_colv(vel, dtby2, lcolv(iconst), &
                                 lambda=lcolv(iconst)%lambda, &
                                 imass=imass)
         END DO
      ELSE
         DO iconst = 1, SIZE(colv_list)
            IF (colv_list(iconst)%restraint%active) CYCLE
            lcolv(iconst)%sigma = rattle_con_eval(lcolv(iconst)%colvar_old, vel)
            fdotf_sum = eval_Jac_colvar(lcolv(iconst)%colvar_old, &
                                        lcolv(iconst)%colvar_old, imass=imass)
            del_lam = 2.0_dp*lcolv(iconst)%sigma/(dt*fdotf_sum)
            lcolv(iconst)%lambda = lcolv(iconst)%lambda+del_lam

            ! Update velocities
            CALL update_con_colv(vel, dtby2, lcolv(iconst), &
                                 lambda=del_lam, &
                                 imass=imass)
         END DO
      END IF

      DO iconst = 1, SIZE(colv_list)
         IF (colv_list(iconst)%restraint%active) CYCLE
         lcolv(iconst)%sigma = rattle_con_eval(lcolv(iconst)%colvar_old, vel)
         max_sigma = MAX(ABS(lcolv(iconst)%sigma), max_sigma)
      END DO

   END SUBROUTINE rattle_colv_low

! **************************************************************************************************
!> \brief Real shake_roll - Low Level
!>      shake algorithm (box allowed to change) for collective variables constraints
!>      updates the multiplier one molecule type at a time
!> \param fixd_list ...
!> \param colv_list ...
!> \param lcolv ...
!> \param particle_set ...
!> \param pos ...
!> \param vel ...
!> \param r_shake ...
!> \param v_shake ...
!> \param dt ...
!> \param ishake ...
!> \param cell ...
!> \param imass ...
!> \param max_sigma ...
!> \par History
!>      none
!> \author Teodoro Laino [tlaino]
! **************************************************************************************************
   SUBROUTINE shake_roll_colv_low(fixd_list, colv_list, lcolv, &
                                  particle_set, pos, vel, r_shake, v_shake, dt, ishake, cell, &
                                  imass, max_sigma)

      TYPE(fixd_constraint_type), DIMENSION(:), POINTER  :: fixd_list
      TYPE(colvar_constraint_type), POINTER              :: colv_list(:)
      TYPE(local_colvar_constraint_type), POINTER        :: lcolv(:)
      TYPE(particle_type), POINTER                       :: particle_set(:)
      REAL(KIND=dp), INTENT(INOUT)                       :: pos(:, :), vel(:, :)
      REAL(KIND=dp), DIMENSION(:, :), INTENT(IN)         :: r_shake, v_shake
      REAL(kind=dp), INTENT(in)                          :: dt
      INTEGER, INTENT(in)                                :: ishake
      TYPE(cell_type), POINTER                           :: cell
      REAL(KIND=dp), DIMENSION(:)                        :: imass
      REAL(KIND=dp), INTENT(INOUT)                       :: max_sigma

      INTEGER                                            :: iconst
      REAL(KIND=dp)                                      :: del_lam, dtby2, dtsqby2, fdotf_sum

      dtsqby2 = dt*dt*.5_dp
      dtby2 = dt*.5_dp
      IF (ishake == 1) THEN
         DO iconst = 1, SIZE(colv_list)
            IF (colv_list(iconst)%restraint%active) CYCLE
            ! Update positions
            CALL update_con_colv(pos, dtsqby2, lcolv(iconst), &
                                 lambda=lcolv(iconst)%lambda, &
                                 roll=.TRUE., rmat=r_shake, imass=imass)
            ! Update velocities
            CALL update_con_colv(vel, dtby2, lcolv(iconst), &
                                 lambda=lcolv(iconst)%lambda, &
                                 roll=.TRUE., rmat=v_shake, imass=imass)
         END DO
      ELSE
         DO iconst = 1, SIZE(colv_list)
            IF (colv_list(iconst)%restraint%active) CYCLE
            ! Update colvar
            CALL colvar_eval_mol_f(lcolv(iconst)%colvar, cell, particles=particle_set, &
                                   pos=pos, fixd_list=fixd_list)
            lcolv(iconst)%sigma = diff_colvar(lcolv(iconst)%colvar, &
                                              colv_list(iconst)%expected_value)
            fdotf_sum = eval_Jac_colvar(lcolv(iconst)%colvar, &
                                        lcolv(iconst)%colvar_old, roll=.TRUE., rmat=r_shake, &
                                        imass=imass)
            del_lam = 2.0_dp*lcolv(iconst)%sigma/(dt*dt*fdotf_sum)
            lcolv(iconst)%lambda = lcolv(iconst)%lambda+del_lam

            ! Update positions
            CALL update_con_colv(pos, dtsqby2, lcolv(iconst), &
                                 lambda=del_lam, &
                                 roll=.TRUE., rmat=r_shake, imass=imass)
            ! Update velocities
            CALL update_con_colv(vel, dtby2, lcolv(iconst), &
                                 lambda=del_lam, &
                                 roll=.TRUE., rmat=v_shake, imass=imass)
         END DO
      END IF
      ! computing the constraint and value of tolerance
      DO iconst = 1, SIZE(colv_list)
         IF (colv_list(iconst)%restraint%active) CYCLE
         CALL colvar_eval_mol_f(lcolv(iconst)%colvar, cell, particles=particle_set, &
                                pos=pos, fixd_list=fixd_list)
         lcolv(iconst)%sigma = diff_colvar(lcolv(iconst)%colvar, &
                                           colv_list(iconst)%expected_value)
         max_sigma = MAX(ABS(lcolv(iconst)%sigma), max_sigma)
      END DO

   END SUBROUTINE shake_roll_colv_low

! **************************************************************************************************
!> \brief Real Rattle_roll - Low Level
!>      rattle algorithm (box allowed to change) for collective variables constraints
!>      updates the multiplier one molecule type at a time
!> \param fixd_list ...
!> \param colv_list ...
!> \param lcolv ...
!> \param particle_set ...
!> \param vel ...
!> \param r_rattle ...
!> \param dt ...
!> \param irattle ...
!> \param veps ...
!> \param cell ...
!> \param imass ...
!> \param max_sigma ...
!> \par History
!>      none
!> \author Teodoro Laino [tlaino]
! **************************************************************************************************
   SUBROUTINE rattle_roll_colv_low(fixd_list, colv_list, lcolv, &
                                   particle_set, vel, r_rattle, dt, irattle, veps, cell, &
                                   imass, max_sigma)

      TYPE(fixd_constraint_type), DIMENSION(:), POINTER  :: fixd_list
      TYPE(colvar_constraint_type), POINTER              :: colv_list(:)
      TYPE(local_colvar_constraint_type), POINTER        :: lcolv(:)
      TYPE(particle_type), POINTER                       :: particle_set(:)
      REAL(KIND=dp), INTENT(INOUT)                       :: vel(:, :)
      REAL(KIND=dp), INTENT(IN)                          :: r_rattle(:, :), dt
      INTEGER, INTENT(in)                                :: irattle
      REAL(KIND=dp), INTENT(IN)                          :: veps(:, :)
      TYPE(cell_type), POINTER                           :: cell
      REAL(KIND=dp), DIMENSION(:)                        :: imass
      REAL(KIND=dp), INTENT(INOUT)                       :: max_sigma

      INTEGER                                            :: iconst
      REAL(KIND=dp)                                      :: del_lam, dtby2, fdotf_sum

      dtby2 = dt*.5_dp
      IF (irattle == 1) THEN
         DO iconst = 1, SIZE(colv_list)
            IF (colv_list(iconst)%restraint%active) CYCLE
            ! Update colvar_old
            CALL colvar_eval_mol_f(lcolv(iconst)%colvar_old, cell, &
                                   particles=particle_set, fixd_list=fixd_list)
            ! Update velocities
            CALL update_con_colv(vel, dtby2, lcolv(iconst), &
                                 lambda=lcolv(iconst)%lambda, &
                                 imass=imass)
         END DO
      ELSE
         DO iconst = 1, SIZE(colv_list)
            IF (colv_list(iconst)%restraint%active) CYCLE
            lcolv(iconst)%sigma = rattle_con_eval(lcolv(iconst)%colvar_old, vel, &
                                                  roll=.TRUE., veps=veps, rmat=r_rattle, particles=particle_set)
            fdotf_sum = eval_Jac_colvar(lcolv(iconst)%colvar_old, &
                                        lcolv(iconst)%colvar_old, roll=.TRUE., &
                                        rmat=r_rattle, imass=imass)
            del_lam = 2.0_dp*lcolv(iconst)%sigma/(dt*fdotf_sum)
            lcolv(iconst)%lambda = lcolv(iconst)%lambda+del_lam
            ! Update velocities
            CALL update_con_colv(vel, dtby2, lcolv(iconst), &
                                 lambda=del_lam, &
                                 roll=.TRUE., rmat=r_rattle, imass=imass)
         END DO
      END IF
      ! computing the constraint and value of the tolerance
      DO iconst = 1, SIZE(colv_list)
         IF (colv_list(iconst)%restraint%active) CYCLE
         lcolv(iconst)%sigma = rattle_con_eval(lcolv(iconst)%colvar_old, vel, &
                                               roll=.TRUE., veps=veps, rmat=r_rattle, particles=particle_set)
         max_sigma = MAX(ABS(lcolv(iconst)%sigma), max_sigma)
      END DO

   END SUBROUTINE rattle_roll_colv_low

! **************************************************************************************************
!> \brief Update position/velocities
!> \param wrk ...
!> \param fac ...
!> \param lcolv ...
!> \param lambda ...
!> \param roll ...
!> \param rmat ...
!> \param imass ...
!> \par History
!>      Teodoro Laino [teo] created 04.2006
!> \author Teodoro Laino [tlaino]
! **************************************************************************************************
   SUBROUTINE update_con_colv(wrk, fac, lcolv, lambda, roll, rmat, imass)
      REAL(KIND=dp), INTENT(INOUT)                       :: wrk(:, :)
      REAL(KIND=dp), INTENT(IN)                          :: fac
      TYPE(local_colvar_constraint_type), INTENT(IN)     :: lcolv
      REAL(KIND=dp), INTENT(IN)                          :: lambda
      LOGICAL, INTENT(in), OPTIONAL                      :: roll
      REAL(KIND=dp), DIMENSION(:, :), INTENT(IN), &
         OPTIONAL                                        :: rmat
      REAL(KIND=dp), DIMENSION(:)                        :: imass

      CHARACTER(len=*), PARAMETER :: routineN = 'update_con_colv', &
         routineP = moduleN//':'//routineN

      INTEGER                                            :: iatm, ind
      LOGICAL                                            :: my_roll
      REAL(KIND=dp), DIMENSION(3)                        :: f_roll

      my_roll = .FALSE.
      IF (PRESENT(roll)) THEN
         my_roll = roll
         IF (my_roll) THEN
            CPASSERT(PRESENT(rmat))
         END IF
      END IF
      DO iatm = 1, SIZE(lcolv%colvar_old%i_atom)
         ind = lcolv%colvar_old%i_atom(iatm)
         !
         IF (my_roll) THEN
            ! If ROLL rotate forces
            CALL matvec_3x3(f_roll, rmat, lcolv%colvar_old%dsdr(:, iatm))
         ELSE
            f_roll = lcolv%colvar_old%dsdr(:, iatm)
         END IF
         wrk(:, ind) = wrk(:, ind)-imass(ind)*fac*lambda*f_roll
      END DO
   END SUBROUTINE update_con_colv

! **************************************************************************************************
!> \brief Evaluates the Jacobian of the collective variables constraints
!> \param colvar ...
!> \param colvar_old ...
!> \param roll ...
!> \param rmat ...
!> \param imass ...
!> \return ...
!> \par History
!>      Teodoro Laino [teo] created 04.2006
!> \author Teodoro Laino [tlaino]
! **************************************************************************************************
   FUNCTION eval_Jac_colvar(colvar, colvar_old, roll, rmat, imass) RESULT(res)
      TYPE(colvar_type), POINTER                         :: colvar, colvar_old
      LOGICAL, INTENT(IN), OPTIONAL                      :: roll
      REAL(KIND=dp), DIMENSION(:, :), INTENT(IN), &
         OPTIONAL                                        :: rmat
      REAL(KIND=dp), DIMENSION(:)                        :: imass
      REAL(KIND=dp)                                      :: res

      CHARACTER(len=*), PARAMETER :: routineN = 'eval_Jac_colvar', &
         routineP = moduleN//':'//routineN

      INTEGER                                            :: i, iatom
      LOGICAL                                            :: my_roll
      REAL(KIND=dp), DIMENSION(3)                        :: tmp1, tmp2, tmp3

      my_roll = .FALSE.
      IF (PRESENT(roll)) THEN
         my_roll = roll
         IF (my_roll) THEN
            CPASSERT(PRESENT(rmat))
         END IF
      END IF

      res = 0.0_dp
      DO i = 1, SIZE(colvar%i_atom)
         iatom = colvar%i_atom(i)
         tmp1 = colvar%dsdr(1:3, i)
         IF (my_roll) THEN
            tmp3 = colvar_old%dsdr(1:3, i)
            CALL matvec_3x3(tmp2, rmat, tmp3)
         ELSE
            tmp2 = colvar_old%dsdr(1:3, i)
         END IF
         res = res+DOT_PRODUCT(tmp1, tmp2)*imass(iatom)
      END DO

   END FUNCTION eval_Jac_colvar

! **************************************************************************************************
!> \brief Evaluates the constraint for the rattle scheme
!> \param colvar ...
!> \param vel ...
!> \param roll ...
!> \param veps ...
!> \param rmat ...
!> \param particles ...
!> \return ...
!> \par History
!>      Teodoro Laino [teo] created 04.2006
!> \author Teodoro Laino [tlaino]
! **************************************************************************************************
   FUNCTION rattle_con_eval(colvar, vel, roll, veps, rmat, particles) RESULT(res)
      TYPE(colvar_type), POINTER                         :: colvar
      REAL(KIND=dp), INTENT(INOUT)                       :: vel(:, :)
      LOGICAL, INTENT(IN), OPTIONAL                      :: roll
      REAL(KIND=dp), DIMENSION(:, :), INTENT(IN), &
         OPTIONAL                                        :: veps, rmat
      TYPE(particle_type), DIMENSION(:), OPTIONAL, &
         POINTER                                         :: particles
      REAL(KIND=dp)                                      :: res

      CHARACTER(len=*), PARAMETER :: routineN = 'rattle_con_eval', &
         routineP = moduleN//':'//routineN

      INTEGER                                            :: iatm, ind
      LOGICAL                                            :: my_roll
      REAL(KIND=dp)                                      :: tmp(3)
      REAL(KIND=dp), DIMENSION(3)                        :: f_roll, pos, v_roll

      my_roll = .FALSE.
      IF (PRESENT(roll)) THEN
         my_roll = roll
         IF (my_roll) THEN
            CPASSERT(PRESENT(rmat))
            CPASSERT(PRESENT(veps))
            CPASSERT(PRESENT(particles))
         END IF
      END IF
      res = 0.0_dp
      DO iatm = 1, SIZE(colvar%i_atom)
         ind = colvar%i_atom(iatm)
         IF (my_roll) THEN
            pos = particles(ind)%r
            CALL matvec_3x3(f_roll, rmat, colvar%dsdr(:, iatm))
            CALL matvec_3x3(tmp, veps, pos)
            v_roll = vel(:, ind)+tmp
         ELSE
            f_roll = colvar%dsdr(:, iatm)
            v_roll = vel(:, ind)
         END IF
         res = res+DOT_PRODUCT(f_roll, v_roll)
      END DO

   END FUNCTION rattle_con_eval

END MODULE constraint_clv
