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

diff --git a/source/libmarklin/train.cpp b/source/libmarklin/train.cpp
index b87447f..0084f38 100644
--- a/source/libmarklin/train.cpp
+++ b/source/libmarklin/train.cpp
@@ -40,19 +40,22 @@ 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),
@@ -60,7 +63,7 @@ Train::Train(Layout &l, const VehicleType &t, unsigned a):
 	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)
 {
@@ -71,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));
 }
 
@@ -209,10 +216,10 @@ bool Train::set_route(const Route *r)
 	free_noncritical_blocks();
 
 	Route *lead = 0;
-	if(r && !cur_blocks.empty())
+	if(r && !blocks.empty())
 	{
-		TrackIter first = cur_blocks.front().track_iter();
-		TrackIter next = (rsv_blocks.empty() ? cur_blocks : rsv_blocks).back().next().track_iter();
+		TrackIter first = blocks.front().track_iter();
+		TrackIter next = blocks.back().next().track_iter();
 		if(!r->has_track(*next))
 		{
 			lead = Route::find(next, *r);
@@ -241,7 +248,7 @@ bool Train::set_route(const Route *r)
 
 bool Train::go_to(Track &to)
 {
-	for(BlockList::const_iterator i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
+	for(BlockList::const_iterator i=blocks.begin(); i!=cur_blocks_end; ++i)
 		if((*i)->has_track(to))
 		{
 			signal_arrived.emit();
@@ -250,7 +257,7 @@ bool Train::go_to(Track &to)
 
 	free_noncritical_blocks();
 
-	TrackIter next = (rsv_blocks.empty() ? cur_blocks : rsv_blocks).back().next().track_iter();
+	TrackIter next = blocks.back().next().track_iter();
 
 	Route *route = Route::find(next, to);
 	if(!route)
@@ -266,51 +273,48 @@ bool Train::divert(Track &from)
 	if(routes.empty())
 		return false;
 
-	int path = -1;
-	unsigned from_ep = 0;
+	unsigned path = 0;
+	unsigned entry = 0;
 	list<RouteRef>::iterator route = routes.begin();
-	BlockIter block = cur_blocks.back();
-	set<const Track *> visited;
 
 	// Follow our routes to find out where we're entering the turnout
-	while(1)
+	for(TrackLoopIter track = blocks.front().track_iter();;)
 	{
-		block = block.next(route->route);
-
-		const Block::Endpoint &entry_ep = block->get_endpoints()[block.entry()];
-
-		if(visited.count(entry_ep.track))
-			return false;
-		visited.insert(entry_ep.track);
-
-		if(!advance_route(route, *entry_ep.track))
+		if(!advance_route(route, *track))
 			return false;
 
-		if(entry_ep.track==&from)
+		if(&*track==&from)
 		{
-			if(block->get_train()==this && !free_block(*block))
+			Block &block = track->get_block();
+			if(block.get_train()==this && !free_block(block))
 				return false;
 
-			from_ep = entry_ep.track_ep;
-			path = route->route->get_turnout(from.get_turnout_id());
-			break;
-		}
-	}
+			int route_path = route->route->get_turnout(from.get_turnout_id());
 
-	// Check that more than one path is available
-	unsigned ep_paths = from.get_type().get_endpoints()[from_ep].paths;
-	if(!(ep_paths&(ep_paths-1)))
-		return false;
+			// 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!=path)
-		{
-			path = i;
+			// 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;
 		}
 
-	TrackIter track = TrackIter(&from, from_ep).next(path);
+		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;
 
@@ -325,7 +329,7 @@ bool Train::divert(Track &from)
 	diversion->add_track(from);
 	diversion->set_turnout(from.get_turnout_id(), path);
 
-	if(!is_valid_diversion(*diversion, from, from_ep))
+	if(!is_valid_diversion(*diversion, TrackIter(&from, entry)))
 		return false;
 
 	// Follow the diversion route until we get back to the original route
@@ -341,8 +345,7 @@ bool Train::divert(Track &from)
 		else if(!diversion->has_track(*track))
 			throw LogicError("Pathfinder returned a bad route");
 
-		unsigned tid = track->get_turnout_id();
-		track = track.next(tid ? diversion->get_turnout(tid) : 0);
+		track = track.next(diversion->get_path(*track));
 	}
 
 	if(route==end)
@@ -370,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;
@@ -382,7 +384,7 @@ void Train::place(Block &block, unsigned entry)
 		return;
 	}
 
-	cur_blocks.push_back(BlockIter(&block, entry));
+	blocks.push_back(BlockIter(&block, entry));
 	if(reverse)
 	{
 		TrackIter track = BlockIter(&block, entry).reverse().track_iter();
@@ -390,7 +392,7 @@ void Train::place(Block &block, unsigned entry)
 	}
 	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);
 	}
 }
