Add unit test and refactor minor stuff