Updated energy function to match one in paper