From 319f90fd59587efc1242c34f307abc29f323642b Mon Sep 17 00:00:00 2001
From: Mikko Rasa <tdb@tdb.fi>
Date: Thu, 21 May 2009 16:57:02 +0000
Subject: [PATCH] Attempt to estimate the exact positions of trains from
 measured speed data Improve track vertex array creation Use Point instead of
 separate coordinate members for Endpoint and TrackPart

---
 source/3d/track.cpp                  |  87 ++++----------
 source/engineer/engineer.cpp         |  22 ++++
 source/libmarklin/block.cpp          |   9 +-
 source/libmarklin/endpoint.h         |  10 +-
 source/libmarklin/geometry.h         |   3 +-
 source/libmarklin/track.cpp          |  68 ++++++++++-
 source/libmarklin/track.h            |   3 +-
 source/libmarklin/trackpart.cpp      |  31 +++--
 source/libmarklin/trackpart.h        |   8 +-
 source/libmarklin/tracktype.cpp      |   6 +-
 source/libmarklin/trafficmanager.cpp |   6 +-
 source/libmarklin/trafficmanager.h   |   3 +-
 source/libmarklin/train.cpp          | 170 ++++++++++++++++-----------
 source/libmarklin/train.h            |  16 ++-
 14 files changed, 276 insertions(+), 166 deletions(-)

diff --git a/source/3d/track.cpp b/source/3d/track.cpp
index d4b7079..5857e52 100644
--- a/source/3d/track.cpp
+++ b/source/3d/track.cpp
@@ -1,7 +1,7 @@
 /* $Id$
 
 This file is part of the MSP Märklin suite
-Copyright © 2006-2008 Mikkosoft Productions, Mikko Rasa
+Copyright © 2006-2009 Mikkosoft Productions, Mikko Rasa
 Distributed under the GPL
 */
 
@@ -14,8 +14,6 @@ Distributed under the GPL
 using namespace std;
 using namespace Msp;
 
-#include <iostream>
-
 namespace Marklin {
 
 Track3D::Track3D(Track &t, unsigned q):
@@ -96,10 +94,10 @@ void Track3D::render_endpoints() const
 		float s=sin(ep.dir);
 
 		glBegin(GL_QUADS);
-		glVertex3f(ep.x-s*0.025, ep.y+c*0.025, 0);
-		glVertex3f(ep.x+s*0.025, ep.y-c*0.025, 0);
-		glVertex3f(ep.x+s*0.025, ep.y-c*0.025, 0.02);
-		glVertex3f(ep.x-s*0.025, ep.y+c*0.025, 0.02);
+		glVertex3f(ep.pos.x-s*0.025, ep.pos.y+c*0.025, 0);
+		glVertex3f(ep.pos.x+s*0.025, ep.pos.y-c*0.025, 0);
+		glVertex3f(ep.pos.x+s*0.025, ep.pos.y-c*0.025, 0.02);
+		glVertex3f(ep.pos.x-s*0.025, ep.pos.y+c*0.025, 0.02);
 		glEnd();
 	}
 
@@ -173,63 +171,30 @@ void Track3D::build_part(const TrackPart &part, GL::VertexArrayBuilder &va_build
 	}
 	static unsigned psize=profile.size();
 
-	const float &radius=part.radius;
-	const float &x=part.x;
-	const float &y=part.y;
-	const float &length=part.length;
-	const float &dir=part.dir;
+	unsigned nsegs=(part.radius ? static_cast<unsigned>(part.length*(1<<quality))+1 : 1);
+	float plen=part.length;
+	if(part.radius)
+		plen*=abs(part.radius);
 
