From 57e0944ee8ed442e2d0aa7c9964cac00450af5e7 Mon Sep 17 00:00:00 2001
From: Mikko Rasa <tdb@tdb.fi>
Date: Tue, 25 Jun 2013 18:39:14 +0300
Subject: [PATCH] Redesign the train activation system

It was getting rather messy and there was one or two bugs where block
allocation didn't resume correctly.  The flag now resides in
BlockAllocator.  Rather than poking at it directly, AIs can express an
intention of moving the train.  This should help keep things in sync
with each other.
---
 source/engineer/engineer.cpp      |  2 +-
 source/libr2c2/aicontrol.cpp      | 11 ++--
 source/libr2c2/aicontrol.h        |  1 +
 source/libr2c2/blockallocator.cpp | 73 ++++++++++++++++++---------
 source/libr2c2/blockallocator.h   | 12 +++--
 source/libr2c2/train.cpp          | 83 ++++++++++++-------------------
 source/libr2c2/train.h            |  9 ++--
 source/libr2c2/trainai.h          |  1 +
 source/libr2c2/trainrouter.cpp    |  7 ++-
 source/libr2c2/trainstatus.cpp    |  2 +-
 10 files changed, 102 insertions(+), 99 deletions(-)

diff --git a/source/engineer/engineer.cpp b/source/engineer/engineer.cpp
index ae803c4..9a3b930 100644
--- a/source/engineer/engineer.cpp
+++ b/source/engineer/engineer.cpp
@@ -271,7 +271,7 @@ void Engineer::button_press(unsigned btn)
 			if(track->get_turnout_id())
 			{
 				Block &block = track->get_block();
-				if(block.get_train() && !block.get_train()->free_block(block))
+				if(block.get_train() && block.get_train()->is_block_critical(block))
 					set_status("Turnout is busy");
 				else
 				{
diff --git a/source/libr2c2/aicontrol.cpp b/source/libr2c2/aicontrol.cpp
index 97b7209..f1f16bf 100644
--- a/source/libr2c2/aicontrol.cpp
+++ b/source/libr2c2/aicontrol.cpp
@@ -21,9 +21,6 @@ AIControl::AIControl(Train &t):
 
 void AIControl::set_target_speed(float s)
 {
-	if(s && !train.is_active())
-		train.set_active(true);
-
 	target_speed = s;
 	need_update = true;
 	signal_event.emit(Message("target-speed-changed", target_speed));
@@ -83,7 +80,7 @@ void AIControl::tick(const Time::TimeDelta &)
 			speed_limit = 0;
 		else if(state==APPROACH)
 			speed_limit = approach_speed;
-		else if(state==FOLLOW && train.get_preceding_train()->is_active())
+		else if(state==FOLLOW && train.get_preceding_train()->get_block_allocator().is_active())
 			speed_limit = train.get_preceding_train()->get_speed();
 
 		if(speed_limit>=0 && target_speed>speed_limit)
@@ -99,9 +96,11 @@ void AIControl::tick(const Time::TimeDelta &)
 		reverse = pending_reverse;
 		train.set_control("reverse", reverse);
 	}
+}
 
-	if(!target_speed && !train.get_controller().get_speed() && train.is_active())
-		train.set_active(false);
+bool AIControl::has_intent_to_move() const
+{
+	return target_speed;
 }
 
 void AIControl::event(TrainAI &, const Message &ev)
diff --git a/source/libr2c2/aicontrol.h b/source/libr2c2/aicontrol.h
index b0c53d5..a9f9de1 100644
--- a/source/libr2c2/aicontrol.h
+++ b/source/libr2c2/aicontrol.h
@@ -35,6 +35,7 @@ public:
 
 	virtual void message(const Message &);
 	virtual void tick(const Msp::Time::TimeDelta &);
+	virtual bool has_intent_to_move() const;
 
 private:
 	void event(TrainAI &, const Message &);
diff --git a/source/libr2c2/blockallocator.cpp b/source/libr2c2/blockallocator.cpp
index 3955039..b38b62d 100644
--- a/source/libr2c2/blockallocator.cpp
+++ b/source/libr2c2/blockallocator.cpp
@@ -39,7 +39,22 @@ BlockAllocator::BlockAllocator(Train &t):
 	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())
+		{
+			(*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)
@@ -51,8 +66,22 @@ void BlockAllocator::start_from(const BlockIter &block)
 	reserve_block(block);
 }
 
