From: Mikko Rasa <tdb@tdb.fi>
Date: Fri, 15 Feb 2013 14:15:20 +0000 (+0200)
Subject: Separate train routing logic to a class derived from TrainAI
X-Git-Url: http://git.tdb.fi/?p=r2c2.git;a=commitdiff_plain;h=d0af7846e05691d65d8021e46c8f81e8ca05199a

Separate train routing logic to a class derived from TrainAI
---

diff --git a/source/engineer/routeselect.cpp b/source/engineer/routeselect.cpp
index 47259e3..e5c090c 100644
--- a/source/engineer/routeselect.cpp
+++ b/source/engineer/routeselect.cpp
@@ -1,6 +1,7 @@
 #include <msp/gltk/label.h>
 #include "engineer.h"
 #include "libr2c2/route.h"
+#include "libr2c2/trainrouter.h"
 #include "routeselect.h"
 
 using namespace std;
@@ -21,13 +22,18 @@ RouteSelect::RouteSelect(Engineer &e, Train &t):
 	drp_route->set_geometry(GLtk::Geometry(10, geom.h-50, geom.w-20, 20));
 	drp_route->append("(none)");
 	drp_route->set_selected_index(0);
+
+	const Route *current_route = 0;
+	if(TrainRouter *router = dynamic_cast<TrainRouter *>(train.get_tagged_ai("router")))
+		current_route = router->get_route();
+
 	const set<Route *> &routes = engineer.get_layout().get_routes();
 	unsigned n = 1;
 	for(set<Route *>::const_iterator i=routes.begin(); i!=routes.end(); ++i)
 		if(!(*i)->is_temporary())
 		{
 			drp_route->append((*i)->get_name());
-			if(*i==train.get_route())
+			if(*i==current_route)
 				drp_route->set_selected_index(n);
 			++n;
 		}
@@ -50,10 +56,9 @@ void RouteSelect::on_ok_clicked()
 			}
 			++i;
 		}
-		
-		if(!train.set_route(*i))
-			engineer.set_status("Could not set route");
+
+		train.ai_message(TrainAI::Message("set-route", *i));
 	}
 	else
-		train.set_route(0);
+		train.ai_message(TrainAI::Message("clear-route"));
 }
diff --git a/source/engineer/trainpanel.cpp b/source/engineer/trainpanel.cpp
index b29836f..97592be 100644
--- a/source/engineer/trainpanel.cpp
+++ b/source/engineer/trainpanel.cpp
@@ -3,6 +3,7 @@
 #include "libr2c2/aicontrol.h"
 #include "libr2c2/timetable.h"
 #include "libr2c2/trackiter.h"
+#include "libr2c2/trainrouter.h"
 #include "libr2c2/vehicletype.h"
 #include "engineer.h"
 #include "routeselect.h"
@@ -65,11 +66,18 @@ TrainPanel::TrainPanel(Engineer &e, Train &t):
 	pnl_extra->set_geometry(GLtk::Geometry(0, 10, geom.w, 135));
 	pnl_extra->set_visible(false);
 
-	const Route *route = train.get_route();
+	TrainRouter *router = dynamic_cast<TrainRouter *>(train.get_tagged_ai("router"));
+	if(!router)
+	{
+		router = new TrainRouter(train);
+		router->set_tag("router");
+	}
+
+	const Route *route = router->get_route();
 	pnl_extra->add(*(lbl_route = new GLtk::Label((route ? route->get_name() : "Free run"))));
 	lbl_route->set_style("digital");
 	lbl_route->set_geometry(GLtk::Geometry(10, 85, geom.w-20, 20));
-	train.signal_route_changed.connect(sigc::mem_fun(this, &TrainPanel::train_route_changed));
+	router->signal_route_changed.connect(sigc::mem_fun(this, &TrainPanel::train_route_changed));
 
 	pnl_extra->add(*(lbl_status = new GLtk::Label(status.get_status())));
 	lbl_status->set_style("digital");
@@ -276,6 +284,7 @@ void TrainPanel::go_to(Track *track, unsigned)
 {
 	pick_conn.disconnect();
 
-	if(!train.go_to(*track))
+	TrainRouter *router = dynamic_cast<TrainRouter *>(train.get_tagged_ai("router"));
+	if(!router || !router->go_to(*track))
 		engineer.set_status("Could not set route");
 }
diff --git a/source/engineer/trainproperties.cpp b/source/engineer/trainproperties.cpp
index 3609235..ecc9b2b 100644
--- a/source/engineer/trainproperties.cpp
+++ b/source/engineer/trainproperties.cpp
@@ -2,6 +2,7 @@
 #include <msp/strings/format.h>
 #include <msp/strings/lexicalcast.h>
 #include "libr2c2/driver.h"
+#include "libr2c2/trainrouter.h"
 #include "libr2c2/vehicle.h"
 #include "libr2c2/vehicletype.h"
 #include "engineer.h"
@@ -89,7 +90,8 @@ TrainProperties::TrainProperties(Engineer &e, Train *t):
 	{
 		ent_addr->set_text(lexical_cast<string>(train->get_address()));
 		ent_name->set_text(train->get_name());
-		drp_priority->set_selected_index(train->get_priority()+2);
+		if(TrainRouter *router = dynamic_cast<TrainRouter *>(train->get_tagged_ai("router")))
+			drp_priority->set_selected_index(router->get_priority()+2);
 
 		unsigned n_vehicles = train->get_n_vehicles();
 		for(unsigned i=1; i<n_vehicles; ++i)
@@ -115,7 +117,8 @@ void TrainProperties::on_ok_clicked()
 	}
 
 	train->set_name(ent_name->get_text());
-	train->set_priority(drp_priority->get_selected_index()-2);
+	if(TrainRouter *router = dynamic_cast<TrainRouter *>(train->get_tagged_ai("router")))
+		router->set_priority(drp_priority->get_selected_index()-2);
 
 	// The locomotive is vehicle 0 so we need to add 1
 	for(set<unsigned>::const_iterator i=rem_vehicles.end(); i!=rem_vehicles.begin();)
