Mostly implmented with a priority queue; still not completely optimal in some cases.