+namespace {
+
+using namespace Marklin;
+
+typedef std::pair<const Track *, unsigned> Key;
+
+struct Node
+{
+ const Track *track;
+ unsigned ep;
+ Node *prev;
+ float dist;
+
+ Node():
+ track(0), ep(0), prev(0), dist(0)
+ { }
+
+ Node(const Track &t, unsigned e):
+ track(&t), ep(e), prev(0), dist(0)
+ { }
+
+ Node(const 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
+{
+ const Track &track;
+
+ TrackMatch(const Track &t): track(t) { }
+
+ bool operator()(const Track &t) const { return &t==&track; }
+};
+
+struct TrackInRoute
+{
+ const Route &route;
+
+ TrackInRoute(const Route &r): route(r) { }
+
+ bool operator()(const Track &t) const { return route.get_tracks().count(&t); }
+};
+
+template<typename Pred>
+list<const Track *> dijkstra(const 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<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)));
+ }
+ }
+
+ if(!final)
+ throw InvalidParameterValue("Could not find a route");
+
+ list<const Track *> result;
+ for(Node *node=final; node; node=node->prev)
+ result.push_front(node->track);
+
+ return result;
+}
+
+unsigned count = 0;
+
+template<typename Pred>
+Route *create_route(const Track &from, unsigned ep, const Pred &goal)
+{
+ list<const Track *> tracks = dijkstra(from, ep, goal);
+
+ Route *route = new Route(from.get_layout(), format("-%d-", ++count));
+ for(list<const Track *>::iterator i=tracks.begin(); i!=tracks.end(); ++i)
+ route->add_track(**i);
+
+ route->set_temporary(true);
+
+ return route;
+}
+
+}
+
+