diff --git a/source/libr2c2/aicontrol.cpp b/source/libr2c2/aicontrol.cpp
index 3e31aaf..6e1c4b1 100644
--- a/source/libr2c2/aicontrol.cpp
+++ b/source/libr2c2/aicontrol.cpp
@@ -16,7 +16,7 @@ AIControl::AIControl(Train &t):
 	state(NORMAL),
 	need_update(false)
 {
-	train.signal_arrived.connect(sigc::mem_fun(this, &AIControl::arrived));
+	train.signal_ai_event.connect(sigc::mem_fun(this, &AIControl::event));
 }
 
 void AIControl::set_target_speed(float s)
@@ -104,9 +104,10 @@ void AIControl::tick(const Time::TimeStamp &, const Time::TimeDelta &)
 		train.set_active(false);
 }
 
-void AIControl::arrived()
+void AIControl::event(TrainAI &, const Message &ev)
 {
-	set_target_speed(0);
+	if(ev.type=="arrived")
+		set_target_speed(0);
 }
 
 } // namespace R2C2
diff --git a/source/libr2c2/aicontrol.h b/source/libr2c2/aicontrol.h
index 7560b99..154a2ea 100644
--- a/source/libr2c2/aicontrol.h
+++ b/source/libr2c2/aicontrol.h
@@ -37,7 +37,7 @@ public:
 	virtual void tick(const Msp::Time::TimeStamp &, const Msp::Time::TimeDelta &);
 
 private:
-	void arrived();
+	void event(TrainAI &, const Message &);
 };
 
 } // namespace R2C2
diff --git a/source/libr2c2/timetable.cpp b/source/libr2c2/timetable.cpp
index 9fd44df..d5a0951 100644
--- a/source/libr2c2/timetable.cpp
+++ b/source/libr2c2/timetable.cpp
@@ -22,7 +22,7 @@ Timetable::Timetable(Train &t):
 	arrived(false)
 {
 	train.signal_advanced.connect(sigc::mem_fun(this, &Timetable::train_advanced));
-	train.signal_arrived.connect(sigc::mem_fun(this, &Timetable::train_arrived));
+	train.signal_ai_event.connect(sigc::mem_fun(this, &Timetable::event));
 	Layout &layout = train.get_layout();
 	layout.signal_block_state_changed.connect(sigc::mem_fun(this, &Timetable::block_state_changed));
 	layout.signal_block_reserved.connect(sigc::mem_fun(this, &Timetable::block_reserved));
@@ -88,13 +88,11 @@ void Timetable::tick(const Time::TimeStamp &t, const Time::TimeDelta &)
 		{
 		case GOTO_SENSOR:
 			arrived = false;
-			if(!train.go_to(get_sensor(row.get_param<unsigned>(0))))
-				set_enabled(false);
+			train.ai_message(Message("go-to-track", &get_sensor(row.get_param<unsigned>(0))));
 			break;
 		case GOTO_ZONE:
 			arrived = false;
-			if(!train.go_to(get_zone(row.get_param<string>(0))))
-				set_enabled(false);
+			train.ai_message(Message("go-to-zone", &get_zone(row.get_param<string>(0))));
 			break;
 		case TRAVEL_TO:
 			{
@@ -153,8 +151,7 @@ void Timetable::tick(const Time::TimeStamp &t, const Time::TimeDelta &)
 			train.ai_message(Message("toggle-reverse"));
 			break;
 		case ROUTE:
-			if(!train.set_route(&train.get_layout().get_route(row.get_param<string>(0))))
-				set_enabled(false);
+			train.ai_message(Message("set-route", &train.get_layout().get_route(row.get_param<string>(0))));
 			break;
 		}
 
@@ -232,19 +229,22 @@ void Timetable::train_advanced(Block &block)
 		pending_train = 0;
 }
 
-void Timetable::train_arrived()
+void Timetable::event(TrainAI &, const Message &ev)
 {
-	if(rows.empty() || !enabled)
-		return;
-
-	Row &row = rows[current_row];
-	if(row.type==ARRIVE)
+	if(ev.type=="arrived")
 	{
-		current_row = (current_row+1)%rows.size();
-		executing = true;
+		if(rows.empty() || !enabled)
+			return;
+
+		Row &row = rows[current_row];
+		if(row.type==ARRIVE)
+		{
+			current_row = (current_row+1)%rows.size();
+			executing = true;
+		}
+		else
+			arrived = true;
 	}
-	else
-		arrived = true;
 }
 
 
diff --git a/source/libr2c2/timetable.h b/source/libr2c2/timetable.h
index 9c99705..67565a2 100644
--- a/source/libr2c2/timetable.h
+++ b/source/libr2c2/timetable.h
@@ -104,7 +104,7 @@ private:
 	void block_state_changed(Block &, Block::State);
 	void block_reserved(Block &, Train *);
 	void train_advanced(Block &);
-	void train_arrived();
+	void event(TrainAI &, const Message &);
 };
 
 } // namespace R2C2
diff --git a/source/libr2c2/train.cpp b/source/libr2c2/train.cpp
index eb5724b..93bd427 100644
--- a/source/libr2c2/train.cpp
+++ b/source/libr2c2/train.cpp
@@ -15,6 +15,7 @@
 #include "trackiter.h"
 #include "tracktype.h"
 #include "train.h"
+#include "trainrouter.h"
 #include "vehicle.h"
 #include "vehicletype.h"
 #include "zone.h"
@@ -42,8 +43,6 @@ Train::Train(Layout &l, const VehicleType &t, unsigned a, const string &p):
 	loco_type(t),
 	address(a),
 	protocol(p),
-	priority(0),
-	yielding_to(0),
 	preceding_train(0),
 	cur_blocks_end(blocks.end()),
 	clear_blocks_end(blocks.end()),
@@ -56,7 +55,6 @@ Train::Train(Layout &l, const VehicleType &t, unsigned a, const string &p):
 	speed_changing(false),
 	reverse(false),
 	functions(0),
-	end_of_route(false),
 	travel_dist(0),
 	pure_speed(false),
 	speed_quantizer(0),
@@ -106,16 +104,6 @@ void Train::set_name(const string &n)
 	signal_name_changed.emit(name);
 }
 
