Finish BFS and path find