Finished task 5, added tests, some refactoring