From: Mikko Rasa <tdb@tdb.fi>
Date: Sun, 3 Oct 2010 18:56:38 +0000 (+0000)
Subject: Add an overload of Block::traverse that takes a Route
X-Git-Url: http://git.tdb.fi/?a=commitdiff_plain;h=98047057e4adae31aa449161ca845db1a6c2db77;p=r2c2.git

Add an overload of Block::traverse that takes a Route
Improved handling of taking blocks from another train
Add yield system for trains of equal priority
---

diff --git a/source/libmarklin/block.cpp b/source/libmarklin/block.cpp
index aa16fff..db1942b 100644
--- a/source/libmarklin/block.cpp
+++ b/source/libmarklin/block.cpp
@@ -8,6 +8,7 @@ Distributed under the GPL
 #include <algorithm>
 #include "block.h"
 #include "layout.h"
+#include "route.h"
 #include "tracktype.h"
 
 using namespace std;
@@ -81,6 +82,11 @@ int Block::get_endpoint_by_link(const Block &other) const
 }
 
 unsigned Block::traverse(unsigned epi, float *len) const
+{
+	return traverse(epi, 0, len);
+}
+
+unsigned Block::traverse(unsigned epi, const Route *route, float *len) const
 {
 	if(epi>=endpoints.size())
 		throw InvalidParameterValue("Endpoint index out of range");
@@ -94,7 +100,11 @@ unsigned Block::traverse(unsigned epi, float *len) const
 
 	while(1)
 	{
-		unsigned cur_path = track->get_active_path();
+		int cur_path = -1;
+		if(track->get_turnout_id() && route)
+			cur_path = route->get_turnout(track->get_turnout_id());
+		if(cur_path==-1)
+			cur_path = track->get_active_path();
 
 		if(len)
 			*len += track->get_type().get_path_length(cur_path);
diff --git a/source/libmarklin/block.h b/source/libmarklin/block.h
index abcd87c..3da5eec 100644
--- a/source/libmarklin/block.h
+++ b/source/libmarklin/block.h
@@ -15,6 +15,7 @@ Distributed under the GPL
 namespace Marklin {
 
 class Layout;
+class Route;
 class Train;
 
 class Block
@@ -50,6 +51,7 @@ public:
 	const std::vector<Endpoint> &get_endpoints() const { return endpoints; }
 	int get_endpoint_by_link(const Block &) const;
 	unsigned traverse(unsigned, float * =0) const;
+	unsigned traverse(unsigned, const Route *, float * =0) const;
 	void check_link(Block &);
 	void break_link(Block &);
 	Block *get_link(unsigned) const;
diff --git a/source/libmarklin/train.cpp b/source/libmarklin/train.cpp
index 8166c09..0c1748a 100644
--- a/source/libmarklin/train.cpp
+++ b/source/libmarklin/train.cpp
@@ -44,6 +44,7 @@ Train::Train(Layout &l, const VehicleType &t, unsigned a):
 	loco_type(t),
 	address(a),
 	priority(0),
+	yielding_to(0),
 	pending_block(0),
 	reserving(false),
 	advancing(false),
@@ -105,6 +106,11 @@ void Train::set_priority(int p)
 	priority = p;
 }
 
+void Train::yield_to(const Train &t)
+{
+	yielding_to = &t;
+}
+
 void Train::add_vehicle(const VehicleType &vt)
 {
 	Vehicle *veh = new Vehicle(layout, vt);
@@ -711,12 +717,12 @@ unsigned Train::reserve_more()
 	if(!active)
 		return 0;
 
-	BlockRef *last = 0;
+	BlockRef *start = 0;
 	if(!rsv_blocks.empty())
-		last = &rsv_blocks.back();
+		start = &rsv_blocks.back();
 	else if(!cur_blocks.empty())
-		last = &cur_blocks.back();
-	if(!last)
+		start = &cur_blocks.back();
+	if(!start)
 		return 0;
 
 	pending_block = 0;
@@ -733,7 +739,7 @@ unsigned Train::reserve_more()
 	const Route *cur_route = 0;
 	if(route)
 	{
-		const set<Track *> &tracks = last->block->get_tracks();
+		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))
@@ -745,13 +751,15 @@ unsigned Train::reserve_more()
 
 	SetFlag setf(reserving);
 
-	bool got_more = false;
-	BlockRef *good = last;
+	BlockRef *last = start;
+	BlockRef *good = start;
 	unsigned good_sens = nsens;
-	while(good_sens<3)
+	Train *blocking_train = 0;
+	std::list<BlockRef> contested_blocks;
+	while(good_sens<3 || !contested_blocks.empty())
 	{
 		// Traverse to the next block
-		unsigned exit = last->block->traverse(last->entry);
+		unsigned exit = last->block->traverse(last->entry, cur_route);
 		Block *link = last->block->get_link(exit);
 		if(!link)
 			break;
@@ -769,9 +777,12 @@ unsigned Train::reserve_more()
 			else if(!cur_route->get_tracks().count(entry_ep.track))
 			{
 				// Keep the blocks if we arrived at the end of the route
-				good = last;
-				good_sens = nsens;
-				end_of_route = true;
+				if(!blocking_train)
+				{
+					good = last;
+					good_sens = nsens;
+					end_of_route = true;
+				}
 				break;
 			}
 		}
@@ -780,30 +791,83 @@ unsigned Train::reserve_more()
 
 		if(link->get_endpoints().size()<2)
 		{
-			good = last;
-			good_sens = nsens;
+			if(!blocking_train)
+			{
+				good = last;
+				good_sens = nsens;
+			}
 			break;
 		}
 
+		if(blocking_train)
+		{
+			if(link->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))
+				{
+					// Roll back and start actually reserving the blocks
+					last = &rsv_blocks.back();
+					if(blocking_train->get_priority()==priority)
+						blocking_train->yield_to(*this);
+					blocking_train = 0;
+					continue;
+				}
+				else
+				{
+					pending_block = contested_blocks.front().block;
+					break;
+				}
+			}
+			else
+			{
+				contested_blocks.push_back(BlockRef(link, entry));
+				last = &contested_blocks.back();
+				continue;
+			}
+		}
+
 		bool reserved = link->reserve(this);
 		if(!reserved)
 		{
-			// Ask a lesser priority train to free the block for us
-			if(link->get_train()->get_priority()<priority)
-				if(link->get_train()->free_block(*link))
-					reserved = link->reserve(this);
+			/* 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);
+			if(other_entry<0)
+				throw LogicError("Block reservation inconsistency");
+
+			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)
+			{
+				/* Same direction, keep the blocks we got so far and wait for the
+				other train to pass */
+				good = last;
+				good_sens = nsens;
+
+				// 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);
+			}
+			else if(other_prio<priority || (other_prio==priority && other_train!=yielding_to && 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();
+				continue;
+			}
 
 			if(!reserved)
 			{
-				// If we found another train and it's not headed straight for us, we can keep the blocks we got
-				int other_entry = link->get_train()->get_entry_to_block(*link);
-				if(other_entry<0)
-					throw LogicError("Block reservation inconsistency");
-				if(static_cast<unsigned>(entry)!=link->traverse(other_entry))
-				{
-					good = last;
-					good_sens = nsens;
-				}
 				pending_block = link;
 				break;
 			}
@@ -843,24 +907,25 @@ unsigned Train::reserve_more()
 			}
 		}
 