+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()
 {
+	active = false;
 	release_blocks_begin(blocks.end());
 	pending_block = 0;
 	stop_at_block = 0;
@@ -61,6 +90,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
@@ -233,29 +264,6 @@ bool BlockAllocator::reserve_block(const BlockIter &block)
 	}
 }
 
-bool BlockAllocator::release_from(const Block &block)
-{
-	bool have_sensor = false;
-	for(BlockList::iterator i=cur_blocks_end; i!=blocks.end(); ++i)
-	{
-		if(i->block()==&block)
-		{
-			if(have_sensor)
-				release_blocks_end(i);
-			return have_sensor;
-		}
-		else if((*i)->get_sensor_id())
-			have_sensor = true;
-	}
-
-	return false;
-}
-
-void BlockAllocator::release_noncurrent()
-{
-	release_blocks_end(cur_blocks_end);
-}
-
 void BlockAllocator::release_blocks_begin(const BlockList::iterator &end)
 {
 	for(BlockList::iterator i=blocks.begin(); i!=end; )
@@ -295,10 +303,24 @@ void BlockAllocator::release_block(const BlockList::iterator &i)
 
 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(blocks.begin(), 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)
@@ -351,6 +373,9 @@ void BlockAllocator::sensor_state_changed(Sensor &sensor, Sensor::State state)
 			for(BlockList::iterator j=cur_blocks_end; j!=end; ++j)
 				train.signal_advanced.emit(**j);
 			cur_blocks_end = end;
+
+			if(active)
+				reserve_more();
 		}
 		else if(result==3)
 			train.get_layout().emergency("Sensor for "+train.get_name()+" triggered out of order");
diff --git a/source/libr2c2/blockallocator.h b/source/libr2c2/blockallocator.h
index 3c974f2..6ad3891 100644
--- a/source/libr2c2/blockallocator.h
+++ b/source/libr2c2/blockallocator.h
@@ -33,6 +33,7 @@ private:
 	typedef std::list<BlockIter> BlockList;
 
 	Train &train;
+	bool active;
 	BlockList blocks;
 	BlockList::iterator cur_blocks_end;
 	Block *pending_block;
@@ -42,7 +43,11 @@ private:
 public:
 	BlockAllocator(Train &);
 
+	void set_active(bool);
+	bool is_active() const { return active; }
+
 	void start_from(const BlockIter &);
+	void rewind_to(const Block &);
 	void clear();
 	bool empty() const { return blocks.empty(); }
 	void stop_at(const Block *);
@@ -55,13 +60,9 @@ public:
 	bool has_block(const Block &) const;
 	bool is_block_current(const Block &) const;
 
-	void reserve_more();
 private:
+	void reserve_more();
 	bool reserve_block(const BlockIter &);
-public:
-	bool release_from(const Block &);
-	void release_noncurrent();
-private:
 	void release_blocks_begin(const BlockList::iterator &);
 	void release_blocks_end(const BlockList::iterator &);
 	void release_block(const BlockList::iterator &);
@@ -69,6 +70,7 @@ public:
 	void reverse();
 
 private:
+	void turnout_path_changing(Track &);
 	void turnout_path_changed(Track &);
 	void block_reserved(Block &, const Train *);
 	void sensor_state_changed(Sensor &, Sensor::State);
diff --git a/source/libr2c2/train.cpp b/source/libr2c2/train.cpp
index 8cfa2d0..6cb3205 100644
--- a/source/libr2c2/train.cpp
+++ b/source/libr2c2/train.cpp
@@ -36,7 +36,6 @@ Train::Train(Layout &l, const VehicleType &t, unsigned a, const string &p):
 	allocator(*this),
 	advancing(false),
 	controller(new SimpleController),
-	active(false),
 	current_speed_step(0),
 	speed_changing(false),
 	reverse(false),
@@ -126,23 +125,6 @@ void Train::set_control(const string &n, float v)
 	controller->set_control(n, v);
 }
 