@@ -400,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;
@@ -419,13 +420,13 @@ bool Train::free_block(Block &block)
 		return false;
 
 	unsigned nsens = 0;
-	for(BlockList::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(nsens<1)
 				return false;
-			release_blocks(rsv_blocks, i, rsv_blocks.end());
+			release_blocks(i, blocks.end());
 			return true;
 		}
 		else if((*i)->get_sensor_id())
@@ -437,12 +438,12 @@ bool Train::free_block(Block &block)
 
 void Train::free_noncritical_blocks()
 {
-	if(cur_blocks.empty() || rsv_blocks.empty())
+	if(blocks.empty())
 		return;
 
 	if(controller->get_speed()==0)
 	{
-		release_blocks(rsv_blocks);
+		release_blocks(cur_blocks_end, blocks.end());
 		return;
 	}
 
@@ -452,16 +453,13 @@ void Train::free_noncritical_blocks()
 	Vehicle &veh = *(reverse ? vehicles.back() : vehicles.front());
 
 	TrackIter track(veh.get_track(), veh.get_entry());
-	BlockList::iterator block = cur_blocks.begin();
+	BlockList::iterator block = blocks.begin();
 	bool in_rsv = false;
-	while(block!=rsv_blocks.end() && !(*block)->has_track(*track))
+	while(block!=blocks.end() && !(*block)->has_track(*track))
 	{
 		++block;
-		if(block==cur_blocks.end())
-		{
-			block = rsv_blocks.begin();
+		if(block==cur_blocks_end)
 			in_rsv = true;
-		}
 	}
 
 	float dist = veh.get_offset();
@@ -479,17 +477,14 @@ void Train::free_noncritical_blocks()
 		if(!(*block)->has_track(*track))
 		{
 			++block;
-			if(block==cur_blocks.end())
-			{
-				block = rsv_blocks.begin();
+			if(block==cur_blocks_end)
 				in_rsv = true;
-			}
-			if(block==rsv_blocks.end())
+			if(block==blocks.end())
 				return;
 
 			if(dist>min_dist && nsens>0)
 			{
-				release_blocks(rsv_blocks, block, rsv_blocks.end());
+				release_blocks(block, blocks.end());
 				return;
 			}
 
@@ -503,10 +498,7 @@ void Train::free_noncritical_blocks()
 
 int Train::get_entry_to_block(Block &block) const
 {
-	for(BlockList::const_iterator i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
-		if(i->block()==&block)
-			return i->entry();
-	for(BlockList::const_iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
+	for(BlockList::const_iterator i=blocks.begin(); i!=blocks.end(); ++i)
 		if(i->block()==&block)
 			return i->entry();
 	return -1;
@@ -521,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();
 	}
 
@@ -532,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");
@@ -566,10 +557,14 @@ void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt)
 		Track *track = vehicle.get_track();
 
 		bool ok = false;
-		for(BlockList::const_iterator i=cur_blocks.begin(); (!ok && i!=cur_blocks.end()); ++i)
+		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);
@@ -585,21 +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()->get_sensor_id())
+	if(!blocks.empty() && !blocks.front()->get_sensor_id())
 	{
-		float dist = get_reserved_distance_until(&*cur_blocks.front(), true);
+		float dist = get_reserved_distance_until(&*blocks.front(), true);
 
 		if(dist>10*layout.get_catalogue().get_scale())
 		{
-			cur_blocks.front()->reserve(0);
-			cur_blocks.pop_front();
+			blocks.front()->reserve(0);
+			blocks.pop_front();
 		}
 	}
 }
