Cleanup, add tests and README