+namespace {
+
+using namespace Marklin;
+
+typedef std::pair<Track *, unsigned> Key;
+
+struct Node
+{
+ Track *track;
+ unsigned ep;
+ Node *prev;
+ float dist;
+
+ Node():
+ track(0), ep(0), prev(0), dist(0)
+ { }
+
+ Node(Track &t, unsigned e):
+ track(&t), ep(e), prev(0), dist(0)
+ { }
+
+ Node(Track &t, unsigned e, Node &r, float d):
+ track(&t), ep(e), prev(&r), dist(prev->dist+d)
+ { }
+
+ bool operator<(const Node &other) const
+ { return dist>other.dist; }
+};
+
+struct TrackMatch
+{
+ Track &track;
+
+ TrackMatch(Track &t): track(t) { }
+
+ bool operator()(Track &t) const { return &t==&track; }
+};
+
+struct TrackInSet
+{
+ const set<Track *> &tracks;
+
+ TrackInSet(const set<Track *> &t): tracks(t) { }
+
+ bool operator()(Track &t) const { return tracks.count(&t); }
+};
+
+template<typename Pred>
+list<Track *> dijkstra(Track &from, unsigned ep, const Pred &goal)
+{
+ map<Key, Node> track_nodes;
+ priority_queue<Node> nodes;
+ Node *final = 0;
+
+ nodes.push(Node(from, ep));
+
+ while(!nodes.empty())
+ {
+ Node lowest = nodes.top();
+ nodes.pop();
+
+ Key key(lowest.track, lowest.ep);
+ if(track_nodes.count(key))
+ continue;
+
+ Node &ref = track_nodes[key] = lowest;
+ if(goal(*lowest.track))
+ {
+ final = &ref;
+ break;
+ }
+
+ const TrackType &type = lowest.track->get_type();
+ const vector<TrackType::Endpoint> &eps = type.get_endpoints();
+ const vector<Track *> &links = lowest.track->get_links();
+ for(unsigned i=0; i<eps.size(); ++i)
+ {
+ if(i==lowest.ep || !links[i])
+ continue;
+
+ unsigned link_ep = links[i]->get_endpoint_by_link(*lowest.track);
+ if(track_nodes.count(Key(links[i], link_ep)))
+ continue;
+
+ unsigned mask = eps[i].paths&eps[lowest.ep].paths;
+ if(!mask)
+ continue;
+
+ unsigned path=0;
+ for(; !(mask&1); ++path, mask>>=1) ;
+ nodes.push(Node(*links[i], links[i]->get_endpoint_by_link(*lowest.track), ref, type.get_path_length(path)));
+ }
+ }
+
+ list<Track *> result;
+ for(Node *node=final; node; node=node->prev)
+ result.push_front(node->track);
+
+ return result;
+}
+
+template<typename Pred>
+Route *create_route(Track &from, unsigned ep, const Pred &goal)
+{
+ list<Track *> tracks = dijkstra(from, ep, goal);
+
+ if(tracks.empty())
+ return 0;
+
+ Route *route = new Route(from.get_layout());
+ for(list<Track *>::iterator i=tracks.begin(); i!=tracks.end(); ++i)
+ route->add_track(**i);
+
+ route->set_name("Pathfinder");
+ route->set_temporary(true);
+
+ return route;
+}
+
+}
+
+