Adding tests, cleanup of some code