Working on A* algorithm