Energy Conservation is now a part of the cost function.

Added function to keep track of change in energy for a single step as
well as for a already calculated trajectory.