Cleanup, removal of hack
Simplify algo + test addition
Merge pull request #5 from JoeTheDentist/testing

Testing