source/libr2c2/layout.cpp

   1 #include <algorithm>
   2 #include <msp/core/maputils.h>
   3 #include <msp/core/raii.h>
   4 #include <msp/core/refptr.h>
   5 #include <msp/datafile/parser.h>
   6 #include <msp/datafile/writer.h>
   7 #include <msp/io/print.h>
   8 #include <msp/time/utils.h>
   9 #include "beamgate.h"
  10 #include "block.h"
  11 #include "catalogue.h"
  12 #include "driver.h"
  13 #include "layout.h"
  14 #include "route.h"
  15 #include "signal.h"
  16 #include "signaltype.h"
  17 #include "terrain.h"
  18 #include "track.h"
  19 #include "trackcircuit.h"
  20 #include "tracktype.h"
  21 #include "train.h"
  22 #include "vehicle.h"
  23 #include "vehicletype.h"
  24 #include "zone.h"
  25
  26 using namespace std;
  27 using namespace Msp;
  28
  29 namespace {
  30
  31 bool zone_order(const R2C2::Zone *z1, const R2C2::Zone *z2)
  32 {
  33         return z1->get_number()<z2->get_number();
  34 }
  35
  36 }
  37
  38
  39 namespace R2C2 {
  40
  41 Layout::Layout(Catalogue &c, Driver *d):
  42         catalogue(c),
  43         driver(d),
  44         next_turnout_addr(0x800),
  45         emitting_block_reserved(false)
  46 {
  47         clock.set_rate(60);
  48 }
  49
  50 Layout::~Layout()
  51 {
  52         delete driver;
  53         driver = 0;
  54
  55         while(!trains.empty())
  56                 delete trains.begin()->second;
  57         track_chains.del<Route>();
  58         track_chains.del<Zone>();
  59         objects.del<Signal>();
  60         objects.del<Track>();
  61         track_chains.del<Block>();
  62 }
  63
  64 Driver &Layout::get_driver() const
  65 {
  66         if(!driver)
  67                 throw logic_error("!driver");
  68         return *driver;
  69 }
  70
  71 void Layout::add(Object &o)
  72 {
  73         if(objects.insert(o))
  74         {
  75                 try
  76                 {
  77                         signal_object_added.emit(o);
  78                 }
  79                 catch(...)
  80                 {
  81                         objects.erase(o);
  82                         throw;
  83                 }
  84         }
  85 }
  86
  87 void Layout::add(Track &t)
  88 {
  89         if(objects.insert(t))
  90         {
  91                 try
  92                 {
  93                         // Blocks must be recreated first
  94                         create_blocks();
  95                         signal_object_added.emit(t);
  96                 }
  97                 catch(...)
  98                 {
  99                         objects.erase(t);
 100                         create_blocks();
 101                         throw;
 102                 }
 103         }
 104 }
 105
 106 void Layout::add(TrackChain &g)
 107 {
 108         if(track_chains.insert(g))
 109         {
 110                 try
 111                 {
 112                         signal_track_chain_added.emit(g);
 113                 }
 114                 catch(...)
 115                 {
 116                         track_chains.erase(g);
 117                         throw;
 118                 }
 119         }
 120 }
 121
 122 void Layout::add(Block &b)
 123 {
 124         if(track_chains.insert(b))
 125         {
 126                 sigc::connection conn = b.signal_reserved.connect(sigc::bind<0>(sigc::mem_fun(this, &Layout::block_reserved), sigc::ref(b)));
 127                 try
 128                 {
 129                         signal_track_chain_added.emit(b);
 130                 }
 131                 catch(...)
 132                 {
 133                         track_chains.erase(b);
 134                         conn.disconnect();
 135                         throw;
 136                 }
 137         }
 138 }
 139
 140 void Layout::add(Sensor &s)
 141 {
 142         if(sensors.insert(s))
 143         {
 144                 s.signal_state_changed.connect(sigc::bind<0>(sigc::mem_fun(this, &Layout::sensor_state_changed), sigc::ref(s)));
 145                 s.signal_state_changed.connect(sigc::bind<0>(signal_sensor_state_changed, sigc::ref(s)));
 146         }
 147 }
 148
 149 void Layout::add(BeamGate &g)
 150 {
 151         add(static_cast<Object &>(g));
 152         add(static_cast<Sensor &>(g));
 153 }
 154
 155 template<>
 156 const set<Object *> &Layout::get_all<Object>() const
 157 {
 158         return objects.get();
 159 }
 160
 161 template<>
 162 const set<Track *> &Layout::get_all<Track>() const
 163 {
 164         return objects.get<Track>();
 165 }
 166
 167 template<>
 168 const set<Signal *> &Layout::get_all<Signal>() const
 169 {
 170         return objects.get<Signal>();
 171 }
 172
 173 template<>
 174 const set<Vehicle *> &Layout::get_all<Vehicle>() const
 175 {
 176         return objects.get<Vehicle>();
 177 }
 178
 179 template<>
 180 const set<Terrain *> &Layout::get_all<Terrain>() const
 181 {
 182         return objects.get<Terrain>();
 183 }
 184
 185 template<>
 186 const set<TrackChain *> &Layout::get_all<TrackChain>() const
 187 {
 188         return track_chains.get();
 189 }
 190
 191 template<>
 192 const set<Block *> &Layout::get_all<Block>() const
 193 {
 194         return track_chains.get<Block>();
 195 }
 196
 197 template<>
 198 const set<Route *> &Layout::get_all<Route>() const
 199 {
 200         return track_chains.get<Route>();
 201 }
 202
 203 template<>
 204 const set<Zone *> &Layout::get_all<Zone>() const
 205 {
 206         return track_chains.get<Zone>();
 207 }
 208
 209 template<>
 210 const set<Sensor *> &Layout::get_all<Sensor>() const
 211 {
 212         return sensors.get<Sensor>();
 213 }
 214
 215 template<>
 216 const set<TrackCircuit *> &Layout::get_all<TrackCircuit>() const
 217 {
 218         return sensors.get<TrackCircuit>();
 219 }
 220
 221 template<>
 222 const set<BeamGate *> &Layout::get_all<BeamGate>() const
 223 {
 224         return sensors.get<BeamGate>();
 225 }
 226
 227 template<typename T>
 228 T *Layout::pick(const Ray &ray)
 229 {
 230         const set<Object *> &objs = objects.get();
 231         T *closest = 0;
 232         float distance = -1;
 233         for(set<Object *>::const_iterator i=objs.begin(); i!=objs.end(); ++i)
 234                 if(T *t = dynamic_cast<T *>(*i))
 235                 {
 236                         float d = -1;
 237                         if(t->collide_ray(ray, &d))
 238                                 if(!closest || d<distance)
 239                                 {
 240                                         closest = t;
 241                                         distance = d;
 242                                 }
 243                 }
 244
 245         return closest;
 246 }
 247
 248 template Object *Layout::pick<Object>(const Ray &);
 249 template Track *Layout::pick<Track>(const Ray &);
 250 template Signal *Layout::pick<Signal>(const Ray &);
 251 template Vehicle *Layout::pick<Vehicle>(const Ray &);
 252
 253 void Layout::remove(Object &o)
 254 {
 255         if(objects.erase(o))
 256                 signal_object_removed.emit(o);
 257 }
 258
 259 void Layout::remove(Track &t)
 260 {
 261         if(objects.erase(t))
 262         {
 263                 // Blocks must be recreated first
 264                 create_blocks(t);
 265                 signal_object_removed.emit(t);
 266         }
 267 }
 268
 269 void Layout::remove(TrackChain &g)
 270 {
 271         if(track_chains.erase(g))
 272                 signal_track_chain_removed.emit(g);
 273 }
 274
 275 void Layout::remove(Sensor &s)
 276 {
 277         sensors.erase(s);
 278 }
 279
 280 void Layout::remove(BeamGate &g)
 281 {
 282         remove(static_cast<Object &>(g));
 283         remove(static_cast<Sensor &>(g));
 284 }
 285
 286 unsigned Layout::allocate_turnout_address()
 287 {
 288         set<unsigned> used_addrs;
 289         const set<Track *> &tracks = objects.get<Track>();
 290         for(set<Track *>::const_iterator i=tracks.begin(); i!=tracks.end(); ++i)
 291                 if((*i)->get_type().is_turnout())
 292                         used_addrs.insert((*i)->get_turnout_address());
 293
 294         unsigned result = next_turnout_addr;
 295         while(used_addrs.count(result))
 296                 ++result;
 297         next_turnout_addr = result+1;
 298
 299         return result;
 300 }
 301
 302 Block &Layout::get_block(unsigned id) const
 303 {
 304         const set<Block *> &blocks = track_chains.get<Block>();
 305         for(set<Block *>::const_iterator i=blocks.begin(); i!=blocks.end(); ++i)
 306                 if((*i)->get_id()==id)
 307                         return **i;
 308
 309         throw key_error(id);
 310 }
 311
 312 void Layout::create_blocks()
 313 {
 314         set<Track *> loose_tracks = objects.get<Track>();
 315         const set<Block *> *blocks = &track_chains.get<Block>();
 316         for(set<Block *>::const_iterator i=blocks->begin(); i!=blocks->end(); ++i)
 317         {
 318                 const set<Track *> &btracks = (*i)->get_tracks();
 319                 for(set<Track *>::const_iterator j=btracks.begin(); j!=btracks.end(); ++j)
 320                         loose_tracks.erase(*j);
 321         }
 322
 323         list<Block *> created_blocks;
 324         while(!loose_tracks.empty())
 325         {
 326                 Block *block = new Block(*this, **loose_tracks.begin());
 327                 created_blocks.push_back(block);
 328
 329                 const set<Track *> &btracks = block->get_tracks();
 330                 for(set<Track *>::const_iterator i=btracks.begin(); i!=btracks.end(); ++i)
 331                         loose_tracks.erase(*i);
 332         }
 333
 334         // The previously obtained set has been invalidated by creating new blocks
 335         blocks = &track_chains.get<Block>();
 336         for(list<Block *>::iterator i=created_blocks.begin(); i!=created_blocks.end(); ++i)
 337                 for(set<Block *>::const_iterator j=blocks->begin(); j!=blocks->end(); ++j)
 338                         if(*j!=*i)
 339                                 (*i)->check_link(**j);
 340 }
 341
 342 void Layout::create_blocks(Track &track)
 343 {
 344         /* Must collect the blocks in a set first while all tracks are still
 345         guaranteed to have blocks and to avoid duplicate deletes */
 346         set<Block *> del_blocks;
 347
 348         del_blocks.insert(&track.get_block());
 349
 350         const vector<Track *> &links = track.get_links();
 351         for(vector<Track *>::const_iterator i=links.begin(); i!=links.end(); ++i)
 352                 if(*i)
 353                         del_blocks.insert(&(*i)->get_block());
 354
 355         for(set<Block *>::iterator i=del_blocks.begin(); i!=del_blocks.end(); ++i)
 356                 delete *i;
 357
 358         create_blocks();
 359 }
 360
 361 Route &Layout::get_route(const string &name) const
 362 {
 363         const set<Route *> &routes = track_chains.get<Route>();
 364         for(set<Route *>::const_iterator i=routes.begin(); i!=routes.end(); ++i)
 365                 if((*i)->get_name()==name)
 366                         return **i;
 367         throw key_error(name);
 368 }
 369
 370 void Layout::update_routes()
 371 {
 372         const set<Route *> &routes = track_chains.get<Route>();
 373         for(set<Route *>::iterator i=routes.begin(); i!=routes.end(); ++i)
 374                 (*i)->update_turnouts();
 375 }
 376
 377 Layout::ZoneArray Layout::get_zones(const string &group) const
 378 {
 379         ZoneArray result;
 380         const set<Zone *> &zones = track_chains.get<Zone>();
 381         for(set<Zone *>::const_iterator i=zones.begin(); i!=zones.end(); ++i)
 382                 if((*i)->get_group()==group)
 383                         result.push_back(*i);
 384
 385         sort(result.begin(), result.end(), zone_order);
 386
 387         return result;
 388 }
 389
 390 Zone &Layout::get_zone(const string &group, unsigned num) const
 391 {
 392         const set<Zone *> &zones = track_chains.get<Zone>();
 393         for(set<Zone *>::const_iterator i=zones.begin(); i!=zones.end(); ++i)
 394                 if((*i)->get_group()==group && (*i)->get_number()==num)
 395                         return **i;
 396
 397         throw key_error(format("%s %d", group, num));
 398 }
 399
 400 void Layout::add_train(Train &t)
 401 {
 402         insert_unique(trains, t.get_address(), &t);
 403         signal_train_added.emit(t);
 404 }
 405
 406 Train &Layout::get_train(unsigned addr) const
 407 {
 408         return *get_item(trains, addr);
 409 }
 410
 411 void Layout::remove_train(Train &t)
 412 {
 413         if(trains.erase(t.get_address()))
 414                 signal_train_removed.emit(t);
 415 }
 416
 417 void Layout::tick()
 418 {
 419         if(driver)
 420                 driver->tick();
 421
 422         Time::TimeStamp t = Time::now();
 423         Time::TimeDelta dt;
 424         if(last_tick)
 425                 dt = t-last_tick;
 426         last_tick = t;
 427
 428         unsigned count = dt/(10*Time::msec)+1;
 429         dt /= count;
 430         while(count--)
 431                 step(dt);
 432 }
 433
 434 void Layout::step(const Time::TimeDelta &dt)
 435 {
 436         if(!driver || !driver->is_halted())
 437                 clock.tick(dt);
 438
 439         for(set<Sensor *>::iterator i=sensors.get().begin(); i!=sensors.get().end(); ++i)
 440                 (*i)->tick(dt);
 441         const set<Signal *> &signals = objects.get<Signal>();
 442         for(set<Signal *>::iterator i=signals.begin(); i!=signals.end(); ++i)
 443                 (*i)->tick(dt);
 444         for(map<unsigned, Train *>::iterator i=trains.begin(); i!=trains.end(); ++i)
 445                 i->second->tick(dt);
 446 }
 447
 448 void Layout::emergency(Block *block, const string &msg)
 449 {
 450         if(driver)
 451                 driver->halt(true);
 452         if(block)
 453                 IO::print("Emergency at %s: %s\n", block->get_name(), msg);
 454         else
 455                 IO::print("Emergency: %s\n", msg);
 456         signal_emergency.emit(block, msg);
 457 }
 458
 459 void Layout::save(const string &fn) const
 460 {
 461         IO::BufferedFile out(fn, IO::M_WRITE);
 462         DataFile::Writer writer(out);
 463
 464         if(!base.empty())
 465                 writer.write((DataFile::Statement("base"), base));
 466
 467         const set<Track *> &tracks = objects.get<Track>();
 468         for(set<Track *>::const_iterator i=tracks.begin(); i!=tracks.end(); ++i)
 469         {
 470                 DataFile::Statement st("track");
 471                 st.append(catalogue.get_name(&(*i)->get_type()));
 472                 (*i)->save(st.sub);
 473                 writer.write(st);
 474         }
 475
 476         const set<Signal *> &signals = objects.get<Signal>();
 477         for(set<Signal *>::const_iterator i=signals.begin(); i!=signals.end(); ++i)
 478         {
 479                 DataFile::Statement st("signal");
 480                 st.append(catalogue.get_name(&(*i)->get_type()));
 481                 (*i)->save(st.sub);
 482                 writer.write(st);
 483         }
 484
 485         const set<Terrain *> &terrains = objects.get<Terrain>();
 486         for(set<Terrain *>::const_iterator i=terrains.begin(); i!=terrains.end(); ++i)
 487         {
 488                 DataFile::Statement st("terrain");
 489                 st.append(catalogue.get_name(&(*i)->get_type()));
 490                 (*i)->save(st.sub);
 491                 writer.write(st);
 492         }
 493
 494         const set<BeamGate *> &gates = objects.get<BeamGate>();
 495         for(set<BeamGate *>::const_iterator i=gates.begin(); i!=gates.end(); ++i)
 496         {
 497                 DataFile::Statement st("beamgate");
 498                 (*i)->save(st.sub);
 499                 writer.write(st);
 500         }
 501
 502         const set<Route *> &routes = track_chains.get<Route>();
 503         for(set<Route *>::const_iterator i=routes.begin(); i!=routes.end(); ++i)
 504         {
 505                 if((*i)->is_temporary())
 506                         continue;
 507
 508                 DataFile::Statement st("route");
 509                 (*i)->save(st.sub);
 510                 writer.write(st);
 511         }
 512
 513         const set<Zone *> &zones = track_chains.get<Zone>();
 514         for(set<Zone *>::const_iterator i=zones.begin(); i!=zones.end(); ++i)
 515         {
 516                 DataFile::Statement st("zone");
 517                 (*i)->save(st.sub);
 518                 writer.write(st);
 519         }
 520 }
 521
 522 void Layout::save_dynamic(const string &fn) const
 523 {
 524         IO::BufferedFile out(fn, IO::M_WRITE);
 525         DataFile::Writer writer(out);
 526
 527         {
 528                 DataFile::Statement st("clock");
 529                 clock.save(st.sub);
 530                 writer.write(st);
 531         }
 532
 533         const set<Track *> &tracks = objects.get<Track>();
 534         for(set<Track *>::const_iterator i=tracks.begin(); i!=tracks.end(); ++i)
 535                 if((*i)->get_type().is_turnout())
 536                 {
 537                         DataFile::Statement st("turnout");
 538                         st.append((*i)->get_turnout_address());
 539                         (*i)->save_dynamic(st.sub);
 540                         writer.write(st);
 541                 }
 542
 543         for(map<unsigned, Train *>::const_iterator i=trains.begin(); i!=trains.end(); ++i)
 544         {
 545                 DataFile::Statement st("train");
 546                 st.append(catalogue.get_name(&i->second->get_locomotive_type()));
 547                 st.append(i->second->get_address());
 548                 st.append(i->second->get_protocol());
 549                 i->second->save(st.sub);
 550                 writer.write(st);
 551         }
 552 }
 553
 554 void Layout::sensor_state_changed(Sensor &sensor, Sensor::State state)
 555 {
 556         if(state==Sensor::ACTIVE)
 557         {
 558                 Block *block = sensor.get_block();
 559                 if(block && !block->get_train())
 560                         emergency(block, "Unreserved sensor triggered");
 561         }
 562 }
 563
 564 void Layout::block_reserved(Block &block, Train *train)
 565 {
 566         if(emitting_block_reserved)
 567                 block_reserve_queue.push_back(BlockReservation(block, train));
 568         else
 569         {
 570                 SetFlag setf(emitting_block_reserved);
 571                 signal_block_reserved.emit(block, train);
 572                 while(!block_reserve_queue.empty())
 573                 {
 574                         BlockReservation br = block_reserve_queue.front();
 575                         block_reserve_queue.pop_front();
 576                         signal_block_reserved.emit(br.block, br.train);
 577                 }
 578         }
 579 }
 580
 581
 582 template<typename B>
 583 Layout::Storage<B>::~Storage()
 584 {
 585         while(!base.empty())
 586                 delete *base.begin();
 587 }
 588
 589 template<typename B>
 590 bool Layout::Storage<B>::insert(B &b)
 591 {
 592         bool result = base.insert(&b).second;
 593         if(result)
 594                 caches.clear();
 595         return result;
 596 }
 597
 598 template<typename B>
 599 bool Layout::Storage<B>::erase(B &b)
 600 {
 601         bool result = base.erase(&b);
 602         if(result)
 603                 caches.clear();
 604         return result;
 605 }
 606
 607 template<typename B>
 608 template<typename T>
 609 const set<T *> &Layout::Storage<B>::get() const
 610 {
 611         for(list<Variant>::const_iterator i=caches.begin(); i!=caches.end(); ++i)
 612                 if(i->check_type<set<T *> >())
 613                         return i->value<set<T *> >();
 614
 615         caches.push_back(set<T *>());
 616         set<T *> &result = caches.back().value<set<T *> >();
 617         for(typename set<B *>::const_iterator i=base.begin(); i!=base.end(); ++i)
 618                 if(T *t = dynamic_cast<T *>(*i))
 619                         result.insert(t);
 620
 621         return result;
 622 }
 623
 624 template<typename B>
 625 template<typename T>
 626 void Layout::Storage<B>::del()
 627 {
 628         set<T *> ts = get<T>();
 629         for(typename set<T *>::iterator i=ts.begin(); i!=ts.end(); ++i)
 630                 if(base.count(*i))
 631                         delete *i;
 632 }
 633
 634
 635 Layout::BlockReservation::BlockReservation(Block &b, Train *t):
 636         block(b),
 637         train(t)
 638 { }
 639
 640
 641 Layout::Loader::Loader(Layout &l):
 642         DataFile::ObjectLoader<Layout>(l)
 643 {
 644         add("base",  &Layout::base);
 645         add("beamgate", &Loader::beamgate);
 646         add("clock", &Loader::clock);
 647         add("route", &Loader::route);
 648         add("signal", &Loader::signal);
 649         add("terrain", &Loader::terrain);
 650         add("track", &Loader::track);
 651         add("train", &Loader::train);
 652         add("turnout", &Loader::turnout);
 653         add("zone",  &Loader::zone);
 654
 655         // Deprecated
 656         add("turnout", &Loader::turnout2);
 657 }
 658
 659 void Layout::Loader::beamgate()
 660 {
 661         BeamGate *gate = new BeamGate(obj);
 662         load_sub(*gate);
 663 }
 664
 665 void Layout::Loader::clock()
 666 {
 667         load_sub(obj.clock);
 668 }
 669
 670 void Layout::Loader::route()
 671 {
 672         Route *rte = new Route(obj);
 673         load_sub(*rte);
 674 }
 675
 676 void Layout::Loader::signal(const string &n)
 677 {
 678         Signal *sig = new Signal(obj, obj.catalogue.get<SignalType>(n));
 679         load_sub(*sig);
 680 }
 681
 682 void Layout::Loader::terrain(const string &n)
 683 {
 684         Terrain *ter = new Terrain(obj, obj.catalogue.get<TerrainType>(n));
 685         load_sub(*ter);
 686 }
 687
 688 void Layout::Loader::track(const string &n)
 689 {
 690         Track *trk = new Track(obj, obj.catalogue.get<TrackType>(n));
 691         load_sub(*trk);
 692         const set<Track *> &tracks = obj.objects.get<Track>();
 693         for(set<Track *>::const_iterator i=tracks.begin(); i!=tracks.end(); ++i)
 694                 if(*i!=trk)
 695                         trk->link_to(**i);
 696 }
 697
 698 void Layout::Loader::train(const string &n, unsigned addr, const std::string &proto)
 699 {
 700         Train *trn = new Train(obj, obj.catalogue.get<VehicleType>(n), addr, proto);
 701         load_sub(*trn);
 702 }
 703
 704 void Layout::Loader::turnout(unsigned addr)
 705 {
 706         /* Kinda slow, but maintaining a map of turnouts seems more trouble than
 707         it's worth at this point. */
 708         const set<Track *> &tracks = obj.objects.get<Track>();
 709         for(set<Track *>::const_iterator i=tracks.begin(); i!=tracks.end(); ++i)
 710                 if((*i)->get_turnout_address()==addr)
 711                 {
 712                         load_sub(**i);
 713                         return;
 714                 }
 715
 716         throw key_error(addr);
 717 }
 718
 719 void Layout::Loader::turnout2(unsigned addr, unsigned path)
 720 {
 721         // XXX Discrepancy between ids and addresses
 722         if(obj.driver)
 723                 obj.driver->set_turnout(addr, path);
 724 }
 725
 726 void Layout::Loader::zone()
 727 {
 728         Zone *zne = new Zone(obj);
 729         load_sub(*zne);
 730 }
 731
 732 } // namespace R2C2