Implemented energy calculation