Indeed that does look slow/painful.  Looks like a priority queue insert into a dequeue data structure.  Does this happen on a path that's used frequently enough to bother looking for a better implementation?