First implementation of AStar pathfinding.