! ===========================================================================
! Linear_Molecule algorithm part 1 - Update positions
subroutine constrain_positions_@PBC@(molecule, dt, box, ions, &
      xyz_now, forces_now, xyz_next, velocity_next)
   implicit none
   ! Passed in
   ! ---------
   type(MW_molecule_t), intent(in) :: molecule
   real(wp), intent(in) :: dt ! time step
   type(MW_box_t), intent(in) :: box ! simulation box
   type(MW_ion_t), intent(in)      :: ions(:)

   real(wp), intent(in) :: xyz_now(:,:)       ! coordinates at t
   real(wp), intent(in) :: forces_now(:,:)       ! coordinates at t

   ! Passed in/out
   ! -------------
   real(wp), intent(inout) :: xyz_next(:,:) !< coodinates at t+dt
   real(wp), intent(inout) :: velocity_next(:,:) !< velocities at t+dt

   ! Local
   ! -----
   integer :: imol
   integer :: num_sites
   integer :: num_constraints

   logical :: satisfied
   integer :: ia, ib, ic, iasite, ibsite, icsite, iatype, ibtype, ictype
   real(wp) :: dr, drsq, delta, tolerance
   real(wp) :: sx, sy, sz, snorm2
   real(wp) :: rijx, rijy, rijz, rijnorm2, sdotrij
   real(wp) :: iamassrec, ibmassrec, alpha
   real(wp) :: g
   integer :: max_iteration

   real(wp) :: a, b, c
   real(wp) :: mass_global, ma, mb, mc, ma_rec, mb_rec, mc_rec
   real(wp) :: ca, cb, fa, fb
   real(wp) :: halfdt

   a = box%length(1)
   b = box%length(2)
   c = box%length(3)

   halfdt = 0.5_wp * dt

   num_sites = molecule%num_sites
   num_constraints = molecule%num_constraints
   max_iteration = molecule%constraint_max_iteration
   tolerance = molecule%constraint_tolerance

   ca = molecule%other_sites_coordinates(1,1)
   cb = molecule%other_sites_coordinates(2,1)

   ! Compute global mass
   iasite = molecule%basis_sites(1)
   iatype = molecule%sites(iasite)
   ma = ions(iatype)%mass
   ma_rec = 1.0_wp / ma

   ibsite = molecule%basis_sites(2)
   ibtype = molecule%sites(ibsite)
   mb = ions(ibtype)%mass
   mb_rec = 1.0_wp / mb

   icsite = molecule%other_sites(1)
   ictype = molecule%sites(icsite)
   mc = ions(ictype)%mass
   mc_rec = 1.0_wp / mc

   mass_global = (ma * mb * mc) / (ma*mb + ca*ca*mc*mb + cb*cb*mc*ma)

   do imol = 1, molecule%n

      ia = ions(iatype)%offset + imol
      ib = ions(ibtype)%offset + imol
      ic = ions(ictype)%offset + imol

      ! Correct forces felt by A
      fa = -ca*ca*mass_global/ma * forces_now(ia,1) - &
            ca*cb * mass_global/mb * forces_now(ib,1) + &
               ca*mass_global/mc * forces_now(ic,1)
      velocity_next(ia,1) = velocity_next(ia,1) + halfdt * ma_rec * fa
      xyz_next(ia,1) = xyz_next(ia,1) + halfdt * dt * ma_rec * fa

      fa = -ca*ca*mass_global/ma * forces_now(ia,2) - &
            ca*cb * mass_global/mb * forces_now(ib,2) + &
               ca*mass_global/mc * forces_now(ic,2)
      velocity_next(ia,2) = velocity_next(ia,2) + halfdt * ma_rec * fa
      xyz_next(ia,2) = xyz_next(ia,2) + halfdt * dt * ma_rec * fa

      fa = -ca*ca*mass_global/ma * forces_now(ia,3) - &
            ca*cb * mass_global/mb * forces_now(ib,3) + &
               ca*mass_global/mc * forces_now(ic,3)
      velocity_next(ia,3) = velocity_next(ia,3) + halfdt * ma_rec * fa
      xyz_next(ia,3) = xyz_next(ia,3) + halfdt * dt * ma_rec * fa

      ! Correct forces felt by B
      fb = -ca*cb*mass_global/ma * forces_now(ia,1) - &
            cb*cb * mass_global/mb * forces_now(ib,1) + &
            cb*mass_global/mc * forces_now(ic,1)
      velocity_next(ib,1) = velocity_next(ib,1) + halfdt * mb_rec * fb
      xyz_next(ib,1) = xyz_next(ib,1) + halfdt * dt * mb_rec * fb

      fb = -ca*cb*mass_global/ma * forces_now(ia,2) - &
            cb*cb * mass_global/mb * forces_now(ib,2) + &
            cb*mass_global/mc * forces_now(ic,2)
      velocity_next(ib,2) = velocity_next(ib,2) + halfdt * mb_rec * fb
      xyz_next(ib,2) = xyz_next(ib,2) + halfdt * dt * mb_rec * fb

      fb = -ca*cb*mass_global/ma * forces_now(ia,3) - &
            cb*cb * mass_global/mb * forces_now(ib,3) + &
            cb*mass_global/mc * forces_now(ic,3)
      velocity_next(ib,3) = velocity_next(ib,3) + halfdt * mb_rec * fb
      xyz_next(ib,3) = xyz_next(ib,3) + halfdt * dt * mb_rec * fb

      call minimum_image_displacement_@PBC@(a, b, c, &
            xyz_next(ia,1), xyz_next(ia,2), xyz_next(ia,3), &
            xyz_next(ib,1), xyz_next(ib,2), xyz_next(ib,3), &
            sx, sy, sz, snorm2)

      ! Test for constraint realization
      dr = molecule%basis_constrained_dr(1)
      drsq = dr*dr
      delta = snorm2 - drsq
      if (abs(delta) > tolerance*drsq) then
         satisfied = .false.

         alpha = ca/ma * (ca/ma - cb/mb) * mass_global - ma_rec - cb/mb * (ca/ma - cb/mb) * mass_global - mb_rec
         iamassrec = 2.0_wp/ma * (ca * (ca/ma - cb/mb) *mass_global - 1.0_wp)
         ibmassrec = 2.0_wp/mb * (cb * (ca/ma - cb/mb) *mass_global + 1.0_wp)

         ! Compute minimum image distance between 2 points at the previous time step
         call minimum_image_displacement_@PBC@(a, b, c, &
               xyz_now(ia,1), xyz_now(ia,2), xyz_now(ia,3), &
               xyz_now(ib,1), xyz_now(ib,2), xyz_now(ib,3), &
               rijx, rijy, rijz, rijnorm2)
         sdotrij = sx*rijx + sy*rijy + sz*rijz
         ! this is g as defined in Andersen's paper (J. of Comp. Chem., 52, 24-34)
         g = delta / (2.0_wp * dt*dt * sdotrij * alpha)

         xyz_next(ia,1) = xyz_next(ia,1) + halfdt*dt*g*iamassrec*rijx
         xyz_next(ia,2) = xyz_next(ia,2) + halfdt*dt*g*iamassrec*rijy
         xyz_next(ia,3) = xyz_next(ia,3) + halfdt*dt*g*iamassrec*rijz

         xyz_next(ib,1) = xyz_next(ib,1) + halfdt*dt*g*ibmassrec*rijx
         xyz_next(ib,2) = xyz_next(ib,2) + halfdt*dt*g*ibmassrec*rijy
         xyz_next(ib,3) = xyz_next(ib,3) + halfdt*dt*g*ibmassrec*rijz

         velocity_next(ia,1) = velocity_next(ia,1) + halfdt*g*iamassrec*rijx
         velocity_next(ia,2) = velocity_next(ia,2) + halfdt*g*iamassrec*rijy
         velocity_next(ia,3) = velocity_next(ia,3) + halfdt*g*iamassrec*rijz

         velocity_next(ib,1) = velocity_next(ib,1) + halfdt*g*ibmassrec*rijx
         velocity_next(ib,2) = velocity_next(ib,2) + halfdt*g*ibmassrec*rijy
         velocity_next(ib,3) = velocity_next(ib,3) + halfdt*g*ibmassrec*rijz

      end if

      ! Compute minimum image distance between 2 points at the previous time step
      call minimum_image_displacement_@PBC@(a, b, c, &
            xyz_next(ia,1), xyz_next(ia,2), xyz_next(ia,3), &
            xyz_next(ib,1), xyz_next(ib,2), xyz_next(ib,3), &
            rijx, rijy, rijz, rijnorm2)

      ! Place c correctly
      xyz_next(ic,1) =  xyz_next(ia,1) + cb * rijx
      xyz_next(ic,2) =  xyz_next(ia,2) + cb * rijy
      xyz_next(ic,3) =  xyz_next(ia,3) + cb * rijz

      velocity_next(ic,1) = ca * velocity_next(ia,1) + cb * velocity_next(ib,1)
      velocity_next(ic,2) = ca * velocity_next(ia,2) + cb * velocity_next(ib,2)
      velocity_next(ic,3) = ca * velocity_next(ia,3) + cb * velocity_next(ib,3)

   end do


