Refactored and remove dependency on heapq