source/libr2c2/train.cpp

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