-void Train::set_priority(int p)
-{
-	priority = p;
-}
-
-void Train::yield_to(const Train &t)
-{
-	yielding_to = &t;
-}
-
 void Train::add_vehicle(const VehicleType &vt)
 {
 	Vehicle *veh = new Vehicle(layout, vt);
@@ -234,97 +222,6 @@ void Train::ai_message(const TrainAI::Message &msg)
 		(*i)->message(msg);
 }
 
-bool Train::set_route(const Route *r)
-{
-	free_noncritical_blocks();
-
-	Route *lead = 0;
-	if(r && !blocks.empty())
-	{
-		TrackIter first = blocks.front().track_iter();
-		TrackIter next = blocks.back().next().track_iter();
-		if(!r->has_track(*next))
-		{
-			lead = Route::find(next, *r);
-			if(!lead)
-				return false;
-			create_lead_route(lead, lead);
-			routes.push_front(lead);
-		}
-		else if(!r->has_track(*first))
-			lead = create_lead_route(0, r);
-	}
-
-	routes.clear();
-	if(lead)
-		routes.push_back(lead);
-	if(r)
-		routes.push_back(r);
-	end_of_route = false;
-
-	reserve_more();
-
-	signal_route_changed.emit(get_route());
-
-	return true;
-}
-
-bool Train::go_to(Track &to)
-{
-	for(BlockList::const_iterator i=blocks.begin(); i!=cur_blocks_end; ++i)
-		if((*i)->has_track(to))
-		{
-			signal_arrived.emit();
-			return set_route(0);
-		}
-
-	free_noncritical_blocks();
-
-	TrackIter next = blocks.back().next().track_iter();
-
-	Route *route = Route::find(next, to);
-	if(!route)
-		return false;
-	create_lead_route(route, route);
-	return set_route(route);
-}
-
-bool Train::go_to(const Zone &to)
-{
-	set<Track *> tracks;
-	for(BlockList::const_iterator i=blocks.begin(); i!=blocks.end(); ++i)
-		tracks.insert((*i)->get_tracks().begin(), (*i)->get_tracks().end());
-
-	const Zone::TrackSet &ztracks = to.get_tracks();
-	unsigned union_size = 0;
-	for(Zone::TrackSet::const_iterator i=ztracks.begin(); i!=ztracks.end(); ++i)
-		union_size += tracks.count(*i);
-
-	if(union_size==tracks.size() || union_size==ztracks.size())
-	{
-		signal_arrived.emit();
-		return set_route(0);
-	}
-
-	free_noncritical_blocks();
-
-	TrackIter next = blocks.back().next().track_iter();
-
-	Route *route = Route::find(next, to);
-	if(!route)
-		return false;
-	create_lead_route(route, route);
-	route->add_tracks(ztracks);
-	return set_route(route);
-}
-
-const Route *Train::get_route() const
-{
-	if(routes.empty())
-		return 0;
-	return routes.front();
-}
-
 void Train::place(Block &block, unsigned entry)
 {
 	if(controller->get_speed())
@@ -368,6 +265,11 @@ void Train::unplace()
 		(*i)->unplace();
 }
 
+void Train::stop_at(Block *block)
+{
+	stop_at_block = block;
+}
+
 bool Train::free_block(Block &block)
 {
 	if(get_reserved_distance_until(&block, false)<controller->get_braking_distance()*1.3)
@@ -450,6 +352,20 @@ void Train::free_noncritical_blocks()
 	}
 }
 
+const BlockIter &Train::get_head_block() const
+{
+	if(blocks.empty())
+		throw logic_error("no blocks");
+	return blocks.back();
+}
+
+const BlockIter &Train::get_tail_block() const
+{
+	if(blocks.empty())
+		throw logic_error("no blocks");
+	return blocks.front();
+}
+
 int Train::get_entry_to_block(const Block &block) const
 {
 	for(BlockList::const_iterator i=blocks.begin(); i!=blocks.end(); ++i)
@@ -543,12 +459,6 @@ void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt)
 			}
 		}
 	}
-	else if(end_of_route && cur_blocks_end==blocks.end())
-	{
-		set_active(false);
-		signal_arrived.emit();
-		set_route(0);
-	}
 
 	if(!blocks.empty() && !blocks.front()->get_sensor_id())
 	{
@@ -567,8 +477,6 @@ void Train::save(list<DataFile::Statement> &st) const
 {
 	st.push_back((DataFile::Statement("name"), name));
 
-	st.push_back((DataFile::Statement("priority"), priority));
-
 	for(vector<Vehicle *>::const_iterator i=vehicles.begin(); i!=vehicles.end(); ++i)
 		if(i!=vehicles.begin())
 			st.push_back((DataFile::Statement("vehicle"), (*i)->get_type().get_article_number()));
@@ -593,22 +501,22 @@ void Train::save(list<DataFile::Statement> &st) const
 			st.push_back((DataFile::Statement("block"), (*i)->get_id()));
 	}
 
-	if(!routes.empty())
-	{
-		list<const Route *>::const_iterator i = routes.begin();
-		for(; (i!=routes.end() && (*i)->is_temporary()); ++i) ;
-		if(i!=routes.end())
-			st.push_back((DataFile::Statement("route"), (*i)->get_name()));
-	}
-
 	// XXX Need more generic way of saving AI state
 	for(list<TrainAI *>::const_iterator i=ais.begin(); i!=ais.end(); ++i)
-		if(Timetable *timetable = dynamic_cast<Timetable *>(*i))
+	{
+		if(TrainRouter *router = dynamic_cast<TrainRouter *>(*i))
+		{
+			DataFile::Statement ss("router");
+			router->save(ss.sub);
+			st.push_back(ss);
+		}
+		else if(Timetable *timetable = dynamic_cast<Timetable *>(*i))
 		{
 			DataFile::Statement ss("timetable");
 			timetable->save(ss.sub);
 			st.push_back(ss);
 		}
+	}
 }
 
 void Train::control_changed(const Controller::Control &ctrl)
@@ -697,20 +605,6 @@ void Train::block_state_changed(Block &block, Block::State state)
 			accurate_position = true;
 			overshoot_dist = 0;
 