-void Train::set_active(bool a)
-{
-	if(a==active)
-		return;
-	if(!a && controller->get_speed())
-		throw logic_error("moving");
-
-	active = a;
-	if(active)
-	{
-		stop_timeout = Time::zero;
-		allocator.reserve_more();
-	}
-	else
-		stop_timeout = 2*Time::sec;
-}
-
 void Train::set_function(unsigned func, bool state)
 {
 	if(!loco_type.get_functions().count(func))
@@ -199,10 +181,8 @@ void Train::place(const BlockIter &block)
 	if(controller->get_speed())
 		throw logic_error("moving");
 
-	set_active(false);
-	accurate_position = false;
-
 	allocator.start_from(block);
+	accurate_position = false;
 
 	if(reverse)
 		vehicles.front()->place(block.reverse().track_iter(), 0, Vehicle::FRONT_BUFFER);
@@ -216,8 +196,6 @@ void Train::unplace()
 		throw logic_error("moving");
 
 	allocator.clear();
-
-	set_active(false);
 	accurate_position = false;
 
 	for(vector<Vehicle *>::iterator i=vehicles.begin(); i!=vehicles.end(); ++i)
@@ -227,48 +205,48 @@ void Train::unplace()
 void Train::stop_at(Block *block)
 {
 	allocator.stop_at(block);
-	if(active && !block)
-		allocator.reserve_more();
 }
 
-bool Train::free_block(Block &block)
+bool Train::is_block_critical(const Block &block) const
 {
-	if(get_reserved_distance_until(&block)<controller->get_braking_distance()*1.3)
-		return false;
-
-	return allocator.release_from(block);
+	return get_reserved_distance_until(&block)<controller->get_braking_distance()*1.3;
 }
 
-void Train::free_noncritical_blocks()
+BlockIter Train::get_first_noncritical_block() const
 {
 	if(allocator.empty())
-		return;
+		return BlockIter();
+
+	BlockIter i = allocator.last_current().next();
 
 	if(controller->get_speed()==0)
-	{
-		allocator.release_noncurrent();
-		return;
-	}
+		return i;
 
 	float margin = 10*layout.get_catalogue().get_scale();
 	float min_dist = controller->get_braking_distance()*1.3+margin;
 
-	BlockIter i = allocator.last_current().next();
 	float dist = 0;
 	bool sensor_seen = false;
 	for(; i->get_train()==this; i=i.next())
 	{
 		if(dist>min_dist && sensor_seen)
-		{
-			allocator.release_from(*i);
-			return;
-		}
+			return i;
 
 		dist += i->get_path_length(i.entry());
 
 		if(i->get_sensor_id())
 			sensor_seen = true;
 	}
+
+	return i;
+}
+
+void Train::refresh_blocks_from(Block &block)
+{
+	if(is_block_critical(block))
+		allocator.rewind_to(*get_first_noncritical_block());
+	else
+		allocator.rewind_to(block);
 }
 
 float Train::get_reserved_distance() const
@@ -284,19 +262,14 @@ float Train::get_reserved_distance() const
 	return max(get_reserved_distance_until(0)-margin, 0.0f);
 }
 
-void Train::reserve_more()
-{
-	allocator.reserve_more();
-}
-
 void Train::tick(const Time::TimeDelta &dt)
 {
-	if(!active && stop_timeout)
+	if(stop_timeout)
 	{
 		stop_timeout -= dt;
 		if(stop_timeout<=Time::zero)
 		{
-			allocator.release_noncurrent();
+			allocator.set_active(false);
 			stop_timeout = Time::TimeDelta();
 		}
 	}
@@ -305,8 +278,14 @@ void Train::tick(const Time::TimeDelta &dt)
 
 	Driver &driver = layout.get_driver();
 
+	bool intent_to_move = false;
 	for(list<TrainAI *>::iterator i=ais.begin(); i!=ais.end(); ++i)
+	{
 		(*i)->tick(dt);
+		if((*i)->has_intent_to_move())
+			intent_to_move = true;
+	}
+
 	controller->tick(dt);
 	float speed = controller->get_speed();
 	bool moving = speed>0;
@@ -320,8 +299,6 @@ void Train::tick(const Time::TimeDelta &dt)
 		driver.set_loco_reverse(address, r);
 
 		allocator.reverse();
-		if(active)
-			allocator.reserve_more();
 	}
 
 	if(speed_quantizer)
@@ -340,8 +317,8 @@ void Train::tick(const Time::TimeDelta &dt)
 
 	if(moving)
 	{
-		if(!active)
-			set_active(true);
+		if(!allocator.is_active())
+			allocator.set_active(true);
 
 		Vehicle &vehicle = *(reverse ? vehicles.back() : vehicles.front());
 
@@ -361,6 +338,8 @@ void Train::tick(const Time::TimeDelta &dt)
 			}
 		}
 	}
