From 1203771e5aa9b12dca147cd1a84ece9a01c0fccd Mon Sep 17 00:00:00 2001
From: Mikko Rasa <tdb@tdb.fi>
Date: Fri, 11 Apr 2014 22:12:39 +0300
Subject: [PATCH] Use a better cost estimator for the route planner

Total time is not good because it doesn't penalize waiting time for
earlier trains.  This can cause the state tree to explode as later ones
start moving.

The sum of travel and wait times for each train works much better.
---
 source/libr2c2/trainrouteplanner.cpp | 20 ++++++++++----------
 source/libr2c2/trainrouteplanner.h   |  4 +++-
 2 files changed, 13 insertions(+), 11 deletions(-)

diff --git a/source/libr2c2/trainrouteplanner.cpp b/source/libr2c2/trainrouteplanner.cpp
index b4d4afd..be335d2 100644
--- a/source/libr2c2/trainrouteplanner.cpp
+++ b/source/libr2c2/trainrouteplanner.cpp
@@ -203,7 +203,9 @@ TrainRoutePlanner::TrainRoutingState::TrainRoutingState(const TrainRoutingState
 	state(other.state),
 	delay(other.delay),
 	waypoint(other.waypoint),
+	distance_traveled(other.distance_traveled),
 	remaining_estimate(other.remaining_estimate),
+	wait_time(other.wait_time),
 	blocked_by(other.blocked_by)
 {
 	++occupied_tracks->refcount;
@@ -270,6 +272,7 @@ void TrainRoutePlanner::TrainRoutingState::advance(float distance)
 		--occupied_tracks->n_tracks;
 	}
 
+	distance_traveled += distance;
 	remaining_estimate -= distance;
 }
 
@@ -290,6 +293,8 @@ void TrainRoutePlanner::TrainRoutingState::advance(const Time::TimeDelta &dt)
 
 	if(state==MOVING)
 		advance(info->speed*secs);
+	else if(state!=ARRIVED)
+		wait_time += secs*Time::sec;
 }
 
 void TrainRoutePlanner::TrainRoutingState::advance_track(unsigned next_path)
@@ -317,7 +322,7 @@ TrainRoutePlanner::RoutingStep::RoutingStep():
 
 TrainRoutePlanner::RoutingStep::RoutingStep(const RoutingStep *p):
 	time(p->time),
-	total_estimate(p->total_estimate),
+	cost_estimate(p->cost_estimate),
 	trains(p->trains),
 	prev(p)
 { }
@@ -463,19 +468,14 @@ void TrainRoutePlanner::RoutingStep::advance(const Time::TimeDelta &dt)
 
 void TrainRoutePlanner::RoutingStep::update_estimate()
 {
+	cost_estimate = Time::zero;
 	for(vector<TrainRoutingState>::const_iterator i=trains.begin(); i!=trains.end(); ++i)
-	{
-		Time::TimeDelta t = time+(i->remaining_estimate/i->info->speed)*Time::sec+i->delay;
-		if(i==trains.begin() || t>total_estimate)
-			total_estimate = t;
-	}
+		if(i->remaining_estimate>=0)
+			cost_estimate += i->wait_time+((i->distance_traveled+i->remaining_estimate)/i->info->speed)*Time::sec;
 }
 
 bool TrainRoutePlanner::RoutingStep::is_viable() const
 {
-	if(total_estimate<Time::zero)
-		return false;
-
 	for(vector<TrainRoutingState>::const_iterator i=trains.begin(); i!=trains.end(); ++i)
 		if(i->remaining_estimate<0)
 			return false;
@@ -497,7 +497,7 @@ bool TrainRoutePlanner::RoutingStep::is_goal() const
 
 bool TrainRoutePlanner::RoutingStep::operator<(const RoutingStep &other) const
 {
-	return total_estimate<other.total_estimate;
+	return cost_estimate<other.cost_estimate;
 }
 
 } // namespace R2C2
diff --git a/source/libr2c2/trainrouteplanner.h b/source/libr2c2/trainrouteplanner.h
index 33e6ef5..57d2de5 100644
--- a/source/libr2c2/trainrouteplanner.h
+++ b/source/libr2c2/trainrouteplanner.h
@@ -63,7 +63,9 @@ private:
 		TrainState state;
 		Msp::Time::TimeDelta delay;
 		int waypoint;
+		float distance_traveled;
 		float remaining_estimate;
+		Msp::Time::TimeDelta wait_time;
 		int blocked_by;
 
 		TrainRoutingState(TrainRoutingInfo &);
@@ -82,7 +84,7 @@ private:
 	struct RoutingStep
 	{
 		Msp::Time::TimeDelta time;
-		Msp::Time::TimeDelta total_estimate;
+		Msp::Time::TimeDelta cost_estimate;
 		std::vector<TrainRoutingState> trains;
 		const RoutingStep *prev;
 
-- 
2.45.2