Algorithm is working.  Adding test set and also starting to clean-up code
and modularize it.