Tryed to improve pathfinding algorithm