-			// Check if we've reached the next route
-			if(routes.size()>1)
-			{
-				const Route &route = **(++routes.begin());
-				for(BlockList::iterator j=cur_blocks_end; j!=end; ++j)
-					if(route.has_track(*j->track_iter()))
-					{
-						routes.pop_front();
-						// XXX Exceptions?
-						signal_route_changed.emit(routes.front());
-						break;
-					}
-			}
-
 			// Move blocks up to the next sensor to our current blocks
 			for(BlockList::iterator j=cur_blocks_end; j!=end; ++j)
 				signal_advanced.emit(**j);
@@ -755,7 +649,12 @@ void Train::turnout_path_changed(Track &track)
 {
 	for(list<BlockIter>::iterator i=blocks.begin(); i!=blocks.end(); ++i)
 		if((*i)->get_turnout_id()==track.get_turnout_id() && !reserving)
-			check_turnout_paths(false);
+		{
+			if(&**i==pending_block)
+				reserve_more();
+			else
+				check_turnout_paths(false);
+		}
 }
 
 void Train::halt_event(bool h)
@@ -772,10 +671,12 @@ void Train::block_reserved(const Block &block, const Train *train)
 
 void Train::reserve_more()
 {
-	if(!active || blocks.empty() || end_of_route)
+	if(!active || blocks.empty())
 		return;
 
 	BlockIter start = blocks.back();
+	if(&*start==stop_at_block)
+		return;
 
 	pending_block = 0;
 	preceding_train = 0;
@@ -791,155 +692,51 @@ void Train::reserve_more()
 			dist += (*i)->get_path_length(i->entry());
 	}
 
-	list<const Route *>::iterator cur_route = routes.begin();
-	advance_route(cur_route, *start.track_iter());
-
 	float approach_margin = 50*layout.get_catalogue().get_scale();
 	float min_dist = controller->get_braking_distance()*1.3+approach_margin*2;
 
 	BlockIter block = start;
-	list<BlockIter>::iterator good_end = blocks.end();
-	Train *blocking_train = 0;
-	BlockList contested_blocks;
 
 	SetFlag setf(reserving);
 
 	while(1)
 	{
 		BlockIter last = block;
-		block = block.next(cur_route!=routes.end() ? *cur_route : 0);
+		block = block.next();
 		if(!block || block->get_endpoints().size()<2)
-		{
-			if(!blocking_train)
-			{
-				good_end = blocks.end();
-				end_of_route = true;
-			}
+			// The track ends here
 			break;
-		}
-
-		TrackIter track = block.track_iter();
-
-		if(cur_route!=routes.end())
-		{
-			if(!advance_route(cur_route, *track))
-			{
-				// Keep the blocks if we arrived at the end of the route
-				if(!blocking_train)
-				{
-					good_end = blocks.end();
-					end_of_route = true;
-				}
-				break;
-			}
-		}
 
 		if(block->get_turnout_id() && !last->get_turnout_id())
 		{
-			/* We can keep the blocks if we arrive at a turnout from a non-turnout
-			block.  Having a turnout block as our last reserved block is not good
-			as it would limit our diversion possibilities for little benefit. */
-			good_end = blocks.end();
+			/* We are arriving at a turnout.  See if we have enough blocks and
+			distance reserved. */
 			if(nsens>=3 && dist>=min_dist)
 				break;
 		}
 
-		if(blocking_train)
-		{
-			if(block->get_train()!=blocking_train)
-			{
-				if(blocking_train->free_block(*contested_blocks.back()))
-				{
-					// Roll back and start actually reserving the blocks
-					block = blocks.back();
-					cur_route = routes.begin();
-					advance_route(cur_route, *block.track_iter().track());
-					if(blocking_train->get_priority()==priority)
-						blocking_train->yield_to(*this);
-					blocking_train = 0;
-					continue;
-				}
-				else
-				{
-					yield_to(*blocking_train);
-					pending_block = contested_blocks.front().block();
-					break;
-				}
-			}
-			else
-			{
-				contested_blocks.push_back(block);
-				continue;
-			}
-		}
-
 		blocks.push_back(block);
-		bool reserved = block->reserve(this);
-		if(!reserved)
+		if(!block->reserve(this))
 		{
 			blocks.pop_back();
-			/* We've found another train.  If it wants to exit the block from the
-			same endpoint we're trying to enter from or the other way around,
-			treat it as coming towards us.  Otherwise treat it as going in the
-			same direction. */
-			Train *other_train = block->get_train();
-			int other_entry = other_train->get_entry_to_block(*block);
-			if(other_entry<0)
-				throw logic_error("block reservation inconsistency");
-
-			unsigned exit = block.reverse().entry();
-			unsigned other_exit = BlockIter(block.block(), other_entry).reverse().entry();
-			bool entry_conflict = (block.entry()==other_exit);
-			bool exit_conflict = (exit==static_cast<unsigned>(other_entry));
-			if(!entry_conflict && !last->get_turnout_id())
-			{
-				/* The other train is not coming to the blocks we're holding, so we
-				can keep them. */
-				good_end = blocks.end();
-
-				if(static_cast<unsigned>(other_entry)==block.entry())
-					preceding_train = other_train;
-			}
-
-			int other_prio = other_train->get_priority();
-
-			if(!entry_conflict && !exit_conflict && other_prio<priority)
-			{
-				/* Ask a lesser priority train going to the same direction to free
-				the block for us */
-				if(other_train->free_block(*block))
-				{
-					blocks.push_back(block);
-					if(!(reserved = block->reserve(this)))
-						blocks.pop_back();
-				}
-			}
-			else if(other_train!=yielding_to && (other_prio<priority || (other_prio==priority && entry_conflict)))
-			{
-				/* A lesser priority train is coming at us, we must ask it to free
-				enough blocks to get clear of it to avoid a potential deadlock */
-				blocking_train = other_train;
-				contested_blocks.clear();
-				contested_blocks.push_back(block);
-				continue;
-			}
-
-			if(!reserved)
-			{
-				pending_block = &*block;
-				break;
-			}
+			pending_block = &*block;
+			break;
 		}
 
-		if(!contested_blocks.empty() && contested_blocks.front()==block)
-			contested_blocks.pop_front();
-
 		if(cur_blocks_end==blocks.end())
 			--cur_blocks_end;
 		if(clear_blocks_end==blocks.end())
 			--clear_blocks_end;
-		if(good_end==blocks.end())
-			--good_end;
+
+		TrackIter track = block.track_iter();
+		if(track->is_path_changing())
+		{
+			pending_block = &*block;
+			break;
+		}
+
+		if(&*block==stop_at_block)
+			break;
 
 		if(block->get_sensor_id())
 			++nsens;
@@ -947,13 +744,6 @@ void Train::reserve_more()
 			dist += block->get_path_length(block.entry());
 	}
 
