Fixed multiple bugs in algorithm