use diagonalization method in CMS-PDFT Lagrangian

Changing the way of solving for Lagrange multipliers for the intermediate-state rotations. Add sanity check for this calculation.

• Documentation updated or not needed.
• Test created or not needed.
• Attribution in source files and CONTRIBUTORS.md updated if necessary.