X-Git-Url: http://git.tdb.fi/?a=blobdiff_plain;f=source%2Flibr2c2%2Fblockallocator.cpp;h=7cca79f795a61c9cad6ad11eb2689639f4574098;hb=4b9ed4362779040bfde6d5cd400a1fbffc894815;hp=c9cc5b44440fe6987dff33fb0939cfbf4ca79363;hpb=54392d65e2053d1eacb4cfcc435f1013993f2973;p=r2c2.git

diff --git a/source/libr2c2/blockallocator.cpp b/source/libr2c2/blockallocator.cpp
index c9cc5b4..7cca79f 100644
--- a/source/libr2c2/blockallocator.cpp
+++ b/source/libr2c2/blockallocator.cpp
@@ -1,9 +1,12 @@
 #include <msp/core/maputils.h>
 #include <msp/core/raii.h>
+#include "beamgate.h"
 #include "blockallocator.h"
+#include "block.h"
 #include "catalogue.h"
 #include "driver.h"
 #include "layout.h"
+#include "trackcircuit.h"
 #include "trackiter.h"
 #include "train.h"
 #include "vehicle.h"
@@ -13,41 +16,119 @@ using namespace Msp;
 
 namespace R2C2 {
 
+struct BlockAllocator::BlockMatch
+{
+	const Block &block;
+
+	BlockMatch(const Block &b): block(b) { }
+
+	bool operator()(const BlockIter &bi) const { return &*bi==&block; }
+};
+
+
 BlockAllocator::BlockAllocator(Train &t):
 	train(t),
+	active(false),
 	cur_blocks_end(blocks.end()),
+	next_sensor(0),
 	pending_block(0),
 	stop_at_block(0),
-	reserving(false)
+	reserving(false),
+	advancing(false)
 {
 	Layout &layout = train.get_layout();
 	layout.signal_block_reserved.connect(sigc::mem_fun(this, &BlockAllocator::block_reserved));
-	layout.signal_block_state_changed.connect(sigc::mem_fun(this, &BlockAllocator::block_state_changed));
+	layout.signal_sensor_state_changed.connect(sigc::mem_fun(this, &BlockAllocator::sensor_state_changed));
+	layout.get_driver().signal_halt.connect(sigc::mem_fun(this, &BlockAllocator::halt_event));
 
-	const set<Track *> &tracks = layout.get_tracks();
+	const set<Track *> &tracks = layout.get_all<Track>();
 	for(set<Track *>::const_iterator i=tracks.begin(); i!=tracks.end(); ++i)
-		if((*i)->get_turnout_id())
+		if((*i)->get_type().is_turnout())
+		{
+			(*i)->signal_path_changing.connect(sigc::hide(sigc::bind(sigc::mem_fun(this, &BlockAllocator::turnout_path_changing), sigc::ref(**i))));
 			(*i)->signal_path_changed.connect(sigc::hide(sigc::bind(sigc::mem_fun(this, &BlockAllocator::turnout_path_changed), sigc::ref(**i))));
+		}
+}
+
+void BlockAllocator::set_active(bool a)
+{
+	active = a;
+	if(active)
+		reserve_more();
+	else
+	{
+		release_blocks_end(cur_blocks_end);
+		pending_block = 0;
+	}
 }
 
-void BlockAllocator::start_from(const BlockIter &block)
+bool BlockAllocator::start_from(const BlockIter &block)
 {
 	if(!block)
 		throw invalid_argument("BlockAllocator::start_from");
 
-	release_blocks(blocks.begin(), blocks.end());
+	float remaining_length = 0;
+	unsigned n_vehs = train.get_n_vehicles();
+	for(unsigned i=0; i<n_vehs; ++i)
+		remaining_length += train.get_vehicle(i).get_type().get_length();
 
-	blocks.push_back(block);
-	if(!block->reserve(&train))
+	clear();
+
+	BlockList blocks_to_reserve;
+	for(BlockIter b=block; b; b=b.next())
 	{
-		blocks.pop_back();
+		if(b->get_train())
+			break;
+		blocks_to_reserve.push_back(b);
+		for(TrackIter t=b.track_iter(); (t && &t->get_block()==&*b); t=t.next())
+			remaining_length -= t->get_path_length();
+		if(remaining_length<=0)
+			break;
+	}
+
+	if(remaining_length>0)
+		return false;
+
+	for(BlockList::iterator i=blocks_to_reserve.begin(); i!=blocks_to_reserve.end(); ++i)
+	{
+		blocks.push_back(*i);
+		try
+		{
+			(*i)->reserve(&train);
+		}
+		catch(...)
+		{
+			blocks.pop_back();
+			while(i!=blocks_to_reserve.begin())
+			{
+				blocks.pop_back();
+				(*--i)->reserve(0);
+			}
+			throw;
+		}
+	}
+
+	return true;
+}
+
+void BlockAllocator::rewind_to(const Block &block)
+{
+	if(!active)
 		return;
+
+	BlockList::iterator i = find_if(cur_blocks_end, blocks.end(), BlockMatch(block));
+	if(i!=blocks.end())
+	{
+		release_blocks_end(i);
+		reserve_more();
 	}
 }
 
 void BlockAllocator::clear()
 {
-	release_blocks(blocks.begin(), blocks.end());
+	release_blocks_begin(blocks.end());
+	active = false;
+	next_sensor = 0;
 	pending_block = 0;
 	stop_at_block = 0;
 }
@@ -55,6 +136,8 @@ void BlockAllocator::clear()
 void BlockAllocator::stop_at(const Block *block)
 {
 	stop_at_block = block;
+	if(active && !block)
+		reserve_more();
 }
 
 const BlockIter &BlockAllocator::first() const
@@ -84,12 +167,23 @@ const BlockIter &BlockAllocator::last_current() const
 	return *--i;
 }
 
