Refactored, added tests
Move tests to proper place