Fixed error in calculateEnergy function