From f06c2b89a2601981aae84d0f971c07816dc1bab4 Mon Sep 17 00:00:00 2001
From: Mikko Rasa <tdb@tdb.fi>
Date: Wed, 14 Apr 2010 21:06:34 +0000
Subject: [PATCH] Fix pathfinder to not block itself in some situations Better
 handling of routes at the beginning of Train::reserve_more Delay clearing
 route until stopping

---
 source/libmarklin/route.cpp | 16 +++++++++++-----
 source/libmarklin/train.cpp | 22 +++++++++++++---------
 2 files changed, 24 insertions(+), 14 deletions(-)

diff --git a/source/libmarklin/route.cpp b/source/libmarklin/route.cpp
index 2ff6962..ace8d0b 100644
--- a/source/libmarklin/route.cpp
+++ b/source/libmarklin/route.cpp
@@ -19,6 +19,8 @@ namespace {
 
 using namespace Marklin;
 
+typedef std::pair<const Track *, unsigned> Key;
+
 struct Node
 {
 	const Track *track;
@@ -63,8 +65,7 @@ struct TrackInRoute
 template<typename Pred>
 list<const Track *> dijkstra(const Track &from, unsigned ep, const Pred &goal)
 {
-	// XXX Fails to find some routes - should use track+ep as key
-	map<const Track *, Node> track_nodes;
+	map<Key, Node> track_nodes;
 	priority_queue<Node> nodes;
 	Node *final = 0;
 
@@ -75,10 +76,11 @@ list<const Track *> dijkstra(const Track &from, unsigned ep, const Pred &goal)
 		Node lowest = nodes.top();
 		nodes.pop();
 
-		if(track_nodes.count(lowest.track))
+		Key key(lowest.track, lowest.ep);
+		if(track_nodes.count(key))
 			continue;
 
-		Node &ref = track_nodes[lowest.track] = lowest;
+		Node &ref = track_nodes[key] = lowest;
 		if(goal(*lowest.track))
 		{
 			final = &ref;
@@ -90,7 +92,11 @@ list<const Track *> dijkstra(const Track &from, unsigned ep, const Pred &goal)
 		const vector<Track *> &links = lowest.track->get_links();
 		for(unsigned i=0; i<eps.size(); ++i)
 		{
-			if(i==lowest.ep || !links[i] || track_nodes.count(links[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;
diff --git a/source/libmarklin/train.cpp b/source/libmarklin/train.cpp
index 8ddb66b..d5b2b22 100644
--- a/source/libmarklin/train.cpp
+++ b/source/libmarklin/train.cpp
@@ -343,6 +343,8 @@ void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt)
 			vehicles[0]->advance(reverse ? -d : d);
 		}
 	}
+	else if(end_of_route)
+		set_route(0);
 }
 
 void Train::save(list<DataFile::Statement> &st) const
@@ -461,10 +463,7 @@ void Train::sensor_event(unsigned addr, bool state)
 			{
 				unsigned nsens = reserve_more();
 				if(!nsens && end_of_route)
-				{
 					signal_arrived.emit();
-					set_route(0);
-				}
 			}
 		}
 	}
@@ -525,15 +524,20 @@ unsigned Train::reserve_more()
 		if(i->block->get_sensor_id())
 			++nsens;
 	
+	if(end_of_route)
+		return nsens;
+
 	const Route *cur_route = 0;
 	if(route)
 	{
-		unsigned exit = last->block->traverse(last->entry);
-		Track *track = last->block->get_endpoints()[exit].track;
-		if(route->get_tracks().count(track))
-			cur_route = route;
-		else if(next_route && next_route->get_tracks().count(track))
-			cur_route = next_route;
+		const set<Track *> &tracks = last->block->get_tracks();
+		for(set<Track *>::const_iterator i=tracks.begin(); (cur_route!=route && i!=tracks.end()); ++i)
+		{
+			if(route->get_tracks().count(*i))
+				cur_route = route;
+			else if(next_route && next_route->get_tracks().count(*i))
+				cur_route = next_route;
+		}
 	}
 
 	bool got_more = false;
-- 
2.45.2