-	unsigned nsegs;
-	if(radius)
-	{
-		nsegs=static_cast<unsigned>(part.length*(1<<quality))+1;
-		Point center(x-sin(dir)*radius, y+cos(dir)*radius, 0);
-		float r=fabs(radius);
-		float start=((radius<0)?M_PI:0)+dir;
-		float angle=(radius<0)?-length:length;
-		int inv=(radius<0)?-1:1;
-		for(unsigned i=0; i<=nsegs; ++i)
-		{
-			float a=start+i*angle/nsegs;
-			float c=cos(a);
-			float s=sin(a);
-
-			for(unsigned j=0; j<profile.size(); ++j)
-			{
-				unsigned k=j&~1;
-				float dy=profile[k+1].y-profile[k].y;
-				float dz=profile[k+1].z-profile[k].z;
-				float d=sqrt(dy*dy+dz*dz);
-				va_builder.normal(s*dz/d, -c*dz/d, dy/d);
-
-				Point p(center.x+s*(r-profile[j].y*inv), center.y-c*(r-profile[j].y*inv), profile[j].z+i*track.get_slope()/nsegs);
-				va_builder.vertex(p.x, p.y, p.z);
-				if(profile[j].z==0)
-					border.push_back(p);
-			}
-		}
-	}
-	else
+	for(unsigned i=0; i<=nsegs; ++i)
 	{
-		nsegs=1;
-		float c=cos(dir);
-		float s=sin(dir);
-		for(unsigned i=0; i<2; ++i)
+		float a=part.dir+(part.radius ? i*plen/nsegs/part.radius : 0);
+		float c=cos(a);
+		float s=sin(a);
+		Point p=part.get_point(i*plen/nsegs);
+
+		for(unsigned j=0; j<profile.size(); ++j)
 		{
-			for(unsigned j=0; j<profile.size(); ++j)
-			{
-				unsigned k=j&~1;
-				float dy=profile[k+1].y-profile[k].y;
-				float dz=profile[k+1].z-profile[k].z;
-				float d=sqrt(dy*dy+dz*dz);
-				va_builder.normal(s*dz/d, -c*dz/d, dy/d);
-
-				float len=(part.dead_end && i==1 && j>=6) ? length/2 : length;
-				Point p(x+c*len*i-s*profile[j].y, y+s*len*i+c*profile[j].y, profile[j].z+i*track.get_slope());
-				va_builder.vertex(p.x, p.y, p.z);
-				if(profile[j].z==0)
-					border.push_back(p);
-			}
+			unsigned k=j&~1;
+			float dy=profile[k+1].y-profile[k].y;
+			float dz=profile[k+1].z-profile[k].z;
+			float d=sqrt(dy*dy+dz*dz);
+			va_builder.normal(s*dz/d, -c*dz/d, dy/d);
+
+			Point v(p.x+c*profile[j].x-s*profile[j].y, p.y+c*profile[j].y+s*profile[j].x, profile[j].z+i*track.get_slope()/nsegs);
+			va_builder.vertex(v.x, v.y, v.z);
+			if(profile[j].z==0)
+				border.push_back(v);
 		}
 	}
 
diff --git a/source/engineer/engineer.cpp b/source/engineer/engineer.cpp
index fc6d75d..5466302 100644
--- a/source/engineer/engineer.cpp
+++ b/source/engineer/engineer.cpp
@@ -263,6 +263,28 @@ void Engineer::tick()
 		GL::pop_matrix();
 	}
 
+	const list<Train *> &trains=trfc_mgr->get_trains();
+	for(list<Train *>::const_iterator i=trains.begin(); i!=trains.end(); ++i)
+	{
+		GL::PushMatrix _push;
+
+		const Point &tp=(*i)->get_position();
+		GL::translate(tp.x, tp.y, 0.02);
+		GL::Immediate imm((GL::COLOR4_UBYTE, GL::VERTEX2));
+		imm.color(0.8f, 0.8f, 1.0f);
+		imm.begin(GL::TRIANGLE_FAN);
+		imm.vertex(0, 0);
+		for(unsigned j=0; j<=12; ++j)
+			imm.vertex(0.02*cos(j*M_PI/6), 0.02*sin(j*M_PI/6));
+		imm.end();
+
+		GL::rotate(cam_rot*180/M_PI, 0, 0, 1);
+		GL::translate(0.03, -0.02, 0);
+		GL::scale_uniform(0.04);
+		ui_res.get_default_font().draw_string((*i)->get_name());
+		GL::Texture::unbind();
+	}
+
 	GL::matrix_mode(GL::PROJECTION);
 	GL::load_identity();
 	GL::ortho_bottomleft(screen_w, screen_h);
