From 5d40b436554081e785d213d2e03da7aa0d6631f0 Mon Sep 17 00:00:00 2001
From: Mikko Rasa <tdb@tdb.fi>
Date: Sun, 22 Mar 2015 18:03:24 +0200
Subject: [PATCH] Penalize running against the preferred direction when
 planning routes

---
 source/libr2c2/trainroutemetric.cpp  | 13 ++++++++++++-
 source/libr2c2/trainroutemetric.h    |  1 +
 source/libr2c2/trainrouteplanner.cpp | 14 ++++++++++----
 source/libr2c2/trainrouteplanner.h   |  1 +
 4 files changed, 24 insertions(+), 5 deletions(-)

diff --git a/source/libr2c2/trainroutemetric.cpp b/source/libr2c2/trainroutemetric.cpp
index d3878d4..6577c84 100644
--- a/source/libr2c2/trainroutemetric.cpp
+++ b/source/libr2c2/trainroutemetric.cpp
@@ -50,6 +50,8 @@ TrainRouteMetric::TrainRouteMetric(const TrackChain &tc, TrackChain::Direction d
 		const Data &data = tracks[Key(track.track(), track.entry())];
 
 		const TrackType::Endpoint &ep = track.endpoint();
+
+		float multiplier = get_travel_multiplier(*track, track.entry());
 		for(unsigned i=0; ep.paths>>i; ++i)
 			if(ep.has_path(i))
 			{
@@ -58,7 +60,7 @@ TrainRouteMetric::TrainRouteMetric(const TrackChain &tc, TrackChain::Direction d
 					continue;
 
 				Data &target = tracks[Key(next.track(), next.entry())];
-				float dist = data.distance+track->get_type().get_path_length(i);
+				float dist = data.distance+track->get_type().get_path_length(i)*multiplier;
 				if(target.distance<0 || target.distance>dist)
 				{
 					target = Data(dist, data.goal);
@@ -99,6 +101,15 @@ float TrainRouteMetric::get_distance_from(const Track &track, unsigned exit) con
 	return i->second.distance+i->second.goal->base_distance;
 }
 
+float TrainRouteMetric::get_travel_multiplier(const Track &track, unsigned exit) const
+{
+	int pref = track.get_preferred_exit();
+	if(pref>=0 && exit!=static_cast<unsigned>(pref))
+		return 5;
+
+	return 1;
+}
+
 
 TrainRouteMetric::Goal::Goal():
 	base_distance(0)
diff --git a/source/libr2c2/trainroutemetric.h b/source/libr2c2/trainroutemetric.h
index cef08cb..6331ec9 100644
--- a/source/libr2c2/trainroutemetric.h
+++ b/source/libr2c2/trainroutemetric.h
@@ -44,6 +44,7 @@ public:
 	void chain_to(const TrainRouteMetric &);
 	float get_distance_from(const Track &) const;
 	float get_distance_from(const Track &, unsigned) const;
+	float get_travel_multiplier(const Track &, unsigned) const;
 };
 
 } // namespace R2C2
diff --git a/source/libr2c2/trainrouteplanner.cpp b/source/libr2c2/trainrouteplanner.cpp
index 7e179f3..b9bbfbc 100644
--- a/source/libr2c2/trainrouteplanner.cpp
+++ b/source/libr2c2/trainrouteplanner.cpp
@@ -331,6 +331,8 @@ TrainRoutePlanner::TrainRoutingState::TrainRoutingState(TrainRoutingInfo &inf):
 		iter = iter.next();
 	}
 
+	travel_multiplier = info->metrics[waypoint]->get_travel_multiplier(*track, track.reverse(path).entry());
+
 	update_estimate();
 }
 
@@ -346,6 +348,7 @@ TrainRoutePlanner::TrainRoutingState::TrainRoutingState(const TrainRoutingState
 	delay(other.delay),
 	duration(other.duration),
 	waypoint(other.waypoint),
+	travel_multiplier(other.travel_multiplier),
 	distance_traveled(other.distance_traveled),
 	remaining_estimate(other.remaining_estimate),
 	wait_time(other.wait_time),
@@ -463,8 +466,8 @@ void TrainRoutePlanner::TrainRoutingState::advance(float distance)
 		occupied_tracks->n_tracks -= count_to_free;
 	}
 
-	distance_traveled += distance;
-	remaining_estimate -= distance;
+	distance_traveled += distance*travel_multiplier;
+	remaining_estimate -= distance*travel_multiplier;
 }
 
 void TrainRoutePlanner::TrainRoutingState::advance(const Time::TimeDelta &dt)
@@ -505,6 +508,7 @@ void TrainRoutePlanner::TrainRoutingState::advance_track(unsigned next_path)
 
 	advance(distance);
 	offset = 0;
+	travel_multiplier = info->metrics[waypoint]->get_travel_multiplier(*track, track.reverse(path).entry());
 }
 
 void TrainRoutePlanner::TrainRoutingState::set_path(unsigned p)
@@ -520,9 +524,11 @@ void TrainRoutePlanner::TrainRoutingState::set_path(unsigned p)
 void TrainRoutePlanner::TrainRoutingState::update_estimate()
 {
 	TrackIter iter = track.reverse(path);
-	remaining_estimate = info->metrics[waypoint]->get_distance_from(*iter.track(), iter.entry());
+	const TrainRouteMetric *metric = info->metrics[waypoint];
+	remaining_estimate = metric->get_distance_from(*iter, iter.entry());
+	travel_multiplier = metric->get_travel_multiplier(*iter, iter.entry());
 	if(remaining_estimate>=0)
-		remaining_estimate += occupied_tracks->path_length-offset;
+		remaining_estimate += (occupied_tracks->path_length-offset)*travel_multiplier;
 }
 
 bool TrainRoutePlanner::TrainRoutingState::is_viable() const
diff --git a/source/libr2c2/trainrouteplanner.h b/source/libr2c2/trainrouteplanner.h
index 47d00b8..85f5f65 100644
--- a/source/libr2c2/trainrouteplanner.h
+++ b/source/libr2c2/trainrouteplanner.h
@@ -79,6 +79,7 @@ private:
 		Msp::Time::TimeDelta delay;
 		Msp::Time::TimeDelta duration;
 		unsigned waypoint;
+		float travel_multiplier;
 		float distance_traveled;
 		float remaining_estimate;
 		Msp::Time::TimeDelta wait_time;
-- 
2.43.0