Refactor to faciliate unit tests
Add simple unit test for algorithm