Remove usage of STL heap, to be manually implemented later