diff --git a/source/libmarklin/block.cpp b/source/libmarklin/block.cpp
index 03cb2dc..94be126 100644
--- a/source/libmarklin/block.cpp
+++ b/source/libmarklin/block.cpp
@@ -5,16 +5,15 @@ Copyright © 2006-2009 Mikkosoft Productions, Mikko Rasa
 Distributed under the GPL
 */
 
+#include <iostream>
 #include "control.h"
 #include "block.h"
 #include "tracktype.h"
 #include "trafficmanager.h"
 #include "turnout.h"
 
-using namespace Msp;
-
-#include <iostream>
 using namespace std;
+using namespace Msp;
 
 namespace Marklin {
 
@@ -135,10 +134,6 @@ bool Block::reserve(const Train *t)
 	if(!t || !train)
 	{
 		train=t;
-		if(train)
-			cout<<"Block "<<this<<" reserved for train "<<train->get_name()<<'\n';
-		else
-			cout<<"Block "<<this<<" freed\n";
 		trfc_mgr.signal_block_reserved.emit(*this, train);
 		return true;
 	}
diff --git a/source/libmarklin/endpoint.h b/source/libmarklin/endpoint.h
index 34acdcd..9eade4d 100644
--- a/source/libmarklin/endpoint.h
+++ b/source/libmarklin/endpoint.h
@@ -8,16 +8,18 @@ Distributed under the GPL
 #ifndef MARKLIN_ENDPOINT_H_
 #define MARKLIN_ENDPOINT_H_
 
+#include "geometry.h"
+
 namespace Marklin {
 
 struct Endpoint
 {
-	float  x, y;
-	float  dir;  // Direction outwards from the endpoint
+	Point pos;
+	float dir;  // Direction outwards from the endpoint
 	unsigned routes;
 
-	Endpoint(): x(0), y(0), dir(0), routes(0) { }
-	Endpoint(float x_, float y_, float d, unsigned r): x(x_), y(y_), dir(d), routes(r) { }
+	Endpoint(): dir(0), routes(0) { }
+	Endpoint(float x, float y, float d, unsigned r): pos(x, y), dir(d), routes(r) { }
 };
 
 } // namespace Marklin
diff --git a/source/libmarklin/geometry.h b/source/libmarklin/geometry.h
index 240bd89..e9b9e27 100644
--- a/source/libmarklin/geometry.h
+++ b/source/libmarklin/geometry.h
@@ -1,7 +1,7 @@
 /* $Id$
 
 This file is part of the MSP Märklin suite
-Copyright © 2006-2008 Mikkosoft Productions, Mikko Rasa
+Copyright © 2006-2009 Mikkosoft Productions, Mikko Rasa
 Distributed under the GPL
 */
 
@@ -17,6 +17,7 @@ struct Point
 	float x, y, z;
 
 	Point(): x(0), y(0), z(0) { }
+	Point(float x_, float y_): x(x_), y(y_), z(0) { }
 	Point(float x_, float y_, float z_): x(x_), y(y_), z(z_) { }
 };
 
diff --git a/source/libmarklin/track.cpp b/source/libmarklin/track.cpp
index 61f0d35..e544081 100644
--- a/source/libmarklin/track.cpp
+++ b/source/libmarklin/track.cpp
@@ -1,7 +1,7 @@
 /* $Id$
 
 This file is part of the MSP Märklin suite
-Copyright © 2006-2008 Mikkosoft Productions, Mikko Rasa
+Copyright © 2006-2009 Mikkosoft Productions, Mikko Rasa
 Distributed under the GPL
 */
 
@@ -87,7 +87,7 @@ Point Track::get_endpoint_position(unsigned epi) const
 	float c=cos(rot);
 	float s=sin(rot);
 
