From 145ba6391812e2b1040a78a8f0fda5888bfea610 Mon Sep 17 00:00:00 2001
From: Mikko Rasa <tdb@tdb.fi>
Date: Thu, 10 Apr 2014 22:01:54 +0300
Subject: [PATCH] Move path coercion to TrackType

---
 source/libr2c2/blockallocator.cpp | 22 ++++++----------------
 source/libr2c2/tracktype.cpp      | 30 ++++++++++++++++++++++++++++++
 source/libr2c2/tracktype.h        |  1 +
 3 files changed, 37 insertions(+), 16 deletions(-)

diff --git a/source/libr2c2/blockallocator.cpp b/source/libr2c2/blockallocator.cpp
index ed32193..88fb12e 100644
--- a/source/libr2c2/blockallocator.cpp
+++ b/source/libr2c2/blockallocator.cpp
@@ -253,26 +253,16 @@ 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;
+				}
 			}
 		}
 
diff --git a/source/libr2c2/tracktype.cpp b/source/libr2c2/tracktype.cpp
index 7a1f27a..59b8f9c 100644
--- a/source/libr2c2/tracktype.cpp
+++ b/source/libr2c2/tracktype.cpp
@@ -44,6 +44,36 @@ unsigned TrackType::get_n_paths() const
 	return n;
 }
 
+unsigned TrackType::coerce_path(unsigned entry, unsigned path) const
+{
+	const Endpoint &ep = get_endpoint(entry);
+	if(ep.has_path(path))
+		return path;
+
+	unsigned paths = get_paths();
+	if(paths>>(1<<state_bits))
+	{
+		/* There are more paths than can be expressed with state_bits, so
+		multiple paths are set at once.  See if one of the alternatives fits. */
+		unsigned step = 1<<state_bits;
+		for(unsigned p=path+step; paths>>p; p+=step)
+			if(ep.has_path(p))
+				return p;
+	}
+
+	// Find an endpoint that's connected to the entry and has the requested path
+	for(vector<Endpoint>::const_iterator i=endpoints.begin(); i!=endpoints.end(); ++i)
+		if(i->has_path(path) && i->has_common_paths(ep))
+		{
+			unsigned p = 1;
+			for(unsigned m=i->paths&ep.paths; m>>p; ++p) ;
+			return p-1;
+		}
+
+	// TODO crossings fall here
+	throw logic_error("TrackType::coerce_path");
+}
+
 bool TrackType::is_turnout() const
 {
 	return endpoints.size()>2;
diff --git a/source/libr2c2/tracktype.h b/source/libr2c2/tracktype.h
index 64efe8c..56d164f 100644
--- a/source/libr2c2/tracktype.h
+++ b/source/libr2c2/tracktype.h
@@ -56,6 +56,7 @@ public:
 	float get_path_length(int) const;
 	unsigned get_paths() const;
 	unsigned get_n_paths() const;
+	unsigned coerce_path(unsigned, unsigned) const;
 	unsigned get_state_bits() const { return state_bits; }
 	bool is_turnout() const;
 	bool is_dead_end() const;
-- 
2.45.2