Add FindShortestPath funck