Fixes and changes to the algorithm