-	Point p(pos.x+c*ep.x-s*ep.y, pos.y+s*ep.x+c*ep.y, pos.z);
+	Point p(pos.x+c*ep.pos.x-s*ep.pos.y, pos.y+s*ep.pos.x+c*ep.pos.y, pos.z);
 	if(eps.size()==2 && epi==1)
 		p.z+=slope;
 	return p;
@@ -125,7 +125,7 @@ bool Track::snap_to(Track &other, bool link)
 			if(dx*dx+dy*dy<limit)
 			{
 				set_rotation(other.rot+other_eps[j].dir-eps[i].dir+M_PI);
-				set_position(Point(epp2.x-(eps[i].x*cos(rot)-eps[i].y*sin(rot)), epp2.y-(eps[i].y*cos(rot)+eps[i].x*sin(rot)), epp2.z));
+				set_position(Point(epp2.x-(eps[i].pos.x*cos(rot)-eps[i].pos.y*sin(rot)), epp2.y-(eps[i].pos.y*cos(rot)+eps[i].pos.x*sin(rot)), epp2.z));
 
 				if(link)
 				{
@@ -246,6 +246,68 @@ int Track::traverse(unsigned i, unsigned route) const
 	return -1;
 }
 
+Point Track::get_point(unsigned epi, unsigned route, float d) const
+{
+	const vector<Endpoint> &eps=type.get_endpoints();
+	if(epi>=eps.size())
+		throw InvalidParameterValue("Endpoint index out of range");
+
+	float x=eps[epi].pos.x;
+	float y=eps[epi].pos.y;
+
+	const vector<TrackPart> &parts=type.get_parts();
+	const TrackPart *last_part=0;
+	while(1)
+	{
+		for(vector<TrackPart>::const_iterator i=parts.begin(); i!=parts.end(); ++i)
+		{
+			if((eps[epi].routes&(1<<route)) && i->route!=route)
+				continue;
+			if(&*i==last_part)
+				continue;
+
+			vector<Endpoint> part_eps;
+			i->collect_endpoints(part_eps);
+			for(unsigned j=0; j<part_eps.size(); ++j)
+			{
+				float dx=part_eps[j].pos.x-x;
+				float dy=part_eps[j].pos.y-y;
+				if(dx*dx+dy*dy<1e-6)
+				{
+					float plen=i->length;
+					if(i->radius)
+						plen*=abs(i->radius);
+					if(d<plen)
+					{
+						if(j==1)
+							d=plen-d;
+						Point p=i->get_point(d);
+						float c=cos(rot);
+						float s=sin(rot);
+						return Point(pos.x+c*p.x-s*p.y, pos.y+c*p.y+s*p.x);
+					}
+					else if(part_eps.size()>1)
+					{
+						d-=plen;
+						x=part_eps[1-j].pos.x;
+						y=part_eps[1-j].pos.y;
+						last_part=&*i;
+						i=parts.begin();
+						break;
+					}
+					else
+						return pos;
+				}
+			}
+		}
+
+		if(!last_part)
+			throw Exception("Internal error (Endpoint does not match any part)");
+		else
+			return pos;
+	}
+}
+
 Track *Track::copy() const
 {
 	Track *trk=new Track(type);
diff --git a/source/libmarklin/track.h b/source/libmarklin/track.h
index c434224..11c9fb3 100644
--- a/source/libmarklin/track.h
+++ b/source/libmarklin/track.h
@@ -1,7 +1,7 @@
 /* $Id$
 
 This file is part of the MSP Märklin suite
-Copyright © 2006-2008 Mikkosoft Productions, Mikko Rasa
+Copyright © 2006-2009 Mikkosoft Productions, Mikko Rasa
 Distributed under the GPL
 */
 
@@ -70,6 +70,7 @@ public:
 	Track           *get_link(unsigned) const;
 	void            check_slope();
 	int             traverse(unsigned, unsigned) const;
+	Point           get_point(unsigned, unsigned, float) const;
 
 	/**
 	Creates a copy of the track.  The new track will be almost identical, but
diff --git a/source/libmarklin/trackpart.cpp b/source/libmarklin/trackpart.cpp
index 1ef6e75..046b927 100644
--- a/source/libmarklin/trackpart.cpp
+++ b/source/libmarklin/trackpart.cpp
@@ -1,7 +1,7 @@
 /* $Id$
 
 This file is part of the MSP Märklin suite
-Copyright © 2006-2008 Mikkosoft Productions, Mikko Rasa
+Copyright © 2006-2009 Mikkosoft Productions, Mikko Rasa
 Distributed under the GPL
 */
 
@@ -13,8 +13,6 @@ using namespace std;
 namespace Marklin {
 
 TrackPart::TrackPart():
-	x(0),
-	y(0),
 	dir(0),
 	length(0),
 	radius(0),
@@ -22,23 +20,35 @@ TrackPart::TrackPart():
 	dead_end(false)
 { }
 
-void TrackPart::collect_endpoints(vector<Endpoint> &eps)
+void TrackPart::collect_endpoints(vector<Endpoint> &eps) const
 {
-	eps.push_back(Endpoint(x, y, dir+M_PI, 1<<route));
+	eps.push_back(Endpoint(pos.x, pos.y, dir+M_PI, 1<<route));
 
 	if(dead_end)
 		;
 	else if(radius)
 	{
 		float a=((radius<0) ? -length : length);
+		Point p=get_point(length*abs(radius));
+		eps.push_back(Endpoint(p.x, p.y, dir+a, 1<<route));
+	}
+	else
+		eps.push_back(Endpoint(pos.x+cos(dir)*length, pos.y+sin(dir)*length, dir, 1<<route));
+}
+
+Point TrackPart::get_point(float d) const
+{
+	if(radius)
+	{
+		float a=d/radius;
 		float c=cos(a);
 		float s=sin(a);
 		float rx=radius*sin(dir);
 		float ry=-radius*cos(dir);
-		eps.push_back(Endpoint(x+c*rx-s*ry-rx, y+c*ry+s*rx-ry, dir+a, 1<<route));
+		return Point(pos.x+c*rx-s*ry-rx, pos.y+c*ry+s*rx-ry);
 	}
 	else
-		eps.push_back(Endpoint(x+cos(dir)*length, y+sin(dir)*length, dir, 1<<route));
+		return Point(pos.x+cos(dir)*d, pos.y+sin(dir)*d);
 }
 
 
@@ -62,15 +72,14 @@ void TrackPart::Loader::finish()
 	else
 		part.length/=1000;
 
-	part.x/=1000;
-	part.y/=1000;
+	part.pos.x/=1000;
+	part.pos.y/=1000;
 	part.dir*=M_PI/180;
 }
 
 void TrackPart::Loader::start(float x, float y, float d)
 {
-	part.x=x;
-	part.y=y;
+	part.pos=Point(x, y);
 	part.dir=d;
 }
 
diff --git a/source/libmarklin/trackpart.h b/source/libmarklin/trackpart.h
index 5aa4b0c..05fa0ba 100644
--- a/source/libmarklin/trackpart.h
+++ b/source/libmarklin/trackpart.h
@@ -1,7 +1,7 @@
 /* $Id$
 
 This file is part of the MSP Märklin suite
-Copyright © 2006-2008 Mikkosoft Productions, Mikko Rasa
+Copyright © 2006-2009 Mikkosoft Productions, Mikko Rasa
 Distributed under the GPL
 */
 
@@ -10,6 +10,7 @@ Distributed under the GPL
 
 #include <msp/datafile/loader.h>
 #include "endpoint.h"
+#include "geometry.h"
 
 namespace Marklin {
 
@@ -29,7 +30,7 @@ struct TrackPart
 		void start(float, float, float);
 	};
 
-	float    x, y;
+	Point    pos;
 	float    dir;
 	float    length;
 	float    radius;
@@ -38,7 +39,8 @@ struct TrackPart
 
 	TrackPart();
 
-	void collect_endpoints(std::vector<Endpoint> &);
+	void collect_endpoints(std::vector<Endpoint> &) const;
+	Point get_point(float) const;
 };
 
 } // namespace Marklin
