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

! **************************************************************************************************
!> \par History
!>      none
! **************************************************************************************************
MODULE constraint_4x6

   USE atomic_kind_types,               ONLY: atomic_kind_type,&
                                              get_atomic_kind
   USE constraint_fxd,                  ONLY: check_fixed_atom_cns_g4x6
   USE kinds,                           ONLY: dp
   USE linear_systems,                  ONLY: solve_system
   USE mathlib,                         ONLY: dotprod_3d,&
                                              matvec_3x3
   USE molecule_kind_types,             ONLY: fixd_constraint_type,&
                                              g4x6_constraint_type,&
                                              get_molecule_kind,&
                                              molecule_kind_type
   USE molecule_types,                  ONLY: get_molecule,&
                                              global_constraint_type,&
                                              local_g4x6_constraint_type,&
                                              molecule_type
   USE particle_types,                  ONLY: particle_type
#include "./base/base_uses.f90"

   IMPLICIT NONE

   PRIVATE
   PUBLIC :: shake_4x6_int, &
             rattle_4x6_int, &
             shake_roll_4x6_int, &
             rattle_roll_4x6_int, &
             shake_4x6_ext, &
             rattle_4x6_ext, &
             shake_roll_4x6_ext, &
             rattle_roll_4x6_ext

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

CONTAINS

