A* pathfinding algorithm working