-int BlockAllocator::get_entry_to_block(const Block &block) const
+const BlockIter &BlockAllocator::iter_for(const Block &block) const
+{
+	BlockList::const_iterator i = find_if(blocks.begin(), blocks.end(), BlockMatch(block));
+	if(i==blocks.end())
+		throw key_error(&block);
+	return *i;
+}
+
+bool BlockAllocator::has_block(const Block &block) const
+{
+	return find_if(blocks.begin(), blocks.end(), BlockMatch(block))!=blocks.end();
+}
+
+bool BlockAllocator::is_block_current(const Block &block) const
 {
-	for(BlockList::const_iterator i=blocks.begin(); i!=blocks.end(); ++i)
-		if(i->block()==&block)
-			return i->entry();
-	return -1;
+	BlockList::const_iterator end = cur_blocks_end;
+	return find_if(blocks.begin(), end, BlockMatch(block))!=cur_blocks_end;
 }
 
 void BlockAllocator::reserve_more()
@@ -97,13 +191,16 @@ void BlockAllocator::reserve_more()
 	if(blocks.empty())
 		throw logic_error("no blocks");
 
+	if(train.get_layout().get_driver().is_halted())
+		return;
+
 	BlockIter start = blocks.back();
 	if(&*start==stop_at_block)
 		return;
 	else if(&*start==pending_block)
 	{
 		TrackIter track = start.track_iter();
-		if(!track.endpoint().has_path(track->get_active_path()))
+		if(track->is_path_changing() || !track.endpoint().has_path(track->get_active_path()))
 			return;
 	}
 
@@ -114,7 +211,7 @@ void BlockAllocator::reserve_more()
 	float dist = 0;
 	for(BlockList::const_iterator i=cur_blocks_end; i!=blocks.end(); ++i)
 	{
-		if((*i)->get_sensor_id())
+		if((*i)->get_sensor_address())
 			++nsens;
 		if(nsens>0)
 			dist += (*i)->get_path_length(i->entry());
@@ -135,7 +232,7 @@ void BlockAllocator::reserve_more()
 			// The track ends here
 			break;
 
-		if(block->get_turnout_id() && !prev->get_turnout_id())
+		if(block->get_turnout_address() && !prev->get_turnout_address())
 		{
 			/* We are arriving at a turnout.  See if we have enough blocks and
 			distance reserved. */
@@ -143,10 +240,8 @@ void BlockAllocator::reserve_more()
 				break;
 		}
 
-		blocks.push_back(block);
-		if(!block->reserve(&train))
+		if(!reserve_block(block))
 		{
-			blocks.pop_back();
 			pending_block = &*block;
 			break;
 		}
@@ -162,103 +257,233 @@ void BlockAllocator::reserve_more()
 		}
 		else
 		{
-			const TrackType::Endpoint &entry_ep = track.endpoint();
 			unsigned path = track->get_active_path();
-			if(!entry_ep.has_path(path))
+			if(!track.endpoint().has_path(path))
 			{
-				const TrackType::Endpoint &exit_ep = track.reverse().endpoint();
-				if(entry_ep.has_common_paths(exit_ep))
+				path = track->get_type().coerce_path(track.entry(), path);
+				track->set_active_path(path);
+				if(track->is_path_changing())
 				{
-					unsigned mask = entry_ep.paths&exit_ep.paths;
-					for(path=0; mask>1; ++path, mask>>=1) ;
-
-					track->set_active_path(path);
-					if(track->is_path_changing())
-					{
-						pending_block = &*block;
-						break;
-					}
-				}
-				else
-					// XXX Do something here
+					pending_block = &*block;
 					break;
+				}
 			}
 		}
 
 		if(&*block==stop_at_block)
 			break;
 
