fixed bugs in algorithm and made better tests