Fixed bug in algorithm