refs #7144 New versions of the algorithm.

First few changes.
refs #7144 Refactored old alg
refs #7144 More refactoring

Work in progress still