source/libr2c2/blockallocator.cpp

   1 #include <msp/core/maputils.h>
   2 #include <msp/core/raii.h>
   3 #include "beamgate.h"
   4 #include "blockallocator.h"
   5 #include "block.h"
   6 #include "catalogue.h"
   7 #include "driver.h"
   8 #include "layout.h"
   9 #include "trackcircuit.h"
  10 #include "trackiter.h"
  11 #include "train.h"
  12 #include "vehicle.h"
  13
  14 using namespace std;
  15 using namespace Msp;
  16
  17 namespace R2C2 {
  18
  19 struct BlockAllocator::BlockMatch
  20 {
  21         const Block &block;
  22
  23         BlockMatch(const Block &b): block(b) { }
  24
  25         bool operator()(const BlockIter &bi) const { return &*bi==&block; }
  26 };
  27
  28
  29 BlockAllocator::BlockAllocator(Train &t):
  30         train(t),
  31         active(false),
  32         cur_blocks_end(blocks.end()),
  33         next_sensor(0),
  34         pending_block(0),
  35         stop_at_block(0),
  36         reserving(false),
  37         advancing(false),
  38         reserve_pending(false)
  39 {
  40         Layout &layout = train.get_layout();
  41         layout.signal_block_reserved.connect(sigc::mem_fun(this, &BlockAllocator::block_reserved));
  42         layout.signal_sensor_state_changed.connect(sigc::mem_fun(this, &BlockAllocator::sensor_state_changed));
  43         layout.get_driver().signal_halt.connect(sigc::mem_fun(this, &BlockAllocator::halt_event));
  44
  45         const set<Track *> &tracks = layout.get_all<Track>();
  46         for(set<Track *>::const_iterator i=tracks.begin(); i!=tracks.end(); ++i)
  47                 if((*i)->get_type().is_turnout())
  48                 {
  49                         (*i)->signal_path_changing.connect(sigc::hide(sigc::bind(sigc::mem_fun(this, &BlockAllocator::turnout_path_changing), sigc::ref(**i))));
  50                         (*i)->signal_path_changed.connect(sigc::hide(sigc::bind(sigc::mem_fun(this, &BlockAllocator::turnout_path_changed), sigc::ref(**i))));
  51                 }
  52 }
  53
  54 void BlockAllocator::set_active(bool a)
  55 {
  56         active = a;
  57         if(active)
  58                 reserve_more();
  59         else
  60         {
  61                 release_blocks_end(cur_blocks_end);
  62                 pending_block = 0;
  63                 reserve_pending = false;
  64         }
  65 }
  66
  67 bool BlockAllocator::start_from(const BlockIter &block)
  68 {
  69         if(!block)
  70                 throw invalid_argument("BlockAllocator::start_from");
  71
  72         float remaining_length = 0;
  73         unsigned n_vehs = train.get_n_vehicles();
  74         for(unsigned i=0; i<n_vehs; ++i)
  75                 remaining_length += train.get_vehicle(i).get_type().get_length();
  76
  77         clear();
  78
  79         BlockList blocks_to_reserve;
  80         for(BlockIter b=block; b; b=b.next())
  81         {
  82                 if(b->get_train())
  83                         break;
  84                 blocks_to_reserve.push_back(b);
  85                 for(TrackIter t=b.track_iter(); (t && &t->get_block()==&*b); t=t.next())
  86                         remaining_length -= t->get_path_length();
  87                 if(remaining_length<=0)
  88                         break;
  89         }
  90
  91         if(remaining_length>0)
  92                 return false;
  93
  94         for(BlockList::iterator i=blocks_to_reserve.begin(); i!=blocks_to_reserve.end(); ++i)
  95         {
  96                 blocks.push_back(*i);
  97                 try
  98                 {
  99                         (*i)->reserve(&train);
 100                 }
 101                 catch(...)
 102                 {
 103                         blocks.pop_back();
 104                         while(i!=blocks_to_reserve.begin())
 105                         {
 106                                 blocks.pop_back();
 107                                 (*--i)->reserve(0);
 108                         }
 109                         throw;
 110                 }
 111         }
 112
 113         return true;
 114 }
 115
 116 void BlockAllocator::rewind_to(const Block &block)
 117 {
 118         if(!active)
 119                 return;
 120
 121         BlockList::iterator i = find_if(cur_blocks_end, blocks.end(), BlockMatch(block));
 122         if(i!=blocks.end())
 123         {
 124                 release_blocks_end(i);
 125                 reserve_more();
 126         }
 127 }
 128
 129 void BlockAllocator::clear()
 130 {
 131         release_blocks_begin(blocks.end());
 132         active = false;
 133         next_sensor = 0;
 134         pending_block = 0;
 135         stop_at_block = 0;
 136 }
 137
 138 void BlockAllocator::stop_at(const Block *block)
 139 {
 140         stop_at_block = block;
 141         if(active && !block)
 142                 reserve_more();
 143 }
 144
 145 const BlockIter &BlockAllocator::first() const
 146 {
 147         if(blocks.empty())
 148                 throw logic_error("no blocks");
 149         return blocks.front();
 150 }
 151
 152 const BlockIter &BlockAllocator::last() const
 153 {
 154         if(blocks.empty())
 155                 throw logic_error("no blocks");
 156         BlockList::const_iterator i = --blocks.end();
 157         if(i->block()==pending_block)
 158                 --i;
 159         return *i;
 160 }
 161
 162 const BlockIter &BlockAllocator::last_current() const
 163 {
 164         if(blocks.empty())
 165                 throw logic_error("no blocks");
 166         if(cur_blocks_end==blocks.begin())
 167                 throw logic_error("internal error (no current blocks)");
 168         BlockList::const_iterator i = cur_blocks_end;
 169         return *--i;
 170 }
 171
 172 const BlockIter &BlockAllocator::iter_for(const Block &block) const
 173 {
 174         BlockList::const_iterator i = find_if(blocks.begin(), blocks.end(), BlockMatch(block));
 175         if(i==blocks.end())
 176                 throw key_error(&block);
 177         return *i;
 178 }
 179
 180 bool BlockAllocator::has_block(const Block &block) const
 181 {
 182         return find_if(blocks.begin(), blocks.end(), BlockMatch(block))!=blocks.end();
 183 }
 184
 185 bool BlockAllocator::is_block_current(const Block &block) const
 186 {
 187         BlockList::const_iterator end = cur_blocks_end;
 188         return find_if(blocks.begin(), end, BlockMatch(block))!=cur_blocks_end;
 189 }
 190
 191 void BlockAllocator::tick()
 192 {
 193         if(reserve_pending)
 194         {
 195                 reserve_pending = false;
 196                 reserve_more();
 197         }
 198 }
 199
 200 void BlockAllocator::reserve_more()
 201 {
 202         if(blocks.empty())
 203                 throw logic_error("no blocks");
 204
 205         if(train.get_layout().get_driver().is_halted())
 206                 return;
 207
 208         BlockIter start = blocks.back();
 209         if(&*start==stop_at_block)
 210                 return;
 211         else if(&*start==pending_block)
 212         {
 213                 TrackIter track = start.track_iter();
 214                 if(track->is_path_changing() || !track.endpoint().has_path(track->get_active_path()))
 215                         return;
 216         }
 217
 218         pending_block = 0;
 219
 220         // See how many sensor blocks and how much track we already have
 221         unsigned nsens = 0;
 222         float dist = 0;
 223         for(BlockList::const_iterator i=cur_blocks_end; i!=blocks.end(); ++i)
 224         {
 225                 if((*i)->get_sensor_address())
 226                         ++nsens;
 227                 if(nsens>0)
 228                         dist += (*i)->get_path_length(i->entry());
 229         }
 230
 231         float approach_margin = 50*train.get_layout().get_catalogue().get_scale();
 232         float min_dist = train.get_controller().get_braking_distance()*1.3+approach_margin*2;
 233
 234         BlockIter block = start;
 235
 236         SetFlag setf(reserving);
 237
 238         while(1)
 239         {
 240                 BlockIter prev = block;
 241                 block = block.next();
 242                 if(!block || block->get_endpoints().size()<2)
 243                         // The track ends here
 244                         break;
 245
 246                 if(block->get_turnout_address() && !prev->get_turnout_address())
 247                 {
 248                         /* We are arriving at a turnout.  See if we have enough blocks and
 249                         distance reserved. */
 250                         if(nsens>=3 && dist>=min_dist)
 251                                 break;
 252                 }
 253
 254                 if(!reserve_block(block))
 255                 {
 256                         pending_block = &*block;
 257                         break;
 258                 }
 259
 260                 if(cur_blocks_end==blocks.end())
 261                         --cur_blocks_end;
 262
 263                 TrackIter track = block.track_iter();
 264                 if(track->is_path_changing())
 265                 {
 266                         pending_block = &*block;
 267                         break;
 268                 }
 269                 else
 270                 {
 271                         unsigned path = track->get_active_path();
 272                         if(!track.endpoint().has_path(path))
 273                         {
 274                                 path = track->get_type().coerce_path(track.entry(), path);
 275                                 track->set_active_path(path);
 276                                 if(track->is_path_changing())
 277                                 {
 278                                         pending_block = &*block;
 279                                         break;
 280                                 }
 281                         }
 282                 }
 283
 284                 if(&*block==stop_at_block)
 285                         break;
 286
 287                 if(block->get_sensor_address())
 288                         ++nsens;
 289                 if(nsens>0)
 290                         dist += block->get_path_length(block.entry());
 291         }
 292
 293         if(!next_sensor)
 294         {
 295                 update_next_sensor(0);
 296                 // Immediately advance to just before the next sensor
 297                 advance_front(next_sensor);
 298         }
 299 }
 300
 301 bool BlockAllocator::reserve_block(const BlockIter &block)
 302 {
 303         /* Add it to the list first to present a consistent state in block_reserved
 304         signal. */
 305         blocks.push_back(block);
 306
 307         bool first_reserve = (cur_blocks_end==blocks.end());
 308         if(first_reserve)
 309                 --cur_blocks_end;
 310
 311         try
 312         {
 313                 if(!block->reserve(&train))
 314                 {
 315                         if(first_reserve)
 316                                 cur_blocks_end = blocks.end();
 317                         blocks.pop_back();
 318                         return false;
 319                 }
 320
 321                 return true;
 322         }
 323         catch(...)
 324         {
 325                 if(first_reserve)
 326                         cur_blocks_end = blocks.end();
 327                 blocks.pop_back();
 328                 throw;
 329         }
 330 }
 331
 332 void BlockAllocator::advance_front(const Block *block, bool inclusive)
 333 {
 334         BlockList::iterator end;
 335         if(block)
 336         {
 337                 end = cur_blocks_end;
 338                 if(inclusive)
 339                         --end;
 340                 end = find_if(end, blocks.end(), BlockMatch(*block));
 341                 if(inclusive && end!=blocks.end())
 342                         ++end;
 343         }
 344         else
 345                 end = blocks.end();
 346
 347         if(end==blocks.end() && blocks.back().block()==pending_block)
 348                 --end;
 349
 350         SetFlag setf(advancing);
 351         BlockList::iterator i = cur_blocks_end;
 352         // Update cur_blocks_end first to keep things consistent.
 353         cur_blocks_end = end;
 354         for(; i!=end; ++i)
 355                 signal_advanced.emit(**i, (*i)->get_sensor());
 356 }
 357
 358 void BlockAllocator::advance_front(const Sensor *sensor)
 359 {
 360         if(sensor)
 361                 advance_front(sensor->get_block(), dynamic_cast<const BeamGate *>(sensor));
 362         else
 363                 advance_front(0, false);
 364 }
 365
 366 void BlockAllocator::advance_back()
 367 {
 368         bool rev = train.get_controller().get_reverse();
 369         const Vehicle &veh = train.get_vehicle(rev ? 0 : train.get_n_vehicles()-1);
 370         const Block &veh_block = veh.get_placement().get_position(rev ? VehiclePlacement::FRONT_AXLE : VehiclePlacement::BACK_AXLE)->get_block();
 371
 372         bool pending_cleared = false;
 373
 374         /* Sensors aren't guaranteed to be detriggered in order.  Go through the
 375         block list and locate the first sensor that's still active. */
 376         BlockList::iterator end = blocks.end();
 377         for(BlockList::iterator i=blocks.begin(); i!=cur_blocks_end; ++i)
 378         {
 379                 Block *block = &**i;
 380                 list<Sensor *> sensors;
 381
 382                 /* Collect all sensors from the block in the order they are expected to
 383                 detrigger. */
 384                 for(TrackIter j=i->track_iter(); (j && &j->get_block()==block); j=j.next())
 385                         if(!j->get_attachments().empty())
 386                         {
 387                                 Track::AttachmentList attachments = j->get_attachments_ordered(j.entry());
 388                                 for(Track::AttachmentList::const_iterator k=attachments.begin(); k!=attachments.end(); ++k)
 389                                         if(BeamGate *gate = dynamic_cast<BeamGate *>(*k))
 390                                                 sensors.push_back(gate);
 391                         }
 392
 393                 if(Sensor *sensor = (*i)->get_sensor())
 394                         sensors.push_back(sensor);
 395
 396                 /* See if any sensor is still active, and record the position of the
 397                 last inactive sensor. */
 398                 bool active_sensor = false;
 399                 for(list<Sensor *>::const_iterator j=sensors.begin(); (!active_sensor && j!=sensors.end()); ++j)
 400                 {
 401                         if((*j)->get_state())
 402                                 active_sensor = true;
 403                         else
 404                                 end = i;
 405                 }
 406
 407                 if(block==pending_block)
 408                         pending_cleared = true;
 409
 410                 // Stop if we encounter an active sensor or the train's last vehicle
 411                 if(block==&veh_block || active_sensor)
 412                 {
 413                         if(end!=blocks.end())
 414                         {
 415                                 /* If the last inactive sensor was in an earlier block, release
 416                                 that block as well. */
 417                                 if(i!=end)
 418                                         ++end;
 419                                 release_blocks_begin(end);
 420                                 signal_rear_advanced.emit(*last());
 421                         }
 422
 423                         if(pending_cleared)
 424                                 reserve_more();
 425
 426                         return;
 427                 }
 428         }
 429 }
 430
 431 void BlockAllocator::release_blocks_begin(const BlockList::iterator &end)
 432 {
 433         for(BlockList::iterator i=blocks.begin(); i!=end; )
 434                 release_block(i++);
 435 }
 436
 437 void BlockAllocator::release_blocks_end(const BlockList::iterator &begin)
 438 {
 439         // Guard against decrementing blocks.begin()
 440         if(begin==blocks.begin())
 441                 return release_blocks_begin(blocks.end());
 442
 443         if(begin==blocks.end())
 444                 return;
 445
 446         /* Release the blocks in reverse order so that a consistent state is
 447         presented in block_reserved signal. */
 448         bool done = false;
 449         for(BlockList::iterator i=--blocks.end(); !done; )
 450         {
 451                 done = (i==begin);
 452                 release_block(i--);
 453         }
 454 }
 455
 456 void BlockAllocator::release_block(const BlockList::iterator &i)
 457 {
 458         if(advancing)
 459                 throw logic_error("cannot release while advancing");
 460         if(i==cur_blocks_end)
 461                 ++cur_blocks_end;
 462         if(next_sensor && &**i==next_sensor->get_block())
 463                 next_sensor = 0;
 464         if(&**i==pending_block)
 465                 pending_block = 0;
 466
 467         Block &block = **i;
 468         blocks.erase(i);
 469         block.reserve(0);
 470 }
 471
 472 void BlockAllocator::reverse()
 473 {
 474         release_blocks_end(cur_blocks_end);
 475         blocks.reverse();
 476         for(BlockList::iterator i=blocks.begin(); i!=blocks.end(); ++i)
 477                 *i = i->reverse();
 478
 479         if(active)
 480                 reserve_more();
 481 }
 482
 483 void BlockAllocator::turnout_path_changing(Track &track)
 484 {
 485         BlockList::iterator i = find_if(cur_blocks_end, blocks.end(), BlockMatch(track.get_block()));
 486         if(i!=blocks.end())
 487         {
 488                 ++i;
 489                 release_blocks_end(i);
 490                 pending_block = &track.get_block();
 491         }
 492 }
 493
 494 void BlockAllocator::turnout_path_changed(Track &track)
 495 {
 496         if(&track.get_block()==pending_block && !reserving)
 497                 reserve_pending = true;
 498 }
 499
 500 void BlockAllocator::block_reserved(Block &block, const Train *tr)
 501 {
 502         if(&block==pending_block && !tr && !reserving)
 503                 reserve_pending = true;
 504 }
 505
 506 void BlockAllocator::sensor_state_changed(Sensor &sensor, Sensor::State state)
 507 {
 508         Block *block = sensor.get_block();
 509         if(!block || block->get_train()!=&train)
 510                 return;
 511
 512         if(state==Sensor::MAYBE_ACTIVE)
 513         {
 514                 if(&sensor==next_sensor)
 515                 {
 516                         if(is_block_current(*block))
 517                                 signal_advanced.emit(*block, &sensor);
 518                         update_next_sensor(next_sensor);
 519                         advance_front(next_sensor);
 520
 521                         if(active)
 522                                 reserve_pending = true;
 523                 }
 524                 else if(!is_block_current(*block))
 525                         train.get_layout().emergency(block, "Sensor for "+train.get_name()+" triggered out of order");
 526         }
 527         else if(state==Sensor::INACTIVE)
 528                 advance_back();
 529 }
 530
 531 void BlockAllocator::update_next_sensor(Sensor *after)
 532 {
 533         BeamGate *after_gate = dynamic_cast<BeamGate *>(after);
 534
 535         BlockList::iterator i = cur_blocks_end;
 536         if(after)
 537         {
 538                 if(after_gate)
 539                         --i;
 540                 i = find_if(i, blocks.end(), BlockMatch(*after->get_block()));
 541         }
 542
 543         for(; i!=blocks.end(); ++i)
 544         {
 545                 if(Sensor *sensor = (*i)->get_sensor())
 546                 {
 547                         if(!after_gate && sensor!=next_sensor)
 548                         {
 549                                 next_sensor = sensor;
 550                                 return;
 551                         }
 552                 }
 553
 554                 Block *block = &**i;
 555                 for(TrackIter j=i->track_iter(); (j && &j->get_block()==block); j=j.next())
 556                         if(!j->get_attachments().empty())
 557                         {
 558                                 Track::AttachmentList attachments = j->get_attachments_ordered(j.entry());
 559                                 for(Track::AttachmentList::const_iterator k=attachments.begin(); k!=attachments.end(); ++k)
 560                                         if(BeamGate *gate = dynamic_cast<BeamGate *>(*k))
 561                                         {
 562                                                 if(after_gate)
 563                                                 {
 564                                                         if(gate==after_gate)
 565                                                                 after_gate = 0;
 566                                                 }
 567                                                 else
 568                                                 {
 569                                                         next_sensor = gate;
 570                                                         return;
 571                                                 }
 572                                         }
 573                         }
 574         }
 575
 576         next_sensor = 0;
 577 }
 578
 579 void BlockAllocator::halt_event(bool halted)
 580 {
 581         if(active && !halted)
 582                 reserve_more();
 583 }
 584
 585 void BlockAllocator::save(list<DataFile::Statement> &st) const
 586 {
 587         if(!blocks.empty() && cur_blocks_end!=blocks.begin())
 588         {
 589                 BlockList cur_blocks(blocks.begin(), BlockList::const_iterator(cur_blocks_end));
 590                 BlockIter prev;
 591                 if(train.get_controller().get_reverse())
 592                 {
 593                         cur_blocks.reverse();
 594                         prev = cur_blocks.front().next();
 595                 }
 596                 else
 597                         prev = cur_blocks.front().flip();
 598
 599                 st.push_back((DataFile::Statement("hint"), prev->get_id()));
 600
 601                 for(BlockList::const_iterator i=cur_blocks.begin(); (i!=cur_blocks.end() && i->block()!=pending_block); ++i)
 602                         st.push_back((DataFile::Statement("block"), (*i)->get_id()));
 603         }
 604 }
 605
 606
 607 BlockAllocator::Loader::Loader(BlockAllocator &ba):
 608         DataFile::ObjectLoader<BlockAllocator>(ba),
 609         valid(true)
 610 {
 611         add("block", &Loader::block);
 612         add("hint",  &Loader::hint);
 613 }
 614
 615 void BlockAllocator::Loader::block(unsigned id)
 616 {
 617         if(!valid)
 618                 return;
 619
 620         Block *blk;
 621         try
 622         {
 623                 blk = &obj.train.get_layout().get_block(id);
 624         }
 625         catch(const key_error &)
 626         {
 627                 valid = false;
 628                 return;
 629         }
 630
 631         int entry = -1;
 632         if(prev_block)
 633                 entry = blk->get_endpoint_by_link(*prev_block);
 634         if(entry<0)
 635                 entry = 0;
 636
 637         obj.blocks.push_back(BlockIter(blk, entry));
 638         blk->reserve(&obj.train);
 639
 640         if(blk->get_sensor_address())
 641                 obj.train.get_layout().get_driver().set_sensor(blk->get_sensor_address(), true);
 642
 643         prev_block = blk;
 644 }
 645
 646 void BlockAllocator::Loader::hint(unsigned id)
 647 {
 648         try
 649         {
 650                 prev_block = &obj.train.get_layout().get_block(id);
 651         }
 652         catch(const key_error &)
 653         {
 654                 valid = false;
 655         }
 656 }
 657
 658 } // namespace R2C2