Added possibility to set initial coefficient matrices. Fewer energy
calculations.
Added separate function calculateEnergy