Some refactoring, tests, and Readme updated