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 const Route *Train::get_route() const
 322 {
 323         if(routes.empty())
 324                 return 0;
 325         return routes.front();
 326 }
 327
 328 void Train::place(Block &block, unsigned entry)
 329 {
 330         if(controller->get_speed())
 331                 throw logic_error("moving");
 332
 333         release_blocks();
 334
 335         set_active(false);
 336         accurate_position = false;
 337
 338         blocks.push_back(BlockIter(&block, entry));
 339         if(!block.reserve(this))
 340         {
 341                 blocks.pop_back();
 342                 return;
 343         }
 344
 345         if(reverse)
 346         {
 347                 TrackIter track = BlockIter(&block, entry).reverse().track_iter();
 348                 vehicles.front()->place(*track, track.entry(), 0, Vehicle::FRONT_BUFFER);
 349         }
 350         else
 351         {
 352                 const Block::Endpoint &bep = block.get_endpoint(entry);
 353                 vehicles.back()->place(*bep.track, bep.track_ep, 0, Vehicle::BACK_BUFFER);
 354         }
 355 }
 356
 357 void Train::unplace()
 358 {
 359         if(controller->get_speed())
 360                 throw logic_error("moving");
 361
 362         release_blocks();
 363
 364         set_active(false);
 365         accurate_position = false;
 366
 367         for(vector<Vehicle *>::iterator i=vehicles.begin(); i!=vehicles.end(); ++i)
 368                 (*i)->unplace();
 369 }
 370
 371 bool Train::free_block(Block &block)
 372 {
 373         if(get_reserved_distance_until(&block, false)<controller->get_braking_distance()*1.3)
 374                 return false;
 375
 376         unsigned nsens = 0;
 377         for(BlockList::iterator i=cur_blocks_end; i!=blocks.end(); ++i)
 378         {
 379                 if(i->block()==&block)
 380                 {
 381                         if(nsens<1)
 382                                 return false;
 383                         release_blocks(i, blocks.end());
 384                         return true;
 385                 }
 386                 else if((*i)->get_sensor_id())
 387                         ++nsens;
 388         }
 389
 390         return false;
 391 }
 392
 393 void Train::free_noncritical_blocks()
 394 {
 395         if(blocks.empty())
 396                 return;
 397
 398         if(controller->get_speed()==0)
 399         {
 400                 release_blocks(cur_blocks_end, blocks.end());
 401                 return;
 402         }
 403
 404         float margin = 10*layout.get_catalogue().get_scale();
 405         float min_dist = controller->get_braking_distance()*1.3+margin;
 406
 407         Vehicle &veh = *(reverse ? vehicles.back() : vehicles.front());
 408
 409         TrackIter track(veh.get_track(), veh.get_entry());
 410         BlockList::iterator block = blocks.begin();
 411         bool in_rsv = false;
 412         while(block!=blocks.end() && !(*block)->has_track(*track))
 413         {
 414                 ++block;
 415                 if(block==cur_blocks_end)
 416                         in_rsv = true;
 417         }
 418
 419         float dist = veh.get_offset();
 420         if(reverse)
 421                 track.reverse();
 422         else
 423                 dist = track->get_type().get_path_length(track->get_active_path())-dist;
 424         dist -= veh.get_type().get_length()/2;
 425
 426         bool nsens = 0;
 427         while(1)
 428         {
 429                 track = track.next();
 430
 431                 if(!(*block)->has_track(*track))
 432                 {
 433                         ++block;
 434                         if(block==cur_blocks_end)
 435                                 in_rsv = true;
 436                         if(block==blocks.end())
 437                                 return;
 438
 439                         if(dist>min_dist && nsens>0)
 440                         {
 441                                 release_blocks(block, blocks.end());
 442                                 return;
 443                         }
 444
 445                         if(in_rsv && (*block)->get_sensor_id())
 446                                 ++nsens;
 447                 }
 448
 449                 dist += track->get_type().get_path_length(track->get_active_path());
 450         }
 451 }
 452
 453 int Train::get_entry_to_block(const Block &block) const
 454 {
 455         for(BlockList::const_iterator i=blocks.begin(); i!=blocks.end(); ++i)
 456                 if(i->block()==&block)
 457                         return i->entry();
 458         return -1;
 459 }
 460
 461 float Train::get_reserved_distance() const
 462 {
 463         if(blocks.empty())
 464                 return 0;
 465
 466         float margin = 0;
 467         TrackIter next = blocks.back().next().track_iter();
 468         if(next && next->get_type().is_turnout())
 469                 margin = 15*layout.get_catalogue().get_scale();
 470
 471         return max(get_reserved_distance_until(0, false)-margin, 0.0f);
 472 }
 473
 474 void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt)
 475 {
 476         if(!active && stop_timeout && t>=stop_timeout)
 477         {
 478                 release_blocks(cur_blocks_end, blocks.end());
 479                 stop_timeout = Time::TimeStamp();
 480         }
 481
 482         Driver &driver = layout.get_driver();
 483
 484         for(list<TrainAI *>::iterator i=ais.begin(); i!=ais.end(); ++i)
 485                 (*i)->tick(t, dt);
 486         controller->tick(dt);
 487         float speed = controller->get_speed();
 488         bool moving = speed>0;
 489
 490         if(controller->get_reverse()!=reverse)
 491         {
 492                 reverse = controller->get_reverse();
 493                 bool r = reverse;
 494                 if(loco_type.get_swap_direction())
 495                         r = !r;
 496                 driver.set_loco_reverse(address, r);
 497
 498                 release_blocks(cur_blocks_end, blocks.end());
 499                 reverse_blocks(blocks);
 500
 501                 reserve_more();
 502         }
 503
 504         if(speed_quantizer)
 505         {
 506                 unsigned speed_step = speed_quantizer->find_speed_step(speed);
 507                 if(speed_step!=current_speed_step && !speed_changing && !driver.is_halted() && driver.get_power())
 508                 {
 509                         speed_changing = true;
 510                         driver.set_loco_speed(address, speed_step);
 511
 512                         pure_speed = false;
 513                 }
 514
 515                 speed = speed_quantizer->get_speed(current_speed_step);
 516         }
 517
 518         if(moving)
 519         {
 520                 if(!active)
 521                         set_active(true);
 522
 523                 Vehicle &vehicle = *(reverse ? vehicles.back() : vehicles.front());
 524                 Track *track = vehicle.get_track();
 525
 526                 bool ok = false;
 527                 for(BlockList::const_iterator i=blocks.begin(); (!ok && i!=cur_blocks_end); ++i)
 528                         ok = (*i)->has_track(*track);
 529
 530                 float d = speed*(dt/Time::sec);
 531                 if(ok)
 532                 {
 533                         SetFlag setf(advancing);
 534                         vehicle.advance(reverse ? -d : d);
 535                 }
 536                 else if(accurate_position)
 537                 {
 538                         overshoot_dist += d;
 539                         if(overshoot_dist>40*layout.get_catalogue().get_scale())
 540                         {
 541                                 layout.emergency(name+" has not arrived at sensor");
 542                                 accurate_position = false;
 543                         }
 544                 }
 545         }
 546         else if(end_of_route && cur_blocks_end==blocks.end())
 547         {
 548                 set_active(false);
 549                 signal_arrived.emit();
 550                 set_route(0);
 551         }
 552
 553         if(!blocks.empty() && !blocks.front()->get_sensor_id())
 554         {
 555                 float dist = get_reserved_distance_until(&*blocks.front(), true);
 556
 557                 if(dist>10*layout.get_catalogue().get_scale())
 558                 {
 559                         Block &block = *blocks.front();
 560                         blocks.pop_front();
 561                         block.reserve(0);
 562                 }
 563         }
 564 }
 565
 566 void Train::save(list<DataFile::Statement> &st) const
 567 {
 568         st.push_back((DataFile::Statement("name"), name));
 569
 570         st.push_back((DataFile::Statement("priority"), priority));
 571
 572         for(vector<Vehicle *>::const_iterator i=vehicles.begin(); i!=vehicles.end(); ++i)
 573                 if(i!=vehicles.begin())
 574                         st.push_back((DataFile::Statement("vehicle"), (*i)->get_type().get_article_number()));
 575
 576         if(speed_quantizer)
 577         {
 578                 DataFile::Statement ss("quantized_speed");
 579                 speed_quantizer->save(ss.sub);
 580                 st.push_back(ss);
 581         }
 582
 583         if(!blocks.empty() && cur_blocks_end!=blocks.begin())
 584         {
 585                 BlockList blks(blocks.begin(), BlockList::const_iterator(cur_blocks_end));
 586                 if(reverse)
 587                         reverse_blocks(blks);
 588
 589                 BlockIter prev = blks.front().flip();
 590                 st.push_back((DataFile::Statement("block_hint"), prev->get_id()));
 591
 592                 for(BlockList::const_iterator i=blks.begin(); i!=blks.end(); ++i)
 593                         st.push_back((DataFile::Statement("block"), (*i)->get_id()));
 594         }
 595
 596         if(!routes.empty())
 597         {
 598                 list<const Route *>::const_iterator i = routes.begin();
 599                 for(; (i!=routes.end() && (*i)->is_temporary()); ++i) ;
 600                 if(i!=routes.end())
 601                         st.push_back((DataFile::Statement("route"), (*i)->get_name()));
 602         }
 603
 604         // XXX Need more generic way of saving AI state
 605         for(list<TrainAI *>::const_iterator i=ais.begin(); i!=ais.end(); ++i)
 606                 if(Timetable *timetable = dynamic_cast<Timetable *>(*i))
 607                 {
 608                         DataFile::Statement ss("timetable");
 609                         timetable->save(ss.sub);
 610                         st.push_back(ss);
 611                 }
 612 }
 613
 614 void Train::control_changed(const Controller::Control &ctrl)
 615 {
 616         signal_control_changed.emit(ctrl.name, ctrl.value);
 617 }
 618
 619 void Train::loco_speed_event(unsigned addr, unsigned speed, bool rev)
 620 {
 621         if(addr==address)
 622         {
 623                 current_speed_step = speed;
 624                 bool r = reverse;
 625                 if(loco_type.get_swap_direction())
 626                         r = !r;
 627                 if(rev!=r)
 628                         layout.get_driver().set_loco_reverse(address, r);
 629                 speed_changing = false;
 630                 pure_speed = false;
 631         }
 632 }
 633
 634 void Train::loco_func_event(unsigned addr, unsigned func, bool state)
 635 {
 636         if(addr==address)
 637         {
 638                 if(state)
 639                         functions |= 1<<func;
 640                 else
 641                         functions &= ~(1<<func);
 642
 643                 signal_function_changed.emit(func, state);
 644         }
 645 }
 646
 647 void Train::block_state_changed(Block &block, Block::State state)
 648 {
 649         if(state==Block::MAYBE_ACTIVE)
 650         {
 651                 // Find the first sensor block from our reserved blocks that isn't this sensor
 652                 BlockList::iterator end;
 653                 unsigned result = 0;
 654                 for(end=cur_blocks_end; end!=blocks.end(); ++end)
 655                         if((*end)->get_sensor_id())
 656                         {
 657                                 if(&**end!=&block)
 658                                 {
 659                                         if(result==0)
 660                                                 result = 2;
 661                                         else if(result==1)
 662                                                 break;
 663                                 }
 664                                 else if(result==0)
 665                                         result = 1;
 666                                 else if(result==2)
 667                                         result = 3;
 668                         }
 669
 670                 if(result==1)
 671                 {
 672                         // Compute speed and update related state
 673                         float travel_time_secs = (Time::now()-last_entry_time)/Time::sec;
 674
 675                         if(pure_speed && speed_quantizer && current_speed_step>0 && travel_time_secs>=2)
 676                                 speed_quantizer->learn(current_speed_step, travel_dist/travel_time_secs, travel_time_secs);
 677
 678                         travel_dist = 0;
 679                         for(BlockList::iterator j=cur_blocks_end; j!=end; ++j)
 680                         {
 681                                 travel_dist += (*j)->get_path_length(j->entry());
 682
 683                                 if(&**j==&block && !advancing)
 684                                 {
 685                                         TrackIter track = j->track_iter();
 686                                         if(reverse)
 687                                         {
 688                                                 track = track.flip();
 689                                                 vehicles.back()->place(*track, track.entry(), 0, Vehicle::BACK_AXLE);
 690                                         }
 691                                         else
 692                                                 vehicles.front()->place(*track, track.entry(), 0, Vehicle::FRONT_AXLE);
 693                                 }
 694                         }
 695                         last_entry_time = Time::now();
 696                         pure_speed = true;
 697                         accurate_position = true;
 698                         overshoot_dist = 0;
 699
 700                         // Check if we've reached the next route
 701                         if(routes.size()>1)
 702                         {
 703                                 const Route &route = **(++routes.begin());
 704                                 for(BlockList::iterator j=cur_blocks_end; j!=end; ++j)
 705                                         if(route.has_track(*j->track_iter()))
 706                                         {
 707                                                 routes.pop_front();
 708                                                 // XXX Exceptions?
 709                                                 signal_route_changed.emit(routes.front());
 710                                                 break;
 711                                         }
 712                         }
 713
 714                         // Move blocks up to the next sensor to our current blocks
 715                         for(BlockList::iterator j=cur_blocks_end; j!=end; ++j)
 716                                 signal_advanced.emit(**j);
 717                         cur_blocks_end = end;
 718
 719                         // Try to get more blocks if we're moving
 720                         if(active)
 721                                 reserve_more();
 722                 }
 723                 else if(result==3)
 724                         layout.emergency("Sensor for "+name+" triggered out of order");
 725         }
 726         else if(state==Block::INACTIVE)
 727         {
 728                 const Vehicle &veh = *(reverse ? vehicles.front() : vehicles.back());
 729
 730                 // Find the first sensor in our current blocks that's still active
 731                 BlockList::iterator end = blocks.begin();
 732                 for(BlockList::iterator i=blocks.begin(); i!=cur_blocks_end; ++i)
 733                 {
 734                         if((*i)->has_track(*veh.get_track()))
 735                                 break;
 736                         if((*i)->get_sensor_id())
 737                         {
 738                                 if(layout.get_driver().get_sensor((*i)->get_sensor_id()))
 739                                         break;
 740                                 else
 741                                 {
 742                                         end = i;
 743                                         ++end;
 744                                 }
 745                         }
 746                 }
 747
 748                 if(end!=blocks.begin() && end!=cur_blocks_end)
 749                         // Free blocks up to the last inactive sensor
 750                         release_blocks(blocks.begin(), end);
 751         }
 752 }
 753
 754 void Train::turnout_path_changed(Track &track)
 755 {
 756         for(list<BlockIter>::iterator i=blocks.begin(); i!=blocks.end(); ++i)
 757                 if((*i)->get_turnout_id()==track.get_turnout_id() && !reserving)
 758                         check_turnout_paths(false);
 759 }
 760
 761 void Train::halt_event(bool h)
 762 {
 763         if(h)
 764                 accurate_position = false;
 765 }
 766
 767 void Train::block_reserved(const Block &block, const Train *train)
 768 {
 769         if(&block==pending_block && !train && !reserving)
 770                 reserve_more();
 771 }
 772
 773 void Train::reserve_more()
 774 {
 775         if(!active || blocks.empty() || end_of_route)
 776                 return;
 777
 778         BlockIter start = blocks.back();
 779
 780         pending_block = 0;
 781         preceding_train = 0;
 782
 783         // See how many sensor blocks and how much track we already have
 784         unsigned nsens = 0;
 785         float dist = 0;
 786         for(BlockList::const_iterator i=cur_blocks_end; i!=blocks.end(); ++i)
 787         {
 788                 if((*i)->get_sensor_id())
 789                         ++nsens;
 790                 if(nsens>0)
 791                         dist += (*i)->get_path_length(i->entry());
 792         }
 793
 794         list<const Route *>::iterator cur_route = routes.begin();
 795         advance_route(cur_route, *start.track_iter());
 796
 797         float approach_margin = 50*layout.get_catalogue().get_scale();
 798         float min_dist = controller->get_braking_distance()*1.3+approach_margin*2;
 799
 800         BlockIter block = start;
 801         list<BlockIter>::iterator good_end = blocks.end();
 802         Train *blocking_train = 0;
 803         BlockList contested_blocks;
 804
 805         SetFlag setf(reserving);
 806
 807         while(1)
 808         {
 809                 BlockIter last = block;
 810                 block = block.next(cur_route!=routes.end() ? *cur_route : 0);
 811                 if(!block || block->get_endpoints().size()<2)
 812                 {
 813                         if(!blocking_train)
 814                         {
 815                                 good_end = blocks.end();
 816                                 end_of_route = true;
 817                         }
 818                         break;
 819                 }
 820
 821                 TrackIter track = block.track_iter();
 822
 823                 if(cur_route!=routes.end())
 824                 {
 825                         if(!advance_route(cur_route, *track))
 826                         {
 827                                 // Keep the blocks if we arrived at the end of the route
 828                                 if(!blocking_train)
 829                                 {
 830                                         good_end = blocks.end();
 831                                         end_of_route = true;
 832                                 }
 833                                 break;
 834                         }
 835                 }
 836
 837                 if(block->get_turnout_id() && !last->get_turnout_id())
 838                 {
 839                         /* We can keep the blocks if we arrive at a turnout from a non-turnout
 840                         block.  Having a turnout block as our last reserved block is not good
 841                         as it would limit our diversion possibilities for little benefit. */
 842                         good_end = blocks.end();
 843                         if(nsens>=3 && dist>=min_dist)
 844                                 break;
 845                 }
 846
 847                 if(blocking_train)
 848                 {
 849                         if(block->get_train()!=blocking_train)
 850                         {
 851                                 if(blocking_train->free_block(*contested_blocks.back()))
 852                                 {
 853                                         // Roll back and start actually reserving the blocks
 854                                         block = blocks.back();
 855                                         cur_route = routes.begin();
 856                                         advance_route(cur_route, *block.track_iter().track());
 857                                         if(blocking_train->get_priority()==priority)
 858                                                 blocking_train->yield_to(*this);
 859                                         blocking_train = 0;
 860                                         continue;
 861                                 }
 862                                 else
 863                                 {
 864                                         yield_to(*blocking_train);
 865                                         pending_block = contested_blocks.front().block();
 866                                         break;
 867                                 }
 868                         }
 869                         else
 870                         {
 871                                 contested_blocks.push_back(block);
 872                                 continue;
 873                         }
 874                 }
 875
 876                 blocks.push_back(block);
 877                 bool reserved = block->reserve(this);
 878                 if(!reserved)
 879                 {
 880                         blocks.pop_back();
 881                         /* We've found another train.  If it wants to exit the block from the
 882                         same endpoint we're trying to enter from or the other way around,
 883                         treat it as coming towards us.  Otherwise treat it as going in the
 884                         same direction. */
 885                         Train *other_train = block->get_train();
 886                         int other_entry = other_train->get_entry_to_block(*block);
 887                         if(other_entry<0)
 888                                 throw logic_error("block reservation inconsistency");
 889
 890                         unsigned exit = block.reverse().entry();
 891                         unsigned other_exit = BlockIter(block.block(), other_entry).reverse().entry();
 892                         bool entry_conflict = (block.entry()==other_exit);
 893                         bool exit_conflict = (exit==static_cast<unsigned>(other_entry));
 894                         if(!entry_conflict && !last->get_turnout_id())
 895                         {
 896                                 /* The other train is not coming to the blocks we're holding, so we
 897                                 can keep them. */
 898                                 good_end = blocks.end();
 899
 900                                 if(static_cast<unsigned>(other_entry)==block.entry())
 901                                         preceding_train = other_train;
 902                         }
 903
 904                         int other_prio = other_train->get_priority();
 905
 906                         if(!entry_conflict && !exit_conflict && other_prio<priority)
 907                         {
 908                                 /* Ask a lesser priority train going to the same direction to free
 909                                 the block for us */
 910                                 if(other_train->free_block(*block))
 911                                 {
 912                                         blocks.push_back(block);
 913                                         if(!(reserved = block->reserve(this)))
 914                                                 blocks.pop_back();
 915                                 }
 916                         }
 917                         else if(other_train!=yielding_to && (other_prio<priority || (other_prio==priority && entry_conflict)))
 918                         {
 919                                 /* A lesser priority train is coming at us, we must ask it to free
 920                                 enough blocks to get clear of it to avoid a potential deadlock */
 921                                 blocking_train = other_train;
 922                                 contested_blocks.clear();
 923                                 contested_blocks.push_back(block);
 924                                 continue;
 925                         }
 926
 927                         if(!reserved)
 928                         {
 929                                 pending_block = &*block;
 930                                 break;
 931                         }
 932                 }
 933
 934                 if(!contested_blocks.empty() && contested_blocks.front()==block)
 935                         contested_blocks.pop_front();
 936
 937                 if(cur_blocks_end==blocks.end())
 938                         --cur_blocks_end;
 939                 if(clear_blocks_end==blocks.end())
 940                         --clear_blocks_end;
 941                 if(good_end==blocks.end())
 942                         --good_end;
 943
 944                 if(block->get_sensor_id())
 945                         ++nsens;
 946                 if(nsens>0)
 947                         dist += block->get_path_length(block.entry());
 948         }
 949
 950         // Unreserve blocks that were not good
 951         release_blocks(good_end, blocks.end());
 952
 953         if(blocks.back()!=start)
 954                 // We got some new blocks, so no longer need to yield
 955                 yielding_to = 0;
 956
 957         check_turnout_paths(true);
 958
 959         // Make any sensorless blocks at the beginning immediately current
 960         while(cur_blocks_end!=clear_blocks_end && !(*cur_blocks_end)->get_sensor_id())
 961                 ++cur_blocks_end;
 962 }
 963
 964 void Train::check_turnout_paths(bool set)
 965 {
 966         if(clear_blocks_end==blocks.end())
 967                 return;
 968
 969         for(list<BlockIter>::iterator i=clear_blocks_end; i!=blocks.end(); ++i)
 970         {
 971                 if((*i)->get_turnout_id())
 972                 {
 973                         TrackIter track = i->track_iter();
 974                         const TrackType::Endpoint &track_ep = track.endpoint();
 975
 976                         unsigned path = 0;
 977                         list<BlockIter>::iterator j = i;
 978                         if(++j!=blocks.end())
 979                         {
 980                                 TrackIter rev = j->track_iter().flip();
 981                                 unsigned mask = rev.endpoint().paths&track_ep.paths;
 982                                 for(path=0; mask>1; mask>>=1, ++path) ;
 983                         }
 984                         else
 985                                 return;
 986
 987                         if(path!=track->get_active_path())
 988                         {
 989                                 if(set)
 990                                         track->set_active_path(path);
 991
 992                                 /* Check again, in case the driver was able to service the request
 993                                 instantly */
 994                                 if(!set || path!=track->get_active_path())
 995                                         continue;
 996                         }
 997                 }
 998
 999                 if(i==clear_blocks_end)
1000                         ++clear_blocks_end;
1001                 if(i==cur_blocks_end && !(*i)->get_sensor_id())
1002                         ++cur_blocks_end;
1003         }
1004 }
1005
1006 float Train::get_reserved_distance_until(const Block *until_block, bool back) const
1007 {
1008         if(blocks.empty())
1009                 return 0;
1010
1011         Vehicle &veh = *(reverse!=back ? vehicles.back() : vehicles.front());
1012         const VehicleType &vtype = veh.get_type();
1013
1014         TrackIter track(veh.get_track(), veh.get_entry());
1015         if(!track)  // XXX Probably unnecessary
1016                 return 0;
1017
1018         BlockList::const_iterator block = blocks.begin();
1019         while(block!=clear_blocks_end && !(*block)->has_track(*track))
1020                 ++block;
1021         if(block==clear_blocks_end || &**block==until_block)
1022                 return 0;
1023
1024         float result = veh.get_offset();
1025         if(reverse!=back)
1026                 track = track.reverse();
1027         else
1028                 result = track->get_type().get_path_length(track->get_active_path())-result;
1029         result -= vtype.get_length()/2;
1030
1031         while(1)
1032         {
1033                 track = track.next();
1034                 if(!track)
1035                         break;
1036
1037                 if(!(*block)->has_track(*track))
1038                 {
1039                         if(back)
1040                         {
1041                                 if(block==blocks.begin())
1042                                         break;
1043                                 --block;
1044                         }
1045                         else
1046                         {
1047                                 ++block;
1048                                 if(block==clear_blocks_end)
1049                                         break;
1050                         }
1051
1052                         if(&**block==until_block)
1053                                 break;
1054                 }
1055
1056                 result += track->get_type().get_path_length(track->get_active_path());
1057         }
1058
1059         return result;
1060 }
1061
1062 void Train::release_blocks()
1063 {
1064         release_blocks(blocks.begin(), blocks.end());
1065 }
1066
1067 void Train::release_blocks(BlockList::iterator begin, BlockList::iterator end)
1068 {
1069         while(begin!=end)
1070         {
1071                 if(begin==cur_blocks_end)
1072                         cur_blocks_end = end;
1073                 if(begin==clear_blocks_end)
1074                         clear_blocks_end = end;
1075
1076                 Block &block = **begin;
1077                 blocks.erase(begin++);
1078                 block.reserve(0);
1079
1080                 if(begin==blocks.end())
1081                         end_of_route = false;
1082         }
1083 }
1084
1085 void Train::reverse_blocks(BlockList &blks) const
1086 {
1087         blks.reverse();
1088         for(BlockList::iterator i=blks.begin(); i!=blks.end(); ++i)
1089                 *i = i->reverse();
1090 }
1091
1092 bool Train::advance_route(list<const Route *>::iterator &iter, Track &track)
1093 {
1094         while(iter!=routes.end() && !(*iter)->has_track(track))
1095                 ++iter;
1096         if(iter==routes.end())
1097                 return false;
1098
1099         return true;
1100 }
1101
1102 Route *Train::create_lead_route(Route *lead, const Route *target)
1103 {
1104         if(!lead)
1105         {
1106                 lead = new Route(layout);
1107                 lead->set_name("Lead");
1108                 lead->set_temporary(true);
1109         }
1110
1111         set<Track *> tracks;
1112         for(BlockList::iterator i=blocks.begin(); i!=blocks.end(); ++i)
1113         {
1114                 const set<Track *> &btracks = (*i)->get_tracks();
1115                 for(set<Track *>::const_iterator j=btracks.begin(); j!=btracks.end(); ++j)
1116                         if(!target || !target->has_track(**j))
1117                                 tracks.insert(*j);
1118         }
1119
1120         lead->add_tracks(tracks);
1121
1122         return lead;
1123 }
1124
1125
1126 Train::Loader::Loader(Train &t):
1127         DataFile::ObjectLoader<Train>(t),
1128         prev_block(0),
1129         blocks_valid(true)
1130 {
1131         add("block",       &Loader::block);
1132         add("block_hint",  &Loader::block_hint);
1133         add("name",        &Loader::name);
1134         add("priority",    &Train::priority);
1135         add("quantized_speed",  &Loader::quantized_speed);
1136         add("route",       &Loader::route);
1137         add("timetable",   &Loader::timetable);
1138         add("vehicle",     &Loader::vehicle);
1139 }
1140
1141 void Train::Loader::finish()
1142 {
1143         if(!obj.blocks.empty())
1144         {
1145                 TrackIter track = obj.blocks.front().track_iter();
1146                 float offset = 2*obj.layout.get_catalogue().get_scale();
1147                 obj.vehicles.back()->place(*track, track.entry(), offset, Vehicle::BACK_BUFFER);
1148         }
1149 }
1150
1151 void Train::Loader::block(unsigned id)
1152 {
1153         if(!blocks_valid)
1154                 return;
1155
1156         Block *blk;
1157         try
1158         {
1159                 blk = &obj.layout.get_block(id);
1160         }
1161         catch(const key_error &)
1162         {
1163                 blocks_valid = false;
1164                 return;
1165         }
1166
1167         int entry = -1;
1168         if(prev_block)
1169                 entry = blk->get_endpoint_by_link(*prev_block);
1170         if(entry<0)
1171                 entry = 0;
1172
1173         obj.blocks.push_back(BlockIter(blk, entry));
1174         blk->reserve(&obj);
1175
1176         if(blk->get_sensor_id())
1177                 obj.layout.get_driver().set_sensor(blk->get_sensor_id(), true);
1178
1179         prev_block = blk;
1180 }
1181
1182 void Train::Loader::block_hint(unsigned id)
1183 {
1184         try
1185         {
1186                 prev_block = &obj.layout.get_block(id);
1187         }
1188         catch(const key_error &)
1189         {
1190                 blocks_valid = false;
1191         }
1192 }
1193
1194 void Train::Loader::name(const string &n)
1195 {
1196         obj.set_name(n);
1197 }
1198
1199 void Train::Loader::quantized_speed()
1200 {
1201         if(obj.speed_quantizer)
1202                 load_sub(*obj.speed_quantizer);
1203 }
1204
1205 void Train::Loader::route(const string &n)
1206 {
1207         obj.set_route(&obj.layout.get_route(n));
1208 }
1209
1210 void Train::Loader::timetable()
1211 {
1212         Timetable *ttbl = new Timetable(obj);
1213         load_sub(*ttbl);
1214 }
1215
1216 void Train::Loader::vehicle(ArticleNumber art_nr)
1217 {
1218         const VehicleType &vtype = obj.layout.get_catalogue().get_vehicle(art_nr);
1219         Vehicle *veh = new Vehicle(obj.layout, vtype);
1220         obj.vehicles.back()->attach_back(*veh);
1221         obj.vehicles.push_back(veh);
1222 }
1223
1224 } // namespace R2C2