From 25b79ab38c2be2e5c89b8240504f641fd9c4899c Mon Sep 17 00:00:00 2001
From: Mikko Rasa <tdb@tdb.fi>
Date: Mon, 2 Jun 2008 12:20:45 +0000
Subject: [PATCH] Measure and display train speeds

---
 source/libmarklin/block.cpp     | 10 ++++++++--
 source/libmarklin/block.h       |  2 +-
 source/libmarklin/tracktype.cpp | 18 ++++++++++++------
 source/libmarklin/tracktype.h   |  1 +
 source/libmarklin/train.cpp     | 30 ++++++++++++++++++++++++++----
 source/libmarklin/train.h       |  3 +++
 6 files changed, 51 insertions(+), 13 deletions(-)

diff --git a/source/libmarklin/block.cpp b/source/libmarklin/block.cpp
index 87654e4..3ca744c 100644
--- a/source/libmarklin/block.cpp
+++ b/source/libmarklin/block.cpp
@@ -59,7 +59,7 @@ int Block::get_endpoint_by_link(const Block &other) const
 	return -1;
 }
 
-int Block::traverse(unsigned epi) const
+int Block::traverse(unsigned epi, float *len) const
 {
 	if(epi>=endpoints.size())
 		throw InvalidParameterValue("Endpoint index out of range");
@@ -68,6 +68,9 @@ int Block::traverse(unsigned epi) const
 	Track *track=ep.track;
 	unsigned track_ep=ep.track_ep;
 
+	if(len)
+		*len=0;
+
 	while(1)
 	{
 		unsigned cur_route=0;
@@ -78,9 +81,12 @@ int Block::traverse(unsigned epi) const
 			cur_route=turnout.get_route();
 		}
 
+		if(len)
+			*len+=track->get_type().get_route_length(cur_route);
+
 		int other_ep=track->traverse(track_ep, cur_route);
 		if(other_ep<0)
-			return 0;
+			return -1;
 
 		for(unsigned i=0; i<endpoints.size(); ++i)
 			if(endpoints[i].track==track && endpoints[i].track_ep==static_cast<unsigned>(other_ep))
diff --git a/source/libmarklin/block.h b/source/libmarklin/block.h
index 0766372..568d173 100644
--- a/source/libmarklin/block.h
+++ b/source/libmarklin/block.h
@@ -38,7 +38,7 @@ public:
 	const std::set<Track *> &get_tracks() const { return tracks; }
 	const std::vector<Endpoint> &get_endpoints() const { return endpoints; }
 	int get_endpoint_by_link(const Block &) const;
-	int traverse(unsigned) const;
+	int traverse(unsigned, float * =0) const;
 	void check_link(Block &);
 	Block *get_link(unsigned) const;
 	bool reserve(const Train *);
diff --git a/source/libmarklin/tracktype.cpp b/source/libmarklin/tracktype.cpp
index 70d0316..cd832eb 100644
--- a/source/libmarklin/tracktype.cpp
+++ b/source/libmarklin/tracktype.cpp
@@ -10,15 +10,21 @@ TrackType::TrackType(unsigned a):
 { }
 
 float TrackType::get_total_length() const
+{
+	return get_route_length(-1);
+}
+
+float TrackType::get_route_length(int r) const
 {
 	float len=0;
 	for(vector<TrackPart>::const_iterator i=parts.begin(); i!=parts.end(); ++i)
-	{
-		float l=i->length;
-		if(i->radius)
-			l*=i->radius;
-		len+=l;
-	}
+		if(r<0 || i->route==static_cast<unsigned>(r))
+		{
+			float l=i->length;
+			if(i->radius)
+				l*=abs(i->radius);
+			len+=l;
+		}
 	return len;
 }
 
diff --git a/source/libmarklin/tracktype.h b/source/libmarklin/tracktype.h
index 9822fb2..df6ba02 100644
--- a/source/libmarklin/tracktype.h
+++ b/source/libmarklin/tracktype.h
@@ -37,6 +37,7 @@ public:
 	unsigned get_article_number() const { return art_nr; }
 	const std::string &get_description() const { return description; }
 	float get_total_length() const;
+	float get_route_length(int) const;
 	unsigned get_n_routes() const;
 	const std::vector<TrackPart> &get_parts() const { return parts; }
 	const std::vector<Endpoint>  &get_endpoints() const { return endpoints; }
diff --git a/source/libmarklin/train.cpp b/source/libmarklin/train.cpp
index 8b34e84..2485c4c 100644
--- a/source/libmarklin/train.cpp
+++ b/source/libmarklin/train.cpp
@@ -1,3 +1,5 @@
+#include <cmath>
+#include <msp/strings/formatter.h>
 #include <msp/time/units.h>
 #include <msp/time/utils.h>
 #include "control.h"
@@ -15,7 +17,9 @@ Train::Train(TrafficManager &tm, Locomotive &l):
 	trfc_mgr(tm),
 	loco(l),
 	target_speed(0),
-	status("Unplaced")
+	status("Unplaced"),
+	travel_dist(0),
+	real_speed(0)
 {
 	trfc_mgr.add_train(this);
 
@@ -33,6 +37,8 @@ void Train::set_name(const string &n)
 
 void Train::set_speed(unsigned speed)
 {
+	unsigned old_speed=target_speed;
+
 	target_speed=speed;
 	if(!target_speed)
 	{
@@ -46,7 +52,10 @@ void Train::set_speed(unsigned speed)
 		return;
 
 	loco.set_speed(speed);
-	set_status(target_speed ? "Traveling" : "Stopped");
+	if(!old_speed && target_speed)
+		set_status("Traveling --- kmh");
+	else if(old_speed && !target_speed)
+		set_status("Stopped");
 }
 
 void Train::place(Block *block, unsigned entry)
@@ -97,7 +106,7 @@ void Train::tick(const Time::TimeStamp &t)
 		if(reserve_more() || !rsv_blocks.empty())
 		{
 			loco.set_speed(target_speed);
-			set_status("Traveling");
+			set_status("Traveling --- kmh");
 			try_reserve=Time::TimeStamp();
 		}
 		else
@@ -118,11 +127,24 @@ void Train::sensor_event(bool state, Sensor *sensor)
 
 		if(i!=rsv_blocks.begin())
 		{
+			real_speed=static_cast<int>(round(travel_dist/((Time::now()-last_entry_time)/Time::sec)*87*3.6/5))*5;
+			set_status(format("Traveling %3d kmh", real_speed));
+
+			travel_dist=0;
+			float block_len;
+			for(list<BlockRef>::iterator j=rsv_blocks.begin(); j!=i; ++j)
+			{
+				j->block->traverse(j->entry, &block_len);
+				cout<<"Advancing: block "<<j->block<<" (sensor "<<j->block->get_sensor_id()<<") length "<<block_len<<'\n';
+				travel_dist+=block_len;
+			}
+			last_entry_time=Time::now();
+
 			cur_blocks.splice(cur_blocks.end(), rsv_blocks, rsv_blocks.begin(), i);
 			cout<<"Train "<<name<<" advanced, "<<cur_blocks.size()<<" cur_blocks, "<<rsv_blocks.size()<<" rsv_blocks\n";
 		}
 
-		if(target_speed && rsv_blocks.empty() && !reserve_more())
+		if(target_speed && !reserve_more() && rsv_blocks.empty())
 		{
 			loco.set_speed(0);
 			try_reserve=Time::now()+2*Time::sec;
diff --git a/source/libmarklin/train.h b/source/libmarklin/train.h
index 24c9db9..0b88d8a 100644
--- a/source/libmarklin/train.h
+++ b/source/libmarklin/train.h
@@ -31,6 +31,9 @@ private:
 	unsigned target_speed;
 	Msp::Time::TimeStamp try_reserve;
 	std::string status;
+	Msp::Time::TimeStamp last_entry_time;
+	float travel_dist;
+	unsigned real_speed;
 
 public:
 	sigc::signal<void, const std::string &> signal_name_changed;
-- 
2.45.2