source/libr2c2/train.cpp

   1 /* $Id$
   2
   3 This file is part of R²C²
   4 Copyright © 2006-2011  Mikkosoft Productions, Mikko Rasa
   5 Distributed under the GPL
   6 */
   7
   8 #include <algorithm>
   9 #include <cmath>
  10 #include <msp/strings/formatter.h>
  11 #include <msp/time/units.h>
  12 #include <msp/time/utils.h>
  13 #include "aicontrol.h"
  14 #include "catalogue.h"
  15 #include "driver.h"
  16 #include "layout.h"
  17 #include "route.h"
  18 #include "simplecontroller.h"
  19 #include "speedquantizer.h"
  20 #include "timetable.h"
  21 #include "trackiter.h"
  22 #include "tracktype.h"
  23 #include "train.h"
  24 #include "vehicle.h"
  25 #include "vehicletype.h"
  26 #include "zone.h"
  27
  28 using namespace std;
  29 using namespace Msp;
  30
  31 namespace {
  32
  33 struct SetFlag
  34 {
  35         bool &flag;
  36
  37         SetFlag(bool &f): flag(f) { flag = true; }
  38         ~SetFlag() { flag = false; }
  39 };
  40
  41 }
  42
  43
  44 namespace R2C2 {
  45
  46 Train::Train(Layout &l, const VehicleType &t, unsigned a, const string &p):
  47         layout(l),
  48         loco_type(t),
  49         address(a),
  50         protocol(p),
  51         priority(0),
  52         yielding_to(0),
  53         preceding_train(0),
  54         cur_blocks_end(blocks.end()),
  55         clear_blocks_end(blocks.end()),
  56         pending_block(0),
  57         reserving(false),
  58         advancing(false),
  59         controller(new SimpleController),
  60         active(false),
  61         current_speed_step(0),
  62         speed_changing(false),
  63         reverse(false),
  64         functions(0),
  65         end_of_route(false),
  66         travel_dist(0),
  67         pure_speed(false),
  68         speed_quantizer(0),
  69         accurate_position(false),
  70         overshoot_dist(false)
  71 {
  72         if(!loco_type.is_locomotive())
  73                 throw InvalidParameterValue("Initial vehicle must be a locomotive");
  74
  75         unsigned speed_steps = layout.get_driver().get_protocol_speed_steps(protocol);
  76         if(speed_steps)
  77                 speed_quantizer = new SpeedQuantizer(speed_steps);
  78
  79         vehicles.push_back(new Vehicle(layout, loco_type));
  80
  81         layout.add_train(*this);
  82
  83         layout.get_driver().add_loco(address, protocol, loco_type);
  84         layout.get_driver().signal_loco_speed.connect(sigc::mem_fun(this, &Train::loco_speed_event));
  85         layout.get_driver().signal_loco_function.connect(sigc::mem_fun(this, &Train::loco_func_event));
  86
  87         layout.signal_block_reserved.connect(sigc::mem_fun(this, &Train::block_reserved));
  88         layout.signal_block_state_changed.connect(sigc::mem_fun(this, &Train::block_state_changed));
  89
  90         layout.get_driver().signal_halt.connect(sigc::mem_fun(this, &Train::halt_event));
  91
  92         const set<Track *> &tracks = layout.get_tracks();
  93         for(set<Track *>::const_iterator i=tracks.begin(); i!=tracks.end(); ++i)
  94                 if((*i)->get_turnout_id())
  95                         (*i)->signal_path_changed.connect(sigc::hide(sigc::bind(sigc::mem_fun(this, &Train::turnout_path_changed), sigc::ref(**i))));
  96
  97         controller->signal_control_changed.connect(sigc::mem_fun(this, &Train::control_changed));
  98 }
  99
 100 Train::~Train()
 101 {
 102         delete controller;
 103         for(vector<Vehicle *>::iterator i=vehicles.begin(); i!=vehicles.end(); ++i)
 104                 delete *i;
 105         layout.remove_train(*this);
 106 }
 107
 108 void Train::set_name(const string &n)
 109 {
 110         name = n;
 111
 112         signal_name_changed.emit(name);
 113 }
 114
 115 void Train::set_priority(int p)
 116 {
 117         priority = p;
 118 }
 119
 120 void Train::yield_to(const Train &t)
 121 {
 122         yielding_to = &t;
 123 }
 124
 125 void Train::add_vehicle(const VehicleType &vt)
 126 {
 127         Vehicle *veh = new Vehicle(layout, vt);
 128         vehicles.back()->attach_back(*veh);
 129         vehicles.push_back(veh);
 130 }
 131
 132 void Train::remove_vehicle(unsigned i)
 133 {
 134         if(i>=vehicles.size())
 135                 throw InvalidParameterValue("Vehicle index out of range");
 136         if(i==0)
 137                 throw InvalidParameterValue("Can't remove the locomotive");
 138         delete vehicles[i];
 139         vehicles.erase(vehicles.begin()+i);
 140         if(i<vehicles.size())
 141                 vehicles[i-1]->attach_back(*vehicles[i]);
 142 }
 143
 144 unsigned Train::get_n_vehicles() const
 145 {
 146         return vehicles.size();
 147 }
 148
 149 Vehicle &Train::get_vehicle(unsigned i)
 150 {
 151         if(i>=vehicles.size())
 152                 throw InvalidParameterValue("Vehicle index out of range");
 153         return *vehicles[i];
 154 }
 155
 156 const Vehicle &Train::get_vehicle(unsigned i) const
 157 {
 158         if(i>=vehicles.size())
 159                 throw InvalidParameterValue("Vehicle index out of range");
 160         return *vehicles[i];
 161 }
 162
 163 void Train::set_control(const string &n, float v)
 164 {
 165         controller->set_control(n, v);
 166 }
 167
 168 void Train::set_active(bool a)
 169 {
 170         if(a==active)
 171                 return;
 172         if(!a && controller->get_speed())
 173                 throw InvalidState("Can't deactivate while moving");
 174
 175         active = a;
 176         if(active)
 177         {
 178                 stop_timeout = Time::TimeStamp();
 179                 reserve_more();
 180         }
 181         else
 182                 stop_timeout = Time::now()+2*Time::sec;
 183 }
 184
 185 void Train::set_function(unsigned func, bool state)
 186 {
 187         if(!loco_type.get_functions().count(func))
 188                 throw InvalidParameterValue("Invalid function");
 189         layout.get_driver().set_loco_function(address, func, state);
 190 }
 191
 192 float Train::get_control(const string &ctrl) const
 193 {
 194         return controller->get_control(ctrl).value;
 195 }
 196
 197 float Train::get_speed() const
 198 {
 199         return controller->get_speed();
 200 }
 201
 202 float Train::get_quantized_speed() const
 203 {
 204         if(speed_quantizer)
 205                 return speed_quantizer->quantize_speed(controller->get_speed());
 206         else
 207                 return controller->get_speed();
 208 }
 209
 210 bool Train::get_function(unsigned func) const
 211 {
 212         return (functions>>func)&1;
 213 }
 214
 215 void Train::add_ai(TrainAI &ai)
 216 {
 217         ais.push_back(&ai);
 218         ai.signal_event.connect(sigc::bind<0>(signal_ai_event, sigc::ref(ai)));
 219 }
 220
 221 void Train::remove_ai(TrainAI &ai)
 222 {
 223         list<TrainAI *>::iterator i = find(ais.begin(), ais.end(), &ai);
 224         if(i!=ais.end())
 225                 ais.erase(i);
 226 }
 227
 228 TrainAI *Train::get_tagged_ai(const string &tag) const
 229 {
 230         for(list<TrainAI *>::const_iterator i=ais.begin(); i!=ais.end(); ++i)
 231                 if((*i)->get_tag()==tag)
 232                         return *i;
 233
 234         return 0;
 235 }
 236
 237 void Train::ai_message(const TrainAI::Message &msg)
 238 {
 239         for(list<TrainAI *>::iterator i=ais.begin(); i!=ais.end(); ++i)
 240                 (*i)->message(msg);
 241 }
 242
 243 bool Train::set_route(const Route *r)
 244 {
 245         free_noncritical_blocks();
 246
 247         Route *lead = 0;
 248         if(r && !blocks.empty())
 249         {
 250                 TrackIter first = blocks.front().track_iter();
 251                 TrackIter next = blocks.back().next().track_iter();
 252                 if(!r->has_track(*next))
 253                 {
 254                         lead = Route::find(next, *r);
 255                         if(!lead)
 256                                 return false;
 257                         create_lead_route(lead, lead);
 258                         routes.push_front(lead);
 259                 }
 260                 else if(!r->has_track(*first))
 261                         lead = create_lead_route(0, r);
 262         }
 263
 264         routes.clear();
 265         if(lead)
 266                 routes.push_back(lead);
 267         if(r)
 268                 routes.push_back(r);
 269         end_of_route = false;
 270
 271         reserve_more();
 272
 273         signal_route_changed.emit(get_route());
 274
 275         return true;
 276 }
 277
 278 bool Train::go_to(Track &to)
 279 {
 280         for(BlockList::const_iterator i=blocks.begin(); i!=cur_blocks_end; ++i)
 281                 if((*i)->has_track(to))
 282                 {
 283                         signal_arrived.emit();
 284                         return set_route(0);
 285                 }
 286
 287         free_noncritical_blocks();
 288
 289         TrackIter next = blocks.back().next().track_iter();
 290
 291         Route *route = Route::find(next, to);
 292         if(!route)
 293                 return false;
 294         create_lead_route(route, route);
 295         return set_route(route);
 296 }
 297
 298 bool Train::go_to(const Zone &to)
 299 {
 300         set<Track *> tracks;
 301         for(BlockList::const_iterator i=blocks.begin(); i!=blocks.end(); ++i)
 302                 tracks.insert((*i)->get_tracks().begin(), (*i)->get_tracks().end());
 303
 304         const Zone::TrackSet &ztracks = to.get_tracks();
 305         unsigned union_size = 0;
 306         for(Zone::TrackSet::const_iterator i=ztracks.begin(); i!=ztracks.end(); ++i)
 307                 union_size += tracks.count(*i);
 308
 309         if(union_size==tracks.size() || union_size==ztracks.size())
 310         {
 311                 signal_arrived.emit();
 312                 return set_route(0);
 313         }
 314
 315         free_noncritical_blocks();
 316
 317         TrackIter next = blocks.back().next().track_iter();
 318
 319         Route *route = Route::find(next, to);
 320         if(!route)
 321                 return false;
 322         create_lead_route(route, route);
 323         route->add_tracks(ztracks);
 324         return set_route(route);
 325 }
 326
 327 bool Train::divert(Track &from)
 328 {
 329         if(!from.get_turnout_id())
 330                 throw InvalidParameterValue("Can't divert from a non-turnout");
 331         if(routes.empty())
 332                 return false;
 333
 334         unsigned path = 0;
 335         unsigned entry = 0;
 336         list<RouteRef>::iterator route = routes.begin();
 337
 338         // Follow our routes to find out where we're entering the turnout
 339         for(TrackLoopIter track = blocks.front().track_iter();;)
 340         {
 341                 if(!advance_route(route, *track))
 342                         return false;
 343
 344                 if(&*track==&from)
 345                 {
 346                         Block &block = track->get_block();
 347                         if(block.get_train()==this && !free_block(block))
 348                                 return false;
 349
 350                         int route_path = route->route->get_turnout(from.get_turnout_id());
 351
 352                         // Check that more than one path is available
 353                         unsigned ep_paths = track.endpoint().paths;
 354                         if(!(ep_paths&(ep_paths-1)))
 355                                 return false;
 356
 357                         // Choose some other path
 358                         for(int i=0; ep_paths>>i; ++i)
 359                                 if((ep_paths&(1<<i)) && i!=route_path)
 360                                 {
 361                                         path = i;
 362                                         break;
 363                                 }
 364
 365                         entry = track.entry();
 366                         break;
 367                 }
 368
 369                 track = track.next(route->route->get_path(*track));
 370
 371                 if(!track || track.looped())
 372                         return false;
 373         }
 374
 375         TrackIter track = TrackIter(&from, entry).next(path);
 376         if(!track)
 377                 return false;
 378
 379         set<Track *> tracks;
 380         for(list<RouteRef>::iterator i=routes.begin(); i!=routes.end(); ++i)
 381                 tracks.insert(i->route->get_tracks().begin(), i->route->get_tracks().end());
 382         RefPtr<Route> diversion = Route::find(track, tracks);
 383         if(!diversion)
 384                 return false;
 385
 386         diversion->set_name("Diversion");
 387         diversion->add_track(from);
 388         diversion->set_turnout(from.get_turnout_id(), path);
 389
 390         if(!is_valid_diversion(*diversion, TrackIter(&from, entry)))
 391                 return false;
 392
 393         // Follow the diversion route until we get back to the original route
 394         list<RouteRef>::iterator end = routes.end();
 395         while(1)
 396         {
 397                 for(list<RouteRef>::iterator i=route; (end==routes.end() && i!=routes.end()); ++i)
 398                         if(i->route->has_track(*track))
 399                                 end = i;
 400
 401                 if(end!=routes.end())
 402                         break;
 403                 else if(!diversion->has_track(*track))
 404                         throw LogicError("Pathfinder returned a bad route");
 405
 406                 track = track.next(diversion->get_path(*track));
 407         }
 408
 409         if(route==end)
 410                 // We are rejoining the same route we diverted from, duplicate it
 411                 routes.insert(end, *route);
 412         else
 413         {
 414                 ++route;
 415                 routes.erase(route, end);
 416         }
 417         routes.insert(end, RouteRef(diversion.release(), from.get_turnout_id()));
 418
 419         return true;
 420 }
 421
 422 const Route *Train::get_route() const
 423 {
 424         if(routes.empty())
 425                 return 0;
 426         return routes.front().route;
 427 }
 428
 429 void Train::place(Block &block, unsigned entry)
 430 {
 431         if(controller->get_speed())
 432                 throw InvalidState("Must be stopped before placing");
 433
 434         release_blocks();
 435
 436         set_active(false);
 437         accurate_position = false;
 438
 439         if(!block.reserve(this))
 440                 return;
 441
 442         blocks.push_back(BlockIter(&block, entry));
 443         if(reverse)
 444         {
 445                 TrackIter track = BlockIter(&block, entry).reverse().track_iter();
 446                 vehicles.front()->place(*track, track.entry(), 0, Vehicle::FRONT_BUFFER);
 447         }
 448         else
 449         {
 450                 const Block::Endpoint &bep = block.get_endpoint(entry);
 451                 vehicles.back()->place(*bep.track, bep.track_ep, 0, Vehicle::BACK_BUFFER);
 452         }
 453 }
 454
 455 void Train::unplace()
 456 {
 457         if(controller->get_speed())
 458                 throw InvalidState("Must be stopped before unplacing");
 459
 460         release_blocks();
 461
 462         set_active(false);
 463         accurate_position = false;
 464
 465         for(vector<Vehicle *>::iterator i=vehicles.begin(); i!=vehicles.end(); ++i)
 466                 (*i)->unplace();
 467 }
 468
 469 bool Train::free_block(Block &block)
 470 {
 471         if(get_reserved_distance_until(&block, false)<controller->get_braking_distance()*1.3)
 472                 return false;
 473
 474         unsigned nsens = 0;
 475         for(BlockList::iterator i=cur_blocks_end; i!=blocks.end(); ++i)
 476         {
 477                 if(i->block()==&block)
 478                 {
 479                         if(nsens<1)
 480                                 return false;
 481                         release_blocks(i, blocks.end());
 482                         return true;
 483                 }
 484                 else if((*i)->get_sensor_id())
 485                         ++nsens;
 486         }
 487
 488         return false;
 489 }
 490
 491 void Train::free_noncritical_blocks()
 492 {
 493         if(blocks.empty())
 494                 return;
 495
 496         if(controller->get_speed()==0)
 497         {
 498                 release_blocks(cur_blocks_end, blocks.end());
 499                 return;
 500         }
 501
 502         float margin = 10*layout.get_catalogue().get_scale();
 503         float min_dist = controller->get_braking_distance()*1.3+margin;
 504
 505         Vehicle &veh = *(reverse ? vehicles.back() : vehicles.front());
 506
 507         TrackIter track(veh.get_track(), veh.get_entry());
 508         BlockList::iterator block = blocks.begin();
 509         bool in_rsv = false;
 510         while(block!=blocks.end() && !(*block)->has_track(*track))
 511         {
 512                 ++block;
 513                 if(block==cur_blocks_end)
 514                         in_rsv = true;
 515         }
 516
 517         float dist = veh.get_offset();
 518         if(reverse)
 519                 track.reverse();
 520         else
 521                 dist = track->get_type().get_path_length(track->get_active_path())-dist;
 522         dist -= veh.get_type().get_length()/2;
 523
 524         bool nsens = 0;
 525         while(1)
 526         {
 527                 track = track.next();
 528
 529                 if(!(*block)->has_track(*track))
 530                 {
 531                         ++block;
 532                         if(block==cur_blocks_end)
 533                                 in_rsv = true;
 534                         if(block==blocks.end())
 535                                 return;
 536
 537                         if(dist>min_dist && nsens>0)
 538                         {
 539                                 release_blocks(block, blocks.end());
 540                                 return;
 541                         }
 542
 543                         if(in_rsv && (*block)->get_sensor_id())
 544                                 ++nsens;
 545                 }
 546
 547                 dist += track->get_type().get_path_length(track->get_active_path());
 548         }
 549 }
 550
 551 int Train::get_entry_to_block(Block &block) const
 552 {
 553         for(BlockList::const_iterator i=blocks.begin(); i!=blocks.end(); ++i)
 554                 if(i->block()==&block)
 555                         return i->entry();
 556         return -1;
 557 }
 558
 559 float Train::get_reserved_distance() const
 560 {
 561         if(blocks.empty())
 562                 return 0;
 563
 564         float margin = 0;
 565         TrackIter next = blocks.back().next().track_iter();
 566         if(next && next->get_type().is_turnout())
 567                 margin = 15*layout.get_catalogue().get_scale();
 568
 569         return max(get_reserved_distance_until(0, false)-margin, 0.0f);
 570 }
 571
 572 void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt)
 573 {
 574         if(!active && stop_timeout && t>=stop_timeout)
 575         {
 576                 release_blocks(cur_blocks_end, blocks.end());
 577                 stop_timeout = Time::TimeStamp();
 578         }
 579
 580         Driver &driver = layout.get_driver();
 581
 582         for(list<TrainAI *>::iterator i=ais.begin(); i!=ais.end(); ++i)
 583                 (*i)->tick(t, dt);
 584         controller->tick(dt);
 585         float speed = controller->get_speed();
 586         bool moving = speed>0;
 587
 588         if(controller->get_reverse()!=reverse)
 589         {
 590                 reverse = controller->get_reverse();
 591                 driver.set_loco_reverse(address, reverse);
 592
 593                 release_blocks(cur_blocks_end, blocks.end());
 594                 reverse_blocks(blocks);
 595
 596                 reserve_more();
 597         }
 598
 599         if(speed_quantizer)
 600         {
 601                 unsigned speed_step = speed_quantizer->find_speed_step(speed);
 602                 if(speed_step!=current_speed_step && !speed_changing && !driver.is_halted() && driver.get_power())
 603                 {
 604                         speed_changing = true;
 605                         driver.set_loco_speed(address, speed_step);
 606
 607                         pure_speed = false;
 608                 }
 609
 610                 speed = speed_quantizer->get_speed(current_speed_step);
 611         }
 612
 613         if(moving)
 614         {
 615                 if(!active)
 616                         set_active(true);
 617
 618                 Vehicle &vehicle = *(reverse ? vehicles.back() : vehicles.front());
 619                 Track *track = vehicle.get_track();
 620
 621                 bool ok = false;
 622                 for(BlockList::const_iterator i=blocks.begin(); (!ok && i!=cur_blocks_end); ++i)
 623                         ok = (*i)->has_track(*track);
 624
 625                 float d = speed*(dt/Time::sec);
 626                 if(ok)
 627                 {
 628                         SetFlag setf(advancing);
 629                         vehicle.advance(reverse ? -d : d);
 630                 }
 631                 else if(accurate_position)
 632                 {
 633                         overshoot_dist += d;
 634                         if(overshoot_dist>40*layout.get_catalogue().get_scale())
 635                         {
 636                                 layout.emergency(name+" has not arrived at sensor");
 637                                 accurate_position = false;
 638                         }
 639                 }
 640         }
 641         else if(end_of_route && cur_blocks_end==blocks.end())
 642         {
 643                 set_active(false);
 644                 signal_arrived.emit();
 645                 set_route(0);
 646         }
 647
 648         if(!blocks.empty() && !blocks.front()->get_sensor_id())
 649         {
 650                 float dist = get_reserved_distance_until(&*blocks.front(), true);
 651
 652                 if(dist>10*layout.get_catalogue().get_scale())
 653                 {
 654                         blocks.front()->reserve(0);
 655                         blocks.pop_front();
 656                 }
 657         }
 658 }
 659
 660 void Train::save(list<DataFile::Statement> &st) const
 661 {
 662         st.push_back((DataFile::Statement("name"), name));
 663
 664         st.push_back((DataFile::Statement("priority"), priority));
 665
 666         for(vector<Vehicle *>::const_iterator i=vehicles.begin(); i!=vehicles.end(); ++i)
 667                 if(i!=vehicles.begin())
 668                         st.push_back((DataFile::Statement("vehicle"), (*i)->get_type().get_article_number()));
 669
 670         if(speed_quantizer)
 671         {
 672                 DataFile::Statement ss("quantized_speed");
 673                 speed_quantizer->save(ss.sub);
 674                 st.push_back(ss);
 675         }
 676
 677         if(!blocks.empty() && cur_blocks_end!=blocks.begin())
 678         {
 679                 BlockList blks(blocks.begin(), BlockList::const_iterator(cur_blocks_end));
 680                 if(reverse)
 681                         reverse_blocks(blks);
 682
 683                 BlockIter prev = blks.front().flip();
 684                 st.push_back((DataFile::Statement("block_hint"), prev->get_id()));
 685
 686                 for(BlockList::const_iterator i=blks.begin(); i!=blks.end(); ++i)
 687                         st.push_back((DataFile::Statement("block"), (*i)->get_id()));
 688         }
 689
 690         if(!routes.empty())
 691         {
 692                 list<RouteRef>::const_iterator i = routes.begin();
 693                 for(; (i!=routes.end() && i->route->is_temporary()); ++i) ;
 694                 if(i!=routes.end())
 695                         st.push_back((DataFile::Statement("route"), i->route->get_name()));
 696         }
 697
 698         // XXX Need more generic way of saving AI state
 699         for(list<TrainAI *>::const_iterator i=ais.begin(); i!=ais.end(); ++i)
 700                 if(Timetable *timetable = dynamic_cast<Timetable *>(*i))
 701                 {
 702                         DataFile::Statement ss("timetable");
 703                         timetable->save(ss.sub);
 704                         st.push_back(ss);
 705                 }
 706 }
 707
 708 void Train::control_changed(const Controller::Control &ctrl)
 709 {
 710         signal_control_changed.emit(ctrl.name, ctrl.value);
 711 }
 712
 713 void Train::loco_speed_event(unsigned addr, unsigned speed, bool rev)
 714 {
 715         if(addr==address)
 716         {
 717                 current_speed_step = speed;
 718                 if(rev!=reverse)
 719                         layout.get_driver().set_loco_reverse(address, reverse);
 720                 speed_changing = false;
 721                 pure_speed = false;
 722         }
 723 }
 724
 725 void Train::loco_func_event(unsigned addr, unsigned func, bool state)
 726 {
 727         if(addr==address)
 728         {
 729                 if(state)
 730                         functions |= 1<<func;
 731                 else
 732                         functions &= ~(1<<func);
 733
 734                 signal_function_changed.emit(func, state);
 735         }
 736 }
 737
 738 void Train::block_state_changed(Block &block, Block::State state)
 739 {
 740         if(state==Block::MAYBE_ACTIVE)
 741         {
 742                 // Find the first sensor block from our reserved blocks that isn't this sensor
 743                 BlockList::iterator end;
 744                 unsigned result = 0;
 745                 for(end=cur_blocks_end; end!=blocks.end(); ++end)
 746                         if((*end)->get_sensor_id())
 747                         {
 748                                 if(&**end!=&block)
 749                                 {
 750                                         if(result==0)
 751                                                 result = 2;
 752                                         else if(result==1)
 753                                                 break;
 754                                 }
 755                                 else if(result==0)
 756                                         result = 1;
 757                                 else if(result==2)
 758                                         result = 3;
 759                         }
 760
 761                 if(result==1)
 762                 {
 763                         // Compute speed and update related state
 764                         float travel_time_secs = (Time::now()-last_entry_time)/Time::sec;
 765
 766                         if(pure_speed && speed_quantizer && current_speed_step>0 && travel_time_secs>=2)
 767                                 speed_quantizer->learn(current_speed_step, travel_dist/travel_time_secs, travel_time_secs);
 768
 769                         travel_dist = 0;
 770                         for(BlockList::iterator j=cur_blocks_end; j!=end; ++j)
 771                         {
 772                                 travel_dist += (*j)->get_path_length(j->entry());
 773
 774                                 if(&**j==&block && !advancing)
 775                                 {
 776                                         TrackIter track = j->track_iter();
 777                                         if(reverse)
 778                                         {
 779                                                 track = track.flip();
 780                                                 vehicles.back()->place(*track, track.entry(), 0, Vehicle::BACK_AXLE);
 781                                         }
 782                                         else
 783                                                 vehicles.front()->place(*track, track.entry(), 0, Vehicle::FRONT_AXLE);
 784                                 }
 785                         }
 786                         last_entry_time = Time::now();
 787                         pure_speed = true;
 788                         accurate_position = true;
 789                         overshoot_dist = 0;
 790
 791                         // Check if we've reached the next route
 792                         if(routes.size()>1)
 793                         {
 794                                 const Route &route = *(++routes.begin())->route;
 795                                 for(BlockList::iterator j=cur_blocks_end; j!=end; ++j)
 796                                         if(route.has_track(*j->track_iter()))
 797                                         {
 798                                                 routes.pop_front();
 799                                                 // XXX Exceptions?
 800                                                 signal_route_changed.emit(routes.front().route);
 801                                                 break;
 802                                         }
 803                         }
 804
 805                         // Move blocks up to the next sensor to our current blocks
 806                         for(BlockList::iterator j=cur_blocks_end; j!=end; ++j)
 807                                 signal_advanced.emit(**j);
 808                         cur_blocks_end = end;
 809
 810                         // Try to get more blocks if we're moving
 811                         if(active)
 812                                 reserve_more();
 813                 }
 814                 else if(result==3)
 815                         layout.emergency("Sensor for "+name+" triggered out of order");
 816         }
 817         else if(state==Block::INACTIVE)
 818         {
 819                 const Vehicle &veh = *(reverse ? vehicles.front() : vehicles.back());
 820
 821                 // Find the first sensor in our current blocks that's still active
 822                 BlockList::iterator end = blocks.begin();
 823                 for(BlockList::iterator i=blocks.begin(); i!=cur_blocks_end; ++i)
 824                 {
 825                         if((*i)->has_track(*veh.get_track()))
 826                                 break;
 827                         if((*i)->get_sensor_id())
 828                         {
 829                                 if(layout.get_driver().get_sensor((*i)->get_sensor_id()))
 830                                         break;
 831                                 else
 832                                 {
 833                                         end = i;
 834                                         ++end;
 835                                 }
 836                         }
 837                 }
 838
 839                 if(end!=blocks.begin() && end!=cur_blocks_end)
 840                         // Free blocks up to the last inactive sensor
 841                         release_blocks(blocks.begin(), end);
 842         }
 843 }
 844
 845 void Train::turnout_path_changed(Track &track)
 846 {
 847         for(list<BlockIter>::iterator i=blocks.begin(); i!=blocks.end(); ++i)
 848                 if((*i)->get_turnout_id()==track.get_turnout_id() && !reserving)
 849                         check_turnout_paths(false);
 850 }
 851
 852 void Train::halt_event(bool h)
 853 {
 854         if(h)
 855                 accurate_position = false;
 856 }
 857
 858 void Train::block_reserved(const Block &block, const Train *train)
 859 {
 860         if(&block==pending_block && !train && !reserving)
 861                 reserve_more();
 862 }
 863
 864 void Train::reserve_more()
 865 {
 866         if(!active || blocks.empty() || end_of_route)
 867                 return;
 868
 869         BlockIter start = blocks.back();
 870
 871         pending_block = 0;
 872         preceding_train = 0;
 873
 874         // See how many sensor blocks and how much track we already have
 875         unsigned nsens = 0;
 876         float dist = 0;
 877         for(BlockList::const_iterator i=cur_blocks_end; i!=blocks.end(); ++i)
 878         {
 879                 if((*i)->get_sensor_id())
 880                         ++nsens;
 881                 if(nsens>0)
 882                         dist += (*i)->get_path_length(i->entry());
 883         }
 884
 885         list<RouteRef>::iterator cur_route = routes.begin();
 886         advance_route(cur_route, *start.track_iter());
 887
 888         float approach_margin = 50*layout.get_catalogue().get_scale();
 889         float min_dist = controller->get_braking_distance()*1.3+approach_margin*2;
 890
 891         BlockIter block = start;
 892         list<BlockIter>::iterator good_end = blocks.end();
 893         Track *divert_track = 0;
 894         bool try_divert = false;
 895         Train *blocking_train = 0;
 896         BlockList contested_blocks;
 897
 898         SetFlag setf(reserving);
 899
 900         while(1)
 901         {
 902                 BlockIter last = block;
 903                 block = block.next(cur_route!=routes.end() ? cur_route->route : 0);
 904                 if(!block || block->get_endpoints().size()<2)
 905                 {
 906                         if(!blocking_train)
 907                         {
 908                                 good_end = blocks.end();
 909                                 end_of_route = true;
 910                         }
 911                         break;
 912                 }
 913
 914                 TrackIter track = block.track_iter();
 915
 916                 if(cur_route!=routes.end())
 917                 {
 918                         if(!advance_route(cur_route, *track))
 919                         {
 920                                 // Keep the blocks if we arrived at the end of the route
 921                                 if(!blocking_train)
 922                                 {
 923                                         good_end = blocks.end();
 924                                         end_of_route = true;
 925                                 }
 926                                 break;
 927                         }
 928                 }
 929
 930                 if(block->get_turnout_id() && !last->get_turnout_id())
 931                 {
 932                         /* We can keep the blocks if we arrive at a turnout from a non-turnout
 933                         block.  Having a turnout block as our last reserved block is not good
 934                         as it would limit our diversion possibilities for little benefit. */
 935                         good_end = blocks.end();
 936                         if(nsens>=3 && dist>=min_dist)
 937                                 break;
 938                 }
 939
 940                 if(blocking_train)
 941                 {
 942                         if(block->get_train()!=blocking_train)
 943                         {
 944                                 if(blocking_train->free_block(*contested_blocks.back()))
 945                                 {
 946                                         // Roll back and start actually reserving the blocks
 947                                         block = blocks.back();
 948                                         cur_route = routes.begin();
 949                                         advance_route(cur_route, *block.track_iter().track());
 950                                         if(blocking_train->get_priority()==priority)
 951                                                 blocking_train->yield_to(*this);
 952                                         blocking_train = 0;
 953                                         continue;
 954                                 }
 955                                 else
 956                                 {
 957                                         yield_to(*blocking_train);
 958                                         pending_block = contested_blocks.front().block();
 959                                         try_divert = divert_track;
 960                                         break;
 961                                 }
 962                         }
 963                         else
 964                         {
 965                                 contested_blocks.push_back(block);
 966                                 continue;
 967                         }
 968                 }
 969
 970                 bool reserved = block->reserve(this);
 971                 if(!reserved)
 972                 {
 973                         /* We've found another train.  If it wants to exit the block from the
 974                         same endpoint we're trying to enter from or the other way around,
 975                         treat it as coming towards us.  Otherwise treat it as going in the
 976                         same direction. */
 977                         Train *other_train = block->get_train();
 978                         int other_entry = other_train->get_entry_to_block(*block);
 979                         if(other_entry<0)
 980                                 throw LogicError("Block reservation inconsistency");
 981
 982                         unsigned exit = block.reverse().entry();
 983                         unsigned other_exit = BlockIter(block.block(), other_entry).reverse().entry();
 984                         bool entry_conflict = (block.entry()==other_exit);
 985                         bool exit_conflict = (exit==static_cast<unsigned>(other_entry));
 986                         if(!entry_conflict && !last->get_turnout_id())
 987                         {
 988                                 /* The other train is not coming to the blocks we're holding, so we
 989                                 can keep them. */
 990                                 good_end = blocks.end();
 991
 992                                 if(static_cast<unsigned>(other_entry)==block.entry())
 993                                         preceding_train = other_train;
 994                         }
 995
 996                         int other_prio = other_train->get_priority();
 997
 998                         if(!entry_conflict && !exit_conflict && other_prio<priority)
 999                         {
1000                                 /* Ask a lesser priority train going to the same direction to free
1001                                 the block for us */
1002                                 if(other_train->free_block(*block))
1003                                         reserved = block->reserve(this);
1004                         }
1005                         else if(other_train!=yielding_to && (other_prio<priority || (other_prio==priority && entry_conflict)))
1006                         {
1007                                 /* A lesser priority train is coming at us, we must ask it to free
1008                                 enough blocks to get clear of it to avoid a potential deadlock */
1009                                 blocking_train = other_train;
1010                                 contested_blocks.clear();
1011                                 contested_blocks.push_back(block);
1012                                 continue;
1013                         }
1014                         else if(divert_track && (entry_conflict || exit_conflict || !other_train->is_active()))
1015                                 // We are blocked, but there's a diversion possibility
1016                                 try_divert = true;
1017
1018                         if(!reserved)
1019                         {
1020                                 pending_block = &*block;
1021                                 break;
1022                         }
1023                 }
1024
1025                 if(block->get_turnout_id())
1026                 {
1027                         const TrackType::Endpoint &track_ep = track.endpoint();
1028                         bool multiple_paths = (track_ep.paths&(track_ep.paths-1));
1029
1030                         if(multiple_paths && cur_route!=routes.end() && cur_route->diversion!=block->get_turnout_id())
1031                                 /* There's multiple paths to be taken and we are on a route - take
1032                                 note of the diversion possibility */
1033                                 divert_track = &*track;
1034                 }
1035
1036                 if(!contested_blocks.empty() && contested_blocks.front()==block)
1037                         contested_blocks.pop_front();
1038
1039                 blocks.push_back(block);
1040
1041                 if(cur_blocks_end==blocks.end())
1042                         --cur_blocks_end;
1043                 if(clear_blocks_end==blocks.end())
1044                         --clear_blocks_end;
1045                 if(good_end==blocks.end())
1046                         --good_end;
1047
1048                 if(block->get_sensor_id())
1049                         ++nsens;
1050                 if(nsens>0)
1051                         dist += block->get_path_length(block.entry());
1052         }
1053
1054         // Unreserve blocks that were not good
1055         release_blocks(good_end, blocks.end());
1056
1057         if(blocks.back()!=start)
1058                 // We got some new blocks, so no longer need to yield
1059                 yielding_to = 0;
1060
1061         check_turnout_paths(true);
1062
1063         // Make any sensorless blocks at the beginning immediately current
1064         while(cur_blocks_end!=clear_blocks_end && !(*cur_blocks_end)->get_sensor_id())
1065                 ++cur_blocks_end;
1066
1067         if(try_divert && divert(*divert_track))
1068                 reserve_more();
1069 }
1070
1071 void Train::check_turnout_paths(bool set)
1072 {
1073         if(clear_blocks_end==blocks.end())
1074                 return;
1075
1076         for(list<BlockIter>::iterator i=clear_blocks_end; i!=blocks.end(); ++i)
1077         {
1078                 if((*i)->get_turnout_id())
1079                 {
1080                         TrackIter track = i->track_iter();
1081                         const TrackType::Endpoint &track_ep = track.endpoint();
1082
1083                         unsigned path = 0;
1084                         list<BlockIter>::iterator j = i;
1085                         if(++j!=blocks.end())
1086                         {
1087                                 TrackIter rev = j->track_iter().flip();
1088                                 unsigned mask = rev.endpoint().paths&track_ep.paths;
1089                                 for(path=0; mask>1; mask>>=1, ++path) ;
1090                         }
1091                         else
1092                                 return;
1093
1094                         if(path!=track->get_active_path())
1095                         {
1096                                 if(set)
1097                                         track->set_active_path(path);
1098
1099                                 /* Check again, in case the driver was able to service the request
1100                                 instantly */
1101                                 if(!set || path!=track->get_active_path())
1102                                         continue;
1103                         }
1104                 }
1105
1106                 if(i==clear_blocks_end)
1107                         ++clear_blocks_end;
1108         }
1109 }
1110
1111 float Train::get_reserved_distance_until(const Block *until_block, bool back) const
1112 {
1113         if(blocks.empty())
1114                 return 0;
1115
1116         Vehicle &veh = *(reverse!=back ? vehicles.back() : vehicles.front());
1117         const VehicleType &vtype = veh.get_type();
1118
1119         TrackIter track(veh.get_track(), veh.get_entry());
1120         if(!track)  // XXX Probably unnecessary
1121                 return 0;
1122
1123         BlockList::const_iterator block = blocks.begin();
1124         while(block!=clear_blocks_end && !(*block)->has_track(*track))
1125                 ++block;
1126         if(block==clear_blocks_end || &**block==until_block)
1127                 return 0;
1128
1129         float result = veh.get_offset();
1130         if(reverse!=back)
1131                 track = track.reverse();
1132         else
1133                 result = track->get_type().get_path_length(track->get_active_path())-result;
1134         result -= vtype.get_length()/2;
1135
1136         while(1)
1137         {
1138                 track = track.next();
1139                 if(!track)
1140                         break;
1141
1142                 if(!(*block)->has_track(*track))
1143                 {
1144                         if(back)
1145                         {
1146                                 if(block==blocks.begin())
1147                                         break;
1148                                 --block;
1149                         }
1150                         else
1151                         {
1152                                 ++block;
1153                                 if(block==clear_blocks_end)
1154                                         break;
1155                         }
1156
1157                         if(&**block==until_block)
1158                                 break;
1159                 }
1160
1161                 result += track->get_type().get_path_length(track->get_active_path());
1162         }
1163
1164         return result;
1165 }
1166
1167 void Train::release_blocks()
1168 {
1169         release_blocks(blocks.begin(), blocks.end());
1170 }
1171
1172 void Train::release_blocks(BlockList::iterator begin, BlockList::iterator end)
1173 {
1174         while(begin!=end)
1175         {
1176                 if(begin==cur_blocks_end)
1177                         cur_blocks_end = end;
1178                 if(begin==clear_blocks_end)
1179                         clear_blocks_end = end;
1180
1181                 Block &block = **begin;
1182                 blocks.erase(begin++);
1183                 block.reserve(0);
1184
1185                 if(begin==blocks.end())
1186                         end_of_route = false;
1187         }
1188 }
1189
1190 void Train::reverse_blocks(BlockList &blks) const
1191 {
1192         blks.reverse();
1193         for(BlockList::iterator i=blks.begin(); i!=blks.end(); ++i)
1194                 *i = i->reverse();
1195 }
1196
1197 bool Train::advance_route(list<RouteRef>::iterator &iter, Track &track)
1198 {
1199         while(iter!=routes.end() && !iter->route->has_track(track))
1200                 ++iter;
1201         if(iter==routes.end())
1202                 return false;
1203
1204         list<RouteRef>::iterator next = iter;
1205         ++next;
1206         if(next!=routes.end() && next->diversion && next->route->has_track(track))
1207                 iter = next;
1208
1209         return true;
1210 }
1211
1212 Route *Train::create_lead_route(Route *lead, const Route *target)
1213 {
1214         if(!lead)
1215         {
1216                 lead = new Route(layout);
1217                 lead->set_name("Lead");
1218                 lead->set_temporary(true);
1219         }
1220
1221         set<Track *> tracks;
1222         for(BlockList::iterator i=blocks.begin(); i!=blocks.end(); ++i)
1223         {
1224                 const set<Track *> &btracks = (*i)->get_tracks();
1225                 for(set<Track *>::const_iterator j=btracks.begin(); j!=btracks.end(); ++j)
1226                         if(!target || !target->has_track(**j))
1227                                 tracks.insert(*j);
1228         }
1229
1230         lead->add_tracks(tracks);
1231
1232         return lead;
1233 }
1234
1235 bool Train::is_valid_diversion(const Route &diversion, const TrackIter &from)
1236 {
1237         float diversion_len = 0;
1238         TrackLoopIter track1 = from;
1239         while(diversion.has_track(*track1))
1240         {
1241                 unsigned path = diversion.get_path(*track1);
1242                 diversion_len += track1->get_type().get_path_length(path);
1243
1244                 track1 = track1.next(path);
1245
1246                 if(!track1 || track1.looped())
1247                         return false;
1248         }
1249
1250         list<RouteRef>::iterator route = routes.begin();
1251         if(!advance_route(route, *from))
1252                 return false;
1253
1254         float route_len = 0;
1255         TrackLoopIter track2 = from;
1256         while(1)
1257         {
1258                 unsigned path = route->route->get_path(*track2);
1259                 route_len += track2->get_type().get_path_length(path);
1260
1261                 bool ok = (track2!=from && diversion.has_track(*track2));
1262
1263                 track2 = track2.next(path);
1264                 if(!track2)
1265                         return false;
1266
1267                 if(ok)
1268                         break;
1269
1270                 if(track2.looped())
1271                         return false;
1272
1273                 if(!advance_route(route, *track2))
1274                         return false;
1275         }
1276
1277         // Must end up at the same place through both routes
1278         if(track2!=track1)
1279                 return false;
1280
1281         return diversion_len<route_len*1.2;
1282 }
1283
1284
1285 Train::RouteRef::RouteRef(const Route *r, unsigned d):
1286         route(r),
1287         diversion(d)
1288 { }
1289
1290
1291 Train::Loader::Loader(Train &t):
1292         DataFile::BasicLoader<Train>(t),
1293         prev_block(0),
1294         blocks_valid(true)
1295 {
1296         add("block",       &Loader::block);
1297         add("block_hint",  &Loader::block_hint);
1298         add("name",        &Loader::name);
1299         add("priority",    &Train::priority);
1300         add("quantized_speed",  &Loader::quantized_speed);
1301         add("route",       &Loader::route);
1302         add("timetable",   &Loader::timetable);
1303         add("vehicle",     &Loader::vehicle);
1304 }
1305
1306 void Train::Loader::finish()
1307 {
1308         if(!obj.blocks.empty())
1309         {
1310                 TrackIter track = obj.blocks.front().track_iter();
1311                 float offset = 2*obj.layout.get_catalogue().get_scale();
1312                 obj.vehicles.back()->place(*track, track.entry(), offset, Vehicle::BACK_BUFFER);
1313         }
1314 }
1315
1316 void Train::Loader::block(unsigned id)
1317 {
1318         if(!blocks_valid)
1319                 return;
1320
1321         Block *blk;
1322         try
1323         {
1324                 blk = &obj.layout.get_block(id);
1325         }
1326         catch(const KeyError &)
1327         {
1328                 blocks_valid = false;
1329                 return;
1330         }
1331
1332         int entry = -1;
1333         if(prev_block)
1334                 entry = blk->get_endpoint_by_link(*prev_block);
1335         if(entry<0)
1336                 entry = 0;
1337
1338         blk->reserve(&obj);
1339         obj.blocks.push_back(BlockIter(blk, entry));
1340
1341         if(blk->get_sensor_id())
1342                 obj.layout.get_driver().set_sensor(blk->get_sensor_id(), true);
1343
1344         prev_block = blk;
1345 }
1346
1347 void Train::Loader::block_hint(unsigned id)
1348 {
1349         try
1350         {
1351                 prev_block = &obj.layout.get_block(id);
1352         }
1353         catch(const KeyError &)
1354         {
1355                 blocks_valid = false;
1356         }
1357 }
1358
1359 void Train::Loader::name(const string &n)
1360 {
1361         obj.set_name(n);
1362 }
1363
1364 void Train::Loader::quantized_speed()
1365 {
1366         if(obj.speed_quantizer)
1367                 load_sub(*obj.speed_quantizer);
1368 }
1369
1370 void Train::Loader::route(const string &n)
1371 {
1372         obj.set_route(&obj.layout.get_route(n));
1373 }
1374
1375 void Train::Loader::timetable()
1376 {
1377         Timetable *ttbl = new Timetable(obj);
1378         load_sub(*ttbl);
1379 }
1380
1381 void Train::Loader::vehicle(ArticleNumber art_nr)
1382 {
1383         const VehicleType &vtype = obj.layout.get_catalogue().get_vehicle(art_nr);
1384         Vehicle *veh = new Vehicle(obj.layout, vtype);
1385         obj.vehicles.back()->attach_back(*veh);
1386         obj.vehicles.push_back(veh);
1387 }
1388
1389 } // namespace R2C2