A* Pathfinding.  Has bugs not working