Fixed bugs in algorithm