+	else if(allocator.is_active() && !intent_to_move && !stop_timeout)
+		stop_timeout = 2*Time::sec;
 }
 
 void Train::save(list<DataFile::Statement> &st) const
diff --git a/source/libr2c2/train.h b/source/libr2c2/train.h
index c0eb52e..f378389 100644
--- a/source/libr2c2/train.h
+++ b/source/libr2c2/train.h
@@ -56,7 +56,6 @@ private:
 	bool advancing;
 	Controller *controller;
 	std::list<TrainAI *> ais;
-	bool active;
 	unsigned current_speed_step;
 	bool speed_changing;
 	bool reverse;
@@ -90,13 +89,11 @@ public:
 	const Vehicle &get_vehicle(unsigned) const;
 
 	void set_control(const std::string &, float);
-	void set_active(bool);
 	void set_function(unsigned, bool);
 	float get_control(const std::string &) const;
 	float get_speed() const;
 	float get_quantized_speed() const;
 	unsigned get_speed_step() const { return current_speed_step; }
-	bool is_active() const { return active; }
 	bool get_function(unsigned) const;
 	unsigned get_functions() const { return functions; }
 
@@ -117,9 +114,9 @@ public:
 	void unplace();
 	bool is_placed() const { return !allocator.empty(); }
 	void stop_at(Block *);
-	bool free_block(Block &);
-	void free_noncritical_blocks();
-	void reserve_more();
+	bool is_block_critical(const Block &) const;
+	BlockIter get_first_noncritical_block() const;
+	void refresh_blocks_from(Block &);
 	const BlockAllocator &get_block_allocator() const { return allocator; }
 	float get_reserved_distance() const;
 
diff --git a/source/libr2c2/trainai.h b/source/libr2c2/trainai.h
index 3f98613..85c27b7 100644
--- a/source/libr2c2/trainai.h
+++ b/source/libr2c2/trainai.h
@@ -44,6 +44,7 @@ public:
 
 	virtual void message(const Message &) { }
 	virtual void tick(const Msp::Time::TimeDelta &) = 0;
+	virtual bool has_intent_to_move() const { return false; }
 };
 
 } // namespace R2C2
diff --git a/source/libr2c2/trainrouter.cpp b/source/libr2c2/trainrouter.cpp
index a5e0d56..8de191b 100644
--- a/source/libr2c2/trainrouter.cpp
+++ b/source/libr2c2/trainrouter.cpp
@@ -30,14 +30,14 @@ void TrainRouter::set_priority(int p)
 
 bool TrainRouter::set_route(const Route *r)
 {
-	train.free_noncritical_blocks();
+	BlockIter fncb = train.get_first_noncritical_block();
 
 	Route *lead = 0;
 	if(r && train.is_placed())
 	{
 		const BlockAllocator &allocator = train.get_block_allocator();
 		TrackIter first = allocator.first().track_iter();
-		TrackIter next = allocator.last().next().track_iter();
+		TrackIter next = fncb.track_iter();
 		if(!r->has_track(*next))
 		{
 			lead = Route::find(next, *r);
@@ -57,7 +57,7 @@ bool TrainRouter::set_route(const Route *r)
 	train.stop_at(0);
 	arriving = false;
 
-	train.reserve_more();
+	train.refresh_blocks_from(*fncb);
 
 	const Route *route = get_route();
 	signal_route_changed.emit(route);
@@ -144,7 +144,6 @@ void TrainRouter::tick(const Time::TimeDelta &)
 
 	if(arriving && !train.get_speed())
 	{
-		train.set_active(false);
 		signal_arrived.emit();
 		signal_event.emit(Message("arrived"));
 		set_route(0);
diff --git a/source/libr2c2/trainstatus.cpp b/source/libr2c2/trainstatus.cpp
index 65c17f7..10651d0 100644
--- a/source/libr2c2/trainstatus.cpp
+++ b/source/libr2c2/trainstatus.cpp
@@ -25,7 +25,7 @@ void TrainStatus::check()
 {
 	float scale = train.get_layout().get_catalogue().get_scale();
 	int s = static_cast<int>(train.get_quantized_speed()*3.6/scale+0.5);
-	if(s==0 && train.is_active())
+	if(s==0 && train.get_block_allocator().is_active())
 		s = -1;
 
 	if(s!=speed)
-- 
2.43.0