Add energy function to close #7