Refactored code a bit, added tests