Implement heap and tests