Fixes to algorithm