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