X-Git-Url: http://git.tdb.fi/?a=blobdiff_plain;ds=sidebyside;f=source%2Flibmarklin%2Ftrain.cpp;h=42d3e01e1f87b1675d9edd32fd7439533ced4af4;hb=010d8321e982d1684fcbff5bf6fc2bdec7cb7bae;hp=4974318fba6e78687b842d5e0ea5baadc2b62378;hpb=80e10207bee4d774f3015a5e946d463d2c828029;p=r2c2.git

diff --git a/source/libmarklin/train.cpp b/source/libmarklin/train.cpp
index 4974318..42d3e01 100644
--- a/source/libmarklin/train.cpp
+++ b/source/libmarklin/train.cpp
@@ -10,13 +10,15 @@ Distributed under the GPL
 #include <msp/time/units.h>
 #include <msp/time/utils.h>
 #include "control.h"
+#include "except.h"
+#include "tracktype.h"
 #include "trafficmanager.h"
 #include "train.h"
 
+using namespace std;
 using namespace Msp;
 
 #include <iostream>
-using namespace std;
 
 namespace Marklin {
 
@@ -26,59 +28,62 @@ 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);
 
-	const map<unsigned, Sensor *> &sensors=trfc_mgr.get_control().get_sensors();
+	loco.signal_reverse_changed.connect(sigc::mem_fun(this, &Train::locomotive_reverse_changed));
+
+	const map<unsigned, Sensor *> &sensors = trfc_mgr.get_control().get_sensors();
 	for(map<unsigned, Sensor *>::const_iterator i=sensors.begin(); i!=sensors.end(); ++i)
 		i->second->signal_state_changed.connect(sigc::bind(sigc::mem_fun(this, &Train::sensor_event), i->second));
+
+	const map<unsigned, Turnout *> &turnouts = trfc_mgr.get_control().get_turnouts();
+	for(map<unsigned, Turnout *>::const_iterator i=turnouts.begin(); i!=turnouts.end(); ++i)
+	{
+		i->second->signal_route_changing.connect(sigc::bind(sigc::mem_fun(this, &Train::turnout_route_changing), i->second));
+		i->second->signal_route_changed.connect(sigc::bind(sigc::mem_fun(this, &Train::turnout_route_changed), i->second));
+	}
 }
 
 void Train::set_name(const string &n)
 {
-	name=n;
+	name = n;
 
 	signal_name_changed.emit(name);
 }
 
 void Train::set_speed(unsigned speed)
 {
-	unsigned old_speed=target_speed;
+	if(speed==target_speed)
+		return;
+	if(!target_speed && speed)
+		travel_speed = static_cast<int>(round(speed*speed_scale*87*3.6/5))*5;
 
-	target_speed=speed;
+	target_speed = speed;
 	if(!target_speed)
 	{
 		// XXX We might roll onto a new sensor and get confused - should delay freeing blocks a bit
 		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");
-		}
-	}
+		reserve_more();
+
+	signal_target_speed_changed.emit(target_speed);
+
+	update_speed();
+	pure_speed = false;
+}
+
+void Train::set_reverse(bool rev)
+{
+	loco.set_reverse(rev);
 }
 
 void Train::place(Block *block, unsigned entry)
@@ -86,13 +91,13 @@ void Train::place(Block *block, unsigned entry)
 	for(list<BlockRef>::iterator i=rsv_blocks.begin(); i!=rsv_blocks.end();)
 	{
 		i->block->reserve(0);
-		i=rsv_blocks.erase(i);
+		i = rsv_blocks.erase(i);
 	}
 
 	for(list<BlockRef>::iterator i=cur_blocks.begin(); i!=cur_blocks.end();)
 	{
 		i->block->reserve(0);
-		i=cur_blocks.erase(i);
+		i = cur_blocks.erase(i);
 	}
 
 	if(!block->reserve(this))
@@ -102,51 +107,91 @@ void Train::place(Block *block, unsigned entry)
 	}
 
 	cur_blocks.push_back(BlockRef(block, entry));
+	set_position(block->get_endpoints()[entry]);
 
 	set_status("Stopped");
 }
 
 bool Train::free_block(Block *block)
 {
+	unsigned nsens = 0;
 	for(list<BlockRef>::iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
+	{
 		if(i->block==block)
 		{
+			if(nsens<1)
+				return false;
 			while(i!=rsv_blocks.end())
 			{
 				i->block->reserve(0);
-				i=rsv_blocks.erase(i);
+				i = rsv_blocks.erase(i);
 			}
+			update_speed();
 			return true;
 		}
+		else if(i->block->get_sensor_id())
+			++nsens;
+	}
 
 	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)
 	{
-		unsigned n=reserve_more();
-		if(n>=2)
-		{
-			loco.set_speed(target_speed);
-			set_status("Traveling --- kmh");
-			try_reserve=Time::TimeStamp();
-		}
-		else if(n==1)
+		reserve_more();
+		update_speed();
+	}
+
+	if(cur_track)
+	{
+		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);
 	}
 }
 