@@ -614,20 +609,20 @@ 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())
 	{
-		BlockList blocks = cur_blocks;
+		BlockList blks(blocks.begin(), BlockList::const_iterator(cur_blocks_end));
 		if(reverse)
-			reverse_blocks(blocks);
+			reverse_blocks(blks);
 
-		BlockIter prev = blocks.front().flip();
+		BlockIter prev = blks.front().flip();
 		st.push_back((DataFile::Statement("block_hint"), prev->get_id()));
 
-		for(BlockList::const_iterator i=blocks.begin(); i!=blocks.end(); ++i)
+		for(BlockList::const_iterator i=blks.begin(); i!=blks.end(); ++i)
 			st.push_back((DataFile::Statement("block"), (*i)->get_id()));
 	}
 
@@ -656,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;
 	}
@@ -682,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
-		BlockList::iterator i;
+		BlockList::iterator end;
 		unsigned result = 0;
-		for(i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
-			if((*i)->get_sensor_id())
+		for(end=cur_blocks_end; end!=blocks.end(); ++end)
+			if((*end)->get_sensor_id())
 			{
-				if((*i)->get_sensor_id()!=addr)
+				if((*end)->get_sensor_id()!=addr)
 				{
 					if(result==0)
 						result = 2;
@@ -700,23 +695,23 @@ 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;
-			for(BlockList::iterator j=rsv_blocks.begin(); j!=i; ++j)
+			for(BlockList::iterator j=cur_blocks_end; j!=end; ++j)
 			{
 				travel_dist += (*j)->get_path_length(j->entry());
 
@@ -740,9 +735,9 @@ void Train::sensor_event(unsigned addr, bool state)
 			// Check if we've reached the next route
 			if(routes.size()>1)
 			{
-				const set<Track *> &rtracks = (++routes.begin())->route->get_tracks();
-				for(BlockList::iterator j=rsv_blocks.begin(); j!=i; ++j)
-					if(rtracks.count((*j)->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()))
 					{
 						routes.pop_front();
 						// XXX Exceptions?
@@ -752,7 +747,7 @@ void Train::sensor_event(unsigned addr, bool state)
 			}
 
 			// 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)
@@ -766,8 +761,8 @@ 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
-		BlockList::iterator end = cur_blocks.begin();
-		for(BlockList::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)->has_track(*veh.get_track()))
 				break;
@@ -783,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 && (!pending_block->get_train() || pending_block->get_train()==this))
-	{
-		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)
@@ -817,34 +803,25 @@ 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;
 
-	BlockIter start;
-	if(!rsv_blocks.empty())
-		start = rsv_blocks.back();
-	else if(!cur_blocks.empty())
-		start = cur_blocks.back();
-	else
-		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(BlockList::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)->get_sensor_id())
 			++nsens;
 		if(nsens>0)
 			dist += (*i)->get_path_length(i->entry());
 	}
-	
-	if(end_of_route)
-		return nsens;
 
 	list<RouteRef>::iterator cur_route = routes.begin();
 	advance_route(cur_route, *start.track_iter());
@@ -853,22 +830,24 @@ unsigned Train::reserve_more()
 	float min_dist = controller->get_braking_distance()*1.3+approach_margin*2;
 
 	BlockIter block = start;
-	BlockIter good = start;
+	list<BlockIter>::iterator good_end = blocks.end();
 	Track *divert_track = 0;
 	bool try_divert = false;
-	unsigned good_sens = nsens;
-	float good_dist = dist;
 	Train *blocking_train = 0;
 	BlockList contested_blocks;
 
 	SetFlag setf(reserving);
 
-	while(good_sens<3 || good_dist<min_dist || !contested_blocks.empty())
+	while(1)
 	{
 		BlockIter last = block;
 		block = block.next(cur_route!=routes.end() ? cur_route->route : 0);
-		if(!block)
+		if(!block || block->get_endpoints().size()<2)
+		{
+			if(!blocking_train)
+				good_end = blocks.end();
 			break;
+		}
 
 		TrackIter track = block.track_iter();
 
@@ -879,26 +858,21 @@ unsigned Train::reserve_more()
 				// 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(!routes.empty() && routes.front().route->has_track(*track))
-			cur_route = routes.begin();
 
-		if(block->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)
@@ -908,7 +882,7 @@ unsigned Train::reserve_more()
 				if(blocking_train->free_block(*contested_blocks.back()))
 				{
 					// Roll back and start actually reserving the blocks
-					block = rsv_blocks.back();
+					block = blocks.back();
 					cur_route = routes.begin();
 					advance_route(cur_route, *block.track_iter().track());
 					if(blocking_train->get_priority()==priority)
@@ -948,13 +922,9 @@ unsigned Train::reserve_more()
 			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 = last;
-				good_sens = nsens;
-				good_dist = dist;
-			}
+				good_end = blocks.end();
 
 			int other_prio = other_train->get_priority();
 
@@ -987,45 +957,9 @@ unsigned Train::reserve_more()
 
 		if(block->get_turnout_id())
 		{
-			const TrackType::Endpoint &track_ep = track->get_type().get_endpoints()[track.entry()];
+			const TrackType::Endpoint &track_ep = track.endpoint();
 			bool multiple_paths = (track_ep.paths&(track_ep.paths-1));
 
-			if(multiple_paths || !last->get_turnout_id())
-			{
-				/* We can keep the blocks reserved so far if we are facing the
-				points or if there was no turnout immediately before this one.
-				With multiple successive turnouts (as is common in crossovers) it's
-				best to hold at one we can divert from. */
-				good = last;
-				good_sens = nsens;
-				good_dist = dist;
-			}
-
-			// Figure out what path we'd like to take on the turnout
-			int path = -1;
-			for(list<RouteRef>::iterator i=cur_route; (path<0 && i!=routes.end()); ++i)
-				path = i->route->get_turnout(block->get_turnout_id());
-			if(path<0)
-				path = track->get_active_path();
-			if(!(track_ep.paths&(1<<path)))
-			{
-				for(unsigned i=0; track_ep.paths>>i; ++i)
-					if(track_ep.paths&(1<<i))
-						path = i;
-			}
-
-			if(path!=static_cast<int>(track->get_active_path()))
-			{
-				// The turnout is set to wrong path - switch and wait for it
-				pending_block = &*block;
-				track->set_active_path(path);
-				if(pending_block)
-				{
-					block->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 */
@@ -1035,7 +969,15 @@ unsigned Train::reserve_more()
 		if(!contested_blocks.empty() && contested_blocks.front()==block)
 			contested_blocks.pop_front();
 
-		rsv_blocks.push_back(block);
+		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)
@@ -1043,52 +985,78 @@ unsigned Train::reserve_more()
 	}
 
 	// Unreserve blocks that were not good
-	while(!rsv_blocks.empty() && rsv_blocks.back()!=good)
-	{
-		rsv_blocks.back()->reserve(0);
-		rsv_blocks.pop_back();
-	}
+	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
-	BlockList::iterator i;
-	for(i=rsv_blocks.begin(); (i!=rsv_blocks.end() && !(*i)->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;
 
 	if(try_divert && divert(*divert_track))
-		return reserve_more();
+		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;
+			}
+		}
 
-	return good_sens;
+		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();
 
 	TrackIter track(veh.get_track(), veh.get_entry());
-	if(!track)
+	if(!track)  // XXX Probably unnecessary
 		return 0;
 
-	BlockList::const_iterator block = cur_blocks.begin();
-	while(block!=rsv_blocks.end() && !(*block)->has_track(*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==until_block)
+	if(block==clear_blocks_end || &**block==until_block)
 		return 0;
 
 	float result = veh.get_offset();
@@ -1108,16 +1076,14 @@ float Train::get_reserved_distance_until(const Block *until_block, bool back) co
 		{
 			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;
 			}
 
@@ -1133,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;
 
@@ -1141,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;
 
@@ -1161,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;
@@ -1180,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);
@@ -1196,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;
 }
@@ -1207,25 +1178,33 @@ void Train::set_status(const string &s)
 	signal_status_changed.emit(s);
 }
 
-void Train::release_blocks(BlockList &blocks)
+void Train::release_blocks()
 {
-	release_blocks(blocks, blocks.begin(), blocks.end());
+	release_blocks(blocks.begin(), blocks.end());
 }
 
-void Train::release_blocks(BlockList &blocks, BlockList::iterator begin, BlockList::iterator end)
+void Train::release_blocks(BlockList::iterator begin, BlockList::iterator end)
 {
 	while(begin!=end)
 	{
+		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);
+
+		if(begin==blocks.end())
+			end_of_route = false;
 	}
 }
 
