Refactors and cleans up code, fixes test