Tidy code and add tests