From 29878263d8faf9b7e5845e8b1c3d52b139a255cf Mon Sep 17 00:00:00 2001
From: Mikko Rasa <tdb@tdb.fi>
Date: Wed, 26 Mar 2014 02:12:06 +0200
Subject: [PATCH] Support routing through multiple waypoints

Currently the result must be valid as a single route.  No crossing one's
own path or doubling back from a reversing loop.
---
 source/libr2c2/trainrouteplanner.cpp | 15 ++++++++++++---
 source/libr2c2/trainrouteplanner.h   |  1 +
 source/libr2c2/trainrouter.cpp       | 22 ++++++++++++++++++++++
 source/libr2c2/trainrouter.h         |  4 ++++
 4 files changed, 39 insertions(+), 3 deletions(-)

diff --git a/source/libr2c2/trainrouteplanner.cpp b/source/libr2c2/trainrouteplanner.cpp
index a7bb13e..b98b37e 100644
--- a/source/libr2c2/trainrouteplanner.cpp
+++ b/source/libr2c2/trainrouteplanner.cpp
@@ -121,13 +121,20 @@ void TrainRoutePlanner::add_steps(RoutingStep &step, unsigned train_index)
 	RoutingStep next(&step);
 	next.advance(dt);
 	TrainRouter &router = *train.info->router;
-	if(router.is_destination(*train.track) && !router.is_destination(*next_track))
+	if(train.waypoint<0 && router.is_destination(*train.track) && !router.is_destination(*next_track))
 	{
 		next.trains[train_index].state = ARRIVED;
 		new_steps.push_back(next);
 	}
 	else
 	{
+		if(train.waypoint>=0 && router.is_waypoint(train.waypoint, *train.track) && !router.is_waypoint(train.waypoint, *next_track))
+		{
+			++next.trains[train_index].waypoint;
+			if(next.trains[train_index].waypoint>=static_cast<int>(router.get_n_waypoints()))
+				next.trains[train_index].waypoint = -1;
+		}
+
 		next.trains[train_index].advance_track(0);
 
 		const TrackType::Endpoint &next_entry_ep = next_track.endpoint();
@@ -225,7 +232,8 @@ TrainRoutePlanner::TrainRoutingState::TrainRoutingState(TrainRoutingInfo &inf):
 	info(&inf),
 	occupied_tracks(0),
 	state(MOVING),
-	delay(info->router->get_departure_delay())
+	delay(info->router->get_departure_delay()),
+	waypoint(info->router->get_n_waypoints() ? 0 : -1)
 {
 	const Vehicle *veh = &info->train->get_vehicle(0);
 	// TODO margins
@@ -256,7 +264,8 @@ TrainRoutePlanner::TrainRoutingState::TrainRoutingState(const TrainRoutingState
 	occupied_tracks(other.occupied_tracks),
 	offset(other.offset),
 	back_offset(other.back_offset),
-	state(other.state)
+	state(other.state),
+	waypoint(other.waypoint)
 {
 	++occupied_tracks->refcount;
 }
diff --git a/source/libr2c2/trainrouteplanner.h b/source/libr2c2/trainrouteplanner.h
index 19fb329..21da3bb 100644
--- a/source/libr2c2/trainrouteplanner.h
+++ b/source/libr2c2/trainrouteplanner.h
@@ -60,6 +60,7 @@ private:
 		float back_offset;
 		TrainState state;
 		Msp::Time::TimeDelta delay;
+		int waypoint;
 
 		TrainRoutingState(TrainRoutingInfo &);
 		TrainRoutingState(const TrainRoutingState &);
diff --git a/source/libr2c2/trainrouter.cpp b/source/libr2c2/trainrouter.cpp
index b2993ee..da3b7d5 100644
--- a/source/libr2c2/trainrouter.cpp
+++ b/source/libr2c2/trainrouter.cpp
@@ -61,6 +61,7 @@ bool TrainRouter::set_route(const Route *r)
 	if(!r)
 	{
 		destination = 0;
+		waypoints.clear();
 	}
 
 	train.refresh_blocks_from(*fncb);
@@ -101,6 +102,20 @@ bool TrainRouter::is_destination(Track &track) const
 		return false;
 }
 
+void TrainRouter::add_waypoint(const TrackChain &wp)
+{
+	waypoints.push_back(&wp);
+	update_pending = true;
+}
+
+bool TrainRouter::is_waypoint(unsigned index, Track &track) const
+{
+	if(index>=waypoints.size())
+		throw out_of_range("TrainRouter::is_waypoint");
+
+	return waypoints[index]->has_track(track);
+}
+
 void TrainRouter::set_departure_delay(const Time::TimeDelta &d)
 {
 	delay = d;
@@ -125,6 +140,13 @@ void TrainRouter::message(const Message &msg)
 		else
 			set_destination(*msg.value.value<const TrackChain *>());
 	}
+	else if(msg.type=="add-waypoint")
+	{
+		if(msg.value.check_type<TrackChain *>())
+			add_waypoint(*msg.value.value<TrackChain *>());
+		else
+			add_waypoint(*msg.value.value<const TrackChain *>());
+	}
 	else if(msg.type=="set-departure-delay")
 		set_departure_delay(msg.value.value<Time::TimeDelta>());
 }
diff --git a/source/libr2c2/trainrouter.h b/source/libr2c2/trainrouter.h
index c518522..a2a73aa 100644
--- a/source/libr2c2/trainrouter.h
+++ b/source/libr2c2/trainrouter.h
@@ -41,6 +41,7 @@ private:
 	RouteList routes;
 	unsigned arriving;
 	const TrackChain *destination;
+	std::vector<const TrackChain *> waypoints;
 	std::list<Wait> waits;
 	Msp::Time::TimeDelta delay;
 
@@ -59,6 +60,9 @@ public:
 	void set_destination(const TrackChain &);
 	const TrackChain *get_destination() const { return destination; }
 	bool is_destination(Track &) const;
+	void add_waypoint(const TrackChain &);
+	unsigned get_n_waypoints() const { return waypoints.size(); }
+	bool is_waypoint(unsigned, Track &) const;
 	void set_departure_delay(const Msp::Time::TimeDelta &);
 	const Msp::Time::TimeDelta &get_departure_delay() const { return delay; }
 
-- 
2.43.0