Completed algorithm. Probably off by one bugs