! **************************************************************************************************
!> \brief Intramolecular shake_4x6
!> \param molecule ...
!> \param particle_set ...
!> \param pos ...
!> \param vel ...
!> \param dt ...
!> \param ishake ...
!> \param max_sigma ...
!> \par History
!>      none
! **************************************************************************************************
   SUBROUTINE shake_4x6_int(molecule, particle_set, pos, vel, dt, ishake, &
                            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
      REAL(KIND=dp), INTENT(INOUT)                       :: max_sigma

      INTEGER                                            :: first_atom, ng4x6
      TYPE(fixd_constraint_type), DIMENSION(:), POINTER  :: fixd_list
      TYPE(g4x6_constraint_type), POINTER                :: g4x6_list(:)
      TYPE(local_g4x6_constraint_type), POINTER          :: lg4x6(:)
      TYPE(molecule_kind_type), POINTER                  :: molecule_kind

      NULLIFY (fixd_list)
      molecule_kind => molecule%molecule_kind
      CALL get_molecule_kind(molecule_kind, ng4x6=ng4x6, &
                             fixd_list=fixd_list, g4x6_list=g4x6_list)
      CALL get_molecule(molecule, first_atom=first_atom, lg4x6=lg4x6)
      ! Real Shake
      CALL shake_4x6_low(fixd_list, g4x6_list, lg4x6, ng4x6, first_atom, &
                         particle_set, pos, vel, dt, ishake, max_sigma)

   END SUBROUTINE shake_4x6_int

! **************************************************************************************************
!> \brief Intramolecular shake_4x6_roll
!> \param molecule ...
!> \param particle_set ...
!> \param pos ...
!> \param vel ...
!> \param r_shake ...
!> \param dt ...
!> \param ishake ...
!> \param max_sigma ...
!> \par History
!>      none
! **************************************************************************************************
   SUBROUTINE shake_roll_4x6_int(molecule, particle_set, pos, vel, r_shake, &
                                 dt, ishake, 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
      REAL(kind=dp), INTENT(in)                          :: dt
      INTEGER, INTENT(IN)                                :: ishake
      REAL(KIND=dp), INTENT(INOUT)                       :: max_sigma

      INTEGER                                            :: first_atom, ng4x6
      TYPE(fixd_constraint_type), DIMENSION(:), POINTER  :: fixd_list
      TYPE(g4x6_constraint_type), POINTER                :: g4x6_list(:)
      TYPE(local_g4x6_constraint_type), POINTER          :: lg4x6(:)
      TYPE(molecule_kind_type), POINTER                  :: molecule_kind

      NULLIFY (fixd_list)
      molecule_kind => molecule%molecule_kind
      CALL get_molecule_kind(molecule_kind, ng4x6=ng4x6, &
                             fixd_list=fixd_list, g4x6_list=g4x6_list)
      CALL get_molecule(molecule, first_atom=first_atom, lg4x6=lg4x6)
      ! Real Shake
      CALL shake_roll_4x6_low(fixd_list, g4x6_list, lg4x6, ng4x6, first_atom, &
                              particle_set, pos, vel, r_shake, dt, ishake, max_sigma)

   END SUBROUTINE shake_roll_4x6_int

! **************************************************************************************************
!> \brief Intramolecular rattle_4x6
!> \param molecule ...
!> \param particle_set ...
!> \param vel ...
!> \param dt ...
!> \par History
!>      none
! **************************************************************************************************
   SUBROUTINE rattle_4x6_int(molecule, particle_set, vel, dt)
      TYPE(molecule_type), POINTER                       :: molecule
      TYPE(particle_type), POINTER                       :: particle_set(:)
      REAL(KIND=dp), INTENT(INOUT)                       :: vel(:, :)
      REAL(kind=dp), INTENT(in)                          :: dt

      INTEGER                                            :: first_atom, ng4x6
      TYPE(fixd_constraint_type), DIMENSION(:), POINTER  :: fixd_list
      TYPE(g4x6_constraint_type), POINTER                :: g4x6_list(:)
      TYPE(local_g4x6_constraint_type), POINTER          :: lg4x6(:)
      TYPE(molecule_kind_type), POINTER                  :: molecule_kind

      NULLIFY (fixd_list)
      molecule_kind => molecule%molecule_kind
      CALL get_molecule_kind(molecule_kind, ng4x6=ng4x6, &
                             fixd_list=fixd_list, g4x6_list=g4x6_list)
      CALL get_molecule(molecule, first_atom=first_atom, lg4x6=lg4x6)
      ! Real Rattle
      CALL rattle_4x6_low(fixd_list, g4x6_list, lg4x6, first_atom, &
                          particle_set, vel, dt)

   END SUBROUTINE rattle_4x6_int

! **************************************************************************************************
!> \brief Intramolecular rattle_4x6_roll
!> \param molecule ...
!> \param particle_set ...
!> \param vel ...
!> \param r_rattle ...
!> \param dt ...
!> \param veps ...
!> \par History
!>      none
! **************************************************************************************************
   SUBROUTINE rattle_roll_4x6_int(molecule, particle_set, vel, r_rattle, dt, veps)
      TYPE(molecule_type), POINTER                       :: molecule
      TYPE(particle_type), POINTER                       :: particle_set(:)
      REAL(KIND=dp), INTENT(INOUT)                       :: vel(:, :)
      REAL(KIND=dp), DIMENSION(:, :), INTENT(IN)         :: r_rattle
      REAL(kind=dp), INTENT(in)                          :: dt
      REAL(KIND=dp), DIMENSION(:, :), INTENT(IN)         :: veps

      INTEGER                                            :: first_atom, ng4x6
      TYPE(fixd_constraint_type), DIMENSION(:), POINTER  :: fixd_list
      TYPE(g4x6_constraint_type), POINTER                :: g4x6_list(:)
      TYPE(local_g4x6_constraint_type), POINTER          :: lg4x6(:)
      TYPE(molecule_kind_type), POINTER                  :: molecule_kind

      NULLIFY (fixd_list)
      molecule_kind => molecule%molecule_kind
      CALL get_molecule_kind(molecule_kind, ng4x6=ng4x6, &
                             fixd_list=fixd_list, g4x6_list=g4x6_list)
      CALL get_molecule(molecule, first_atom=first_atom, lg4x6=lg4x6)
      ! Real Rattle
      CALL rattle_roll_4x6_low(fixd_list, g4x6_list, lg4x6, first_atom, &
                               particle_set, vel, r_rattle, dt, veps)

   END SUBROUTINE rattle_roll_4x6_int

! **************************************************************************************************
!> \brief Intramolecular shake_4x6
!> \param gci ...
!> \param particle_set ...
!> \param pos ...
!> \param vel ...
!> \param dt ...
!> \param ishake ...
!> \param max_sigma ...
!> \par History
!>      none
! **************************************************************************************************
   SUBROUTINE shake_4x6_ext(gci, particle_set, pos, vel, dt, ishake, &
                            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
      REAL(KIND=dp), INTENT(INOUT)                       :: max_sigma

      INTEGER                                            :: first_atom, ng4x6
      TYPE(fixd_constraint_type), DIMENSION(:), POINTER  :: fixd_list
      TYPE(g4x6_constraint_type), POINTER                :: g4x6_list(:)
      TYPE(local_g4x6_constraint_type), POINTER          :: lg4x6(:)

      first_atom = 1
      ng4x6 = gci%ng4x6
      fixd_list => gci%fixd_list
      g4x6_list => gci%g4x6_list
      lg4x6 => gci%lg4x6
      ! Real Shake
      CALL shake_4x6_low(fixd_list, g4x6_list, lg4x6, ng4x6, first_atom, &
                         particle_set, pos, vel, dt, ishake, max_sigma)

   END SUBROUTINE shake_4x6_ext

! **************************************************************************************************
!> \brief Intramolecular shake_4x6_roll
!> \param gci ...
!> \param particle_set ...
!> \param pos ...
!> \param vel ...
!> \param r_shake ...
!> \param dt ...
!> \param ishake ...
!> \param max_sigma ...
!> \par History
!>      none
! **************************************************************************************************
   SUBROUTINE shake_roll_4x6_ext(gci, particle_set, pos, vel, r_shake, &
                                 dt, ishake, 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
      REAL(kind=dp), INTENT(in)                          :: dt
      INTEGER, INTENT(IN)                                :: ishake
      REAL(KIND=dp), INTENT(INOUT)                       :: max_sigma

      INTEGER                                            :: first_atom, ng4x6
      TYPE(fixd_constraint_type), DIMENSION(:), POINTER  :: fixd_list
      TYPE(g4x6_constraint_type), POINTER                :: g4x6_list(:)
      TYPE(local_g4x6_constraint_type), POINTER          :: lg4x6(:)

      first_atom = 1
      ng4x6 = gci%ng4x6
      fixd_list => gci%fixd_list
      g4x6_list => gci%g4x6_list
      lg4x6 => gci%lg4x6
      ! Real Shake
      CALL shake_roll_4x6_low(fixd_list, g4x6_list, lg4x6, ng4x6, first_atom, &
                              particle_set, pos, vel, r_shake, dt, ishake, max_sigma)

   END SUBROUTINE shake_roll_4x6_ext

! **************************************************************************************************
!> \brief Intramolecular rattle_4x6
!> \param gci ...
!> \param particle_set ...
!> \param vel ...
!> \param dt ...
!> \par History
!>      none
! **************************************************************************************************
   SUBROUTINE rattle_4x6_ext(gci, particle_set, vel, dt)

      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                                            :: first_atom, ng4x6
      TYPE(fixd_constraint_type), DIMENSION(:), POINTER  :: fixd_list
      TYPE(g4x6_constraint_type), POINTER                :: g4x6_list(:)
      TYPE(local_g4x6_constraint_type), POINTER          :: lg4x6(:)

      first_atom = 1
      ng4x6 = gci%ng4x6
      fixd_list => gci%fixd_list
      g4x6_list => gci%g4x6_list
      lg4x6 => gci%lg4x6
      ! Real Rattle
      CALL rattle_4x6_low(fixd_list, g4x6_list, lg4x6, first_atom, &
                          particle_set, vel, dt)

   END SUBROUTINE rattle_4x6_ext

! **************************************************************************************************
!> \brief Intramolecular rattle_4x6_roll
!> \param gci ...
!> \param particle_set ...
!> \param vel ...
!> \param r_rattle ...
!> \param dt ...
!> \param veps ...
!> \par History
!>      none
! **************************************************************************************************
   SUBROUTINE rattle_roll_4x6_ext(gci, particle_set, vel, r_rattle, dt, veps)

      TYPE(global_constraint_type), POINTER              :: gci
      TYPE(particle_type), POINTER                       :: particle_set(:)
      REAL(KIND=dp), INTENT(INOUT)                       :: vel(:, :)
      REAL(KIND=dp), DIMENSION(:, :), INTENT(IN)         :: r_rattle
      REAL(kind=dp), INTENT(in)                          :: dt
      REAL(KIND=dp), DIMENSION(:, :), INTENT(IN)         :: veps

      INTEGER                                            :: first_atom, ng4x6
      TYPE(fixd_constraint_type), DIMENSION(:), POINTER  :: fixd_list
      TYPE(g4x6_constraint_type), POINTER                :: g4x6_list(:)
      TYPE(local_g4x6_constraint_type), POINTER          :: lg4x6(:)

      first_atom = 1
      ng4x6 = gci%ng4x6
      fixd_list => gci%fixd_list
      g4x6_list => gci%g4x6_list
      lg4x6 => gci%lg4x6
      ! Real Rattle
      CALL rattle_roll_4x6_low(fixd_list, g4x6_list, lg4x6, first_atom, &
                               particle_set, vel, r_rattle, dt, veps)

   END SUBROUTINE rattle_roll_4x6_ext

! **************************************************************************************************
!> \brief ...
!> \param fixd_list ...
!> \param g4x6_list ...
!> \param lg4x6 ...
!> \param ng4x6 ...
!> \param first_atom ...
!> \param particle_set ...
!> \param pos ...
!> \param vel ...
!> \param dt ...
!> \param ishake ...
!> \param max_sigma ...
!> \par History
!>      none
! **************************************************************************************************
   SUBROUTINE shake_4x6_low(fixd_list, g4x6_list, lg4x6, ng4x6, first_atom, &
                            particle_set, pos, vel, dt, ishake, max_sigma)
      TYPE(fixd_constraint_type), DIMENSION(:), POINTER  :: fixd_list
      TYPE(g4x6_constraint_type), POINTER                :: g4x6_list(:)
      TYPE(local_g4x6_constraint_type), POINTER          :: lg4x6(:)
      INTEGER, INTENT(IN)                                :: ng4x6, first_atom
      TYPE(particle_type), POINTER                       :: particle_set(:)
      REAL(KIND=dp), INTENT(INOUT)                       :: pos(:, :), vel(:, :)
      REAL(kind=dp), INTENT(in)                          :: dt
      INTEGER, INTENT(IN)                                :: ishake
      REAL(KIND=dp), INTENT(INOUT)                       :: max_sigma

      INTEGER                                            :: iconst, index_a, index_b, index_c, &
                                                            index_d
      REAL(KIND=dp)                                      :: dtby2, dtsqby2, idtsq, imass1, imass2, &
                                                            imass3, imass4, sigma
      REAL(KIND=dp), DIMENSION(3)                        :: fc1, fc2, fc3, fc4, r0_12, r0_13, r0_14, &
                                                            r0_23, r0_24, r0_34, r1, r12, r13, &
                                                            r14, r2, r23, r24, r3, r34, r4, v1, &
                                                            v2, v3, v4, vec
      REAL(KIND=dp), DIMENSION(6, 1)                     :: bvec
      REAL(KIND=dp), DIMENSION(6, 6)                     :: amat, atemp
      TYPE(atomic_kind_type), POINTER                    :: atomic_kind

      dtsqby2 = dt*dt*.5_dp
      idtsq = 1.0_dp/dt/dt
      dtby2 = dt*.5_dp
      DO iconst = 1, ng4x6
         IF (g4x6_list(iconst)%restraint%active) CYCLE
         index_a = g4x6_list(iconst)%a+first_atom-1
         index_b = g4x6_list(iconst)%b+first_atom-1
         index_c = g4x6_list(iconst)%c+first_atom-1
         index_d = g4x6_list(iconst)%d+first_atom-1
         IF (ishake == 1) THEN
            r0_12(:) = pos(:, index_a)-pos(:, index_b)
            r0_13(:) = pos(:, index_a)-pos(:, index_c)
            r0_14(:) = pos(:, index_a)-pos(:, index_d)
            r0_23(:) = pos(:, index_b)-pos(:, index_c)
            r0_24(:) = pos(:, index_b)-pos(:, index_d)
            r0_34(:) = pos(:, index_c)-pos(:, index_d)
            atomic_kind => particle_set(index_a)%atomic_kind
            imass1 = 1.0_dp/atomic_kind%mass
            atomic_kind => particle_set(index_b)%atomic_kind
            imass2 = 1.0_dp/atomic_kind%mass
            atomic_kind => particle_set(index_c)%atomic_kind
            imass3 = 1.0_dp/atomic_kind%mass
            atomic_kind => particle_set(index_d)%atomic_kind
            imass4 = 1.0_dp/atomic_kind%mass
            lg4x6(iconst)%fa = -2.0_dp*(lg4x6(iconst)%ra_old- &
                                        lg4x6(iconst)%rb_old)
            lg4x6(iconst)%fb = -2.0_dp*(lg4x6(iconst)%ra_old- &
                                        lg4x6(iconst)%rc_old)
            lg4x6(iconst)%fc = -2.0_dp*(lg4x6(iconst)%ra_old- &
                                        lg4x6(iconst)%rd_old)
            lg4x6(iconst)%fd = -2.0_dp*(lg4x6(iconst)%rb_old- &
                                        lg4x6(iconst)%rc_old)
            lg4x6(iconst)%fe = -2.0_dp*(lg4x6(iconst)%rb_old- &
                                        lg4x6(iconst)%rd_old)
            lg4x6(iconst)%ff = -2.0_dp*(lg4x6(iconst)%rc_old- &
                                        lg4x6(iconst)%rd_old)
            ! Check for fixed atom constraints
            CALL check_fixed_atom_cns_g4x6(imass1, imass2, imass3, imass4, &
                                           index_a, index_b, index_c, index_d, fixd_list, lg4x6(iconst))
            ! construct matrix
            amat(1, 1) = (imass1+imass2)*DOTPROD_3D(r0_12, lg4x6(iconst)%fa)
            amat(1, 2) = imass1*DOTPROD_3D(r0_12, lg4x6(iconst)%fb)
            amat(1, 3) = imass1*DOTPROD_3D(r0_12, lg4x6(iconst)%fc)
            amat(1, 4) = -imass2*DOTPROD_3D(r0_12, lg4x6(iconst)%fd)
            amat(1, 5) = -imass2*DOTPROD_3D(r0_12, lg4x6(iconst)%fe)
            amat(1, 6) = 0.0_dp

            amat(2, 1) = imass1*DOTPROD_3D(r0_13, lg4x6(iconst)%fa)
            amat(2, 2) = (imass1+imass3)*DOTPROD_3D(r0_13, lg4x6(iconst)%fb)
            amat(2, 3) = imass1*DOTPROD_3D(r0_13, lg4x6(iconst)%fc)
            amat(2, 4) = imass3*DOTPROD_3D(r0_13, lg4x6(iconst)%fd)
            amat(2, 5) = 0.0_dp
            amat(2, 6) = -imass3*DOTPROD_3D(r0_13, lg4x6(iconst)%ff)

            amat(3, 1) = imass1*DOTPROD_3D(r0_14, lg4x6(iconst)%fa)
            amat(3, 2) = imass1*DOTPROD_3D(r0_14, lg4x6(iconst)%fb)
            amat(3, 3) = (imass1+imass4)*DOTPROD_3D(r0_14, lg4x6(iconst)%fc)
            amat(3, 4) = 0.0_dp
            amat(3, 5) = imass4*DOTPROD_3D(r0_14, lg4x6(iconst)%fe)
            amat(3, 6) = imass4*DOTPROD_3D(r0_14, lg4x6(iconst)%ff)

            amat(4, 1) = -imass2*DOTPROD_3D(r0_23, lg4x6(iconst)%fa)
            amat(4, 2) = imass3*DOTPROD_3D(r0_23, lg4x6(iconst)%fb)
            amat(4, 3) = 0.0_dp
            amat(4, 4) = (imass3+imass2)*DOTPROD_3D(r0_23, lg4x6(iconst)%fd)
            amat(4, 5) = imass2*DOTPROD_3D(r0_23, lg4x6(iconst)%fe)
            amat(4, 6) = -imass3*DOTPROD_3D(r0_23, lg4x6(iconst)%ff)

            amat(5, 1) = -imass2*DOTPROD_3D(r0_24, lg4x6(iconst)%fa)
            amat(5, 2) = 0.0_dp
            amat(5, 3) = imass4*DOTPROD_3D(r0_24, lg4x6(iconst)%fc)
            amat(5, 4) = imass2*DOTPROD_3D(r0_24, lg4x6(iconst)%fd)
            amat(5, 5) = (imass4+imass2)*DOTPROD_3D(r0_24, lg4x6(iconst)%fe)
            amat(5, 6) = imass4*DOTPROD_3D(r0_24, lg4x6(iconst)%ff)

            amat(6, 1) = 0.0_dp
            amat(6, 2) = -imass3*DOTPROD_3D(r0_34, lg4x6(iconst)%fb)
            amat(6, 3) = imass4*DOTPROD_3D(r0_34, lg4x6(iconst)%fc)
            amat(6, 4) = -imass3*DOTPROD_3D(r0_34, lg4x6(iconst)%fd)
            amat(6, 5) = imass4*DOTPROD_3D(r0_34, lg4x6(iconst)%fe)
            amat(6, 6) = (imass3+imass4)*DOTPROD_3D(r0_34, lg4x6(iconst)%ff)
            ! Store values
            lg4x6(iconst)%r0_12 = r0_12
            lg4x6(iconst)%r0_13 = r0_13
            lg4x6(iconst)%r0_14 = r0_14
            lg4x6(iconst)%r0_23 = r0_23
            lg4x6(iconst)%r0_24 = r0_24
            lg4x6(iconst)%r0_34 = r0_34
            lg4x6(iconst)%amat = amat
            lg4x6(iconst)%imass1 = imass1
            lg4x6(iconst)%imass2 = imass2
            lg4x6(iconst)%imass3 = imass3
            lg4x6(iconst)%imass4 = imass4
            lg4x6(iconst)%lambda_old = 0.0_dp
            lg4x6(iconst)%del_lambda = 0.0_dp
         ELSE
            ! Retrieve values
            r0_12 = lg4x6(iconst)%r0_12
            r0_13 = lg4x6(iconst)%r0_13
            r0_14 = lg4x6(iconst)%r0_14
            r0_23 = lg4x6(iconst)%r0_23
            r0_24 = lg4x6(iconst)%r0_24
            r0_34 = lg4x6(iconst)%r0_34
            amat = lg4x6(iconst)%amat
            imass1 = lg4x6(iconst)%imass1
            imass2 = lg4x6(iconst)%imass2
            imass3 = lg4x6(iconst)%imass3
            imass4 = lg4x6(iconst)%imass4
         END IF

         ! Iterate until convergence:
         vec = lg4x6(iconst)%lambda(1)*lg4x6(iconst)%fa*(imass1+imass2)+ &
               lg4x6(iconst)%lambda(2)*imass1*lg4x6(iconst)%fb+ &
               lg4x6(iconst)%lambda(3)*imass1*lg4x6(iconst)%fc- &
               lg4x6(iconst)%lambda(4)*imass2*lg4x6(iconst)%fd- &
               lg4x6(iconst)%lambda(5)*imass2*lg4x6(iconst)%fe
         bvec(1, 1) = g4x6_list(iconst)%dab*g4x6_list(iconst)%dab &
                      -dtsqby2*dtsqby2*DOTPROD_3D(vec, vec)-DOTPROD_3D(r0_12, r0_12)
         vec = lg4x6(iconst)%lambda(2)*lg4x6(iconst)%fb*(imass1+imass3)+ &
               lg4x6(iconst)%lambda(1)*imass1*lg4x6(iconst)%fa+ &
               lg4x6(iconst)%lambda(3)*imass1*lg4x6(iconst)%fc+ &
               lg4x6(iconst)%lambda(4)*imass3*lg4x6(iconst)%fd- &
               lg4x6(iconst)%lambda(6)*imass3*lg4x6(iconst)%ff
         bvec(2, 1) = g4x6_list(iconst)%dac*g4x6_list(iconst)%dac &
                      -dtsqby2*dtsqby2*DOTPROD_3D(vec, vec)-DOTPROD_3D(r0_13, r0_13)
         vec = lg4x6(iconst)%lambda(3)*lg4x6(iconst)%fc*(imass1+imass4)+ &
               lg4x6(iconst)%lambda(1)*imass1*lg4x6(iconst)%fa+ &
               lg4x6(iconst)%lambda(2)*imass1*lg4x6(iconst)%fb+ &
               lg4x6(iconst)%lambda(5)*imass4*lg4x6(iconst)%fe+ &
               lg4x6(iconst)%lambda(6)*imass4*lg4x6(iconst)%ff
         bvec(3, 1) = g4x6_list(iconst)%dad*g4x6_list(iconst)%dad &
                      -dtsqby2*dtsqby2*DOTPROD_3D(vec, vec)-DOTPROD_3D(r0_14, r0_14)
         vec = lg4x6(iconst)%lambda(4)*lg4x6(iconst)%fd*(imass2+imass3)- &
               lg4x6(iconst)%lambda(1)*imass2*lg4x6(iconst)%fa+ &
               lg4x6(iconst)%lambda(2)*imass3*lg4x6(iconst)%fb+ &
               lg4x6(iconst)%lambda(5)*imass2*lg4x6(iconst)%fe- &
               lg4x6(iconst)%lambda(6)*imass3*lg4x6(iconst)%ff
         bvec(4, 1) = g4x6_list(iconst)%dbc*g4x6_list(iconst)%dbc &
                      -dtsqby2*dtsqby2*DOTPROD_3D(vec, vec)-DOTPROD_3D(r0_23, r0_23)
         vec = lg4x6(iconst)%lambda(5)*lg4x6(iconst)%fe*(imass2+imass4)- &
               lg4x6(iconst)%lambda(1)*imass2*lg4x6(iconst)%fa+ &
               lg4x6(iconst)%lambda(3)*imass4*lg4x6(iconst)%fc+ &
               lg4x6(iconst)%lambda(4)*imass2*lg4x6(iconst)%fd+ &
               lg4x6(iconst)%lambda(6)*imass4*lg4x6(iconst)%ff
         bvec(5, 1) = g4x6_list(iconst)%dbd*g4x6_list(iconst)%dbd &
                      -dtsqby2*dtsqby2*DOTPROD_3D(vec, vec)-DOTPROD_3D(r0_24, r0_24)
         vec = lg4x6(iconst)%lambda(6)*lg4x6(iconst)%ff*(imass3+imass4)- &
               lg4x6(iconst)%lambda(2)*imass3*lg4x6(iconst)%fb+ &
               lg4x6(iconst)%lambda(3)*imass4*lg4x6(iconst)%fc- &
               lg4x6(iconst)%lambda(4)*imass3*lg4x6(iconst)%fd+ &
               lg4x6(iconst)%lambda(5)*imass4*lg4x6(iconst)%fe
         bvec(6, 1) = g4x6_list(iconst)%dcd*g4x6_list(iconst)%dcd &
                      -dtsqby2*dtsqby2*DOTPROD_3D(vec, vec)-DOTPROD_3D(r0_34, r0_34)
         bvec = bvec*idtsq

         ! get lambda
         atemp = amat
         CALL solve_system(atemp, 6, bvec)
         lg4x6(iconst)%lambda(:) = bvec(:, 1)
         lg4x6(iconst)%del_lambda(:) = lg4x6(iconst)%lambda(:)- &
                                       lg4x6(iconst)%lambda_old(:)
         lg4x6(iconst)%lambda_old(:) = lg4x6(iconst)%lambda(:)

         fc1 = lg4x6(iconst)%del_lambda(1)*lg4x6(iconst)%fa+ &
               lg4x6(iconst)%del_lambda(2)*lg4x6(iconst)%fb+ &
               lg4x6(iconst)%del_lambda(3)*lg4x6(iconst)%fc
         fc2 = -lg4x6(iconst)%del_lambda(1)*lg4x6(iconst)%fa+ &
               lg4x6(iconst)%del_lambda(4)*lg4x6(iconst)%fd+ &
               lg4x6(iconst)%del_lambda(5)*lg4x6(iconst)%fe
         fc3 = -lg4x6(iconst)%del_lambda(2)*lg4x6(iconst)%fb- &
               lg4x6(iconst)%del_lambda(4)*lg4x6(iconst)%fd+ &
               lg4x6(iconst)%del_lambda(6)*lg4x6(iconst)%ff
         fc4 = -lg4x6(iconst)%del_lambda(3)*lg4x6(iconst)%fc- &
               lg4x6(iconst)%del_lambda(5)*lg4x6(iconst)%fe- &
               lg4x6(iconst)%del_lambda(6)*lg4x6(iconst)%ff
         r1(:) = pos(:, index_a)+imass1*dtsqby2*fc1(:)
         r2(:) = pos(:, index_b)+imass2*dtsqby2*fc2(:)
         r3(:) = pos(:, index_c)+imass3*dtsqby2*fc3(:)
         r4(:) = pos(:, index_d)+imass4*dtsqby2*fc4(:)
         v1(:) = vel(:, index_a)+imass1*dtby2*fc1(:)
         v2(:) = vel(:, index_b)+imass2*dtby2*fc2(:)
         v3(:) = vel(:, index_c)+imass3*dtby2*fc3(:)
         v4(:) = vel(:, index_d)+imass4*dtby2*fc4(:)
         r12 = r1-r2
         r13 = r1-r3
         r14 = r1-r4
         r23 = r2-r3
         r24 = r2-r4
         r34 = r3-r4
         ! compute the tolerance:
         sigma = DOT_PRODUCT(r12, r12)-g4x6_list(iconst)%dab* &
                 g4x6_list(iconst)%dab
         max_sigma = MAX(max_sigma, ABS(sigma))
         sigma = DOT_PRODUCT(r13, r13)-g4x6_list(iconst)%dac* &
                 g4x6_list(iconst)%dac
         max_sigma = MAX(max_sigma, ABS(sigma))
         sigma = DOT_PRODUCT(r14, r14)-g4x6_list(iconst)%dad* &
                 g4x6_list(iconst)%dad
         max_sigma = MAX(max_sigma, ABS(sigma))
         sigma = DOT_PRODUCT(r23, r23)-g4x6_list(iconst)%dbc* &
                 g4x6_list(iconst)%dbc
         max_sigma = MAX(max_sigma, ABS(sigma))
         sigma = DOT_PRODUCT(r24, r24)-g4x6_list(iconst)%dbd* &
                 g4x6_list(iconst)%dbd
         max_sigma = MAX(max_sigma, ABS(sigma))
         sigma = DOT_PRODUCT(r34, r34)-g4x6_list(iconst)%dcd* &
                 g4x6_list(iconst)%dcd
         max_sigma = MAX(max_sigma, ABS(sigma))

         ! update positions with full multiplier
         pos(:, index_a) = r1(:)
         pos(:, index_b) = r2(:)
         pos(:, index_c) = r3(:)
         pos(:, index_d) = r4(:)

         ! update velocites with full multiplier
         vel(:, index_a) = v1(:)
         vel(:, index_b) = v2(:)
         vel(:, index_c) = v3(:)
         vel(:, index_d) = v4(:)
      END DO
   END SUBROUTINE shake_4x6_low

! **************************************************************************************************
!> \brief ...
!> \param fixd_list ...
!> \param g4x6_list ...
!> \param lg4x6 ...
!> \param ng4x6 ...
!> \param first_atom ...
!> \param particle_set ...
!> \param pos ...
!> \param vel ...
!> \param r_shake ...
!> \param dt ...
!> \param ishake ...
!> \param max_sigma ...
!> \par History
!>      none
! **************************************************************************************************
   SUBROUTINE shake_roll_4x6_low(fixd_list, g4x6_list, lg4x6, ng4x6, first_atom, &
                                 particle_set, pos, vel, r_shake, dt, ishake, max_sigma)
      TYPE(fixd_constraint_type), DIMENSION(:), POINTER  :: fixd_list
      TYPE(g4x6_constraint_type), POINTER                :: g4x6_list(:)
      TYPE(local_g4x6_constraint_type), POINTER          :: lg4x6(:)
      INTEGER, INTENT(IN)                                :: ng4x6, first_atom
      TYPE(particle_type), POINTER                       :: particle_set(:)
      REAL(KIND=dp), INTENT(INOUT)                       :: pos(:, :), vel(:, :)
      REAL(KIND=dp), DIMENSION(:, :), INTENT(IN)         :: r_shake
      REAL(kind=dp), INTENT(in)                          :: dt
      INTEGER, INTENT(IN)                                :: ishake
      REAL(KIND=dp), INTENT(INOUT)                       :: max_sigma

      INTEGER                                            :: iconst, index_a, index_b, index_c, &
                                                            index_d
      REAL(KIND=dp)                                      :: dtby2, dtsqby2, idtsq, imass1, imass2, &
                                                            imass3, imass4, sigma
      REAL(KIND=dp), DIMENSION(3) :: f_roll1, f_roll2, f_roll3, f_roll4, f_roll5, f_roll6, fc1, &
         fc2, fc3, fc4, r0_12, r0_13, r0_14, r0_23, r0_24, r0_34, r1, r12, r13, r14, r2, r23, r24, &
         r3, r34, r4, v1, v2, v3, v4, vec
      REAL(KIND=dp), DIMENSION(6, 1)                     :: bvec
      REAL(KIND=dp), DIMENSION(6, 6)                     :: amat, atemp
      TYPE(atomic_kind_type), POINTER                    :: atomic_kind

      dtsqby2 = dt*dt*.5_dp
      idtsq = 1.0_dp/dt/dt
      dtby2 = dt*.5_dp
      DO iconst = 1, ng4x6
         IF (g4x6_list(iconst)%restraint%active) CYCLE
         index_a = g4x6_list(iconst)%a+first_atom-1
         index_b = g4x6_list(iconst)%b+first_atom-1
         index_c = g4x6_list(iconst)%c+first_atom-1
         index_d = g4x6_list(iconst)%d+first_atom-1
         IF (ishake == 1) THEN
            r0_12(:) = pos(:, index_a)-pos(:, index_b)
            r0_13(:) = pos(:, index_a)-pos(:, index_c)
            r0_23(:) = pos(:, index_b)-pos(:, index_c)
            r0_14(:) = pos(:, index_a)-pos(:, index_d)
            r0_24(:) = pos(:, index_b)-pos(:, index_d)
            r0_34(:) = pos(:, index_c)-pos(:, index_d)
            atomic_kind => particle_set(index_a)%atomic_kind
            imass1 = 1.0_dp/atomic_kind%mass
            atomic_kind => particle_set(index_b)%atomic_kind
            imass2 = 1.0_dp/atomic_kind%mass
            atomic_kind => particle_set(index_c)%atomic_kind
            imass3 = 1.0_dp/atomic_kind%mass
            atomic_kind => particle_set(index_d)%atomic_kind
            imass4 = 1.0_dp/atomic_kind%mass
            lg4x6(iconst)%fa = -2.0_dp*(lg4x6(iconst)%ra_old- &
                                        lg4x6(iconst)%rb_old)
            lg4x6(iconst)%fb = -2.0_dp*(lg4x6(iconst)%ra_old- &
                                        lg4x6(iconst)%rc_old)
            lg4x6(iconst)%fc = -2.0_dp*(lg4x6(iconst)%ra_old- &
                                        lg4x6(iconst)%rd_old)
            lg4x6(iconst)%fd = -2.0_dp*(lg4x6(iconst)%rb_old- &
                                        lg4x6(iconst)%rc_old)
            lg4x6(iconst)%fe = -2.0_dp*(lg4x6(iconst)%rb_old- &
                                        lg4x6(iconst)%rd_old)
            lg4x6(iconst)%ff = -2.0_dp*(lg4x6(iconst)%rc_old- &
                                        lg4x6(iconst)%rd_old)
            ! Check for fixed atom constraints
            CALL check_fixed_atom_cns_g4x6(imass1, imass2, imass3, imass4, &
                                           index_a, index_b, index_c, index_d, fixd_list, lg4x6(iconst))
            ! rotate fconst:
            CALL matvec_3x3(f_roll1, r_shake, lg4x6(iconst)%fa)
            CALL matvec_3x3(f_roll2, r_shake, lg4x6(iconst)%fb)
            CALL matvec_3x3(f_roll3, r_shake, lg4x6(iconst)%fc)
            CALL matvec_3x3(f_roll4, r_shake, lg4x6(iconst)%fd)
            CALL matvec_3x3(f_roll5, r_shake, lg4x6(iconst)%fe)
            CALL matvec_3x3(f_roll6, r_shake, lg4x6(iconst)%ff)

            ! construct matrix
            amat(1, 1) = (imass1+imass2)*DOTPROD_3D(r0_12, f_roll1)
            amat(1, 2) = imass1*DOTPROD_3D(r0_12, f_roll2)
            amat(1, 3) = imass1*DOTPROD_3D(r0_12, f_roll3)
            amat(1, 4) = -imass2*DOTPROD_3D(r0_12, f_roll4)
            amat(1, 5) = -imass2*DOTPROD_3D(r0_12, f_roll5)
            amat(1, 6) = 0.0_dp

            amat(2, 1) = imass1*DOTPROD_3D(r0_13, f_roll1)
            amat(2, 2) = (imass1+imass3)*DOTPROD_3D(r0_13, f_roll2)
            amat(2, 3) = imass1*DOTPROD_3D(r0_13, f_roll3)
            amat(2, 4) = imass3*DOTPROD_3D(r0_13, f_roll4)
            amat(2, 5) = 0.0_dp
            amat(2, 6) = -imass3*DOTPROD_3D(r0_13, f_roll6)

            amat(3, 1) = imass1*DOTPROD_3D(r0_14, f_roll1)
            amat(3, 2) = imass1*DOTPROD_3D(r0_14, f_roll2)
            amat(3, 3) = (imass1+imass4)*DOTPROD_3D(r0_14, f_roll3)
            amat(3, 4) = 0.0_dp
            amat(3, 5) = imass4*DOTPROD_3D(r0_14, f_roll5)
            amat(3, 6) = imass4*DOTPROD_3D(r0_14, f_roll6)

            amat(4, 1) = -imass2*DOTPROD_3D(r0_23, f_roll1)
            amat(4, 2) = imass3*DOTPROD_3D(r0_23, f_roll2)
            amat(4, 3) = 0.0_dp
            amat(4, 4) = (imass3+imass2)*DOTPROD_3D(r0_23, f_roll4)
            amat(4, 5) = imass2*DOTPROD_3D(r0_23, f_roll5)
            amat(4, 6) = -imass3*DOTPROD_3D(r0_23, f_roll6)

            amat(5, 1) = -imass2*DOTPROD_3D(r0_24, f_roll1)
            amat(5, 2) = 0.0_dp
            amat(5, 3) = imass4*DOTPROD_3D(r0_24, f_roll3)
            amat(5, 4) = imass2*DOTPROD_3D(r0_24, f_roll4)
            amat(5, 5) = (imass4+imass2)*DOTPROD_3D(r0_24, f_roll5)
            amat(5, 6) = imass4*DOTPROD_3D(r0_24, f_roll6)

            amat(6, 1) = 0.0_dp
            amat(6, 2) = -imass3*DOTPROD_3D(r0_34, f_roll2)
            amat(6, 3) = imass4*DOTPROD_3D(r0_34, f_roll3)
            amat(6, 4) = -imass3*DOTPROD_3D(r0_34, f_roll4)
            amat(6, 5) = imass4*DOTPROD_3D(r0_34, f_roll5)
            amat(6, 6) = (imass3+imass4)*DOTPROD_3D(r0_34, f_roll6)
            ! Store values
            lg4x6(iconst)%r0_12 = r0_12
            lg4x6(iconst)%r0_13 = r0_13
            lg4x6(iconst)%r0_14 = r0_14
            lg4x6(iconst)%r0_23 = r0_23
            lg4x6(iconst)%r0_24 = r0_24
            lg4x6(iconst)%r0_34 = r0_34
            lg4x6(iconst)%f_roll1 = f_roll1
            lg4x6(iconst)%f_roll2 = f_roll2
            lg4x6(iconst)%f_roll3 = f_roll3
            lg4x6(iconst)%f_roll4 = f_roll4
            lg4x6(iconst)%f_roll5 = f_roll5
            lg4x6(iconst)%f_roll6 = f_roll6
            lg4x6(iconst)%amat = amat
            lg4x6(iconst)%imass1 = imass1
            lg4x6(iconst)%imass2 = imass2
            lg4x6(iconst)%imass3 = imass3
            lg4x6(iconst)%imass4 = imass4
            lg4x6(iconst)%lambda_old = 0.0_dp
            lg4x6(iconst)%del_lambda = 0.0_dp
         ELSE
            ! Retrieve values
            r0_12 = lg4x6(iconst)%r0_12
            r0_13 = lg4x6(iconst)%r0_13
            r0_14 = lg4x6(iconst)%r0_14
            r0_23 = lg4x6(iconst)%r0_23
            r0_24 = lg4x6(iconst)%r0_24
            r0_34 = lg4x6(iconst)%r0_34
            f_roll1 = lg4x6(iconst)%f_roll1
            f_roll2 = lg4x6(iconst)%f_roll2
            f_roll3 = lg4x6(iconst)%f_roll3
            f_roll4 = lg4x6(iconst)%f_roll4
            f_roll5 = lg4x6(iconst)%f_roll5
            f_roll6 = lg4x6(iconst)%f_roll6
            amat = lg4x6(iconst)%amat
            imass1 = lg4x6(iconst)%imass1
            imass2 = lg4x6(iconst)%imass2
            imass3 = lg4x6(iconst)%imass3
            imass4 = lg4x6(iconst)%imass4
         END IF

         ! Iterate until convergence:
         vec = lg4x6(iconst)%lambda(1)*f_roll1*(imass1+imass2)+ &
               lg4x6(iconst)%lambda(2)*imass1*f_roll2+ &
               lg4x6(iconst)%lambda(3)*imass1*f_roll3- &
               lg4x6(iconst)%lambda(4)*imass2*f_roll4- &
               lg4x6(iconst)%lambda(5)*imass2*f_roll5
         bvec(1, 1) = g4x6_list(iconst)%dab*g4x6_list(iconst)%dab &
                      -dtsqby2*dtsqby2*DOTPROD_3D(vec, vec)-DOTPROD_3D(r0_12, r0_12)
         vec = lg4x6(iconst)%lambda(2)*f_roll2*(imass1+imass3)+ &
               lg4x6(iconst)%lambda(1)*imass1*f_roll1+ &
               lg4x6(iconst)%lambda(3)*imass1*f_roll3+ &
               lg4x6(iconst)%lambda(4)*imass3*f_roll4- &
               lg4x6(iconst)%lambda(6)*imass3*f_roll6
         bvec(2, 1) = g4x6_list(iconst)%dac*g4x6_list(iconst)%dac &
                      -dtsqby2*dtsqby2*DOTPROD_3D(vec, vec)-DOTPROD_3D(r0_13, r0_13)
         vec = lg4x6(iconst)%lambda(3)*f_roll3*(imass1+imass4)+ &
               lg4x6(iconst)%lambda(1)*imass1*f_roll1+ &
               lg4x6(iconst)%lambda(2)*imass1*f_roll2+ &
               lg4x6(iconst)%lambda(5)*imass4*f_roll5+ &
               lg4x6(iconst)%lambda(6)*imass4*f_roll6
         bvec(3, 1) = g4x6_list(iconst)%dad*g4x6_list(iconst)%dad &
                      -dtsqby2*dtsqby2*DOTPROD_3D(vec, vec)-DOTPROD_3D(r0_14, r0_14)
         vec = lg4x6(iconst)%lambda(4)*f_roll4*(imass2+imass3)- &
               lg4x6(iconst)%lambda(1)*imass2*f_roll1+ &
               lg4x6(iconst)%lambda(2)*imass3*f_roll2+ &
               lg4x6(iconst)%lambda(5)*imass2*f_roll5- &
               lg4x6(iconst)%lambda(6)*imass3*f_roll6
         bvec(4, 1) = g4x6_list(iconst)%dbc*g4x6_list(iconst)%dbc &
                      -dtsqby2*dtsqby2*DOTPROD_3D(vec, vec)-DOTPROD_3D(r0_23, r0_23)
         vec = lg4x6(iconst)%lambda(5)*f_roll5*(imass2+imass4)- &
               lg4x6(iconst)%lambda(1)*imass2*f_roll1+ &
               lg4x6(iconst)%lambda(3)*imass4*f_roll3+ &
               lg4x6(iconst)%lambda(4)*imass2*f_roll4+ &
               lg4x6(iconst)%lambda(6)*imass4*f_roll6
         bvec(5, 1) = g4x6_list(iconst)%dbd*g4x6_list(iconst)%dbd &
                      -dtsqby2*dtsqby2*DOTPROD_3D(vec, vec)-DOTPROD_3D(r0_24, r0_24)
         vec = lg4x6(iconst)%lambda(6)*f_roll6*(imass3+imass4)- &
               lg4x6(iconst)%lambda(2)*imass3*f_roll2+ &
               lg4x6(iconst)%lambda(3)*imass4*f_roll3- &
               lg4x6(iconst)%lambda(4)*imass3*f_roll4+ &
               lg4x6(iconst)%lambda(5)*imass4*f_roll5
         bvec(6, 1) = g4x6_list(iconst)%dcd*g4x6_list(iconst)%dcd &
                      -dtsqby2*dtsqby2*DOTPROD_3D(vec, vec)-DOTPROD_3D(r0_34, r0_34)
         bvec = bvec*idtsq

         ! get lambda
         atemp = amat
         CALL solve_system(atemp, 6, bvec)
         lg4x6(iconst)%lambda(:) = bvec(:, 1)
         lg4x6(iconst)%del_lambda(:) = lg4x6(iconst)%lambda(:)- &
                                       lg4x6(iconst)%lambda_old(:)
         lg4x6(iconst)%lambda_old(:) = lg4x6(iconst)%lambda(:)

         fc1 = lg4x6(iconst)%del_lambda(1)*lg4x6(iconst)%fa+ &
               lg4x6(iconst)%del_lambda(2)*lg4x6(iconst)%fb+ &
               lg4x6(iconst)%del_lambda(3)*lg4x6(iconst)%fc
         fc2 = -lg4x6(iconst)%del_lambda(1)*lg4x6(iconst)%fa+ &
               lg4x6(iconst)%del_lambda(4)*lg4x6(iconst)%fd+ &
               lg4x6(iconst)%del_lambda(5)*lg4x6(iconst)%fe
         fc3 = -lg4x6(iconst)%del_lambda(2)*lg4x6(iconst)%fb- &
               lg4x6(iconst)%del_lambda(4)*lg4x6(iconst)%fd+ &
               lg4x6(iconst)%del_lambda(6)*lg4x6(iconst)%ff
         fc4 = -lg4x6(iconst)%del_lambda(3)*lg4x6(iconst)%fc- &
               lg4x6(iconst)%del_lambda(5)*lg4x6(iconst)%fe- &
               lg4x6(iconst)%del_lambda(6)*lg4x6(iconst)%ff
         CALL MATVEC_3x3(vec, r_shake, fc1)
         r1(:) = pos(:, index_a)+imass1*dtsqby2*vec
         CALL MATVEC_3x3(vec, r_shake, fc2)
         r2(:) = pos(:, index_b)+imass2*dtsqby2*vec
         CALL MATVEC_3x3(vec, r_shake, fc3)
         r3(:) = pos(:, index_c)+imass3*dtsqby2*vec
         CALL MATVEC_3x3(vec, r_shake, fc4)
         r4(:) = pos(:, index_d)+imass4*dtsqby2*vec
         CALL MATVEC_3x3(vec, r_shake, fc1)
         v1(:) = vel(:, index_a)+imass1*dtby2*vec
         CALL MATVEC_3x3(vec, r_shake, fc2)
         v2(:) = vel(:, index_b)+imass2*dtby2*vec
         CALL MATVEC_3x3(vec, r_shake, fc3)
         v3(:) = vel(:, index_c)+imass3*dtby2*vec
         CALL MATVEC_3x3(vec, r_shake, fc4)
         v4(:) = vel(:, index_d)+imass4*dtby2*vec

         r12 = r1-r2
         r13 = r1-r3
         r23 = r2-r3
         r14 = r1-r4
         r24 = r2-r4
         r34 = r3-r4
         ! compute the tolerance:
         sigma = DOT_PRODUCT(r12, r12)-g4x6_list(iconst)%dab* &
                 g4x6_list(iconst)%dab
         max_sigma = MAX(max_sigma, ABS(sigma))
         sigma = DOT_PRODUCT(r13, r13)-g4x6_list(iconst)%dac* &
                 g4x6_list(iconst)%dac
         max_sigma = MAX(max_sigma, ABS(sigma))
         sigma = DOT_PRODUCT(r14, r14)-g4x6_list(iconst)%dad* &
                 g4x6_list(iconst)%dad
         max_sigma = MAX(max_sigma, ABS(sigma))
         sigma = DOT_PRODUCT(r23, r23)-g4x6_list(iconst)%dbc* &
                 g4x6_list(iconst)%dbc
         max_sigma = MAX(max_sigma, ABS(sigma))
         sigma = DOT_PRODUCT(r24, r24)-g4x6_list(iconst)%dbd* &
                 g4x6_list(iconst)%dbd
         max_sigma = MAX(max_sigma, ABS(sigma))
         sigma = DOT_PRODUCT(r34, r34)-g4x6_list(iconst)%dcd* &
                 g4x6_list(iconst)%dcd
         max_sigma = MAX(max_sigma, ABS(sigma))

         ! update positions with full multiplier
         pos(:, index_a) = r1(:)
         pos(:, index_b) = r2(:)
         pos(:, index_c) = r3(:)
         pos(:, index_d) = r4(:)

         ! update velocites with full multiplier
         vel(:, index_a) = v1(:)
         vel(:, index_b) = v2(:)
         vel(:, index_c) = v3(:)
         vel(:, index_d) = v4(:)
      END DO
   END SUBROUTINE shake_roll_4x6_low

! **************************************************************************************************
!> \brief ...
!> \param fixd_list ...
!> \param g4x6_list ...
!> \param lg4x6 ...
!> \param first_atom ...
!> \param particle_set ...
!> \param vel ...
!> \param dt ...
!> \par History
!>      none
! **************************************************************************************************
   SUBROUTINE rattle_4x6_low(fixd_list, g4x6_list, lg4x6, first_atom, &
                             particle_set, vel, dt)

      TYPE(fixd_constraint_type), DIMENSION(:), POINTER  :: fixd_list
      TYPE(g4x6_constraint_type), POINTER                :: g4x6_list(:)
      TYPE(local_g4x6_constraint_type), POINTER          :: lg4x6(:)
      INTEGER, INTENT(IN)                                :: first_atom
      TYPE(particle_type), POINTER                       :: particle_set(:)
      REAL(KIND=dp), INTENT(INOUT)                       :: vel(:, :)
      REAL(kind=dp), INTENT(in)                          :: dt

      INTEGER                                            :: iconst, index_a, index_b, index_c, &
                                                            index_d
      REAL(KIND=dp)                                      :: dtby2, idt, imass1, imass2, imass3, &
                                                            imass4, mass
      REAL(KIND=dp), DIMENSION(3)                        :: fc1, fc2, fc3, fc4, r12, r13, r14, r23, &
                                                            r24, r34, v12, v13, v14, v23, v24, v34
      REAL(KIND=dp), DIMENSION(6, 1)                     :: bvec
      REAL(KIND=dp), DIMENSION(6, 6)                     :: amat
      TYPE(atomic_kind_type), POINTER                    :: atomic_kind

      idt = 1.0_dp/dt
      dtby2 = dt*.5_dp
      DO iconst = 1, SIZE(g4x6_list)
         IF (g4x6_list(iconst)%restraint%active) CYCLE
         index_a = g4x6_list(iconst)%a+first_atom-1
         index_b = g4x6_list(iconst)%b+first_atom-1
         index_c = g4x6_list(iconst)%c+first_atom-1
         index_d = g4x6_list(iconst)%d+first_atom-1
         v12(:) = vel(:, index_a)-vel(:, index_b)
         v13(:) = vel(:, index_a)-vel(:, index_c)
         v14(:) = vel(:, index_a)-vel(:, index_d)
         v23(:) = vel(:, index_b)-vel(:, index_c)
         v24(:) = vel(:, index_b)-vel(:, index_d)
         v34(:) = vel(:, index_c)-vel(:, index_d)

         r12(:) = particle_set(index_a)%r(:)-particle_set(index_b)%r(:)
         r13(:) = particle_set(index_a)%r(:)-particle_set(index_c)%r(:)
         r14(:) = particle_set(index_a)%r(:)-particle_set(index_d)%r(:)
         r23(:) = particle_set(index_b)%r(:)-particle_set(index_c)%r(:)
         r24(:) = particle_set(index_b)%r(:)-particle_set(index_d)%r(:)
         r34(:) = particle_set(index_c)%r(:)-particle_set(index_d)%r(:)
         atomic_kind => particle_set(index_a)%atomic_kind
         CALL get_atomic_kind(atomic_kind=atomic_kind, mass=mass)
         imass1 = 1.0_dp/mass
         atomic_kind => particle_set(index_b)%atomic_kind
         CALL get_atomic_kind(atomic_kind=atomic_kind, mass=mass)
         imass2 = 1.0_dp/mass
         atomic_kind => particle_set(index_c)%atomic_kind
         CALL get_atomic_kind(atomic_kind=atomic_kind, mass=mass)
         imass3 = 1.0_dp/mass
         atomic_kind => particle_set(index_d)%atomic_kind
         CALL get_atomic_kind(atomic_kind=atomic_kind, mass=mass)
         imass4 = 1.0_dp/mass
         lg4x6(iconst)%fa = -2.0_dp*r12
         lg4x6(iconst)%fb = -2.0_dp*r13
         lg4x6(iconst)%fc = -2.0_dp*r14
         lg4x6(iconst)%fd = -2.0_dp*r23
         lg4x6(iconst)%fe = -2.0_dp*r24
         lg4x6(iconst)%ff = -2.0_dp*r34
         ! Check for fixed atom constraints
         CALL check_fixed_atom_cns_g4x6(imass1, imass2, imass3, imass4, &
                                        index_a, index_b, index_c, index_d, fixd_list, lg4x6(iconst))
         ! construct matrix
         amat(1, 1) = (imass1+imass2)*DOTPROD_3D(r12, lg4x6(iconst)%fa)
         amat(1, 2) = imass1*DOTPROD_3D(r12, lg4x6(iconst)%fb)
         amat(1, 3) = imass1*DOTPROD_3D(r12, lg4x6(iconst)%fc)
         amat(1, 4) = -imass2*DOTPROD_3D(r12, lg4x6(iconst)%fd)
         amat(1, 5) = -imass2*DOTPROD_3D(r12, lg4x6(iconst)%fe)
         amat(1, 6) = 0.0_dp

         amat(2, 1) = imass1*DOTPROD_3D(r13, lg4x6(iconst)%fa)
         amat(2, 2) = (imass1+imass3)*DOTPROD_3D(r13, lg4x6(iconst)%fb)
         amat(2, 3) = imass1*DOTPROD_3D(r13, lg4x6(iconst)%fc)
         amat(2, 4) = imass3*DOTPROD_3D(r13, lg4x6(iconst)%fd)
         amat(2, 5) = 0.0_dp
         amat(2, 6) = -imass3*DOTPROD_3D(r13, lg4x6(iconst)%ff)

         amat(3, 1) = imass1*DOTPROD_3D(r14, lg4x6(iconst)%fa)
         amat(3, 2) = imass1*DOTPROD_3D(r14, lg4x6(iconst)%fb)
         amat(3, 3) = (imass1+imass4)*DOTPROD_3D(r14, lg4x6(iconst)%fc)
         amat(3, 4) = 0.0_dp
         amat(3, 5) = imass4*DOTPROD_3D(r14, lg4x6(iconst)%fe)
         amat(3, 6) = imass4*DOTPROD_3D(r14, lg4x6(iconst)%ff)

         amat(4, 1) = -imass2*DOTPROD_3D(r23, lg4x6(iconst)%fa)
         amat(4, 2) = imass3*DOTPROD_3D(r23, lg4x6(iconst)%fb)
         amat(4, 3) = 0.0_dp
         amat(4, 4) = (imass3+imass2)*DOTPROD_3D(r23, lg4x6(iconst)%fd)
         amat(4, 5) = imass2*DOTPROD_3D(r23, lg4x6(iconst)%fe)
         amat(4, 6) = -imass3*DOTPROD_3D(r23, lg4x6(iconst)%ff)

         amat(5, 1) = -imass2*DOTPROD_3D(r24, lg4x6(iconst)%fa)
         amat(5, 2) = 0.0_dp
         amat(5, 3) = imass4*DOTPROD_3D(r24, lg4x6(iconst)%fc)
         amat(5, 4) = imass2*DOTPROD_3D(r24, lg4x6(iconst)%fd)
         amat(5, 5) = (imass4+imass2)*DOTPROD_3D(r24, lg4x6(iconst)%fe)
         amat(5, 6) = imass4*DOTPROD_3D(r24, lg4x6(iconst)%ff)

         amat(6, 1) = 0.0_dp
         amat(6, 2) = -imass3*DOTPROD_3D(r34, lg4x6(iconst)%fb)
         amat(6, 3) = imass4*DOTPROD_3D(r34, lg4x6(iconst)%fc)
         amat(6, 4) = -imass3*DOTPROD_3D(r34, lg4x6(iconst)%fd)
         amat(6, 5) = imass4*DOTPROD_3D(r34, lg4x6(iconst)%fe)
         amat(6, 6) = (imass3+imass4)*DOTPROD_3D(r34, lg4x6(iconst)%ff)

         ! construct solution vector
         bvec(1, 1) = DOTPROD_3D(r12, v12)
         bvec(2, 1) = DOTPROD_3D(r13, v13)
         bvec(3, 1) = DOTPROD_3D(r14, v14)
         bvec(4, 1) = DOTPROD_3D(r23, v23)
         bvec(5, 1) = DOTPROD_3D(r24, v24)
         bvec(6, 1) = DOTPROD_3D(r34, v34)
         bvec = -bvec*2.0_dp*idt

         ! get lambda
         CALL solve_system(amat, 6, bvec)
         lg4x6(iconst)%lambda(:) = bvec(:, 1)

         fc1 = lg4x6(iconst)%lambda(1)*lg4x6(iconst)%fa+ &
               lg4x6(iconst)%lambda(2)*lg4x6(iconst)%fb+ &
               lg4x6(iconst)%lambda(3)*lg4x6(iconst)%fc
         fc2 = -lg4x6(iconst)%lambda(1)*lg4x6(iconst)%fa+ &
               lg4x6(iconst)%lambda(4)*lg4x6(iconst)%fd+ &
               lg4x6(iconst)%lambda(5)*lg4x6(iconst)%fe
         fc3 = -lg4x6(iconst)%lambda(2)*lg4x6(iconst)%fb- &
               lg4x6(iconst)%lambda(4)*lg4x6(iconst)%fd+ &
               lg4x6(iconst)%lambda(6)*lg4x6(iconst)%ff
         fc4 = -lg4x6(iconst)%lambda(3)*lg4x6(iconst)%fc- &
               lg4x6(iconst)%lambda(5)*lg4x6(iconst)%fe- &
               lg4x6(iconst)%lambda(6)*lg4x6(iconst)%ff
         vel(:, index_a) = vel(:, index_a)+imass1*dtby2*fc1(:)
         vel(:, index_b) = vel(:, index_b)+imass2*dtby2*fc2(:)
         vel(:, index_c) = vel(:, index_c)+imass3*dtby2*fc3(:)
         vel(:, index_d) = vel(:, index_d)+imass4*dtby2*fc4(:)
      END DO
   END SUBROUTINE rattle_4x6_low

! **************************************************************************************************
!> \brief ...
!> \param fixd_list ...
!> \param g4x6_list ...
!> \param lg4x6 ...
!> \param first_atom ...
!> \param particle_set ...
!> \param vel ...
!> \param r_rattle ...
!> \param dt ...
!> \param veps ...
!> \par History
!>      none
! **************************************************************************************************
   SUBROUTINE rattle_roll_4x6_low(fixd_list, g4x6_list, lg4x6, first_atom, &
                                  particle_set, vel, r_rattle, dt, veps)

      TYPE(fixd_constraint_type), DIMENSION(:), POINTER  :: fixd_list
      TYPE(g4x6_constraint_type), POINTER                :: g4x6_list(:)
      TYPE(local_g4x6_constraint_type), POINTER          :: lg4x6(:)
      INTEGER, INTENT(IN)                                :: first_atom
      TYPE(particle_type), POINTER                       :: particle_set(:)
      REAL(KIND=dp), INTENT(INOUT)                       :: vel(:, :)
      REAL(KIND=dp), DIMENSION(:, :), INTENT(IN)         :: r_rattle
      REAL(kind=dp), INTENT(in)                          :: dt
      REAL(KIND=dp), DIMENSION(:, :), INTENT(IN)         :: veps

      INTEGER                                            :: iconst, index_a, index_b, index_c, &
                                                            index_d
      REAL(KIND=dp)                                      :: dtby2, idt, imass1, imass2, imass3, &
                                                            imass4, mass
      REAL(KIND=dp), DIMENSION(3)                        :: f_roll1, f_roll2, f_roll3, f_roll4, &
                                                            f_roll5, f_roll6, fc1, fc2, fc3, fc4, &
                                                            r12, r13, r14, r23, r24, r34, v12, &
                                                            v13, v14, v23, v24, v34, vec
      REAL(KIND=dp), DIMENSION(6)                        :: lambda
      REAL(KIND=dp), DIMENSION(6, 1)                     :: bvec
      REAL(KIND=dp), DIMENSION(6, 6)                     :: amat
      TYPE(atomic_kind_type), POINTER                    :: atomic_kind

      idt = 1.0_dp/dt
      dtby2 = dt*.5_dp
      DO iconst = 1, SIZE(g4x6_list)
         IF (g4x6_list(iconst)%restraint%active) CYCLE
         index_a = g4x6_list(iconst)%a+first_atom-1
         index_b = g4x6_list(iconst)%b+first_atom-1
         index_c = g4x6_list(iconst)%c+first_atom-1
         index_d = g4x6_list(iconst)%d+first_atom-1
         v12(:) = vel(:, index_a)-vel(:, index_b)
         v13(:) = vel(:, index_a)-vel(:, index_c)
         v14(:) = vel(:, index_a)-vel(:, index_d)
         v23(:) = vel(:, index_b)-vel(:, index_c)
         v24(:) = vel(:, index_b)-vel(:, index_d)
         v34(:) = vel(:, index_c)-vel(:, index_d)

         r12(:) = particle_set(index_a)%r(:)-particle_set(index_b)%r(:)
         r13(:) = particle_set(index_a)%r(:)-particle_set(index_c)%r(:)
         r14(:) = particle_set(index_a)%r(:)-particle_set(index_d)%r(:)
         r23(:) = particle_set(index_b)%r(:)-particle_set(index_c)%r(:)
         r24(:) = particle_set(index_b)%r(:)-particle_set(index_d)%r(:)
         r34(:) = particle_set(index_c)%r(:)-particle_set(index_d)%r(:)
         atomic_kind => particle_set(index_a)%atomic_kind
         CALL get_atomic_kind(atomic_kind=atomic_kind, mass=mass)
         imass1 = 1.0_dp/mass
         atomic_kind => particle_set(index_b)%atomic_kind
         CALL get_atomic_kind(atomic_kind=atomic_kind, mass=mass)
         imass2 = 1.0_dp/mass
         atomic_kind => particle_set(index_c)%atomic_kind
         CALL get_atomic_kind(atomic_kind=atomic_kind, mass=mass)
         imass3 = 1.0_dp/mass
         atomic_kind => particle_set(index_d)%atomic_kind
         CALL get_atomic_kind(atomic_kind=atomic_kind, mass=mass)
         imass4 = 1.0_dp/mass
         lg4x6(iconst)%fa = -2.0_dp*r12
         lg4x6(iconst)%fb = -2.0_dp*r13
         lg4x6(iconst)%fc = -2.0_dp*r14
         lg4x6(iconst)%fd = -2.0_dp*r23
         lg4x6(iconst)%fe = -2.0_dp*r24
         lg4x6(iconst)%ff = -2.0_dp*r34
         ! Check for fixed atom constraints
         CALL check_fixed_atom_cns_g4x6(imass1, imass2, imass3, imass4, &
                                        index_a, index_b, index_c, index_d, fixd_list, lg4x6(iconst))
         ! roll the fc
         CALL MATVEC_3x3(f_roll1, r_rattle, lg4x6(iconst)%fa)
         CALL MATVEC_3x3(f_roll2, r_rattle, lg4x6(iconst)%fb)
         CALL MATVEC_3x3(f_roll3, r_rattle, lg4x6(iconst)%fc)
         CALL MATVEC_3x3(f_roll4, r_rattle, lg4x6(iconst)%fd)
         CALL MATVEC_3x3(f_roll5, r_rattle, lg4x6(iconst)%fe)
         CALL MATVEC_3x3(f_roll6, r_rattle, lg4x6(iconst)%ff)
         ! construct matrix
         amat(1, 1) = (imass1+imass2)*DOTPROD_3D(r12, f_roll1)
         amat(1, 2) = imass1*DOTPROD_3D(r12, f_roll2)
         amat(1, 3) = imass1*DOTPROD_3D(r12, f_roll3)
         amat(1, 4) = -imass2*DOTPROD_3D(r12, f_roll4)
         amat(1, 5) = -imass2*DOTPROD_3D(r12, f_roll5)
         amat(1, 6) = 0.0_dp

         amat(2, 1) = imass1*DOTPROD_3D(r13, f_roll1)
         amat(2, 2) = (imass1+imass3)*DOTPROD_3D(r13, f_roll2)
         amat(2, 3) = imass1*DOTPROD_3D(r13, f_roll3)
         amat(2, 4) = imass3*DOTPROD_3D(r13, f_roll4)
         amat(2, 5) = 0.0_dp
         amat(2, 6) = -imass3*DOTPROD_3D(r13, f_roll6)

         amat(3, 1) = imass1*DOTPROD_3D(r14, f_roll1)
         amat(3, 2) = imass1*DOTPROD_3D(r14, f_roll2)
         amat(3, 3) = (imass1+imass4)*DOTPROD_3D(r14, f_roll3)
         amat(3, 4) = 0.0_dp
         amat(3, 5) = imass4*DOTPROD_3D(r14, f_roll5)
         amat(3, 6) = imass4*DOTPROD_3D(r14, f_roll6)

         amat(4, 1) = -imass2*DOTPROD_3D(r23, f_roll1)
         amat(4, 2) = imass3*DOTPROD_3D(r23, f_roll2)
         amat(4, 3) = 0.0_dp
         amat(4, 4) = (imass3+imass2)*DOTPROD_3D(r23, f_roll4)
         amat(4, 5) = imass2*DOTPROD_3D(r23, f_roll5)
         amat(4, 6) = -imass3*DOTPROD_3D(r23, f_roll6)

         amat(5, 1) = -imass2*DOTPROD_3D(r24, f_roll1)
         amat(5, 2) = 0.0_dp
         amat(5, 3) = imass4*DOTPROD_3D(r24, f_roll3)
         amat(5, 4) = imass2*DOTPROD_3D(r24, f_roll4)
         amat(5, 5) = (imass4+imass2)*DOTPROD_3D(r24, f_roll5)
         amat(5, 6) = imass4*DOTPROD_3D(r24, f_roll6)

         amat(6, 1) = 0.0_dp
         amat(6, 2) = -imass3*DOTPROD_3D(r34, f_roll2)
         amat(6, 3) = imass4*DOTPROD_3D(r34, f_roll3)
         amat(6, 4) = -imass3*DOTPROD_3D(r34, f_roll4)
         amat(6, 5) = imass4*DOTPROD_3D(r34, f_roll5)
         amat(6, 6) = (imass3+imass4)*DOTPROD_3D(r34, f_roll6)

         ! construct solution vector
         CALL matvec_3x3(vec, veps, r12)
         bvec(1, 1) = DOTPROD_3D(r12, v12+vec)
         CALL matvec_3x3(vec, veps, r13)
         bvec(2, 1) = DOTPROD_3D(r13, v13+vec)
         CALL matvec_3x3(vec, veps, r14)
         bvec(3, 1) = DOTPROD_3D(r14, v14+vec)
         CALL matvec_3x3(vec, veps, r23)
         bvec(4, 1) = DOTPROD_3D(r23, v23+vec)
         CALL matvec_3x3(vec, veps, r24)
         bvec(5, 1) = DOTPROD_3D(r24, v24+vec)
         CALL matvec_3x3(vec, veps, r34)
         bvec(6, 1) = DOTPROD_3D(r34, v34+vec)
         bvec = -bvec*2.0_dp*idt

         ! get lambda
         CALL solve_system(amat, 6, bvec)
         lambda(:) = bvec(:, 1)
         lg4x6(iconst)%lambda(:) = lambda

         fc1 = lambda(1)*f_roll1+ &
               lambda(2)*f_roll2+ &
               lambda(3)*f_roll3
         fc2 = -lambda(1)*f_roll1+ &
               lambda(4)*f_roll4+ &
               lambda(5)*f_roll5
         fc3 = -lambda(2)*f_roll2- &
               lambda(4)*f_roll4+ &
               lambda(6)*f_roll6
         fc4 = -lambda(3)*f_roll3- &
               lambda(5)*f_roll5- &
               lambda(6)*f_roll6
         vel(:, index_a) = vel(:, index_a)+imass1*dtby2*fc1(:)
         vel(:, index_b) = vel(:, index_b)+imass2*dtby2*fc2(:)
         vel(:, index_c) = vel(:, index_c)+imass3*dtby2*fc3(:)
         vel(:, index_d) = vel(:, index_d)+imass4*dtby2*fc4(:)
      END DO
   END SUBROUTINE rattle_roll_4x6_low

END MODULE constraint_4x6
