X-Git-Url: http://git.tdb.fi/?a=blobdiff_plain;f=source%2Flibmarklin%2Ftrain.cpp;h=0084f38fab8d4764c79c15873134b9f4a31d3049;hb=975ea87cc7be179618b06291cb2506a2523cad1f;hp=f0a0bf32dcf502c4e6728ed79812a40b90ef5906;hpb=8c721bbf508ddcf1f1a375c8d3d99241219b36af;p=r2c2.git

diff --git a/source/libmarklin/train.cpp b/source/libmarklin/train.cpp
index f0a0bf3..0084f38 100644
--- a/source/libmarklin/train.cpp
+++ b/source/libmarklin/train.cpp
@@ -16,6 +16,7 @@ Distributed under the GPL
 #include "route.h"
 #include "simplecontroller.h"
 #include "timetable.h"
+#include "trackiter.h"
 #include "tracktype.h"
 #include "train.h"
 #include "vehicle.h"
@@ -39,29 +40,30 @@ struct SetFlag
 
 namespace Marklin {
 
-Train::Train(Layout &l, const VehicleType &t, unsigned a):
+Train::Train(Layout &l, const VehicleType &t, unsigned a, const string &p):
 	layout(l),
 	loco_type(t),
 	address(a),
+	protocol(p),
 	priority(0),
 	yielding_to(0),
+	cur_blocks_end(blocks.end()),
+	clear_blocks_end(blocks.end()),
 	pending_block(0),
 	reserving(false),
 	advancing(false),
 	controller(new AIControl(*this, new SimpleController)),
 	timetable(0),
 	active(false),
-	current_speed(0),
+	current_speed_step(0),
 	speed_changing(false),
 	reverse(false),
 	functions(0),
-	route(0),
-	next_route(0),
 	end_of_route(false),
 	status("Unplaced"),
 	travel_dist(0),
 	pure_speed(false),
-	real_speed(15),
+	real_speed(layout.get_driver().get_protocol_speed_steps(protocol)+1),
 	accurate_position(false),
 	overshoot_dist(false)
 {
@@ -72,16 +74,20 @@ Train::Train(Layout &l, const VehicleType &t, unsigned a):
 
 	layout.add_train(*this);
 
-	layout.get_driver().add_loco(address);
+	layout.get_driver().add_loco(address, protocol);
 	layout.get_driver().signal_loco_speed.connect(sigc::mem_fun(this, &Train::loco_speed_event));
 	layout.get_driver().signal_loco_function.connect(sigc::mem_fun(this, &Train::loco_func_event));
 
 	layout.signal_block_reserved.connect(sigc::mem_fun(this, &Train::block_reserved));
 	layout.get_driver().signal_sensor.connect(sigc::mem_fun(this, &Train::sensor_event));
-	layout.get_driver().signal_turnout.connect(sigc::mem_fun(this, &Train::turnout_event));
 
 	layout.get_driver().signal_halt.connect(sigc::mem_fun(this, &Train::halt_event));
 
+	const set<Track *> &tracks = layout.get_tracks();
+	for(set<Track *>::const_iterator i=tracks.begin(); i!=tracks.end(); ++i)
+		if((*i)->get_turnout_id())
+			(*i)->signal_path_changed.connect(sigc::hide(sigc::bind(sigc::mem_fun(this, &Train::turnout_path_changed), sigc::ref(**i))));
+
 	controller->signal_control_changed.connect(sigc::mem_fun(this, &Train::control_changed));
 }
 
@@ -205,63 +211,161 @@ void Train::set_timetable(Timetable *tt)
 	timetable = tt;
 }
 
