From 2225814e69913aecaee53b0505d1b92197621b10 Mon Sep 17 00:00:00 2001
From: Mikko Rasa <tdb@tdb.fi>
Date: Sun, 22 Feb 2015 20:09:51 +0200
Subject: [PATCH] Add an actual heuristic for wait time

---
 source/libr2c2/trainroutemetric.cpp  | 16 ++++++++++++++
 source/libr2c2/trainroutemetric.h    |  1 +
 source/libr2c2/trainrouteplanner.cpp | 32 ++++++++++++++++++++++++++--
 source/libr2c2/trainrouteplanner.h   |  2 ++
 4 files changed, 49 insertions(+), 2 deletions(-)

diff --git a/source/libr2c2/trainroutemetric.cpp b/source/libr2c2/trainroutemetric.cpp
index 469fc94..1fc4fce 100644
--- a/source/libr2c2/trainroutemetric.cpp
+++ b/source/libr2c2/trainroutemetric.cpp
@@ -68,6 +68,22 @@ void TrainRouteMetric::chain_to(const TrainRouteMetric &metric)
 		i->base_distance = metric.get_distance_from(*i->track.track(), i->track.entry());
 }
 
+float TrainRouteMetric::get_distance_from(const Track &track) const
+{
+	map<Key, Data>::const_iterator i = tracks.lower_bound(Key(&track, 0));
+	map<Key, Data>::const_iterator j = tracks.upper_bound(Key(&track, 255));
+
+	float result = -1;
+	for(; i!=j; ++i)
+	{
+		float d = i->second.distance+i->second.goal->base_distance;
+		if(result<0 || d<result)
+			result = d;
+	}
+
+	return result;
+}
+
 float TrainRouteMetric::get_distance_from(const Track &track, unsigned exit) const
 {
 	map<Key, Data>::const_iterator i = tracks.find(Key(&track, exit));
diff --git a/source/libr2c2/trainroutemetric.h b/source/libr2c2/trainroutemetric.h
index 156fe88..cef08cb 100644
--- a/source/libr2c2/trainroutemetric.h
+++ b/source/libr2c2/trainroutemetric.h
@@ -42,6 +42,7 @@ public:
 	TrainRouteMetric(const TrackChain &, TrackChain::Direction = TrackChain::UNSPECIFIED);
 
 	void chain_to(const TrainRouteMetric &);
+	float get_distance_from(const Track &) const;
 	float get_distance_from(const Track &, unsigned) const;
 };
 
diff --git a/source/libr2c2/trainrouteplanner.cpp b/source/libr2c2/trainrouteplanner.cpp
index 7d1b0c2..e083087 100644
--- a/source/libr2c2/trainrouteplanner.cpp
+++ b/source/libr2c2/trainrouteplanner.cpp
@@ -338,6 +338,7 @@ TrainRoutePlanner::TrainRoutingState::TrainRoutingState(const TrainRoutingState
 	distance_traveled(other.distance_traveled),
 	remaining_estimate(other.remaining_estimate),
 	wait_time(other.wait_time),
+	estimated_wait(other.estimated_wait),
 	blocked_by(other.blocked_by)
 {
 	++occupied_tracks->refcount;
@@ -354,6 +355,21 @@ Time::TimeDelta TrainRoutePlanner::TrainRoutingState::get_time_to_next_track() c
 	return ((track->get_type().get_path_length(path)-offset)/info->speed)*Time::sec+delay;
 }
 
+Time::TimeDelta TrainRoutePlanner::TrainRoutingState::get_time_to_pass(Track &trk) const
+{
+	if(is_occupying(trk))
+		return Time::zero;
+
+	for(unsigned wp=waypoint; wp<info->waypoints.size(); ++wp)
+	{
+		float distance = info->metrics[wp]->get_distance_from(trk);
+		if(distance>=0 && distance<remaining_estimate)
+			return ((remaining_estimate-distance)/info->speed)*Time::sec+delay;
+	}
+
+	return Time::day;
+}
+
 bool TrainRoutePlanner::TrainRoutingState::is_occupying(Track &trk) const
 {
 	if(state==ARRIVED && !duration && info->has_duration)
@@ -454,6 +470,9 @@ void TrainRoutePlanner::TrainRoutingState::advance(const Time::TimeDelta &dt)
 	if(duration)
 		duration = max(duration-secs*Time::sec, Time::zero);
 
+	if(estimated_wait)
+		estimated_wait = max(estimated_wait-secs*Time::sec, Time::zero);
+
 	if(state==MOVING)
 		advance(info->speed*secs);
 	else if(state!=ARRIVED)
@@ -555,7 +574,16 @@ void TrainRoutePlanner::RoutingStep::create_successors(list<RoutingStep> &new_st
 		RoutingStep wait(this);
 		wait.advance(dt);
 		wait.trains[train_index].state = WAITING;
-		wait.penalty += 15*Time::sec;
+
+		Time::TimeDelta estimated_wait = Time::day;
+		for(unsigned i=0; i<wait.trains.size(); ++i)
+			if(i!=static_cast<unsigned>(train_index) && wait.trains[i].state!=ARRIVED)
+			{
+				Time::TimeDelta ttp = wait.trains[i].get_time_to_pass(*train.track);
+				estimated_wait = min(estimated_wait, ttp);
+			}
+		wait.trains[train_index].estimated_wait = estimated_wait;
+
 		wait.update_estimate();
 		if(wait.is_viable())
 			new_steps.push_back(wait);
@@ -678,7 +706,7 @@ void TrainRoutePlanner::RoutingStep::update_estimate()
 	cost_estimate = penalty;
 	for(vector<TrainRoutingState>::const_iterator i=trains.begin(); i!=trains.end(); ++i)
 		if(i->remaining_estimate>=0)
-			cost_estimate += i->wait_time+((i->distance_traveled+i->remaining_estimate)/i->info->speed)*Time::sec;
+			cost_estimate += i->wait_time+i->estimated_wait+((i->distance_traveled+i->remaining_estimate)/i->info->speed)*Time::sec;
 }
 
 bool TrainRoutePlanner::RoutingStep::is_viable() const
diff --git a/source/libr2c2/trainrouteplanner.h b/source/libr2c2/trainrouteplanner.h
index 3076c39..5d52f10 100644
--- a/source/libr2c2/trainrouteplanner.h
+++ b/source/libr2c2/trainrouteplanner.h
@@ -81,6 +81,7 @@ private:
 		float distance_traveled;
 		float remaining_estimate;
 		Msp::Time::TimeDelta wait_time;
+		Msp::Time::TimeDelta estimated_wait;
 		int blocked_by;
 
 		TrainRoutingState(TrainRoutingInfo &);
@@ -88,6 +89,7 @@ private:
 		~TrainRoutingState();
 
 		Msp::Time::TimeDelta get_time_to_next_track() const;
+		Msp::Time::TimeDelta get_time_to_pass(Track &) const;
 		bool is_occupying(Track &) const;
 		bool check_arrival();
 		void advance(float);
-- 
2.43.0