Add a friend function to trace path
Fix a bug exposed while replacing queue with stack