-void Train::set_route(const Route *r)
+bool Train::set_route(const Route *r)
 {
-	if(!rsv_blocks.empty())
-	{
-		for(list<BlockRef>::iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
-			if(i->block->get_sensor_id())
-			{
-				release_blocks(rsv_blocks, ++i, rsv_blocks.end());
-				break;
-			}
-	}
+	free_noncritical_blocks();
 
-	route = r;
-	next_route = 0;
-	end_of_route = false;
-
-	if(route && !cur_blocks.empty())
+	Route *lead = 0;
+	if(r && !blocks.empty())
 	{
-		BlockRef &last = (rsv_blocks.empty() ? cur_blocks.back() : rsv_blocks.back());
-		BlockRef next = last.next();
-		const Block::Endpoint &ep = next.block->get_endpoints()[next.entry];
-		if(!route->get_tracks().count(ep.track))
+		TrackIter first = blocks.front().track_iter();
+		TrackIter next = blocks.back().next().track_iter();
+		if(!r->has_track(*next))
 		{
-			next_route = route;
-			route = Route::find(*ep.track, ep.track_ep, *next_route);
+			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(route);
+	signal_route_changed.emit(get_route());
+
+	return true;
 }
 
-void Train::go_to(const Track &to)
+bool Train::go_to(Track &to)
 {
-	for(list<BlockRef>::const_iterator i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
-		if(i->block->get_tracks().count(const_cast<Track *>(&to)))
+	for(BlockList::const_iterator i=blocks.begin(); i!=cur_blocks_end; ++i)
+		if((*i)->has_track(to))
 		{
 			signal_arrived.emit();
-			set_route(0);
-			return;
+			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::divert(Track &from)
+{
+	if(!from.get_turnout_id())
+		throw InvalidParameterValue("Can't divert from a non-turnout");
+	if(routes.empty())
+		return false;
+
+	unsigned path = 0;
+	unsigned entry = 0;
+	list<RouteRef>::iterator route = routes.begin();
+
+	// Follow our routes to find out where we're entering the turnout
+	for(TrackLoopIter track = blocks.front().track_iter();;)
+	{
+		if(!advance_route(route, *track))
+			return false;
+
+		if(&*track==&from)
+		{
+			Block &block = track->get_block();
+			if(block.get_train()==this && !free_block(block))
+				return false;
+
+			int route_path = route->route->get_turnout(from.get_turnout_id());
+
+			// Check that more than one path is available
+			unsigned ep_paths = track.endpoint().paths;
+			if(!(ep_paths&(ep_paths-1)))
+				return false;
+
+			// Choose some other path
+			for(int i=0; ep_paths>>i; ++i)
+				if((ep_paths&(1<<i)) && i!=route_path)
+				{
+					path = i;
+					break;
+				}
+
+			entry = track.entry();
+			break;
 		}
 
-	BlockRef *last = 0;
-	if(rsv_blocks.empty())
-		last = &cur_blocks.back();
+		track = track.next(route->route->get_path(*track));
+
+		if(!track || track.looped())
+			return false;
+	}
+
+	TrackIter track = TrackIter(&from, entry).next(path);
+	if(!track)
+		return false;
+
+	set<Track *> tracks;
+	for(list<RouteRef>::iterator i=routes.begin(); i!=routes.end(); ++i)
+		tracks.insert(i->route->get_tracks().begin(), i->route->get_tracks().end());
+	RefPtr<Route> diversion = Route::find(track, tracks);
+	if(!diversion)
+		return false;
+
+	diversion->set_name("Diversion");
+	diversion->add_track(from);
+	diversion->set_turnout(from.get_turnout_id(), path);
+
+	if(!is_valid_diversion(*diversion, TrackIter(&from, entry)))
+		return false;
+
+	// Follow the diversion route until we get back to the original route
+	list<RouteRef>::iterator end = routes.end();
+	while(1)
+	{
+		for(list<RouteRef>::iterator i=route; (end==routes.end() && i!=routes.end()); ++i)
+			if(i->route->has_track(*track))
+				end = i;
+
+		if(end!=routes.end())
+			break;
+		else if(!diversion->has_track(*track))
+			throw LogicError("Pathfinder returned a bad route");
+
+		track = track.next(diversion->get_path(*track));
+	}
+
+	if(route==end)
+		// We are rejoining the same route we diverted from, duplicate it
+		routes.insert(end, *route);
 	else
 	{
-		for(list<BlockRef>::iterator i=rsv_blocks.begin(); (i!=rsv_blocks.end() && !last); ++i)
-			if(i->block->get_sensor_id())
-				last = &*i;
+		++route;
+		routes.erase(route, end);
 	}
+	routes.insert(end, RouteRef(diversion.release(), from.get_turnout_id()));
 
-	BlockRef next = last->next();
-	const Block::Endpoint &ep = next.block->get_endpoints()[next.entry];
+	return true;
+}
 
-	set_route(Route::find(*ep.track, ep.track_ep, to));
+const Route *Train::get_route() const
+{
+	if(routes.empty())
+		return 0;
+	return routes.front().route;
 }
 
 void Train::place(Block &block, unsigned entry)
@@ -269,8 +373,7 @@ void Train::place(Block &block, unsigned entry)
 	if(controller->get_speed())
 		throw InvalidState("Must be stopped before placing");
 
-	release_blocks(rsv_blocks);
-	release_blocks(cur_blocks);
+	release_blocks();
 
 	set_active(false);
 	accurate_position = false;
@@ -281,18 +384,15 @@ void Train::place(Block &block, unsigned entry)
 		return;
 	}
 
-	cur_blocks.push_back(BlockRef(&block, entry));
+	blocks.push_back(BlockIter(&block, entry));
 	if(reverse)
 	{
-		unsigned exit = block.traverse(entry);
-		const Block::Endpoint &bep = block.get_endpoints()[exit];
-		Track *track = bep.track->get_link(bep.track_ep);
-		unsigned ep = track->get_endpoint_by_link(*bep.track);
-		vehicles.front()->place(*track, ep, 0, Vehicle::FRONT_BUFFER);
+		TrackIter track = BlockIter(&block, entry).reverse().track_iter();
+		vehicles.front()->place(*track, track.entry(), 0, Vehicle::FRONT_BUFFER);
 	}
 	else
 	{
-		const Block::Endpoint &bep = block.get_endpoints()[entry];
+		const Block::Endpoint &bep = block.get_endpoint(entry);
 		vehicles.back()->place(*bep.track, bep.track_ep, 0, Vehicle::BACK_BUFFER);
 	}
 }
@@ -302,8 +402,7 @@ void Train::unplace()
 	if(controller->get_speed())
 		throw InvalidState("Must be stopped before unplacing");
 
-	release_blocks(rsv_blocks);
-	release_blocks(cur_blocks);
+	release_blocks();
 
 	set_active(false);
 	accurate_position = false;
@@ -321,30 +420,87 @@ bool Train::free_block(Block &block)
 		return false;
 
 	unsigned nsens = 0;
-	for(list<BlockRef>::iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
+	for(BlockList::iterator i=cur_blocks_end; i!=blocks.end(); ++i)
 	{
-		if(i->block==&block)
+		if(i->block()==&block)
 		{
 			if(nsens<1)
 				return false;
-			release_blocks(rsv_blocks, i, rsv_blocks.end());
+			release_blocks(i, blocks.end());
 			return true;
 		}
-		else if(i->block->get_sensor_id())
+		else if((*i)->get_sensor_id())
 			++nsens;
 	}
 
 	return false;
 }
 
+void Train::free_noncritical_blocks()
+{
+	if(blocks.empty())
+		return;
+
+	if(controller->get_speed()==0)
+	{
+		release_blocks(cur_blocks_end, blocks.end());
+		return;
+	}
+
+	float margin = 10*layout.get_catalogue().get_scale();
+	float min_dist = controller->get_braking_distance()*1.3+margin;
+
+	Vehicle &veh = *(reverse ? vehicles.back() : vehicles.front());
+
+	TrackIter track(veh.get_track(), veh.get_entry());
+	BlockList::iterator block = blocks.begin();
+	bool in_rsv = false;
+	while(block!=blocks.end() && !(*block)->has_track(*track))
+	{
+		++block;
+		if(block==cur_blocks_end)
+			in_rsv = true;
+	}
+
+	float dist = veh.get_offset();
+	if(reverse)
+		track.reverse();
+	else
+		dist = track->get_type().get_path_length(track->get_active_path())-dist;
+	dist -= veh.get_type().get_length()/2;
+
+	bool nsens = 0;
+	while(1)
+	{
+		track = track.next();
+
+		if(!(*block)->has_track(*track))
+		{
+			++block;
+			if(block==cur_blocks_end)
+				in_rsv = true;
+			if(block==blocks.end())
+				return;
+
+			if(dist>min_dist && nsens>0)
+			{
+				release_blocks(block, blocks.end());
+				return;
+			}
+
+			if(in_rsv && (*block)->get_sensor_id())
+				++nsens;
+		}
+
+		dist += track->get_type().get_path_length(track->get_active_path());
+	}
+}
+
 int Train::get_entry_to_block(Block &block) const
 {
-	for(list<BlockRef>::const_iterator i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
-		if(i->block==&block)
-			return i->entry;
-	for(list<BlockRef>::const_iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
-		if(i->block==&block)
-			return i->entry;
+	for(BlockList::const_iterator i=blocks.begin(); i!=blocks.end(); ++i)
+		if(i->block()==&block)
+			return i->entry();
 	return -1;
 }
 
@@ -357,8 +513,7 @@ void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt)
 {
 	if(!active && stop_timeout && t>=stop_timeout)
 	{
-		release_blocks(rsv_blocks);
-		end_of_route = false;
+		release_blocks(cur_blocks_end, blocks.end());
 		stop_timeout = Time::TimeStamp();
 	}
 
@@ -368,26 +523,26 @@ void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt)
 		timetable->tick(t);
 	controller->tick(dt);
 	float speed = controller->get_speed();
-	unsigned speed_notch = find_speed(speed);
+	unsigned speed_step = find_speed_step(speed);
 
 	if(controller->get_reverse()!=reverse)
 	{
 		reverse = controller->get_reverse();
 		driver.set_loco_reverse(address, reverse);
 
-		release_blocks(rsv_blocks);
-		reverse_blocks(cur_blocks);
+		release_blocks(cur_blocks_end, blocks.end());
+		reverse_blocks(blocks);
 
 		reserve_more();
 	}
-	if(speed_notch!=current_speed && !speed_changing && !driver.is_halted() && driver.get_power())
+	if(speed_step!=current_speed_step && !speed_changing && !driver.is_halted() && driver.get_power())
 	{
 		speed_changing = true;
-		driver.set_loco_speed(address, speed_notch);
+		driver.set_loco_speed(address, speed_step);
 
 		pure_speed = false;
 
-		if(speed_notch)
+		if(speed_step)
 			set_status(format("Traveling %d kmh", get_travel_speed()));
 		else
 			set_status("Waiting");
@@ -402,10 +557,14 @@ void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt)
 		Track *track = vehicle.get_track();
 
 		bool ok = false;
-		for(list<BlockRef>::const_iterator i=cur_blocks.begin(); (!ok && i!=cur_blocks.end()); ++i)
-			ok = i->block->get_tracks().count(track);
+		for(BlockList::const_iterator i=blocks.begin(); (!ok && i!=cur_blocks_end); ++i)
+			ok = (*i)->has_track(*track);
 
-		float d = get_real_speed(current_speed)*(dt/Time::sec);
+		float d;
+		if(real_speed.size()>1)
+			d = get_real_speed(current_speed_step)*(dt/Time::sec);
+		else
+			d = speed*(dt/Time::sec);
 		if(ok)
 		{
 			SetFlag setf(advancing);
@@ -421,20 +580,21 @@ void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt)
 			}
 		}
 	}
-	else if(end_of_route && rsv_blocks.empty())
+	else if(end_of_route && cur_blocks_end==blocks.end())
 	{
+		set_active(false);
 		signal_arrived.emit();
 		set_route(0);
 	}
 
-	if(!cur_blocks.empty() && !cur_blocks.front().block->get_sensor_id())
+	if(!blocks.empty() && !blocks.front()->get_sensor_id())
 	{
-		float dist = get_reserved_distance_until(cur_blocks.front().block, true);
+		float dist = get_reserved_distance_until(&*blocks.front(), true);
 
 		if(dist>10*layout.get_catalogue().get_scale())
 		{
-			cur_blocks.front().block->reserve(0);
-			cur_blocks.erase(cur_blocks.begin());
+			blocks.front()->reserve(0);
+			blocks.pop_front();
 		}
 	}
 }
@@ -449,29 +609,29 @@ void Train::save(list<DataFile::Statement> &st) const
 		if(i!=vehicles.begin())
 			st.push_back((DataFile::Statement("vehicle"), (*i)->get_type().get_article_number()));
 
-	for(unsigned i=0; i<=14; ++i)
+	for(unsigned i=0; i<real_speed.size(); ++i)
 		if(real_speed[i].weight)
 			st.push_back((DataFile::Statement("real_speed"), i, real_speed[i].speed, real_speed[i].weight));
 
-	if(!cur_blocks.empty())
+	if(!blocks.empty() && cur_blocks_end!=blocks.begin())
 	{
-		list<BlockRef> blocks = cur_blocks;
+		BlockList blks(blocks.begin(), BlockList::const_iterator(cur_blocks_end));
 		if(reverse)
-			reverse_blocks(blocks);
+			reverse_blocks(blks);
 
-		Block *prev = blocks.front().block->get_endpoints()[blocks.front().entry].link;
+		BlockIter prev = blks.front().flip();
 		st.push_back((DataFile::Statement("block_hint"), prev->get_id()));
 
-		for(list<BlockRef>::const_iterator i=blocks.begin(); i!=blocks.end(); ++i)
-			st.push_back((DataFile::Statement("block"), i->block->get_id()));
+		for(BlockList::const_iterator i=blks.begin(); i!=blks.end(); ++i)
+			st.push_back((DataFile::Statement("block"), (*i)->get_id()));
 	}
 
-	if(route)
+	if(!routes.empty())
 	{
-		if(!route->is_temporary())
-			st.push_back((DataFile::Statement("route"), route->get_name()));
-		else if(next_route && !next_route->is_temporary())
-			st.push_back((DataFile::Statement("route"), next_route->get_name()));
+		list<RouteRef>::const_iterator i = routes.begin();
+		for(; (i!=routes.end() && i->route->is_temporary()); ++i) ;
+		if(i!=routes.end())
+			st.push_back((DataFile::Statement("route"), i->route->get_name()));
 	}
 
 	if(timetable)
@@ -491,7 +651,7 @@ void Train::loco_speed_event(unsigned addr, unsigned speed, bool)
 {
 	if(addr==address)
 	{
-		current_speed = speed;
+		current_speed_step = speed;
 		speed_changing = false;
 		pure_speed = false;
 	}
@@ -517,12 +677,12 @@ void Train::sensor_event(unsigned addr, bool state)
 	if(state)
 	{
 		// Find the first sensor block from our reserved blocks that isn't this sensor
-		list<BlockRef>::iterator i;
+		BlockList::iterator end;
 		unsigned result = 0;
-		for(i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
-			if(i->block->get_sensor_id())
+		for(end=cur_blocks_end; end!=blocks.end(); ++end)
+			if((*end)->get_sensor_id())
 			{
-				if(i->block->get_sensor_id()!=addr)
+				if((*end)->get_sensor_id()!=addr)
 				{
 					if(result==0)
 						result = 2;
@@ -535,39 +695,36 @@ void Train::sensor_event(unsigned addr, bool state)
 					result = 3;
 			}
 
-		if(result==1 && i!=rsv_blocks.begin())
+		if(result==1)
 		{
 			// Compute speed and update related state
 			float travel_time_secs = (Time::now()-last_entry_time)/Time::sec;
 
 			if(pure_speed)
 			{
-				if(current_speed)
+				if(current_speed_step>0)
 				{
-					RealSpeed &rs = real_speed[current_speed];
+					RealSpeed &rs = real_speed[current_speed_step];
 					rs.add(travel_dist/travel_time_secs, travel_time_secs);
 				}
 				set_status(format("Traveling %d kmh", get_travel_speed()));
 			}
 
 			travel_dist = 0;
-			float block_len;
-			for(list<BlockRef>::iterator j=rsv_blocks.begin(); j!=i; ++j)
+			for(BlockList::iterator j=cur_blocks_end; j!=end; ++j)
 			{
-				j->block->traverse(j->entry, &block_len);
-				travel_dist += block_len;
+				travel_dist += (*j)->get_path_length(j->entry());
 
-				if(j->block->get_sensor_id()==addr && !advancing)
+				if((*j)->get_sensor_id()==addr && !advancing)
 				{
-					const Block::Endpoint &bep = j->block->get_endpoints()[j->entry];
+					TrackIter track = j->track_iter();
 					if(reverse)
 					{
-						Track *track = bep.track->get_link(bep.track_ep);
-						unsigned ep = track->get_endpoint_by_link(*bep.track);
-						vehicles.back()->place(*track, ep, 0, Vehicle::BACK_AXLE);
+						track = track.flip();
+						vehicles.back()->place(*track, track.entry(), 0, Vehicle::BACK_AXLE);
 					}
 					else
-						vehicles.front()->place(*bep.track, bep.track_ep, 0, Vehicle::FRONT_AXLE);
+						vehicles.front()->place(*track, track.entry(), 0, Vehicle::FRONT_AXLE);
 				}
 			}
 			last_entry_time = Time::now();
@@ -576,22 +733,21 @@ void Train::sensor_event(unsigned addr, bool state)
 			overshoot_dist = 0;
 
 			// Check if we've reached the next route
-			if(next_route)
+			if(routes.size()>1)
 			{
-				const set<const Track *> &rtracks = next_route->get_tracks();
-				for(list<BlockRef>::iterator j=rsv_blocks.begin(); j!=i; ++j)
-					if(rtracks.count(j->block->get_endpoints()[j->entry].track))
+				const Route &route = *(++routes.begin())->route;
+				for(BlockList::iterator j=cur_blocks_end; j!=end; ++j)
+					if(route.has_track(*j->track_iter()))
 					{
-						route = next_route;
-						next_route = 0;
+						routes.pop_front();
 						// XXX Exceptions?
-						signal_route_changed.emit(route);
+						signal_route_changed.emit(routes.front().route);
 						break;
 					}
 			}
 
 			// Move blocks up to the next sensor to our current blocks
-			cur_blocks.splice(cur_blocks.end(), rsv_blocks, rsv_blocks.begin(), i);
+			cur_blocks_end = end;
 
 			// Try to get more blocks if we're moving
 			if(active)
@@ -605,14 +761,14 @@ void Train::sensor_event(unsigned addr, bool state)
 		const Vehicle &veh = *(reverse ? vehicles.front() : vehicles.back());
 
 		// Find the first sensor in our current blocks that's still active
-		list<BlockRef>::iterator end = cur_blocks.begin();
-		for(list<BlockRef>::iterator i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
+		BlockList::iterator end = blocks.begin();
+		for(BlockList::iterator i=blocks.begin(); i!=cur_blocks_end; ++i)
 		{
-			if(i->block->get_tracks().count(veh.get_track()))
+			if((*i)->has_track(*veh.get_track()))
 				break;
-			if(i->block->get_sensor_id())
+			if((*i)->get_sensor_id())
 			{
-				if(layout.get_driver().get_sensor(i->block->get_sensor_id()))
+				if(layout.get_driver().get_sensor((*i)->get_sensor_id()))
 					break;
 				else
 				{
@@ -622,26 +778,17 @@ void Train::sensor_event(unsigned addr, bool state)
 			}
 		}
 		
-		if(end!=cur_blocks.begin() && end!=cur_blocks.end())
+		if(end!=blocks.begin() && end!=cur_blocks_end)
 			// Free blocks up to the last inactive sensor
-			release_blocks(cur_blocks, cur_blocks.begin(), end);
+			release_blocks(blocks.begin(), end);
 	}
 }
 
-void Train::turnout_event(unsigned addr, bool)
+void Train::turnout_path_changed(Track &track)
 {
-	if(pending_block)
-	{
-		unsigned pending_addr = pending_block->get_turnout_id();
-		bool double_addr = (*pending_block->get_tracks().begin())->get_type().is_double_address();
-		if(addr==pending_addr || (double_addr && addr==pending_addr+1))
-		{
-			if(reserving)
-				pending_block = 0;
-			else
-				reserve_more();
-		}
-	}
+	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);
 }
 
 void Train::halt_event(bool h)
@@ -656,119 +803,88 @@ void Train::block_reserved(const Block &block, const Train *train)
 		reserve_more();
 }
 
-unsigned Train::reserve_more()
+void Train::reserve_more()
 {
-	if(!active)
-		return 0;
+	if(!active || blocks.empty() || end_of_route)
+		return;
 
-	BlockRef *start = 0;
-	if(!rsv_blocks.empty())
-		start = &rsv_blocks.back();
-	else if(!cur_blocks.empty())
-		start = &cur_blocks.back();
-	if(!start)
-		return 0;
+	BlockIter start = blocks.back();
 
 	pending_block = 0;
 
 	// See how many sensor blocks and how much track we already have
 	unsigned nsens = 0;
 	float dist = 0;
-	for(list<BlockRef>::const_iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
+	for(BlockList::const_iterator i=cur_blocks_end; i!=blocks.end(); ++i)
 	{
-		if(i->block->get_sensor_id())
+		if((*i)->get_sensor_id())
 			++nsens;
 		if(nsens>0)
-		{
-			float length = 0;
-			i->block->traverse(i->entry, &length);
-			dist += length;
-		}
+			dist += (*i)->get_path_length(i->entry());
 	}
-	
-	if(end_of_route)
-		return nsens;
 
-	const Route *cur_route = 0;
-	if(route)
-	{
-		const set<Track *> &tracks = start->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;
-		}
-	}
+	list<RouteRef>::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;
 
-	BlockRef *last = start;
-	BlockRef *good = start;
-	unsigned good_sens = nsens;
-	float good_dist = dist;
+	BlockIter block = start;
+	list<BlockIter>::iterator good_end = blocks.end();
+	Track *divert_track = 0;
+	bool try_divert = false;
 	Train *blocking_train = 0;
-	std::list<BlockRef> contested_blocks;
+	BlockList contested_blocks;
 
 	SetFlag setf(reserving);
 
-	while(good_sens<3 || good_dist<min_dist || !contested_blocks.empty())
+	while(1)
 	{
-		// Traverse to the next block
-		float length = 0;
-		unsigned exit = last->block->traverse(last->entry, cur_route, &length);
-		Block *link = last->block->get_link(exit);
-		if(!link)
+		BlockIter last = block;
+		block = block.next(cur_route!=routes.end() ? cur_route->route : 0);
+		if(!block || block->get_endpoints().size()<2)
+		{
+			if(!blocking_train)
+				good_end = blocks.end();
 			break;
+		}
 
-		int entry = link->get_endpoint_by_link(*last->block);
-		if(entry<0)
-			throw LogicError("Block links are inconsistent!");
-
-		const Block::Endpoint &entry_ep = link->get_endpoints()[entry];
+		TrackIter track = block.track_iter();
 
-		if(cur_route)
+		if(cur_route!=routes.end())
 		{
-			if(cur_route!=next_route && next_route && next_route->get_tracks().count(entry_ep.track))
-				cur_route = next_route;
-			else if(!cur_route->get_tracks().count(entry_ep.track))
+			if(!advance_route(cur_route, *track))
 			{
 				// Keep the blocks if we arrived at the end of the route
 				if(!blocking_train)
 				{
-					good = last;
-					good_sens = nsens;
-					good_dist = dist;
+					good_end = blocks.end();
 					end_of_route = true;
 				}
 				break;
 			}
 		}
-		else if(route && route->get_tracks().count(entry_ep.track))
-			cur_route = route;
 
-		if(link->get_endpoints().size()<2)
+		if(block->get_turnout_id() && !last->get_turnout_id())
 		{
-			if(!blocking_train)
-			{
-				good = last;
-				good_sens = nsens;
-				good_dist = dist;
-			}
-			break;
+			/* 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();
+			if(nsens>=3 && dist>=min_dist)
+				break;
 		}
 
 		if(blocking_train)
 		{
-			if(link->get_train()!=blocking_train)
+			if(block->get_train()!=blocking_train)
 			{
-				// XXX is it possible that this won't free all the blocks we want?
-				if(blocking_train->free_block(*contested_blocks.back().block))
+				if(blocking_train->free_block(*contested_blocks.back()))
 				{
 					// Roll back and start actually reserving the blocks
-					last = &rsv_blocks.back();
+					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;
@@ -776,197 +892,205 @@ unsigned Train::reserve_more()
 				}
 				else
 				{
-					pending_block = contested_blocks.front().block;
+					yield_to(*blocking_train);
+					pending_block = contested_blocks.front().block();
+					try_divert = divert_track;
 					break;
 				}
 			}
 			else
 			{
-				contested_blocks.push_back(BlockRef(link, entry));
-				last = &contested_blocks.back();
+				contested_blocks.push_back(block);
 				continue;
 			}
 		}
 
-		bool reserved = link->reserve(this);
+		bool reserved = block->reserve(this);
 		if(!reserved)
 		{
 			/* 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 = link->get_train();
-			int other_entry = other_train->get_entry_to_block(*link);
+			Train *other_train = block->get_train();
+			int other_entry = other_train->get_entry_to_block(*block);
 			if(other_entry<0)
 				throw LogicError("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();
+
 			int other_prio = other_train->get_priority();
 
-			bool entry_conflict = (static_cast<unsigned>(entry)==link->traverse(other_entry));
-			bool exit_conflict = (link->traverse(entry)==static_cast<unsigned>(other_entry));
-			if(!entry_conflict && !exit_conflict)
+			if(!entry_conflict && !exit_conflict && other_prio<priority)
 			{
-				/* Same direction, keep the blocks we got so far and wait for the
-				other train to pass */
-				good = last;
-				good_sens = nsens;
-				good_dist = dist;
-
-				// Ask a lesser priority train to free the block for us
-				if(other_train->get_priority()<priority)
-					if(other_train->free_block(*link))
-						reserved = link->reserve(this);
+				/* Ask a lesser priority train going to the same direction to free
+				the block for us */
+				if(other_train->free_block(*block))
+					reserved = block->reserve(this);
 			}
-			else if(other_prio<priority || (other_prio==priority && other_train!=yielding_to && entry_conflict))
+			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(BlockRef(link, entry));
-				last = &contested_blocks.back();
+				contested_blocks.push_back(block);
 				continue;
 			}
+			else if(divert_track && (entry_conflict || exit_conflict))
+				// We are blocked, but there's a diversion possibility
+				try_divert = true;
 
 			if(!reserved)
 			{
-				pending_block = link;
+				pending_block = &*block;
 				break;
 			}
 		}
 
-		if(link->get_turnout_id())
+		if(block->get_turnout_id())
 		{
-			const Endpoint &track_ep = entry_ep.track->get_type().get_endpoints()[entry_ep.track_ep];
-
-			// Keep the blocks reserved so far, as either us or the other train can diverge
-			good = last;
-			good_sens = nsens;
-			good_dist = dist;
-
-			// Figure out what path we'd like to take on the turnout
-			int path = -1;
-			if(cur_route)
-				path = cur_route->get_turnout(link->get_turnout_id());
-			if(path<0)
-				path = entry_ep.track->get_active_path();
-			if(!((track_ep.paths>>path)&1))
-			{
-				for(unsigned i=0; track_ep.paths>>i; ++i)
-					if((track_ep.paths>>i)&1)
-						path = i;
-			}
+			const TrackType::Endpoint &track_ep = track.endpoint();
+			bool multiple_paths = (track_ep.paths&(track_ep.paths-1));
 
-			if(path!=static_cast<int>(entry_ep.track->get_active_path()))
-			{
-				// The turnout is set to wrong path - switch and wait for it
-				pending_block = link;
-				entry_ep.track->set_active_path(path);
-				if(pending_block)
-				{
-					link->reserve(0);
-					break;
-				}
-			}
+			if(multiple_paths && cur_route!=routes.end() && cur_route->diversion!=block->get_turnout_id())
+				/* There's multiple paths to be taken and we are on a route - take
+				note of the diversion possibility */
+				divert_track = &*track;
 		}
 
-		if(!contested_blocks.empty() && contested_blocks.front().block==link)
+		if(!contested_blocks.empty() && contested_blocks.front()==block)
 			contested_blocks.pop_front();
 
-		rsv_blocks.push_back(BlockRef(link, entry));
-		last = &rsv_blocks.back();
-		if(last->block->get_sensor_id())
+		blocks.push_back(block);
+
+		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;
+
+		if(block->get_sensor_id())
 			++nsens;
 		if(nsens>0)
-			dist += length;
+			dist += block->get_path_length(block.entry());
 	}
 
 	// Unreserve blocks that were not good
-	while(!rsv_blocks.empty() && &rsv_blocks.back()!=good)
-	{
-		rsv_blocks.back().block->reserve(0);
-		rsv_blocks.erase(--rsv_blocks.end());
-	}
+	release_blocks(good_end, blocks.end());
 
-	if(!rsv_blocks.empty() && &rsv_blocks.back()!=start)
+	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
-	list<BlockRef>::iterator i;
-	for(i=rsv_blocks.begin(); (i!=rsv_blocks.end() && !i->block->get_sensor_id()); ++i) ;
-	if(i!=rsv_blocks.begin())
-		cur_blocks.splice(cur_blocks.end(), rsv_blocks, rsv_blocks.begin(), i);
+	while(cur_blocks_end!=clear_blocks_end && !(*cur_blocks_end)->get_sensor_id())
+		++cur_blocks_end;
 
-	return good_sens;
+	if(try_divert && divert(*divert_track))
+		reserve_more();
+}
+
+void Train::check_turnout_paths(bool set)
+{
+	if(clear_blocks_end==blocks.end())
+		return;
+
+	for(list<BlockIter>::iterator i=clear_blocks_end; i!=blocks.end(); ++i)
+	{
+		if((*i)->get_turnout_id())
+		{
+			TrackIter track = i->track_iter();
+			const TrackType::Endpoint &track_ep = track.endpoint();
+
+			unsigned path = 0;
+			list<BlockIter>::iterator j = i;
+			if(++j!=blocks.end())
+			{
+				TrackIter rev = j->track_iter().flip();
+				unsigned mask = rev.endpoint().paths&track_ep.paths;
+				for(path=0; mask>1; mask>>=1, ++path) ;
+			}
+			else
+				return;
+
+			if(path!=track->get_active_path())
+			{
+				if(set)
+					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())
+					continue;
+			}
+		}
+
+		if(i==clear_blocks_end)
+			++clear_blocks_end;
+	}
 }
 
 float Train::get_reserved_distance_until(const Block *until_block, bool back) const
 {
-	if(cur_blocks.empty())
+	if(blocks.empty())
 		return 0;
 
 	Vehicle &veh = *(reverse!=back ? vehicles.back() : vehicles.front());
 	const VehicleType &vtype = veh.get_type();
 
-	Track *track = veh.get_track();
-	if(!track)
+	TrackIter track(veh.get_track(), veh.get_entry());
+	if(!track)  // XXX Probably unnecessary
 		return 0;
 
-	list<BlockRef>::const_iterator block = cur_blocks.begin();
-	while(block!=rsv_blocks.end() && !block->block->get_tracks().count(track))
-	{
+	BlockList::const_iterator block = blocks.begin();
+	while(block!=clear_blocks_end && !(*block)->has_track(*track))
 		++block;
-		if(block==cur_blocks.end())
-		{
-			if(back)
-				return 0;
-			block = rsv_blocks.begin();
-		}
-	}
-	if(block==rsv_blocks.end() || block->block==until_block)
+	if(block==clear_blocks_end || &**block==until_block)
 		return 0;
 
-	unsigned entry = veh.get_entry();
-
 	float result = veh.get_offset();
 	if(reverse!=back)
-		entry = track->traverse(entry);
+		track = track.reverse();
 	else
 		result = track->get_type().get_path_length(track->get_active_path())-result;
 	result -= vtype.get_length()/2;
 
 	while(1)
 	{
-		if(track->get_type().get_endpoints().size()<2)
+		track = track.next();
+		if(!track)
 			break;
 
-		Track *next = track->get_link(track->traverse(entry));
-
-		if(!block->block->get_tracks().count(next))
+		if(!(*block)->has_track(*track))
 		{
 			if(back)
 			{
-				if(block==cur_blocks.begin())
+				if(block==blocks.begin())
 					break;
 				--block;
 			}
 			else
 			{
 				++block;
-				if(block==cur_blocks.end())
-					block = rsv_blocks.begin();
-				if(block==rsv_blocks.end())
+				if(block==clear_blocks_end)
 					break;
 			}
 
-			if(block->block==until_block)
+			if(&**block==until_block)
 				break;
 		}
 
-		entry = next->get_endpoint_by_link(*track);
-		track = next;
-
 		result += track->get_type().get_path_length(track->get_active_path());
 	}
 
@@ -975,6 +1099,8 @@ float Train::get_reserved_distance_until(const Block *until_block, bool back) co
 
 float Train::get_real_speed(unsigned i) const
 {
+	if(i==0)
+		return 0;
 	if(real_speed[i].weight)
 		return real_speed[i].speed;
 
@@ -983,7 +1109,7 @@ float Train::get_real_speed(unsigned i) const
 	for(low=i; low>0; --low)
 		if(real_speed[low].weight)
 			break;
-	for(high=i; high<14; ++high)
+	for(high=i; high<real_speed.size(); ++high)
 		if(real_speed[high].weight)
 			break;
 
@@ -1003,15 +1129,17 @@ float Train::get_real_speed(unsigned i) const
 		return 0;
 }
 
-unsigned Train::find_speed(float real) const
+unsigned Train::find_speed_step(float real) const
 {
-	if(real<=real_speed[0].speed)
+	if(real_speed.size()<=1)
+		return 0;
+	if(real<=real_speed[1].speed*0.5)
 		return 0;
 
 	unsigned low = 0;
 	unsigned high = 0;
 	unsigned last = 0;
-	for(unsigned i=0; (!high && i<=14); ++i)
+	for(unsigned i=0; (!high && i<real_speed.size()); ++i)
 		if(real_speed[i].weight)
 		{
 			last = i;
@@ -1022,14 +1150,15 @@ unsigned Train::find_speed(float real) const
 		}
 	if(!high)
 	{
+		unsigned limit = real_speed.size()/5;
 		if(!low)
 		{
 			if(real)
-				return 3;
+				return limit;
 			else
 				return 0;
 		}
-		return min(min(static_cast<unsigned>(low*real/real_speed[low].speed), 14U), last+3);
+		return min(min(static_cast<unsigned>(low*real/real_speed[low].speed), real_speed.size()-1), last+limit);
 	}
 
 	float f = (real-real_speed[low].speed)/(real_speed[high].speed-real_speed[low].speed);
@@ -1038,7 +1167,7 @@ unsigned Train::find_speed(float real) const
 
 float Train::get_travel_speed() const
 {
-	float speed = get_real_speed(current_speed);
+	float speed = get_real_speed(current_speed_step);
 	float scale = layout.get_catalogue().get_scale();
 	return static_cast<int>(round(speed/scale*3.6/5))*5;
 }
@@ -1049,47 +1178,127 @@ void Train::set_status(const string &s)
 	signal_status_changed.emit(s);
 }
 
-void Train::release_blocks(list<BlockRef> &blocks)
+void Train::release_blocks()
 {
-	release_blocks(blocks, blocks.begin(), blocks.end());
+	release_blocks(blocks.begin(), blocks.end());
 }
 
-void Train::release_blocks(list<BlockRef> &blocks, list<BlockRef>::iterator begin, list<BlockRef>::iterator end)
+void Train::release_blocks(BlockList::iterator begin, BlockList::iterator end)
 {
 	while(begin!=end)
 	{
-		Block *block = begin->block;
+		if(begin==cur_blocks_end)
+			cur_blocks_end = end;
+		if(begin==clear_blocks_end)
+			clear_blocks_end = end;
+
+		Block &block = **begin;
 		blocks.erase(begin++);
-		block->reserve(0);
+		block.reserve(0);
+
+		if(begin==blocks.end())
+			end_of_route = false;
 	}
 }
 
-void Train::reverse_blocks(list<BlockRef> &blocks) const
+void Train::reverse_blocks(BlockList &blks) const
 {
-	blocks.reverse();
-	for(list<BlockRef>::iterator i=blocks.begin(); i!=blocks.end(); ++i)
-		i->entry = i->block->traverse(i->entry);
+	blks.reverse();
+	for(BlockList::iterator i=blks.begin(); i!=blks.end(); ++i)
+		*i = i->reverse();
 }
 
+bool Train::advance_route(list<RouteRef>::iterator &iter, Track &track)
+{
+	while(iter!=routes.end() && !iter->route->has_track(track))
+		++iter;
+	if(iter==routes.end())
+		return false;
 
-Train::BlockRef::BlockRef(Block *b, unsigned e):
-	block(b),
-	entry(e)
-{ }
+	list<RouteRef>::iterator next = iter;
+	++next;
+	if(next!=routes.end() && next->diversion && next->route->has_track(track))
+		iter = next;
+
+	return true;
+}
 
-Train::BlockRef Train::BlockRef::next() const
+Route *Train::create_lead_route(Route *lead, const Route *target)
 {
-	Block *blk = block->get_endpoints()[block->traverse(entry)].link;
-	if(!blk)
-		throw InvalidState("At end of line");
+	if(!lead)
+	{
+		lead = new Route(layout);
+		lead->set_name("Lead");
+		lead->set_temporary(true);
+	}
 
-	int ep = blk->get_endpoint_by_link(*block);
-	if(ep<0)
-		throw LogicError("Block links are inconsistent");
+	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 BlockRef(blk, ep);
+	return lead;
 }
 
+bool Train::is_valid_diversion(const Route &diversion, const TrackIter &from)
+{
+	float diversion_len = 0;
+	TrackLoopIter track1 = from;
+	while(diversion.has_track(*track1))
+	{
+		unsigned path = diversion.get_path(*track1);
+		diversion_len += track1->get_type().get_path_length(path);
+
+		track1 = track1.next(path);
+
+		if(track1.looped())
+			return false;
+	}
+
+	list<RouteRef>::iterator route = routes.begin();
+	if(!advance_route(route, *from))
+		return false;
+
+	float route_len = 0;
+	TrackLoopIter track2 = from;
+	while(1)
+	{
+		unsigned path = route->route->get_path(*track2);
+		route_len += track2->get_type().get_path_length(path);
+
+		bool ok = (track2!=from && diversion.has_track(*track2));
+
+		track2 = track2.next(path);
+
+		if(ok)
+			break;
+
+		if(track2.looped())
+			return false;
+
+		if(!advance_route(route, *track2))
+			return false;
+	}
+
+	// Must end up at the same place through both routes
+	if(track2!=track1)
+		return false;
+
+	return diversion_len<route_len*1.2;
+}
+
+
+Train::RouteRef::RouteRef(const Route *r, unsigned d):
+	route(r),
+	diversion(d)
+{ }
+
 
 Train::RealSpeed::RealSpeed():
 	speed(0),
@@ -1120,11 +1329,11 @@ Train::Loader::Loader(Train &t):
 
 void Train::Loader::finish()
 {
-	if(!obj.cur_blocks.empty())
+	if(!obj.blocks.empty())
 	{
-		const BlockRef &blkref = obj.cur_blocks.front();
-		const Block::Endpoint &bep = blkref.block->get_endpoints()[blkref.entry];
-		obj.vehicles.back()->place(*bep.track, bep.track_ep, 0, Vehicle::BACK_BUFFER);
+		TrackIter track = obj.blocks.front().track_iter();
+		float offset = 2*obj.layout.get_catalogue().get_scale();
+		obj.vehicles.back()->place(*track, track.entry(), offset, Vehicle::BACK_BUFFER);
 
 		obj.set_status("Stopped");
 	}
@@ -1153,7 +1362,7 @@ void Train::Loader::block(unsigned id)
 		entry = 0;
 
 	blk->reserve(&obj);
-	obj.cur_blocks.push_back(BlockRef(blk, entry));
+	obj.blocks.push_back(BlockIter(blk, entry));
 
 	if(blk->get_sensor_id())
 		obj.layout.get_driver().set_sensor(blk->get_sensor_id(), true);
@@ -1180,6 +1389,8 @@ void Train::Loader::name(const string &n)
 
 void Train::Loader::real_speed(unsigned i, float speed, float weight)
 {
+	if(i>=obj.real_speed.size())
+		return;
 	obj.real_speed[i].speed = speed;
 	obj.real_speed[i].weight = weight;
 }
@@ -1198,9 +1409,9 @@ void Train::Loader::timetable()
 	load_sub(*obj.timetable);
 }
 
-void Train::Loader::vehicle(unsigned n)
+void Train::Loader::vehicle(ArticleNumber art_nr)
 {
-	const VehicleType &vtype = obj.layout.get_catalogue().get_vehicle(n);
+	const VehicleType &vtype = obj.layout.get_catalogue().get_vehicle(art_nr);
 	Vehicle *veh = new Vehicle(obj.layout, vtype);
 	obj.vehicles.back()->attach_back(*veh);
 	obj.vehicles.push_back(veh);