Refactor and add some tests for sanity