Refactor and add some asserts