Initial work on new algorithm.

Hopefully I didn't break anything.