Working pathfinding A*