-	// Unreserve blocks that were not good
-	release_blocks(good_end, blocks.end());
-
-	if(blocks.back()!=start)
-		// We got some new blocks, so no longer need to yield
-		yielding_to = 0;
-
 	check_turnout_paths(true);
 
 	// Make any sensorless blocks at the beginning immediately current
@@ -986,12 +776,13 @@ void Train::check_turnout_paths(bool set)
 
 			if(path!=track->get_active_path())
 			{
-				if(set)
+				if(set && !track->is_path_changing())
+				{
 					track->set_active_path(path);
-
-				/* Check again, in case the driver was able to service the request
-				instantly */
-				if(!set || path!=track->get_active_path())
+					if(track->is_path_changing())
+						continue;
+				}
+				else
 					continue;
 			}
 		}
@@ -1076,9 +867,6 @@ void Train::release_blocks(BlockList::iterator begin, BlockList::iterator end)
 		Block &block = **begin;
 		blocks.erase(begin++);
 		block.reserve(0);
-
-		if(begin==blocks.end())
-			end_of_route = false;
 	}
 }
 
@@ -1089,39 +877,6 @@ void Train::reverse_blocks(BlockList &blks) const
 		*i = i->reverse();
 }
 
-bool Train::advance_route(list<const Route *>::iterator &iter, Track &track)
-{
-	while(iter!=routes.end() && !(*iter)->has_track(track))
-		++iter;
-	if(iter==routes.end())
-		return false;
-
-	return true;
-}
-
-Route *Train::create_lead_route(Route *lead, const Route *target)
-{
-	if(!lead)
-	{
-		lead = new Route(layout);
-		lead->set_name("Lead");
-		lead->set_temporary(true);
-	}
-
-	set<Track *> tracks;
-	for(BlockList::iterator i=blocks.begin(); i!=blocks.end(); ++i)
-	{
-		const set<Track *> &btracks = (*i)->get_tracks();
-		for(set<Track *>::const_iterator j=btracks.begin(); j!=btracks.end(); ++j)
-			if(!target || !target->has_track(**j))
-				tracks.insert(*j);
-	}
-
-	lead->add_tracks(tracks);
-
-	return lead;
-}
-
 
 Train::Loader::Loader(Train &t):
 	DataFile::ObjectLoader<Train>(t),
@@ -1131,9 +886,8 @@ Train::Loader::Loader(Train &t):
 	add("block",       &Loader::block);
 	add("block_hint",  &Loader::block_hint);
 	add("name",        &Loader::name);
-	add("priority",    &Train::priority);
 	add("quantized_speed",  &Loader::quantized_speed);
-	add("route",       &Loader::route);
+	add("router",      &Loader::router);
 	add("timetable",   &Loader::timetable);
 	add("vehicle",     &Loader::vehicle);
 }
@@ -1202,9 +956,10 @@ void Train::Loader::quantized_speed()
 		load_sub(*obj.speed_quantizer);
 }
 
-void Train::Loader::route(const string &n)
+void Train::Loader::router()
 {
-	obj.set_route(&obj.layout.get_route(n));
+	TrainRouter *rtr = new TrainRouter(obj);
+	load_sub(*rtr);
 }
 
 void Train::Loader::timetable()
diff --git a/source/libr2c2/train.h b/source/libr2c2/train.h
index 709140c..7c388ad 100644
--- a/source/libr2c2/train.h
+++ b/source/libr2c2/train.h
@@ -13,11 +13,9 @@
 namespace R2C2 {
 
 class ArticleNumber;
-class Route;
 class SpeedQuantizer;
 class Vehicle;
 class VehicleType;
-class Zone;
 
 class Train: public sigc::trackable
 {
@@ -36,7 +34,7 @@ public:
 		void block_hint(unsigned);
 		void name(const std::string &);
 		void quantized_speed();
-		void route(const std::string &);
+		void router();
 		void timetable();
 		void vehicle(ArticleNumber);
 	};
@@ -45,10 +43,7 @@ public:
 	sigc::signal<void, const std::string &, float> signal_control_changed;
 	sigc::signal<void, unsigned, bool> signal_function_changed;
 	sigc::signal<void, TrainAI &, const TrainAI::Message &> signal_ai_event;
-	sigc::signal<void, const Route *> signal_route_changed;
 	sigc::signal<void, Block &> signal_advanced;
-	sigc::signal<void> signal_arrived;
-	sigc::signal<void, const std::string &> signal_status_changed;
 
 private:
 	typedef std::list<BlockIter> BlockList;
@@ -58,14 +53,13 @@ private:
 	unsigned address;
 	std::string protocol;
 	std::string name;
-	int priority;
-	const Train *yielding_to;
 	const Train *preceding_train;
 	std::vector<Vehicle *> vehicles;
 	BlockList blocks;
 	BlockList::iterator cur_blocks_end;
 	BlockList::iterator clear_blocks_end;
 	Block *pending_block;
+	Block *stop_at_block;
 	bool reserving;
 	bool advancing;
 	Controller *controller;
@@ -76,8 +70,6 @@ private:
 	bool reverse;
 	Msp::Time::TimeStamp stop_timeout;
 	unsigned functions;
-	std::list<const Route *> routes;
-	bool end_of_route;
 
 	Msp::Time::TimeStamp last_entry_time;
 	float travel_dist;
@@ -96,9 +88,6 @@ public:
 	const std::string &get_protocol() const { return protocol; }
 	void set_name(const std::string &);
 	const std::string &get_name() const { return name; }
-	void set_priority(int);
-	void yield_to(const Train &);
-	int get_priority() const { return priority; }
 	const Train *get_preceding_train() const { return preceding_train; }
 	Controller &get_controller() const { return *controller; }
 
@@ -124,15 +113,14 @@ public:
 	TrainAI *get_tagged_ai(const std::string &) const;
 	void ai_message(const TrainAI::Message &);
 
-	bool set_route(const Route *);
-	bool go_to(Track &);
-	bool go_to(const Zone &);
-	const Route *get_route() const;
 	void place(Block &, unsigned);
 	void unplace();
 	bool is_placed() const { return !blocks.empty(); }
+	void stop_at(Block *);
 	bool free_block(Block &);
 	void free_noncritical_blocks();
+	const BlockIter &get_head_block() const;
+	const BlockIter &get_tail_block() const;
 	int get_entry_to_block(const Block &) const;
 	float get_reserved_distance() const;
 
@@ -147,14 +135,14 @@ private:
 	void turnout_path_changed(Track &);
 	void halt_event(bool);
 	void block_reserved(const Block &, const Train *);
+public:
 	void reserve_more();
+private:
 	void check_turnout_paths(bool);
 	float get_reserved_distance_until(const Block *, bool) const;
 	void release_blocks();
 	void release_blocks(BlockList::iterator, BlockList::iterator);
 	void reverse_blocks(BlockList &) const;
-	bool advance_route(std::list<const Route *>::iterator &, Track &);
-	Route *create_lead_route(Route *, const Route *);
 };
 
 } // namespace R2C2
