Finished off algorithm. Needs further testing.

Refs #10169