Fixed, cleaned, and finished the basic algorithm.