bug fix
add temp variable for debugging
bug fix energy function
Remove std::vector<std::array<std::pair>>> data structure for keep track of previous smooth cost
Four experiments to show Yuri