-		if(block->get_sensor_id())
+		if(block->get_sensor_address())
 			++nsens;
 		if(nsens>0)
 			dist += block->get_path_length(block.entry());
 	}
 
-	// Make any sensorless blocks at the beginning immediately current
-	while(cur_blocks_end!=blocks.end() && !(*cur_blocks_end)->get_sensor_id())
-		++cur_blocks_end;
+	if(!next_sensor)
+	{
+		update_next_sensor(0);
+		// Immediately advance to just before the next sensor
+		advance_front(next_sensor);
+	}
 }
 
-void BlockAllocator::release_until(const Block &block)
+bool BlockAllocator::reserve_block(const BlockIter &block)
 {
-	for(BlockList::iterator i=blocks.begin(); i!=cur_blocks_end; ++i)
-		if(i->block()==&block)
+	/* Add it to the list first to present a consistent state in block_reserved
+	signal. */
+	blocks.push_back(block);
+
+	bool first_reserve = (cur_blocks_end==blocks.end());
+	if(first_reserve)
+		--cur_blocks_end;
+
+	try
+	{
+		if(!block->reserve(&train))
 		{
-			if(++i!=cur_blocks_end)
-				release_blocks(blocks.begin(), i);
-			return;
+			if(first_reserve)
+				cur_blocks_end = blocks.end();
+			blocks.pop_back();
+			return false;
 		}
+
+		return true;
+	}
+	catch(...)
+	{
+		if(first_reserve)
+			cur_blocks_end = blocks.end();
+		blocks.pop_back();
+		throw;
+	}
 }
 
-bool BlockAllocator::release_from(const Block &block)
+void BlockAllocator::advance_front(const Block *block, bool inclusive)
 {
-	bool have_sensor = false;
-	for(BlockList::iterator i=cur_blocks_end; i!=blocks.end(); ++i)
+	BlockList::iterator end;
+	if(block)
 	{
-		if(i->block()==&block)
+		end = cur_blocks_end;
+		if(inclusive)
+			--end;
+		end = find_if(end, blocks.end(), BlockMatch(*block));
+		if(inclusive && end!=blocks.end())
+			++end;
+	}
+	else
+		end = blocks.end();
+
+	if(end==blocks.end() && blocks.back().block()==pending_block)
+		--end;
+
+	SetFlag setf(advancing);
+	BlockList::iterator i = cur_blocks_end;
+	// Update cur_blocks_end first to keep things consistent.
+	cur_blocks_end = end;
+	for(; i!=end; ++i)
+		signal_advanced.emit(**i, (*i)->get_sensor());
+}
+
+void BlockAllocator::advance_front(const Sensor *sensor)
+{
+	if(sensor)
+		advance_front(sensor->get_block(), dynamic_cast<const BeamGate *>(sensor));
+	else
+		advance_front(0, false);
+}
+
+void BlockAllocator::advance_back()
+{
+	bool rev = train.get_controller().get_reverse();
+	const Vehicle &veh = train.get_vehicle(rev ? 0 : train.get_n_vehicles()-1);
+	const Block &veh_block = veh.get_placement().get_position(rev ? VehiclePlacement::FRONT_AXLE : VehiclePlacement::BACK_AXLE)->get_block();
+
+	bool pending_cleared = false;
+
+	/* Sensors aren't guaranteed to be detriggered in order.  Go through the
+	block list and locate the first sensor that's still active. */
+	BlockList::iterator end = blocks.end();
+	for(BlockList::iterator i=blocks.begin(); i!=cur_blocks_end; ++i)
+	{
+		Block *block = &**i;
+		list<Sensor *> sensors;
+
+		/* Collect all sensors from the block in the order they are expected to
+		detrigger. */
+		for(TrackIter j=i->track_iter(); (j && &j->get_block()==block); j=j.next())
+			if(!j->get_attachments().empty())
+			{
+				Track::AttachmentList attachments = j->get_attachments_ordered(j.entry());
+				for(Track::AttachmentList::const_iterator k=attachments.begin(); k!=attachments.end(); ++k)
+					if(BeamGate *gate = dynamic_cast<BeamGate *>(*k))
+						sensors.push_back(gate);
+			}
+
+		if(Sensor *sensor = (*i)->get_sensor())
+			sensors.push_back(sensor);
+
+		/* See if any sensor is still active, and record the position of the
+		last inactive sensor. */
+		bool active_sensor = false;
+		for(list<Sensor *>::const_iterator j=sensors.begin(); (!active_sensor && j!=sensors.end()); ++j)
 		{
-			if(have_sensor)
-				release_blocks(i, blocks.end());
-			return have_sensor;
+			if((*j)->get_state())
+				active_sensor = true;
+			else
+				end = i;
 		}
-		else if((*i)->get_sensor_id())
-			have_sensor = true;
-	}
 
-	return false;
+		if(block==pending_block)
+			pending_cleared = true;
+
+		// Stop if we encounter an active sensor or the train's last vehicle
+		if(block==&veh_block || active_sensor)
+		{
+			if(end!=blocks.end())
+			{
+				/* If the last inactive sensor was in an earlier block, release
+				that block as well. */
+				if(i!=end)
+					++end;
+				release_blocks_begin(end);
+				signal_rear_advanced.emit(*last());
+			}
+
+			if(pending_cleared)
+				reserve_more();
+
+			return;
+		}
+	}
 }
 
