Fix bug in algorithm