diff --git a/source/libmarklin/tracktype.cpp b/source/libmarklin/tracktype.cpp
index 0e84a31..2376e90 100644
--- a/source/libmarklin/tracktype.cpp
+++ b/source/libmarklin/tracktype.cpp
@@ -1,7 +1,7 @@
 /* $Id$
 
 This file is part of the MSP Märklin suite
-Copyright © 2006-2008 Mikkosoft Productions, Mikko Rasa
+Copyright © 2006-2009 Mikkosoft Productions, Mikko Rasa
 Distributed under the GPL
 */
 
@@ -56,8 +56,8 @@ void TrackType::collect_endpoints()
 		bool rm=false;
 		for(vector<Endpoint>::iterator j=i+1; j!=endpoints.end();)
 		{
-			float dx=i->x-j->x;
-			float dy=i->y-j->y;
+			float dx=i->pos.x-j->pos.x;
+			float dy=i->pos.y-j->pos.y;
 			if(dx*dx+dy*dy<0.0001)
 			{
 				float da=i->dir-j->dir;
diff --git a/source/libmarklin/trafficmanager.cpp b/source/libmarklin/trafficmanager.cpp
index 4cc186b..6de46a2 100644
--- a/source/libmarklin/trafficmanager.cpp
+++ b/source/libmarklin/trafficmanager.cpp
@@ -75,9 +75,13 @@ void TrafficManager::add_train(Train *t)
 void TrafficManager::tick()
 {
 	Time::TimeStamp t=Time::now();
+	Time::TimeDelta dt;
+	if(last_tick)
+		dt=t-last_tick;
+	last_tick=t;
 
 	for(list<Train *>::iterator i=trains.begin(); i!=trains.end(); ++i)
-		(*i)->tick(t);
+		(*i)->tick(t, dt);
 }
 
 void TrafficManager::turnout_route_changed(unsigned, Turnout *)
diff --git a/source/libmarklin/trafficmanager.h b/source/libmarklin/trafficmanager.h
index 293783f..ac3f395 100644
--- a/source/libmarklin/trafficmanager.h
+++ b/source/libmarklin/trafficmanager.h
@@ -1,7 +1,7 @@
 /* $Id$
 
 This file is part of the MSP Märklin suite
-Copyright © 2006-2008 Mikkosoft Productions, Mikko Rasa
+Copyright © 2006-2009 Mikkosoft Productions, Mikko Rasa
 Distributed under the GPL
 */
 
@@ -24,6 +24,7 @@ private:
 	Layout &layout;
 	std::list<Block *> blocks;
 	std::list<Train *> trains;
+	Msp::Time::TimeStamp last_tick;
 
 public:
 	sigc::signal<void, const Block &, const Train *> signal_block_reserved;
diff --git a/source/libmarklin/train.cpp b/source/libmarklin/train.cpp
index 4974318..f55d7ad 100644
--- a/source/libmarklin/train.cpp
+++ b/source/libmarklin/train.cpp
@@ -10,13 +10,14 @@ Distributed under the GPL
 #include <msp/time/units.h>
 #include <msp/time/utils.h>
 #include "control.h"
+#include "tracktype.h"
 #include "trafficmanager.h"
 #include "train.h"
 
+using namespace std;
 using namespace Msp;
 
 #include <iostream>
-using namespace std;
 
 namespace Marklin {
 
@@ -26,7 +27,11 @@ Train::Train(TrafficManager &tm, Locomotive &l):
 	target_speed(0),
 	status("Unplaced"),
 	travel_dist(0),
-	real_speed(0)
+	travel_speed(0),
+	pure_speed(false),
+	speed_scale(0.02),
+	speed_scale_weight(0),
+	cur_track(0)
 {
 	trfc_mgr.add_train(this);
 
@@ -44,7 +49,8 @@ void Train::set_name(const string &n)
 
 void Train::set_speed(unsigned speed)
 {
-	unsigned old_speed=target_speed;
+	if(!target_speed && speed)
+		travel_speed=static_cast<int>(round(speed*speed_scale*87*3.6/5))*5;
 
 	target_speed=speed;
 	if(!target_speed)
@@ -53,32 +59,10 @@ void Train::set_speed(unsigned speed)
 		for(list<BlockRef>::iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
 			i->block->reserve(0);
 		rsv_blocks.clear();
-		try_reserve=Time::TimeStamp();
-		loco.set_speed(0);
-		if(old_speed)
-			set_status("Stopped");
-	}
-	else
-	{
-		unsigned n=reserve_more();
-		if(n==0)
-		{
-			try_reserve=Time::now()+2*Time::sec;
-			set_status("Blocked");
-		}
-		else if(n==1)
-		{
-			loco.set_speed(3);
-			try_reserve=Time::now()+2*Time::sec;
-			set_status("Slow");
-		}
-		else
-		{
-			loco.set_speed(speed);
-			if(!old_speed)
-				set_status("Traveling --- kmh");
-		}
 	}
+
+	update_speed();
+	pure_speed=false;
 }
 
 void Train::place(Block *block, unsigned entry)
@@ -102,6 +86,7 @@ void Train::place(Block *block, unsigned entry)
 	}
 
 	cur_blocks.push_back(BlockRef(block, entry));
+	set_position(block->get_endpoints()[entry]);
 
 	set_status("Stopped");
 }
@@ -122,25 +107,35 @@ bool Train::free_block(Block *block)
 	return false;
 }
 
-void Train::tick(const Time::TimeStamp &t)
+void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt)
 {
 	if(try_reserve && t>try_reserve)
+		update_speed();
+
+	if(cur_track)
 	{
-		unsigned n=reserve_more();
-		if(n>=2)
-		{
-			loco.set_speed(target_speed);
-			set_status("Traveling --- kmh");
-			try_reserve=Time::TimeStamp();
-		}
-		else if(n==1)
+		unsigned route=0;
+		if(cur_track->get_turnout_id())
+			route=trfc_mgr.get_control().get_turnout(cur_track->get_turnout_id()).get_route();
+
+		offset+=speed_scale*loco.get_speed()*(dt/Time::sec);
+		if(offset>cur_track->get_type().get_route_length(route))
 		{
-			loco.set_speed(3);
-			set_status("Slow");
-			try_reserve=t+2*Time::sec;
+			int out=cur_track->traverse(cur_track_ep, route);
+			if(out>=0)
+			{
+				Track *next=cur_track->get_link(out);
+				if(next)
+					cur_track_ep=next->get_endpoint_by_link(*cur_track);
+				cur_track=next;
+				offset=0;
+			}
+			else
+				cur_track=0;
 		}
-		else
-			try_reserve=t+2*Time::sec;
+
+		if(cur_track)
+			pos=cur_track->get_point(cur_track_ep, route, offset);
 	}
 }
 
