Add total energy test to testEnergy.cpp