+void Train::save(list<DataFile::Statement> &st) const
+{
+	st.push_back((DataFile::Statement("name"), name));
+	st.push_back((DataFile::Statement("speed_scale"), speed_scale, speed_scale_weight));
+}
+
+void Train::locomotive_reverse_changed(bool)
+{
+	for(list<BlockRef>::iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
+		i->block->reserve(0);
+	rsv_blocks.clear();
+	cur_blocks.reverse();
+	for(list<BlockRef>::iterator i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
+		i->entry = i->block->traverse(i->entry);
+	reserve_more();
+	update_speed();
+}
+
 void Train::sensor_event(bool state, Sensor *sensor)
 {
-	unsigned addr=sensor->get_address();
+	unsigned addr = sensor->get_address();
 
 	if(state)
 	{
@@ -157,52 +202,56 @@ 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;
+			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;
+				travel_dist += block_len;
 			}
-			last_entry_time=Time::now();
+			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");
-			}
+			reserve_more();
+			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)
@@ -210,38 +259,74 @@ void Train::sensor_event(bool state, Sensor *sensor)
 	}
 }
 
+void Train::turnout_route_changing(unsigned, Turnout *turnout)
+{
+	unsigned tid = turnout->get_address();
+	for(list<BlockRef>::const_iterator i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
+		if(i->block->get_turnout_id()==tid)
+			throw TurnoutBusy(this);
+	
+	unsigned nsens = 0;
+	for(list<BlockRef>::const_iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
+	{
+		if(i->block->get_turnout_id()==tid)
+		{
+			if(nsens<1)
+				throw TurnoutBusy(this);
+			break;
+		}
+		else if(i->block->get_sensor_id())
+			++nsens;
+	}
+}
+
+void Train::turnout_route_changed(unsigned, Turnout *turnout)
+{
+	unsigned tid = turnout->get_address();
+	for(list<BlockRef>::iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
+		if(i->block->get_turnout_id()==tid)
+		{
+			while(i!=rsv_blocks.end())
+			{
+				i->block->reserve(0);
+				i = rsv_blocks.erase(i);
+			}
+			reserve_more();
+			update_speed();
+			return;
+		}
+}
+
 unsigned Train::reserve_more()
 {
-	BlockRef *last=0;
+	BlockRef *last = 0;
 	if(!rsv_blocks.empty())
-		last=&rsv_blocks.back();
+		last = &rsv_blocks.back();
 	else if(!cur_blocks.empty())
-		last=&cur_blocks.back();
+		last = &cur_blocks.back();
 	if(!last)
 		return 0;
 
-	cout<<"Train "<<name<<" reserving more blocks\n";
-
-	unsigned nsens=0;
+	unsigned nsens = 0;
 	for(list<BlockRef>::const_iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
 		if(i->block->get_sensor_id())
 			++nsens;
 
-	bool result=false;
+	bool result = false;
 	while(nsens<2)
 	{
-		int exit=last->block->traverse(last->entry);
+		int exit = last->block->traverse(last->entry);
 		if(exit>=0) 
 		{
-			Block *link=last->block->get_link(exit);
+			Block *link = last->block->get_link(exit);
 			if(link && link->reserve(this))
 			{
 				rsv_blocks.push_back(BlockRef(link, link->get_endpoint_by_link(*last->block)));
-				last=&rsv_blocks.back();
+				last = &rsv_blocks.back();
 				if(last->block->get_sensor_id())
 				{
 					++nsens;
-					result=true;
+					result = true;
 				}
 			}
 			else
@@ -256,20 +341,72 @@ unsigned Train::reserve_more()
 		last->block->reserve(0);
 		rsv_blocks.erase(--rsv_blocks.end());
 		if(!rsv_blocks.empty())
-			last=&rsv_blocks.back();
+			last = &rsv_blocks.back();
 		else
-			last=0;
+			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 nsens = 0;
+		for(list<BlockRef>::const_iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
+			if(i->block->get_sensor_id())
+				++nsens;
+
+		if(nsens==0)
+		{
+			loco.set_speed(0);
+			pure_speed = false;
+			try_reserve = Time::now()+2*Time::sec;
+			set_status("Blocked");
+		}
+		else if(nsens==1 && target_speed>3)
+		{
+			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;
+	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);
+}
+
+
+Train::Loader::Loader(Train &t):
+	DataFile::BasicLoader<Train>(t)
+{
+	add("name",        &Train::name);
+	add("speed_scale", &Train::speed_scale, &Train::speed_scale_weight);
+}
+
 } // namespace Marklin