Refactored a bit, added test