Stole A* Implementation

from http://scriptogr.am/jdp/post/pathfinding-with-python-graphs-and-a-star