-void BlockAllocator::release_noncurrent()
+void BlockAllocator::release_blocks_begin(const BlockList::iterator &end)
 {
-	release_blocks(cur_blocks_end, blocks.end());
+	for(BlockList::iterator i=blocks.begin(); i!=end; )
+		release_block(i++);
 }
 
-void BlockAllocator::release_blocks(const BlockList::iterator &b, const BlockList::iterator &e)
+void BlockAllocator::release_blocks_end(const BlockList::iterator &begin)
 {
-	for(BlockList::iterator i=b; i!=e; )
-	{
-		if(cur_blocks_end==i)
-			cur_blocks_end = e;
+	// Guard against decrementing blocks.begin()
+	if(begin==blocks.begin())
+		return release_blocks_begin(blocks.end());
 
-		Block &block = **i;
-		blocks.erase(i++);
-		block.reserve(0);
+	if(begin==blocks.end())
+		return;
+
+	/* Release the blocks in reverse order so that a consistent state is
+	presented in block_reserved signal. */
+	bool done = false;
+	for(BlockList::iterator i=--blocks.end(); !done; )
+	{
+		done = (i==begin);
+		release_block(i--);
 	}
 }
 
+void BlockAllocator::release_block(const BlockList::iterator &i)
+{
+	if(advancing)
+		throw logic_error("cannot release while advancing");
+	if(i==cur_blocks_end)
+		++cur_blocks_end;
+	if(next_sensor && &**i==next_sensor->get_block())
+		next_sensor = 0;
+	if(&**i==pending_block)
+		pending_block = 0;
+
+	Block &block = **i;
+	blocks.erase(i);
+	block.reserve(0);
+}
+
 void BlockAllocator::reverse()
 {
-	release_noncurrent();
+	release_blocks_end(cur_blocks_end);
 	blocks.reverse();
 	for(BlockList::iterator i=blocks.begin(); i!=blocks.end(); ++i)
 		*i = i->reverse();
+
+	if(active)
+		reserve_more();
+}
+
+void BlockAllocator::turnout_path_changing(Track &track)
+{
+	BlockList::iterator i = find_if(cur_blocks_end, blocks.end(), BlockMatch(track.get_block()));
+	if(i!=blocks.end())
+	{
+		++i;
+		release_blocks_end(i);
+		pending_block = &track.get_block();
+	}
 }
 
 void BlockAllocator::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 && &**i==pending_block)
-			reserve_more();
+	if(&track.get_block()==pending_block && !reserving)
+		reserve_more();
 }
 
 void BlockAllocator::block_reserved(Block &block, const Train *tr)
@@ -267,65 +492,83 @@ void BlockAllocator::block_reserved(Block &block, const Train *tr)
 		reserve_more();
 }
 
