Clean-up test code and add some functionality to do that