Use a priority queue for finding lowest cost path node.
Optimize pathfinder.

About 2x as fast == fast enough.