-void BlockAllocator::block_state_changed(Block &block, Block::State state)
+void BlockAllocator::sensor_state_changed(Sensor &sensor, Sensor::State state)
 {
-	if(state==Block::MAYBE_ACTIVE)
-	{
-		// Find the first sensor block from our reserved blocks that isn't this sensor
-		BlockList::iterator end;
-		unsigned result = 0;
-		for(end=cur_blocks_end; end!=blocks.end(); ++end)
-			if((*end)->get_sensor_id())
-			{
-				if(&**end!=&block)
-				{
-					if(result==0)
-						result = 2;
-					else if(result==1)
-						break;
-				}
-				else if(result==0)
-					result = 1;
-				else if(result==2)
-					result = 3;
-			}
+	Block *block = sensor.get_block();
+	if(!block || block->get_train()!=&train)
+		return;
 
-		if(result==1)
+	if(state==Sensor::MAYBE_ACTIVE)
+	{
+		if(&sensor==next_sensor)
 		{
-			// Move blocks up to the next sensor to our current blocks
-			for(BlockList::iterator j=cur_blocks_end; j!=end; ++j)
-				train.signal_advanced.emit(**j);
-			cur_blocks_end = end;
+			if(is_block_current(*block))
+				signal_advanced.emit(*block, &sensor);
+			update_next_sensor(next_sensor);
+			advance_front(next_sensor);
+
+			if(active)
+				reserve_more();
 		}
-		else if(result==3)
-			train.get_layout().emergency("Sensor for "+train.get_name()+" triggered out of order");
+		else if(!is_block_current(*block))
+			train.get_layout().emergency(block, "Sensor for "+train.get_name()+" triggered out of order");
 	}
-	else if(state==Block::INACTIVE)
+	else if(state==Sensor::INACTIVE)
+		advance_back();
+}
+
+void BlockAllocator::update_next_sensor(Sensor *after)
+{
+	BeamGate *after_gate = dynamic_cast<BeamGate *>(after);
+
+	BlockList::iterator i = cur_blocks_end;
+	if(after)
 	{
-		const Vehicle &veh = train.get_controller().get_reverse() ? train.get_vehicle(0) : train.get_vehicle(train.get_n_vehicles()-1);
+		if(after_gate)
+			--i;
+		i = find_if(i, blocks.end(), BlockMatch(*after->get_block()));
+	}
 
-		// Find the first sensor in our current blocks that's still active
-		BlockList::iterator end = blocks.begin();
-		for(BlockList::iterator i=blocks.begin(); i!=cur_blocks_end; ++i)
+	for(; i!=blocks.end(); ++i)
+	{
+		if(Sensor *sensor = (*i)->get_sensor())
 		{
-			if((*i)->has_track(*veh.get_track()))
-				break;
-			if((*i)->get_sensor_id())
+			if(!after_gate && sensor!=next_sensor)
 			{
-				if(train.get_layout().get_driver().get_sensor((*i)->get_sensor_id()))
-					break;
-				else
-				{
-					end = i;
-					++end;
-				}
+				next_sensor = sensor;
+				return;
 			}
 		}
-		
-		if(end!=blocks.begin() && end!=cur_blocks_end)
-			// Free blocks up to the last inactive sensor
-			release_blocks(blocks.begin(), end);
+
+		Block *block = &**i;
+		for(TrackIter j=i->track_iter(); (j && &j->get_block()==block); j=j.next())
+			if(!j->get_attachments().empty())
+			{
+				Track::AttachmentList attachments = j->get_attachments_ordered(j.entry());
+				for(Track::AttachmentList::const_iterator k=attachments.begin(); k!=attachments.end(); ++k)
+					if(BeamGate *gate = dynamic_cast<BeamGate *>(*k))
+					{
+						if(after_gate)
+						{
+							if(gate==after_gate)
+								after_gate = 0;
+						}
+						else
+						{
+							next_sensor = gate;
+							return;
+						}
+					}
+			}
 	}
+
+	next_sensor = 0;
+}
+
+void BlockAllocator::halt_event(bool halted)
+{
+	if(active && !halted)
+		reserve_more();
 }
 
 void BlockAllocator::save(list<DataFile::Statement> &st) const
@@ -344,7 +587,7 @@ void BlockAllocator::save(list<DataFile::Statement> &st) const
 
 		st.push_back((DataFile::Statement("hint"), prev->get_id()));
 
-		for(BlockList::const_iterator i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
+		for(BlockList::const_iterator i=cur_blocks.begin(); (i!=cur_blocks.end() && i->block()!=pending_block); ++i)
 			st.push_back((DataFile::Statement("block"), (*i)->get_id()));
 	}
 }
@@ -383,8 +626,8 @@ void BlockAllocator::Loader::block(unsigned id)
 	obj.blocks.push_back(BlockIter(blk, entry));
 	blk->reserve(&obj.train);
 
-	if(blk->get_sensor_id())
-		obj.train.get_layout().get_driver().set_sensor(blk->get_sensor_id(), true);
+	if(blk->get_sensor_address())
+		obj.train.get_layout().get_driver().set_sensor(blk->get_sensor_address(), true);
 
 	prev_block = blk;
 }