First implementation, use heap or priority queue