FullShake Subroutine

    subroutine FullShake()

        ! This method calculates the lambdas by solving the full matrix
        ! equation.

        integer :: l, n, m, info, ipiv(TotNoConstraints)
        character(len=*), parameter :: this_routine = 'FullShake'

        call set_timer(FullShake_Time, 30)

! FULL MATRIX INVERSION METHOD

! Calculate matrix from the derivatives of the constraints w.r.t the the coefficients at t1 and t2. I.e. the initial
! coefficients and those that have been moved by the corrected force.

        DerivConstrT1T2(:, :) = 0.0_dp
        do l = 1, TotNoConstraints
            do n = 1, TotNoConstraints
                do m = 1, NoOrbs
                    ! Product of constraint i, j at time t1, mult by constraint l, n.
                    ! Add these over all m for a specific constraints to get matrix elements
                    DerivConstrT1T2(n, l) = DerivConstrT1T2(n, l) + (Dot_Product(DerivConstrT2(:, m, l), DerivConstrT1(:, m, n)))
                end do
            end do
        end do       ! have filled up whole matrix

! Invert the matrix to calculate the lambda values.
! LU decomposition.
        call dgetrf(TotNoConstraints, TotNoConstraints, DerivConstrT1T2, TotNoConstraints, ipiv, info)
        if (info /= 0) then
            write(stdout, *) 'info ', info
            call Stop_All(this_routine, "The LU decomposition of matrix inversion failed...")
        end if

        do n = 1, TotNoConstraints
            ShakeLambdaNew(n) = Constraint(n) / (TimeStep * (-1))
        end do
        ! These are actually still the constraint values, but now Lambda(n)
        ! can go into dgetrs as the constraints (B in AX = B), and come out
        ! as the computed lambdas (X).

        call dgetrs('N', TotNoConstraints, 1, DerivConstrT1T2, TotNoConstraints, ipiv, ShakeLambdaNew, TotNoConstraints, info)
        if (info /= 0) call Stop_All(this_routine, "Error in dgetrs, solving for the lambdas...")

        call halt_timer(FullShake_Time)

    end subroutine FullShake