diff --git a/source/libr2c2/trainrouter.cpp b/source/libr2c2/trainrouter.cpp
new file mode 100644
index 0000000..160c5b6
--- /dev/null
+++ b/source/libr2c2/trainrouter.cpp
@@ -0,0 +1,362 @@
+#include "layout.h"
+#include "route.h"
+#include "trackiter.h"
+#include "train.h"
+#include "trainrouter.h"
+#include "zone.h"
+
+using namespace std;
+using namespace Msp;
+
+namespace R2C2 {
+
+TrainRouter::TrainRouter(Train &t):
+	TrainAI(t),
+	priority(0),
+	arriving(false),
+	yielding_to(0)
+{
+	train.get_layout().signal_block_reserved.connect(sigc::mem_fun(this, &TrainRouter::block_reserved));
+	train.signal_advanced.connect(sigc::mem_fun(this, &TrainRouter::train_advanced));
+}
+
+void TrainRouter::set_priority(int p)
+{
+	priority = p;
+}
+
+void TrainRouter::yield_to(const Train &t)
+{
+	yielding_to = &t;
+}
+
+bool TrainRouter::set_route(const Route *r)
+{
+	train.free_noncritical_blocks();
+
+	Route *lead = 0;
+	if(r && train.is_placed())
+	{
+		TrackIter first = train.get_tail_block().track_iter();
+		TrackIter next = train.get_head_block().next().track_iter();
+		if(!r->has_track(*next))
+		{
+			lead = Route::find(next, *r);
+			if(!lead)
+				return false;
+			create_lead_route(lead, lead);
+		}
+		else if(!r->has_track(*first))
+			lead = create_lead_route(0, r);
+	}
+
+	routes.clear();
+	if(lead)
+		routes.push_back(lead);
+	if(r)
+		routes.push_back(r);
+	train.stop_at(0);
+	arriving = false;
+
+	train.reserve_more();
+
+	const Route *route = get_route();
+	signal_route_changed.emit(route);
+	signal_event.emit(Message("route-changed", route));
+
+	return true;
+}
+
+bool TrainRouter::go_to(Track &to)
+{
+	if(!train.get_speed())
+	{
+		for(BlockIter i=train.get_tail_block(); (i && i->get_train()==&train); i=i.next())
+			if(i->has_track(to))
+			{
+				signal_arrived.emit();
+				signal_event.emit(Message("arrived"));
+				return set_route(0);
+			}
+	}
+
+	train.free_noncritical_blocks();
+
+	TrackIter next = train.get_head_block().next().track_iter();
+
+	Route *route = Route::find(next, to);
+	if(!route)
+		return false;
+	create_lead_route(route, route);
+	return set_route(route);
+}
+
+bool TrainRouter::go_to(const Zone &to)
+{
+	set<Track *> tracks;
+	for(BlockIter i=train.get_tail_block(); (i && i->get_train()==&train); i=i.next())
+		tracks.insert(i->get_tracks().begin(), i->get_tracks().end());
+
+	const Zone::TrackSet &ztracks = to.get_tracks();
+	unsigned union_size = 0;
+	for(Zone::TrackSet::const_iterator i=ztracks.begin(); i!=ztracks.end(); ++i)
+		union_size += tracks.count(*i);
+
+	if(union_size==tracks.size() || union_size==ztracks.size())
+	{
+		signal_arrived.emit();
+		signal_event.emit(Message("arrived"));
+		return set_route(0);
+	}
+
+	train.free_noncritical_blocks();
+
+	TrackIter next = train.get_head_block().next().track_iter();
+
+	Route *route = Route::find(next, to);
+	if(!route)
+		return false;
+	create_lead_route(route, route);
+	route->add_tracks(ztracks);
+	return set_route(route);
+}
+
+const Route *TrainRouter::get_route() const
+{
+	if(routes.empty())
+		return 0;
+	return routes.front();
+}
+
+void TrainRouter::message(const Message &msg)
+{
+	if(msg.type=="set-route")
+	{
+		if(msg.value.check_type<Route *>())
+			set_route(msg.value.value<Route *>());
+		else
+			set_route(msg.value.value<const Route *>());
+	}
+	else if(msg.type=="clear-route")
+		set_route(0);
+	else if(msg.type=="go-to-track")
+		go_to(*msg.value.value<Track *>());
+	else if(msg.type=="go-to-zone")
+	{
+		if(msg.value.check_type<Zone *>())
+			go_to(*msg.value.value<Zone *>());
+		else
+			go_to(*msg.value.value<const Zone *>());
+	}
+}
+
+void TrainRouter::tick(const Time::TimeStamp &, const Time::TimeDelta &)
+{
+	if(arriving && !train.get_speed())
+	{
+		train.set_active(false);
+		signal_arrived.emit();
+		signal_event.emit(Message("arrived"));
+		set_route(0);
+	}
+}
+
+void TrainRouter::save(list<DataFile::Statement> &st) const
+{
+	if(!tag.empty())
+		st.push_back((DataFile::Statement("tag"), tag));
+
+	st.push_back((DataFile::Statement("priority"), priority));
+
+	if(!routes.empty())
+	{
+		RouteList::const_iterator i = routes.begin();
+		for(; (i!=routes.end() && (*i)->is_temporary()); ++i) ;
+		if(i!=routes.end())
+			st.push_back((DataFile::Statement("route"), (*i)->get_name()));
+	}
+}
+
+void TrainRouter::block_reserved(Block &block, Train *t)
+{
+	if(t!=&train)
+		return;
+
+	yielding_to = 0;
+
+	BlockIter b_iter(&block, t->get_entry_to_block(block));
+	BlockIter b_iter_next;
+
+	RouteList::iterator route = routes.begin();
+	if(advance_route(route, block))
+	{
+		// Check if the block is a turnout and set it to proper path
+		if(unsigned tid = block.get_turnout_id())
+		{
+			int path = (*route)->get_turnout(tid);
+			if(path>=0)
+				b_iter.track_iter()->set_active_path(path);
+		}
+
+		// Check if the next block is still part of the designated route
+		b_iter_next = b_iter.next(*route);
+
+		RouteList::iterator next_route = route;
+		if(!advance_route(next_route, *b_iter_next))
+		{
+			train.stop_at(&block);
+			return;
+		}
+
+	}
+	else
+		b_iter_next = b_iter.next();
+
+	// Check if there's another train and ask it to free the block if appropriate
+	if(b_iter_next)
+	{
+		if(Train *other_train = b_iter_next->get_train())
+		{
+			/* There's another train ahead of us.  If it wants to exit the block
+			from the same endpoint we're trying to enter from or the other way
+			around, treat it as coming towards us.  Otherwise treat it as going
+			in the same direction. */
+			int other_entry = other_train->get_entry_to_block(*b_iter_next);
+			if(other_entry<0)
+				throw logic_error("block reservation inconsistency");
+
+			unsigned exit = b_iter_next.reverse().entry();
+			unsigned other_exit = BlockIter(b_iter_next.block(), other_entry).reverse().entry();
+			bool entry_conflict = (b_iter_next.entry()==other_exit);
+			bool exit_conflict = (exit==static_cast<unsigned>(other_entry));
+			// TODO: speed matching with preceding train
+
+			// XXX Should invent a better way to get our counterpart from the other train
+			TrainRouter *other_router = dynamic_cast<TrainRouter *>(other_train->get_tagged_ai("router"));
+			int other_prio = (other_router ? other_router->get_priority() : 0);
+
+			if(!entry_conflict && !exit_conflict && other_prio<priority)
+			{
+				/* Ask a lesser priority train going to the same direction to free
+				the block for us */
+				other_train->free_block(*b_iter_next);
+			}
+			else if(other_train!=yielding_to && (other_prio<priority || (other_prio==priority && entry_conflict)))
+			{
+				/* A lesser priority train is coming at us, we must ask it to free
+				enough blocks to get clear of it to avoid a potential deadlock */
+				BlockIter last_contested;
+				RouteList::iterator j = route;
+				for(BlockIter i=b_iter_next; (i && i->get_train()==other_train);)
+				{
+					if(!advance_route(j, *i))
+						break;
+					last_contested = i;
+					i = i.next(*j);
+				}
+
+				if(last_contested)
+				{
+					if(other_train->free_block(*last_contested))
+						other_router->yield_to(train);
+					else
+						yield_to(*other_train);
+				}
+			}
+		}
+	}
+}
+
+void TrainRouter::train_advanced(Block &block)
+{
+	// Check if we've reached the next route
+	if(routes.size()>1)
+	{
+		unsigned entry = train.get_entry_to_block(block);
+		Track &track = *block.get_endpoint(entry).track;
+		const Route &route = **++routes.begin();
+		if(route.has_track(track))
+		{
+			routes.pop_front();
+			// XXX Exceptions?
+			signal_event.emit(Message("route-changed", get_route()));
+		}
+	}
+
+	BlockIter iter(&block, train.get_entry_to_block(block));
+	iter = iter.next();
+	if(iter && !is_on_route(*iter))
+		arriving = true;
+}
+
+const Route *TrainRouter::get_route_for_block(const Block &block) const
+{
+	const set<Track *> &tracks = block.get_tracks();
+	for(RouteList::const_iterator i=routes.begin(); i!=routes.end(); ++i)
+		for(set<Track *>::const_iterator j=tracks.begin(); j!=tracks.end(); ++j)
+			if((*i)->has_track(**j))
+				return *i;
+
+	return 0;
+}
+
+Route *TrainRouter::create_lead_route(Route *lead, const Route *target)
+{
+	if(!lead)
+	{
+		lead = new Route(train.get_layout());
+		lead->set_name("Lead");
+		lead->set_temporary(true);
+	}
+
+	set<Track *> tracks;
+	for(BlockIter i=train.get_tail_block(); (i && i->get_train()==&train); i=i.next())
+	{
+		const set<Track *> &btracks = i->get_tracks();
+		for(set<Track *>::const_iterator j=btracks.begin(); j!=btracks.end(); ++j)
+			if(!target || !target->has_track(**j))
+				tracks.insert(*j);
+	}
+
+	lead->add_tracks(tracks);
+
+	return lead;
+}
+
+bool TrainRouter::advance_route(RouteList::iterator &iter, const Block &block)
+{
+	const set<Track *> &tracks = block.get_tracks();
+	for(; iter!=routes.end(); ++iter)
+		for(set<Track *>::const_iterator j=tracks.begin(); j!=tracks.end(); ++j)
+			if((*iter)->has_track(**j))
+				return true;
+
+	return false;
+}
+
+bool TrainRouter::is_on_route(const Block &block)
+{
+	RouteList::iterator iter = routes.begin();
+	return advance_route(iter, block);
+}
+
+
+TrainRouter::Loader::Loader(TrainRouter &r):
+	DataFile::ObjectLoader<TrainRouter>(r)
+{
+	add("priority", &TrainRouter::priority);
+	add("route",    &Loader::route);
+	add("tag",      &Loader::tag);
+}
+
+void TrainRouter::Loader::route(const string &n)
+{
+	obj.set_route(&obj.train.get_layout().get_route(n));
+}
+
+void TrainRouter::Loader::tag(const string &t)
+{
+	obj.set_tag(t);
+}
+
+} // namespace R2C2
diff --git a/source/libr2c2/trainrouter.h b/source/libr2c2/trainrouter.h
new file mode 100644
index 0000000..6aae439
--- /dev/null
+++ b/source/libr2c2/trainrouter.h
@@ -0,0 +1,67 @@
+#ifndef LIBR2C2_TRAINROUTER_H_
+#define LIBR2C2_TRAINROUTER_H_
+
+#include <list>
+#include <msp/datafile/objectloader.h>
+#include "trainai.h"
+
+namespace R2C2 {
+
+class Block;
+class Route;
+class Track;
+class Zone;
+
+class TrainRouter: public TrainAI
+{
+public:
+	class Loader: public Msp::DataFile::ObjectLoader<TrainRouter>
+	{
+	public:
+		Loader(TrainRouter &);
+	private:
+		void route(const std::string &);
+		void tag(const std::string &);
+	};
+
+	sigc::signal<void, const Route *> signal_route_changed;
+	sigc::signal<void> signal_arrived;
+
+private:
+	typedef std::list<const Route *> RouteList;
+
+	int priority;
+	RouteList routes;
+	bool arriving;
+	const Train *yielding_to;
+
+public:
+	TrainRouter(Train &);
+
+	void set_priority(int);
+	int get_priority() const { return priority; }
+	void yield_to(const Train &);
+
+	bool set_route(const Route *);
+	bool go_to(Track &);
+	bool go_to(const Zone &);
+	const Route *get_route() const;
+
+	virtual void message(const Message &);
+	virtual void tick(const Msp::Time::TimeStamp &, const Msp::Time::TimeDelta &);
+
+	void save(std::list<Msp::DataFile::Statement> &) const;
+
+private:
+	void block_reserved(Block &, Train *);
+	void train_advanced(Block &);
+	const Route *get_route_for_block(const Block &) const;
+
+	Route *create_lead_route(Route *, const Route *);
+	bool advance_route(RouteList::iterator &, const Block &);
+	bool is_on_route(const Block &);
+};
+
+} // namespace R2C2
+
+#endif
diff --git a/source/network/server.cpp b/source/network/server.cpp
index d4e2049..73cee24 100644
--- a/source/network/server.cpp
+++ b/source/network/server.cpp
@@ -4,6 +4,8 @@
 #include "libr2c2/driver.h"
 #include "libr2c2/route.h"
 #include "libr2c2/train.h"