end subroutine constrain_positions_@PBC@

! ===========================================================================
! Linear_Molecule algorithm part 2 - Update velocities
subroutine constrain_velocities_@PBC@(molecule, dt, box, ions, &
      xyz_next, forces_next, velocity_next)
   implicit none
   ! Passed in
   ! ---------
   type(MW_molecule_t), intent(in) :: molecule
   real(wp), intent(in) :: dt !< time step
   type(MW_box_t), intent(in) :: box
   type(MW_ion_t), intent(in) :: ions(:)
   real(wp), intent(in) :: xyz_next(:,:) ! coordinates at t + dt
   real(wp), intent(in) :: forces_next(:,:) ! forces at t + dt

   ! Passed out
   ! ----------
   real(wp), intent(inout) :: velocity_next(:,:)   ! coordinates at t+dt

   ! Local
   ! -----
   integer :: imol
   integer :: num_sites, num_constraints

   logical :: satisfied
   integer :: ia, ib, ic, iasite, ibsite, icsite, iatype, ibtype, ictype
   real(wp) :: rijx, rijy, rijz, rijnorm2
   real(wp) :: vijx, vijy, vijz
   real(wp) :: dr, rdotvij, g
   real(wp) :: iamassrec, ibmassrec, alpha
   integer :: max_iteration
   real(wp) :: tolerance

   real(wp) :: a, b, c
   real(wp) :: mass_global, ma, mb, mc, ma_rec, mb_rec
   real(wp) :: ca, cb, fa, fb

   real(wp) :: halfdt

   a = box%length(1)
   b = box%length(2)
   c = box%length(3)

   halfdt = 0.5_wp * dt

   num_sites = molecule%num_sites
   num_constraints = molecule%num_constraints
   max_iteration = molecule%constraint_max_iteration
   tolerance = molecule%constraint_tolerance

   ca = molecule%other_sites_coordinates(1,1)
   cb = molecule%other_sites_coordinates(2,1)

   ! Compute global mass
   iasite = molecule%basis_sites(1)
   iatype = molecule%sites(iasite)
   ma = ions(iatype)%mass
   ma_rec = 1.0_wp / ma

   ibsite = molecule%basis_sites(2)
   ibtype = molecule%sites(ibsite)
   mb = ions(ibtype)%mass
   mb_rec = 1.0_wp / mb

   icsite = molecule%other_sites(1)
   ictype = molecule%sites(icsite)
   mc = ions(ictype)%mass

   mass_global = (ma * mb * mc) / (ma*mb + ca*ca*mc*mb + cb*cb*mc*ma)

   do imol = 1, molecule%n

      ia = ions(iatype)%offset + imol
      ib = ions(ibtype)%offset + imol
      ic = ions(ictype)%offset + imol

      ! Correct forces felt by A
      fa = -ca*ca*mass_global/ma * forces_next(ia,1) - &
            ca*cb * mass_global/mb * forces_next(ib,1) + &
            ca*mass_global/mc * forces_next(ic,1)
      velocity_next(ia,1) = velocity_next(ia,1) + halfdt * ma_rec * fa

      fa = -ca*ca*mass_global/ma * forces_next(ia,2) - &
            ca*cb * mass_global/mb * forces_next(ib,2) + &
            ca*mass_global/mc * forces_next(ic,2)
      velocity_next(ia,2) = velocity_next(ia,2) + halfdt * ma_rec * fa

      fa = -ca*ca*mass_global/ma * forces_next(ia,3) - &
            ca*cb * mass_global/mb * forces_next(ib,3) + &
            ca*mass_global/mc * forces_next(ic,3)
      velocity_next(ia,3) = velocity_next(ia,3) + halfdt * ma_rec * fa

      ! Correct forces felt by B
      fb = -ca*cb*mass_global/ma * forces_next(ia,1) - &
            cb*cb * mass_global/mb * forces_next(ib,1) + &
            cb*mass_global/mc * forces_next(ic,1)
      velocity_next(ib,1) = velocity_next(ib,1) + halfdt * mb_rec * fb

      fb = -ca*cb*mass_global/ma * forces_next(ia,2) - &
            cb*cb * mass_global/mb * forces_next(ib,2) + &
            cb*mass_global/mc * forces_next(ic,2)
      velocity_next(ib,2) = velocity_next(ib,2) + halfdt * mb_rec * fb

      fb = -ca*cb*mass_global/ma * forces_next(ia,3) - &
            cb*cb * mass_global/mb * forces_next(ib,3) + &
            cb*mass_global/mc * forces_next(ic,3)
      velocity_next(ib,3) = velocity_next(ib,3) + halfdt * mb_rec * fb

      call minimum_image_displacement_@PBC@(a, b, c, &
            xyz_next(ia,1), xyz_next(ia,2), xyz_next(ia,3), &
            xyz_next(ib,1), xyz_next(ib,2), xyz_next(ib,3), &
            rijx, rijy, rijz, rijnorm2)

      vijx = velocity_next(ib,1) - velocity_next(ia,1)
      vijy = velocity_next(ib,2) - velocity_next(ia,2)
      vijz = velocity_next(ib,3) - velocity_next(ia,3)

      ! Test for constraint realization
      rdotvij = rijx*vijx + rijy*vijy + rijz*vijz
      if (abs(rdotvij) >= tolerance) then
         satisfied = .false.

         dr = molecule%basis_constrained_dr(1)
         alpha = ca/ma * (ca/ma - cb/mb) * mass_global - ma_rec - cb/mb * (ca/ma - cb/mb) * mass_global - mb_rec
         iamassrec = 2.0_wp/ma * (ca * (ca/ma - cb/mb) *mass_global - 1.0_wp)
         ibmassrec = 2.0_wp/mb * (cb * (ca/ma - cb/mb) *mass_global + 1.0_wp)

         ! this is k as defined in Andersen's paper (J. of Comp. Chem., 52, 24-34)
         g = rdotvij / (rijnorm2 * dt * alpha)

         velocity_next(ia,1) = velocity_next(ia,1) + halfdt*g*iamassrec*rijx
         velocity_next(ia,2) = velocity_next(ia,2) + halfdt*g*iamassrec*rijy
         velocity_next(ia,3) = velocity_next(ia,3) + halfdt*g*iamassrec*rijz

         velocity_next(ib,1) = velocity_next(ib,1) + halfdt*g*ibmassrec*rijx
         velocity_next(ib,2) = velocity_next(ib,2) + halfdt*g*ibmassrec*rijy
         velocity_next(ib,3) = velocity_next(ib,3) + halfdt*g*ibmassrec*rijz

      end if

      ! Set c correctly from rigidity constraint
      velocity_next(ic,1) = ca * velocity_next(ia,1) + cb * velocity_next(ib,1)
      velocity_next(ic,2) = ca * velocity_next(ia,2) + cb * velocity_next(ib,2)
      velocity_next(ic,3) = ca * velocity_next(ia,3) + cb * velocity_next(ib,3)

   end do

end subroutine constrain_velocities_@PBC@
