Working A* algorithm