Bugs in algorithm, need to fix