Added second set of functions which consider cost function that includes energy conservation.