AStar in Pathfinding fixed