WIP: Refactor and add some tests for sanity