Step 2 working. Shortest path return correctly.