source/libr2c2/train.cpp

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