@@ -157,52 +152,53 @@ 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));
+			float travel_time_secs=(Time::now()-last_entry_time)/Time::sec;
+			travel_speed=static_cast<int>(round(travel_dist/travel_time_secs*87*3.6/5))*5;
+
+			if(pure_speed)
+			{
+				float weight=loco.get_speed()*travel_dist;
+				if(weight)
+				{
+					weight*=weight;
+					float scale=travel_dist/travel_time_secs/loco.get_speed();
+					cout<<"Updating speed_scale: "<<speed_scale<<'x'<<speed_scale_weight<<" + "<<scale<<'x'<<weight<<'\n';
+					speed_scale=(speed_scale*speed_scale_weight+scale*weight)/(speed_scale_weight+weight);
+					speed_scale_weight+=weight;
+					cout<<"  Result: "<<speed_scale<<'x'<<speed_scale_weight<<'\n';
+				}
+			}
 
 			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();
+			pure_speed=true;
 
 			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";
 		}
 
+		for(i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
+			if(i->block->get_sensor_id()==addr)
+				set_position(i->block->get_endpoints()[i->entry]);
+
 		if(target_speed)
-		{
-			unsigned n=reserve_more();
-			if(n==0)
-			{
-				loco.set_speed(0);
-				try_reserve=Time::now()+2*Time::sec;
-				set_status("Blocked");
-			}
-			else if(n==1)
-			{
-				loco.set_speed(3);
-				try_reserve=Time::now()+2*Time::sec;
-				set_status("Slow");
-			}
-		}
+			update_speed();
 	}
 	else
 	{
-		cout<<"Train "<<name<<" finding blocks to free\n";
 		for(list<BlockRef>::iterator i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
 			if(i->block->get_sensor_id()==addr)
 			{
 				++i;
 				for(list<BlockRef>::iterator j=cur_blocks.begin(); j!=i; ++j)
 					j->block->reserve(0);
-				cout<<"  "<<distance(cur_blocks.begin(), i)<<" blocks freed, ";
 				cur_blocks.erase(cur_blocks.begin(), i);
-				cout<<cur_blocks.size()<<" cur_blocks\n";
+				break;
 			}
 
 		if(target_speed)
@@ -220,8 +216,6 @@ unsigned Train::reserve_more()
 	if(!last)
 		return 0;
 
-	cout<<"Train "<<name<<" reserving more blocks\n";
-
 	unsigned nsens=0;
 	for(list<BlockRef>::const_iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
 		if(i->block->get_sensor_id())
@@ -261,15 +255,55 @@ unsigned Train::reserve_more()
 			last=0;
 	}
 
-	cout<<"  "<<rsv_blocks.size()<<" rsv_blocks\n";
-
 	return nsens;
 }
 
+void Train::update_speed()
+{
+	if(!target_speed)
+	{
+		loco.set_speed(0);
+		try_reserve=Time::TimeStamp();
+		set_status("Stopped");
+	}
+	else
+	{
+		unsigned n=reserve_more();
+		if(n==0)
+		{
+			loco.set_speed(0);
+			pure_speed=false;
+			try_reserve=Time::now()+2*Time::sec;
+			set_status("Blocked");
+		}
+		else if(n==1)
+		{
+			loco.set_speed(3);
+			pure_speed=false;
+			try_reserve=Time::now()+2*Time::sec;
+			set_status("Slow");
+		}
+		else
+		{
+			loco.set_speed(target_speed);
+			try_reserve=Time::TimeStamp();
+			set_status(format("Traveling %d kmh", travel_speed));
+		}
+	}
+}
+
 void Train::set_status(const string &s)
 {
 	status=s;
 	signal_status_changed.emit(s);
 }
 
+void Train::set_position(const Block::Endpoint &bep)
+{
+	cur_track=bep.track;
+	cur_track_ep=bep.track_ep;
+	offset=0;
+	pos=cur_track->get_endpoint_position(cur_track_ep);
+}
+
 } // namespace Marklin