+#include "libr2c2/trainrouter.h"
+#include "libr2c2/trainstatus.h"
 #include "libr2c2/vehicletype.h"
 #include "server.h"
 
@@ -65,8 +67,7 @@ void Server::train_added(Train &train)
 {
 	train.signal_control_changed.connect(sigc::bind<0>(sigc::mem_fun(this, &Server::train_control_changed), sigc::ref(train)));
 	train.signal_function_changed.connect(sigc::bind<0>(sigc::mem_fun(this, &Server::train_function_changed), sigc::ref(train)));
-	train.signal_route_changed.connect(sigc::bind<0>(sigc::mem_fun(this, &Server::train_route_changed), sigc::ref(train)));
-	train.signal_status_changed.connect(sigc::bind<0>(sigc::mem_fun(this, &Server::train_status_changed), sigc::ref(train)));
+	train.signal_ai_event.connect(sigc::bind<0>(sigc::mem_fun(this, &Server::train_ai_event), sigc::ref(train)));
 
 	TrainInfoPacket pkt;
 	pkt.address = train.get_address();
@@ -96,13 +97,16 @@ void Server::train_function_changed(const Train &train, unsigned, bool)
 	send(pkt);
 }
 
-void Server::train_route_changed(const Train &train, const Route *route)
+void Server::train_ai_event(const Train &train, TrainAI &, const TrainAI::Message &ev)
 {
-	TrainRoutePacket pkt;
-	pkt.address = train.get_address();
-	if(route)
-		pkt.route = route->get_name();
-	send(pkt);
+	if(ev.type=="route-changed")
+	{
+		TrainRoutePacket pkt;
+		pkt.address = train.get_address();
+		if(const Route *route = ev.value.value<const Route *>())
+			pkt.route = route->get_name();
+		send(pkt);
+	}
 }
 
 void Server::train_status_changed(const Train &train, const string &status)
