Completed algorithm. Untested.