Initial implementation of fully working algorithm.