Fixed 2 bugs with algorithm