ShakeConstraints Subroutine

    subroutine ShakeConstraints()

        ! DerivCoeff(k,a) is the unconstrained force on the original coefficients (CoeffT1(a,k)).

        integer :: w, l, a, m, ShakeIteration, ConvergeCount, SymM, SymMin
        real(dp) :: TotCorConstraints, TotConstraints, TotLambdas
        real(dp) :: TotUncorForce, TotDiffUncorCoeffs, TotDiffCorCoeffs
        logical :: tShakeNotConverged
        integer, save :: shake_io

        if (Iteration == 1) then
            shake_io = get_free_unit()
            open(shake_io, file='SHAKEstats', status='unknown')
            write(shake_io, '(A20, 4A35, A20)') 'Shake Iteration', 'Sum Lambdas', 'Total of corrected forces', &
                & 'Sum unconstrained constraints',&
                                        &'Sum corrected constraints', 'Converge count'
        end if
        if (Mod(Iteration, 10) == 0) write(shake_io, *) 'Orbital rotation iteration  =  ', Iteration

        ShakeIteration = 0
        tShakeNotConverged = .true.

! Before we start iterating, take the current coefficients and find the derivative of the constraints with respect to them.

        call CalcDerivConstr(CoeffT1, DerivConstrT1)

! Then find the coefficients at time t2, when moved by the completely unconstrained force and the values of the each
! constraint at these positions.

        Correction(:, :) = 0.0_dp
        call FindandUsetheForce(TotUncorForce, TotDiffUncorCoeffs, CoeffUncorT2)

        call CalcConstraints(CoeffUncorT2, Constraint, TotConstraints)

        call set_timer(Shake_Time, 30)

        if (tShakeDelay) then
            if (Iteration < ShakeStart) then
                ShakeIterMax = 1
            else
                ShakeIterMax = ShakeIterInput
            end if
        end if

        ! Actually starting the calculation.
        do while (tShakeNotConverged)

            ShakeIteration = ShakeIteration + 1

            ForceCorrect(:, :) = 0.0_dp          ! Zeroing terms that are re-calculated each iteration.
            CoeffCorT2(:, :) = 0.0_dp
            ConstraintCor(:) = 0.0_dp
            DerivConstrT2(:, :, :) = 0.0_dp
            TotLambdas = 0.0_dp
            TotCorrectedForce = 0.0_dp
            TotDiffCorCoeffs = 0.0_dp

            if (ShakeIteration /= 1) then
                call UpdateLambdas()
            end if

            ShakeLambdaNew(:) = 0.0_dp

            ! For a particular set of coefficients cm:
            ! Force(corrected) = Force(uncorrected)-Lambdas.DerivConstrT1
            ! Use these derivatives, and the current lambdas to find the trial corrected force.
            ! Then use this to get the (trial) shifted coefficients.

            ! Use the lambdas of this iteration to calculate the correction to the force due to the constraints.
            Correction(:, :) = 0.0_dp

            do w = MinOccVirt, MaxOccVirt
                if (w == 1) then
                    SymMin = 1
                    MinMZ = 1
                    if (tSeparateOccVirt) then
                        MaxMZ = NoOcc
                    else
                        MaxMZ = NoOrbs
                    end if
                else
                    SymMin = 9
                    MinMZ = NoOcc + 1
                    MaxMZ = NoOrbs
                end if

                do m = MinMZ, MaxMZ
                    if (tStoreSpinOrbs) then
                        SymM = int(G1(SymLabelList2_rot(m))%sym%S)
                    else
                        SymM = int(G1(SymLabelList2_rot(m) * 2)%sym%S)
                    end if
                    do a = SymLabelCounts2_rot(1, SymM + SymMin), &
                        (SymLabelCounts2_rot(1, SymM + SymMin) + &
                         SymLabelCounts2_rot(2, SymM + SymMin) - 1)
                        do l = 1, TotNoConstraints
                            Correction(a, m) = Correction(a, m) + (ShakeLambda(l) * DerivConstrT1(a, m, l))
                        end do
                    end do
                end do
            end do

            call FindandUsetheForce(TotCorrectedForce, TotDiffCorCoeffs, CoeffCorT2)

            ! Use these new shifted coefficients to calculate the derivative of the constraints
            ! (at time t2).

            call CalcDerivConstr(CoeffCorT2, DerivConstrT2)

! Test for convergence, if convergence is reached, make the new coefficients the original ones to start the whole process again.
! Then exit out of this do loop and hence the subroutine.
            call TestShakeConvergence(ConvergeCount, TotCorConstraints, ShakeIteration, tShakeNotConverged)

! If the convergence criteria is met, exit out of this subroutine, a rotation has been made which keeps the coefficients
! orthogonal.

! and to SHAKEstats file:
            call neci_flush(stdout)
            call neci_flush(shake_io)
            if (Mod(Iteration, 10) == 0) then
                write(shake_io, '(I20, 4F35.20, I20)') ShakeIteration, TotLambdas, TotCorrectedForce, TotConstraints, &
                    TotCorConstraints, ConvergeCount
            end if

! If the convergence criteria is not met, use either the full matrix inversion method to
!find a new set of lambdas, or the shake algorithm
! (in which case SHAKEAPPROX is required in the system block of the input).

            if (tShakeApprox .and. tShakeNotConverged) then
                call ShakeApproximation()
            else if (tShakeNotConverged) then
                call FullShake()
            else
                DistCs = TotDiffCorCoeffs
            end if

        end do

        call halt_timer(Shake_Time)

    end subroutine ShakeConstraints