Wrote getEnergy Function and a test for it