+		if(!contested_blocks.empty() && contested_blocks.front().block==link)
+			contested_blocks.pop_front();
+
 		rsv_blocks.push_back(BlockRef(link, entry));
 		last = &rsv_blocks.back();
 		if(last->block->get_sensor_id())
-		{
 			++nsens;
-			got_more = true;
-		}
 	}
 
 	// Unreserve blocks that were not good
-	while(!rsv_blocks.empty() && last!=good)
+	while(!rsv_blocks.empty() && &rsv_blocks.back()!=good)
 	{
-		last->block->reserve(0);
+		rsv_blocks.back().block->reserve(0);
 		rsv_blocks.erase(--rsv_blocks.end());
-		if(!rsv_blocks.empty())
-			last = &rsv_blocks.back();
 	}
 
+	if(!rsv_blocks.empty() && &rsv_blocks.back()!=start)
+		// We got some new blocks, so no longer need to yield
+		yielding_to = 0;
 
 	// Make any sensorless blocks at the beginning immediately current
 	list<BlockRef>::iterator i;
diff --git a/source/libmarklin/train.h b/source/libmarklin/train.h
index 3e7a56b..2b08289 100644
--- a/source/libmarklin/train.h
+++ b/source/libmarklin/train.h
@@ -74,6 +74,7 @@ private:
 	unsigned address;
 	std::string name;
 	int priority;
+	const Train *yielding_to;
 	std::vector<Vehicle *> vehicles;
 	std::list<BlockRef> cur_blocks;
 	std::list<BlockRef> rsv_blocks;
@@ -110,6 +111,7 @@ public:
 	void set_name(const std::string &);
 	const std::string &get_name() const { return name; }
 	void set_priority(int);
+	void yield_to(const Train &);
 	int get_priority() const { return priority; }
 	Controller &get_controller() const { return *controller; }