Finished refactoring heap and re writing tests so that they all pass