diff --git a/source/libmarklin/train.h b/source/libmarklin/train.h
index 43ee84d..6a7d2de 100644
--- a/source/libmarklin/train.h
+++ b/source/libmarklin/train.h
@@ -38,9 +38,18 @@ private:
 	unsigned target_speed;
 	Msp::Time::TimeStamp try_reserve;
 	std::string status;
+
 	Msp::Time::TimeStamp last_entry_time;
 	float travel_dist;
-	unsigned real_speed;
+	unsigned travel_speed;
+	bool pure_speed;
+	float speed_scale;
+	float speed_scale_weight;
+
+	Track *cur_track;
+	unsigned cur_track_ep;
+	float offset;
+	Point pos;
 
 public:
 	sigc::signal<void, const std::string &> signal_name_changed;
@@ -53,13 +62,16 @@ public:
 	const std::string &get_name() const { return name; }
 	Locomotive &get_locomotive() const { return loco; }
 	const std::string &get_status() const { return status; }
+	const Point &get_position() const { return pos; }
 	void place(Block *, unsigned);
 	bool free_block(Block *);
-	void tick(const Msp::Time::TimeStamp &);
+	void tick(const Msp::Time::TimeStamp &, const Msp::Time::TimeDelta &);
 private:
 	void sensor_event(bool, Sensor *);
 	unsigned reserve_more();
+	void update_speed();
 	void set_status(const std::string &);
+	void set_position(const Block::Endpoint &);
 };
 
 } // namespace Marklin
-- 
2.43.0