From 90f9efba8d88f41c7a180734b7bcbdd1501d995e Mon Sep 17 00:00:00 2001
From: Mikko Rasa <tdb@tdb.fi>
Date: Sun, 21 Nov 2010 21:07:37 +0000
Subject: [PATCH] Make AIControl states clearer Match the speed of a preceding
 train to avoid oscillating

---
 source/libr2c2/aicontrol.cpp | 49 +++++++++++++++++++++---------------
 source/libr2c2/aicontrol.h   | 11 ++++++--
 source/libr2c2/train.cpp     |  7 ++++++
 source/libr2c2/train.h       |  2 ++
 4 files changed, 47 insertions(+), 22 deletions(-)

diff --git a/source/libr2c2/aicontrol.cpp b/source/libr2c2/aicontrol.cpp
index c2f16e3..2d48bf7 100644
--- a/source/libr2c2/aicontrol.cpp
+++ b/source/libr2c2/aicontrol.cpp
@@ -19,8 +19,7 @@ AIControl::AIControl(Train &t, Controller *n):
 	train(t),
 	next_ctrl(n),
 	target_speed(Control::continuous("speed", 0, 1000)),
-	blocked(false),
-	approach(false)
+	state(NORMAL)
 {
 	target_speed.set(0);
 
@@ -41,10 +40,10 @@ void AIControl::set_control(const string &n, float v)
 			train.set_active(true);
 
 		target_speed.set(v);
-		if(!blocked)
+		if(state!=BLOCKED)
 		{
 			float approach_speed = 5*train.get_layout().get_catalogue().get_scale();
-			if(approach && target_speed.value>approach_speed)
+			if(state==APPROACH && target_speed.value>approach_speed)
 				next_ctrl->set_control("speed", approach_speed);
 			else
 				next_ctrl->set_control("speed", target_speed.value);
@@ -88,26 +87,36 @@ void AIControl::tick(const Time::TimeDelta &dt)
 	float approach_speed = 5*scale;
 	float margin = 10*scale;
 
-	if(!blocked && rsv_dist<brake_dist+margin)
-	{
-		blocked = true;
-		next_ctrl->set_control("speed", 0);
-	}
-	else if((!approach && rsv_dist<brake_dist*1.3+approach_margin) || (blocked && rsv_dist>brake_dist+margin*2))
+	State old_state = state;
+
+	if(state==FOLLOW && !train.get_preceding_train())
+		state = NORMAL;
+
+	if(rsv_dist<brake_dist+margin)
+		state = BLOCKED;
+	else if(rsv_dist>brake_dist+margin*2 && rsv_dist<brake_dist*1.3+approach_margin)
+		state = APPROACH;
+	else if(rsv_dist>brake_dist*1.3+approach_margin*2)
+		state = NORMAL;
+
+	if(state==NORMAL && train.get_preceding_train())
+		state = FOLLOW;
+
+	if(state!=old_state || state==FOLLOW)
 	{
-		blocked = false;
-		approach = true;
-		if(target_speed.value>approach_speed)
-			next_ctrl->set_control("speed", approach_speed);
+		float speed_limit = -1;
+		if(state==BLOCKED)
+			speed_limit = 0;
+		else if(state==APPROACH)
+			speed_limit = approach_speed;
+		else if(state==FOLLOW)
+			speed_limit = train.get_preceding_train()->get_speed();
+
+		if(speed_limit>=0 && target_speed.value>speed_limit)
+			next_ctrl->set_control("speed", speed_limit);
 		else
 			next_ctrl->set_control("speed", target_speed.value);
 	}
-	else if((blocked || approach) && rsv_dist>brake_dist*1.3+approach_margin*2)
-	{
-		blocked = false;
-		approach = false;
-		next_ctrl->set_control("speed", target_speed.value);
-	}
 
 	next_ctrl->tick(dt);
 
diff --git a/source/libr2c2/aicontrol.h b/source/libr2c2/aicontrol.h
index 0d84218..9e0808e 100644
--- a/source/libr2c2/aicontrol.h
+++ b/source/libr2c2/aicontrol.h
@@ -18,11 +18,18 @@ class Train;
 class AIControl: public Controller, public sigc::trackable
 {
 private:
+	enum State
+	{
+		NORMAL,
+		APPROACH,
+		BLOCKED,
+		FOLLOW
+	};
+
 	Train &train;
 	Controller *next_ctrl;
 	Control target_speed;
-	bool blocked;
-	bool approach;
+	State state;
 
 public:
 	AIControl(Train &, Controller *);
diff --git a/source/libr2c2/train.cpp b/source/libr2c2/train.cpp
index ffa436b..0cbe306 100644
--- a/source/libr2c2/train.cpp
+++ b/source/libr2c2/train.cpp
@@ -48,6 +48,7 @@ Train::Train(Layout &l, const VehicleType &t, unsigned a, const string &p):
 	protocol(p),
 	priority(0),
 	yielding_to(0),
+	preceding_train(0),
 	cur_blocks_end(blocks.end()),
 	clear_blocks_end(blocks.end()),
 	pending_block(0),
@@ -841,6 +842,7 @@ void Train::reserve_more()
 	BlockIter start = blocks.back();
 
 	pending_block = 0;
+	preceding_train = 0;
 
 	// See how many sensor blocks and how much track we already have
 	unsigned nsens = 0;
@@ -952,10 +954,15 @@ void 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_end = blocks.end();
 
+				if(static_cast<unsigned>(other_entry)==block.entry())
+					preceding_train = other_train;
+			}
+
 			int other_prio = other_train->get_priority();
 
 			if(!entry_conflict && !exit_conflict && other_prio<priority)
diff --git a/source/libr2c2/train.h b/source/libr2c2/train.h
index 330eadd..81f0e2a 100644
--- a/source/libr2c2/train.h
+++ b/source/libr2c2/train.h
@@ -80,6 +80,7 @@ private:
 	std::string name;
 	int priority;
 	const Train *yielding_to;
+	const Train *preceding_train;
 	std::vector<Vehicle *> vehicles;
 	BlockList blocks;
 	BlockList::iterator cur_blocks_end;
@@ -119,6 +120,7 @@ public:
 	void set_priority(int);
 	void yield_to(const Train &);
 	int get_priority() const { return priority; }
+	const Train *get_preceding_train() const { return preceding_train; }
 	Controller &get_controller() const { return *controller; }
 
 	void add_vehicle(const VehicleType &);
-- 
2.43.0