-/* $Id$
-
-This file is part of R²C²
-Copyright © 2006-2010 Mikkosoft Productions, Mikko Rasa
-Distributed under the GPL
-*/
-
+#include <algorithm>
#include <cmath>
-#include <msp/strings/formatter.h>
+#include <msp/core/maputils.h>
+#include <msp/strings/format.h>
#include <msp/time/units.h>
#include <msp/time/utils.h>
#include "aicontrol.h"
#include "layout.h"
#include "route.h"
#include "simplecontroller.h"
+#include "speedquantizer.h"
#include "timetable.h"
#include "trackiter.h"
#include "tracktype.h"
pending_block(0),
reserving(false),
advancing(false),
- controller(new AIControl(*this, new SimpleController)),
- timetable(0),
+ controller(new SimpleController),
active(false),
current_speed_step(0),
speed_changing(false),
reverse(false),
functions(0),
end_of_route(false),
- status("Unplaced"),
travel_dist(0),
pure_speed(false),
- real_speed(layout.get_driver().get_protocol_speed_steps(protocol)+1),
+ speed_quantizer(0),
accurate_position(false),
overshoot_dist(false)
{
if(!loco_type.is_locomotive())
- throw InvalidParameterValue("Initial vehicle must be a locomotive");
+ throw invalid_argument("Train::Train");
+
+ unsigned speed_steps = layout.get_driver().get_protocol_speed_steps(protocol);
+ if(speed_steps)
+ speed_quantizer = new SpeedQuantizer(speed_steps);
vehicles.push_back(new Vehicle(layout, loco_type));
layout.get_driver().signal_loco_function.connect(sigc::mem_fun(this, &Train::loco_func_event));
layout.signal_block_reserved.connect(sigc::mem_fun(this, &Train::block_reserved));
- layout.get_driver().signal_sensor.connect(sigc::mem_fun(this, &Train::sensor_event));
+ layout.signal_block_state_changed.connect(sigc::mem_fun(this, &Train::block_state_changed));
layout.get_driver().signal_halt.connect(sigc::mem_fun(this, &Train::halt_event));
Train::~Train()
{
delete controller;
- delete timetable;
for(vector<Vehicle *>::iterator i=vehicles.begin(); i!=vehicles.end(); ++i)
delete *i;
layout.remove_train(*this);
void Train::remove_vehicle(unsigned i)
{
if(i>=vehicles.size())
- throw InvalidParameterValue("Vehicle index out of range");
+ throw out_of_range("Train::remove_vehicle");
if(i==0)
- throw InvalidParameterValue("Can't remove the locomotive");
+ throw logic_error("can't remove locomotive");
delete vehicles[i];
vehicles.erase(vehicles.begin()+i);
if(i<vehicles.size())
Vehicle &Train::get_vehicle(unsigned i)
{
if(i>=vehicles.size())
- throw InvalidParameterValue("Vehicle index out of range");
+ throw out_of_range("Train::get_vehicle");
return *vehicles[i];
}
const Vehicle &Train::get_vehicle(unsigned i) const
{
if(i>=vehicles.size())
- throw InvalidParameterValue("Vehicle index out of range");
+ throw out_of_range("Train::get_vehicle");
return *vehicles[i];
}
if(a==active)
return;
if(!a && controller->get_speed())
- throw InvalidState("Can't deactivate while moving");
+ throw logic_error("moving");
active = a;
if(active)
reserve_more();
}
else
- {
stop_timeout = Time::now()+2*Time::sec;
- set_status("Stopped");
- }
}
void Train::set_function(unsigned func, bool state)
{
if(!loco_type.get_functions().count(func))
- throw InvalidParameterValue("Invalid function");
+ throw invalid_argument("Train::set_function");
layout.get_driver().set_loco_function(address, func, state);
}
return controller->get_speed();
}
+float Train::get_quantized_speed() const
+{
+ if(speed_quantizer)
+ return speed_quantizer->quantize_speed(controller->get_speed());
+ else
+ return controller->get_speed();
+}
+
bool Train::get_function(unsigned func) const
{
return (functions>>func)&1;
}
-void Train::set_timetable(Timetable *tt)
+void Train::add_ai(TrainAI &ai)
+{
+ ais.push_back(&ai);
+ ai.signal_event.connect(sigc::bind<0>(signal_ai_event, sigc::ref(ai)));
+}
+
+void Train::remove_ai(TrainAI &ai)
{
- delete timetable;
- timetable = tt;
+ list<TrainAI *>::iterator i = find(ais.begin(), ais.end(), &ai);
+ if(i!=ais.end())
+ ais.erase(i);
+}
+
+TrainAI *Train::get_tagged_ai(const string &tag) const
+{
+ for(list<TrainAI *>::const_iterator i=ais.begin(); i!=ais.end(); ++i)
+ if((*i)->get_tag()==tag)
+ return *i;
+
+ return 0;
+}
+
+void Train::ai_message(const TrainAI::Message &msg)
+{
+ for(list<TrainAI *>::iterator i=ais.begin(); i!=ais.end(); ++i)
+ (*i)->message(msg);
}
bool Train::set_route(const Route *r)
bool Train::divert(Track &from)
{
if(!from.get_turnout_id())
- throw InvalidParameterValue("Can't divert from a non-turnout");
+ throw invalid_argument("Train::divert");
if(routes.empty())
return false;
if(end!=routes.end())
break;
else if(!diversion->has_track(*track))
- throw LogicError("Pathfinder returned a bad route");
+ throw logic_error("bad diversion");
track = track.next(diversion->get_path(*track));
}
void Train::place(Block &block, unsigned entry)
{
if(controller->get_speed())
- throw InvalidState("Must be stopped before placing");
+ throw logic_error("moving");
release_blocks();
set_active(false);
accurate_position = false;
+ blocks.push_back(BlockIter(&block, entry));
if(!block.reserve(this))
{
- set_status("Unplaced");
+ blocks.pop_back();
return;
}
- blocks.push_back(BlockIter(&block, entry));
if(reverse)
{
TrackIter track = BlockIter(&block, entry).reverse().track_iter();
void Train::unplace()
{
if(controller->get_speed())
- throw InvalidState("Must be stopped before unplacing");
+ throw logic_error("moving");
release_blocks();
for(vector<Vehicle *>::iterator i=vehicles.begin(); i!=vehicles.end(); ++i)
(*i)->unplace();
-
- set_status("Unplaced");
}
bool Train::free_block(Block &block)
}
}
-int Train::get_entry_to_block(Block &block) const
+int Train::get_entry_to_block(const Block &block) const
{
for(BlockList::const_iterator i=blocks.begin(); i!=blocks.end(); ++i)
if(i->block()==&block)
float margin = 0;
TrackIter next = blocks.back().next().track_iter();
- if(next->get_type().is_turnout())
+ if(next && next->get_type().is_turnout())
margin = 15*layout.get_catalogue().get_scale();
return max(get_reserved_distance_until(0, false)-margin, 0.0f);
Driver &driver = layout.get_driver();
- if(timetable)
- timetable->tick(t);
+ for(list<TrainAI *>::iterator i=ais.begin(); i!=ais.end(); ++i)
+ (*i)->tick(t, dt);
controller->tick(dt);
float speed = controller->get_speed();
- unsigned speed_step = find_speed_step(speed);
+ bool moving = speed>0;
if(controller->get_reverse()!=reverse)
{
reverse = controller->get_reverse();
- driver.set_loco_reverse(address, reverse);
+ bool r = reverse;
+ if(loco_type.get_swap_direction())
+ r = !r;
+ driver.set_loco_reverse(address, r);
release_blocks(cur_blocks_end, blocks.end());
reverse_blocks(blocks);
reserve_more();
}
- if(speed_step!=current_speed_step && !speed_changing && !driver.is_halted() && driver.get_power())
+
+ if(speed_quantizer)
{
- speed_changing = true;
- driver.set_loco_speed(address, speed_step);
+ unsigned speed_step = speed_quantizer->find_speed_step(speed);
+ if(speed_step!=current_speed_step && !speed_changing && !driver.is_halted() && driver.get_power())
+ {
+ speed_changing = true;
+ driver.set_loco_speed(address, speed_step);
- pure_speed = false;
+ pure_speed = false;
+ }
- if(speed_step)
- set_status(format("Traveling %d kmh", get_travel_speed()));
- else
- set_status("Waiting");
+ speed = speed_quantizer->get_speed(current_speed_step);
}
- if(speed)
+ if(moving)
{
if(!active)
set_active(true);
for(BlockList::const_iterator i=blocks.begin(); (!ok && i!=cur_blocks_end); ++i)
ok = (*i)->has_track(*track);
- float d;
- if(real_speed.size()>1)
- d = get_real_speed(current_speed_step)*(dt/Time::sec);
- else
- d = speed*(dt/Time::sec);
+ float d = speed*(dt/Time::sec);
if(ok)
{
SetFlag setf(advancing);
if(dist>10*layout.get_catalogue().get_scale())
{
- blocks.front()->reserve(0);
+ Block &block = *blocks.front();
blocks.pop_front();
+ block.reserve(0);
}
}
}
if(i!=vehicles.begin())
st.push_back((DataFile::Statement("vehicle"), (*i)->get_type().get_article_number()));
- for(unsigned i=0; i<real_speed.size(); ++i)
- if(real_speed[i].weight)
- st.push_back((DataFile::Statement("real_speed"), i, real_speed[i].speed, real_speed[i].weight));
+ if(speed_quantizer)
+ {
+ DataFile::Statement ss("quantized_speed");
+ speed_quantizer->save(ss.sub);
+ st.push_back(ss);
+ }
if(!blocks.empty() && cur_blocks_end!=blocks.begin())
{
st.push_back((DataFile::Statement("route"), i->route->get_name()));
}
- if(timetable)
- {
- DataFile::Statement ss("timetable");
- timetable->save(ss.sub);
- st.push_back(ss);
- }
+ // XXX Need more generic way of saving AI state
+ for(list<TrainAI *>::const_iterator i=ais.begin(); i!=ais.end(); ++i)
+ if(Timetable *timetable = dynamic_cast<Timetable *>(*i))
+ {
+ DataFile::Statement ss("timetable");
+ timetable->save(ss.sub);
+ st.push_back(ss);
+ }
}
void Train::control_changed(const Controller::Control &ctrl)
if(addr==address)
{
current_speed_step = speed;
- if(rev!=reverse)
- layout.get_driver().set_loco_reverse(address, reverse);
+ bool r = reverse;
+ if(loco_type.get_swap_direction())
+ r = !r;
+ if(rev!=r)
+ layout.get_driver().set_loco_reverse(address, r);
speed_changing = false;
pure_speed = false;
}
}
}
-void Train::sensor_event(unsigned addr, bool state)
+void Train::block_state_changed(Block &block, Block::State state)
{
- if(state)
+ if(state==Block::MAYBE_ACTIVE)
{
// Find the first sensor block from our reserved blocks that isn't this sensor
BlockList::iterator end;
for(end=cur_blocks_end; end!=blocks.end(); ++end)
if((*end)->get_sensor_id())
{
- if((*end)->get_sensor_id()!=addr)
+ if(&**end!=&block)
{
if(result==0)
result = 2;
// Compute speed and update related state
float travel_time_secs = (Time::now()-last_entry_time)/Time::sec;
- if(pure_speed)
- {
- if(current_speed_step>0)
- {
- RealSpeed &rs = real_speed[current_speed_step];
- rs.add(travel_dist/travel_time_secs, travel_time_secs);
- }
- set_status(format("Traveling %d kmh", get_travel_speed()));
- }
+ if(pure_speed && speed_quantizer && current_speed_step>0 && travel_time_secs>=2)
+ speed_quantizer->learn(current_speed_step, travel_dist/travel_time_secs, travel_time_secs);
travel_dist = 0;
for(BlockList::iterator j=cur_blocks_end; j!=end; ++j)
{
travel_dist += (*j)->get_path_length(j->entry());
- if((*j)->get_sensor_id()==addr && !advancing)
+ if(&**j==&block && !advancing)
{
TrackIter track = j->track_iter();
if(reverse)
else if(result==3)
layout.emergency("Sensor for "+name+" triggered out of order");
}
- else
+ else if(state==Block::INACTIVE)
{
const Vehicle &veh = *(reverse ? vehicles.front() : vehicles.back());
}
}
+ blocks.push_back(block);
bool reserved = block->reserve(this);
if(!reserved)
{
+ blocks.pop_back();
/* We've found another train. If it wants to exit the block from the
same endpoint we're trying to enter from or the other way around,
treat it as coming towards us. Otherwise treat it as going in the
Train *other_train = block->get_train();
int other_entry = other_train->get_entry_to_block(*block);
if(other_entry<0)
- throw LogicError("Block reservation inconsistency");
+ throw logic_error("block reservation inconsistency");
unsigned exit = block.reverse().entry();
unsigned other_exit = BlockIter(block.block(), other_entry).reverse().entry();
/* Ask a lesser priority train going to the same direction to free
the block for us */
if(other_train->free_block(*block))
- reserved = block->reserve(this);
+ {
+ blocks.push_back(block);
+ if(!(reserved = block->reserve(this)))
+ blocks.pop_back();
+ }
}
else if(other_train!=yielding_to && (other_prio<priority || (other_prio==priority && entry_conflict)))
{
if(!contested_blocks.empty() && contested_blocks.front()==block)
contested_blocks.pop_front();
- blocks.push_back(block);
-
if(cur_blocks_end==blocks.end())
--cur_blocks_end;
if(clear_blocks_end==blocks.end())
if(i==clear_blocks_end)
++clear_blocks_end;
+ if(i==cur_blocks_end && !(*i)->get_sensor_id())
+ ++cur_blocks_end;
}
}
return result;
}
-float Train::get_real_speed(unsigned i) const
-{
- if(i==0)
- return 0;
- if(real_speed[i].weight)
- return real_speed[i].speed;
-
- unsigned low;
- unsigned high;
- for(low=i; low>0; --low)
- if(real_speed[low].weight)
- break;
- for(high=i; high+1<real_speed.size(); ++high)
- if(real_speed[high].weight)
- break;
-
- if(real_speed[high].weight)
- {
- if(real_speed[low].weight)
- {
- float f = float(i-low)/(high-low);
- return real_speed[low].speed*(1-f)+real_speed[high].speed*f;
- }
- else
- return real_speed[high].speed*float(i)/high;
- }
- else if(real_speed[low].weight)
- return real_speed[low].speed*float(i)/low;
- else
- return 0;
-}
-
-unsigned Train::find_speed_step(float real) const
-{
- if(real_speed.size()<=1)
- return 0;
- if(real<=real_speed[1].speed*0.5)
- return 0;
-
- unsigned low = 0;
- unsigned high = 0;
- unsigned last = 0;
- for(unsigned i=0; (!high && i<real_speed.size()); ++i)
- if(real_speed[i].weight)
- {
- last = i;
- if(real_speed[i].speed>=real)
- high = i;
- else if(real_speed[i].speed>real_speed[low].speed)
- low = i;
- }
- if(!high)
- {
- unsigned limit = real_speed.size()/5;
- if(!low)
- {
- if(real)
- return limit;
- else
- return 0;
- }
- return min(min(static_cast<unsigned>(low*real/real_speed[low].speed), real_speed.size()-1), last+limit);
- }
-
- float f = (real-real_speed[low].speed)/(real_speed[high].speed-real_speed[low].speed);
- return static_cast<unsigned>(low*(1-f)+high*f+0.5);
-}
-
-float Train::get_travel_speed() const
-{
- float speed = get_real_speed(current_speed_step);
- float scale = layout.get_catalogue().get_scale();
- return static_cast<int>(round(speed/scale*3.6/5))*5;
-}
-
-void Train::set_status(const string &s)
-{
- status = s;
- signal_status_changed.emit(s);
-}
-
void Train::release_blocks()
{
release_blocks(blocks.begin(), blocks.end());
{ }
-Train::RealSpeed::RealSpeed():
- speed(0),
- weight(0)
-{ }
-
-void Train::RealSpeed::add(float s, float w)
-{
- speed = (speed*weight+s*w)/(weight+w);
- weight = min(weight+w, 300.0f);
-}
-
-
Train::Loader::Loader(Train &t):
- DataFile::BasicLoader<Train>(t),
+ DataFile::ObjectLoader<Train>(t),
prev_block(0),
blocks_valid(true)
{
add("block_hint", &Loader::block_hint);
add("name", &Loader::name);
add("priority", &Train::priority);
- add("real_speed", &Loader::real_speed);
+ add("quantized_speed", &Loader::quantized_speed);
add("route", &Loader::route);
add("timetable", &Loader::timetable);
add("vehicle", &Loader::vehicle);
TrackIter track = obj.blocks.front().track_iter();
float offset = 2*obj.layout.get_catalogue().get_scale();
obj.vehicles.back()->place(*track, track.entry(), offset, Vehicle::BACK_BUFFER);
-
- obj.set_status("Stopped");
}
}
{
blk = &obj.layout.get_block(id);
}
- catch(const KeyError &)
+ catch(const key_error &)
{
blocks_valid = false;
return;
if(entry<0)
entry = 0;
- blk->reserve(&obj);
obj.blocks.push_back(BlockIter(blk, entry));
+ blk->reserve(&obj);
if(blk->get_sensor_id())
obj.layout.get_driver().set_sensor(blk->get_sensor_id(), true);
{
prev_block = &obj.layout.get_block(id);
}
- catch(const KeyError &)
+ catch(const key_error &)
{
blocks_valid = false;
}
obj.set_name(n);
}
-void Train::Loader::real_speed(unsigned i, float speed, float weight)
+void Train::Loader::quantized_speed()
{
- if(i>=obj.real_speed.size())
- return;
- obj.real_speed[i].speed = speed;
- obj.real_speed[i].weight = weight;
+ if(obj.speed_quantizer)
+ load_sub(*obj.speed_quantizer);
}
void Train::Loader::route(const string &n)
void Train::Loader::timetable()
{
- if(obj.timetable)
- throw InvalidState("A timetable has already been loaded");
-
- obj.timetable = new Timetable(obj);
- load_sub(*obj.timetable);
+ Timetable *ttbl = new Timetable(obj);
+ load_sub(*ttbl);
}
void Train::Loader::vehicle(ArticleNumber art_nr)