Some bug fixes. Still need to set manipulator properly for the algorithm to work.
Oops needed this