Iteration function complete
Add basic energy calculation--diff from green
Update deformation algo and energy func