-void Train::reverse_blocks(BlockList &blocks) const
+void Train::reverse_blocks(BlockList &blks) const
 {
-	blocks.reverse();
-	for(BlockList::iterator i=blocks.begin(); i!=blocks.end(); ++i)
+	blks.reverse();
+	for(BlockList::iterator i=blks.begin(); i!=blks.end(); ++i)
 		*i = i->reverse();
 }
 
@@ -1254,15 +1233,12 @@ Route *Train::create_lead_route(Route *lead, const Route *target)
 	}
 
 	set<Track *> tracks;
-	for(BlockList::iterator i=cur_blocks.begin(); i!=rsv_blocks.end(); )
+	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);
-
-		if(++i==cur_blocks.end())
-			i = rsv_blocks.begin();
 	}
 
 	lead->add_tracks(tracks);
@@ -1270,48 +1246,50 @@ Route *Train::create_lead_route(Route *lead, const Route *target)
 	return lead;
 }
 
-bool Train::is_valid_diversion(const Route &diversion, Track &from, unsigned from_ep)
+bool Train::is_valid_diversion(const Route &diversion, const TrackIter &from)
 {
 	float diversion_len = 0;
-	TrackIter track(&from, from_ep);
-	while(diversion.has_track(*track))
+	TrackLoopIter track1 = from;
+	while(diversion.has_track(*track1))
 	{
-		unsigned tid = track->get_turnout_id();
-		unsigned path = (tid ? diversion.get_turnout(tid) : 0);
-		diversion_len += track->get_type().get_path_length(path);
+		unsigned path = diversion.get_path(*track1);
+		diversion_len += track1->get_type().get_path_length(path);
 
-		track = track.next(path);
+		track1 = track1.next(path);
 
-		if(&*track==&from)
+		if(track1.looped())
 			return false;
 	}
 
 	list<RouteRef>::iterator route = routes.begin();
-	if(!advance_route(route, from))
+	if(!advance_route(route, *from))
 		return false;
 
-	set<Track *> visited;
 	float route_len = 0;
-	track = TrackIter(&from, from_ep);
+	TrackLoopIter track2 = from;
 	while(1)
 	{
-		unsigned tid = track->get_turnout_id();
-		unsigned path = (tid ? route->route->get_turnout(tid) : 0);
-		route_len += track->get_type().get_path_length(path);
+		unsigned path = route->route->get_path(*track2);
+		route_len += track2->get_type().get_path_length(path);
+
+		bool ok = (track2!=from && diversion.has_track(*track2));
 
-		if(&*track!=&from && diversion.has_track(*track))
+		track2 = track2.next(path);
+
+		if(ok)
 			break;
 
-		if(visited.count(&*track))
+		if(track2.looped())
 			return false;
-		visited.insert(&*track);
 
-		track = track.next(path);
-
-		if(!advance_route(route, *track))
+		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;
 }
 
@@ -1351,9 +1329,9 @@ Train::Loader::Loader(Train &t):
 
 void Train::Loader::finish()
 {
-	if(!obj.cur_blocks.empty())
+	if(!obj.blocks.empty())
 	{
-		TrackIter track = obj.cur_blocks.front().track_iter();
+		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);
 
@@ -1384,7 +1362,7 @@ void Train::Loader::block(unsigned id)
 		entry = 0;
 
 	blk->reserve(&obj);
-	obj.cur_blocks.push_back(BlockIter(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);
@@ -1411,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;
 }