@@ -201,11 +205,11 @@ void Server::Connection::handshake_done()
 			pkt.status = status->get_status();
 			comm.send(pkt);
 		}
-		if(train.get_route())
+		if(TrainRouter *router = dynamic_cast<TrainRouter *>(train.get_tagged_ai("router")))
 		{
 			TrainRoutePacket pkt;
 			pkt.address = train.get_address();
-			pkt.route = train.get_route()->get_name();
+			pkt.route = router->get_route()->get_name();
 			comm.send(pkt);
 		}
 	}
@@ -259,11 +263,11 @@ void Server::Connection::receive(const TrainRoutePacket &pkt)
 	{
 		Train &train = server.layout.get_train(pkt.address);
 		if(pkt.route.empty())
-			train.set_route(0);
+			train.ai_message(TrainAI::Message("clear-route"));
 		else
 		{
 			Route &route = server.layout.get_route(pkt.route);
-			train.set_route(&route);
+			train.ai_message(TrainAI::Message("set-route", &route));
 		}
 	}
 	catch(const exception &e)
diff --git a/source/network/server.h b/source/network/server.h
index febd567..cfc8d76 100644
--- a/source/network/server.h
+++ b/source/network/server.h
@@ -6,7 +6,7 @@
 #include <msp/net/streamsocket.h>
 #include <msp/net/streamserversocket.h>
 #include "libr2c2/layout.h"
-#include "libr2c2/trainstatus.h"
+#include "libr2c2/trainai.h"
 #include "packets.h"
 #include "protocol.h"
 
@@ -54,7 +54,7 @@ private:
 	void train_added(Train &);
 	void train_control_changed(const Train &, const std::string &, float);
 	void train_function_changed(const Train &, unsigned, bool);
-	void train_route_changed(const Train &, const Route *);
+	void train_ai_event(const Train &, TrainAI &, const TrainAI::Message &);
 	void train_status_changed(const Train &, const std::string &);
 
 	template<typename P>