Bug fix in algorithm