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         if(!driver || !driver->is_halted())
 429                 clock.tick(dt);
 430
 431         for(set<Sensor *>::iterator i=sensors.get().begin(); i!=sensors.get().end(); ++i)
 432                 (*i)->tick(dt);
 433         const set<Signal *> &signals = objects.get<Signal>();
 434         for(set<Signal *>::iterator i=signals.begin(); i!=signals.end(); ++i)
 435                 (*i)->tick(dt);
 436         for(map<unsigned, Train *>::iterator i=trains.begin(); i!=trains.end(); ++i)
 437                 i->second->tick(dt);
 438 }
 439
 440 void Layout::emergency(Block *block, const string &msg)
 441 {
 442         if(driver)
 443                 driver->halt(true);
 444         if(block)
 445                 IO::print("Emergency at %s: %s\n", block->get_name(), msg);
 446         else
 447                 IO::print("Emergency: %s\n", msg);
 448         signal_emergency.emit(block, msg);
 449 }
 450
 451 void Layout::save(const string &fn) const
 452 {
 453         IO::BufferedFile out(fn, IO::M_WRITE);
 454         DataFile::Writer writer(out);
 455
 456         if(!base.empty())
 457                 writer.write((DataFile::Statement("base"), base));
 458
 459         const set<Track *> &tracks = objects.get<Track>();
 460         for(set<Track *>::const_iterator i=tracks.begin(); i!=tracks.end(); ++i)
 461         {
 462                 DataFile::Statement st("track");
 463                 st.append(catalogue.get_name(&(*i)->get_type()));
 464                 (*i)->save(st.sub);
 465                 writer.write(st);
 466         }
 467
 468         const set<Signal *> &signals = objects.get<Signal>();
 469         for(set<Signal *>::const_iterator i=signals.begin(); i!=signals.end(); ++i)
 470         {
 471                 DataFile::Statement st("signal");
 472                 st.append(catalogue.get_name(&(*i)->get_type()));
 473                 (*i)->save(st.sub);
 474                 writer.write(st);
 475         }
 476
 477         const set<Terrain *> &terrains = objects.get<Terrain>();
 478         for(set<Terrain *>::const_iterator i=terrains.begin(); i!=terrains.end(); ++i)
 479         {
 480                 DataFile::Statement st("terrain");
 481                 st.append(catalogue.get_name(&(*i)->get_type()));
 482                 (*i)->save(st.sub);
 483                 writer.write(st);
 484         }
 485
 486         const set<BeamGate *> &gates = objects.get<BeamGate>();
 487         for(set<BeamGate *>::const_iterator i=gates.begin(); i!=gates.end(); ++i)
 488         {
 489                 DataFile::Statement st("beamgate");
 490                 (*i)->save(st.sub);
 491                 writer.write(st);
 492         }
 493
 494         const set<Route *> &routes = track_chains.get<Route>();
 495         for(set<Route *>::const_iterator i=routes.begin(); i!=routes.end(); ++i)
 496         {
 497                 if((*i)->is_temporary())
 498                         continue;
 499
 500                 DataFile::Statement st("route");
 501                 (*i)->save(st.sub);
 502                 writer.write(st);
 503         }
 504
 505         const set<Zone *> &zones = track_chains.get<Zone>();
 506         for(set<Zone *>::const_iterator i=zones.begin(); i!=zones.end(); ++i)
 507         {
 508                 DataFile::Statement st("zone");
 509                 (*i)->save(st.sub);
 510                 writer.write(st);
 511         }
 512 }
 513
 514 void Layout::save_dynamic(const string &fn) const
 515 {
 516         IO::BufferedFile out(fn, IO::M_WRITE);
 517         DataFile::Writer writer(out);
 518
 519         {
 520                 DataFile::Statement st("clock");
 521                 clock.save(st.sub);
 522                 writer.write(st);
 523         }
 524
 525         const set<Track *> &tracks = objects.get<Track>();
 526         for(set<Track *>::const_iterator i=tracks.begin(); i!=tracks.end(); ++i)
 527                 if((*i)->get_type().is_turnout())
 528                         writer.write((DataFile::Statement("turnout"), (*i)->get_turnout_address(), (*i)->get_active_path()));
 529
 530         for(map<unsigned, Train *>::const_iterator i=trains.begin(); i!=trains.end(); ++i)
 531         {
 532                 DataFile::Statement st("train");
 533                 st.append(catalogue.get_name(&i->second->get_locomotive_type()));
 534                 st.append(i->second->get_address());
 535                 st.append(i->second->get_protocol());
 536                 i->second->save(st.sub);
 537                 writer.write(st);
 538         }
 539 }
 540
 541 void Layout::sensor_state_changed(Sensor &sensor, Sensor::State state)
 542 {
 543         if(state==Sensor::ACTIVE)
 544         {
 545                 Block *block = sensor.get_block();
 546                 if(block && !block->get_train())
 547                         emergency(block, "Unreserved sensor triggered");
 548         }
 549 }
 550
 551 void Layout::block_reserved(Block &block, Train *train)
 552 {
 553         if(emitting_block_reserved)
 554                 block_reserve_queue.push_back(BlockReservation(block, train));
 555         else
 556         {
 557                 SetFlag setf(emitting_block_reserved);
 558                 signal_block_reserved.emit(block, train);
 559                 while(!block_reserve_queue.empty())
 560                 {
 561                         BlockReservation br = block_reserve_queue.front();
 562                         block_reserve_queue.pop_front();
 563                         signal_block_reserved.emit(br.block, br.train);
 564                 }
 565         }
 566 }
 567
 568
 569 template<typename B>
 570 Layout::Storage<B>::~Storage()
 571 {
 572         while(!base.empty())
 573                 delete *base.begin();
 574 }
 575
 576 template<typename B>
 577 bool Layout::Storage<B>::insert(B &b)
 578 {
 579         bool result = base.insert(&b).second;
 580         if(result)
 581                 caches.clear();
 582         return result;
 583 }
 584
 585 template<typename B>
 586 bool Layout::Storage<B>::erase(B &b)
 587 {
 588         bool result = base.erase(&b);
 589         if(result)
 590                 caches.clear();
 591         return result;
 592 }
 593
 594 template<typename B>
 595 template<typename T>
 596 const set<T *> &Layout::Storage<B>::get() const
 597 {
 598         for(list<Variant>::const_iterator i=caches.begin(); i!=caches.end(); ++i)
 599                 if(i->check_type<set<T *> >())
 600                         return i->value<set<T *> >();
 601
 602         caches.push_back(set<T *>());
 603         set<T *> &result = caches.back().value<set<T *> >();
 604         for(typename set<B *>::const_iterator i=base.begin(); i!=base.end(); ++i)
 605                 if(T *t = dynamic_cast<T *>(*i))
 606                         result.insert(t);
 607
 608         return result;
 609 }
 610
 611 template<typename B>
 612 template<typename T>
 613 void Layout::Storage<B>::del()
 614 {
 615         set<T *> ts = get<T>();
 616         for(typename set<T *>::iterator i=ts.begin(); i!=ts.end(); ++i)
 617                 if(base.count(*i))
 618                         delete *i;
 619 }
 620
 621
 622 Layout::BlockReservation::BlockReservation(Block &b, Train *t):
 623         block(b),
 624         train(t)
 625 { }
 626
 627
 628 Layout::Loader::Loader(Layout &l):
 629         DataFile::ObjectLoader<Layout>(l)
 630 {
 631         add("base",  &Layout::base);
 632         add("beamgate", &Loader::beamgate);
 633         add("clock", &Loader::clock);
 634         add("route", &Loader::route);
 635         add("signal", &Loader::signal);
 636         add("terrain", &Loader::terrain);
 637         add("track", &Loader::track);
 638         add("train", &Loader::train);
 639         add("turnout", &Loader::turnout);
 640         add("zone",  &Loader::zone);
 641 }
 642
 643 void Layout::Loader::beamgate()
 644 {
 645         BeamGate *gate = new BeamGate(obj);
 646         load_sub(*gate);
 647 }
 648
 649 void Layout::Loader::clock()
 650 {
 651         load_sub(obj.clock);
 652 }
 653
 654 void Layout::Loader::route()
 655 {
 656         Route *rte = new Route(obj);
 657         load_sub(*rte);
 658 }
 659
 660 void Layout::Loader::signal(const string &n)
 661 {
 662         Signal *sig = new Signal(obj, obj.catalogue.get<SignalType>(n));
 663         load_sub(*sig);
 664 }
 665
 666 void Layout::Loader::terrain(const string &n)
 667 {
 668         Terrain *ter = new Terrain(obj, obj.catalogue.get<TerrainType>(n));
 669         load_sub(*ter);
 670 }
 671
 672 void Layout::Loader::track(const string &n)
 673 {
 674         Track *trk = new Track(obj, obj.catalogue.get<TrackType>(n));
 675         load_sub(*trk);
 676         const set<Track *> &tracks = obj.objects.get<Track>();
 677         for(set<Track *>::const_iterator i=tracks.begin(); i!=tracks.end(); ++i)
 678                 if(*i!=trk)
 679                         trk->link_to(**i);
 680 }
 681
 682 void Layout::Loader::train(const string &n, unsigned addr, const std::string &proto)
 683 {
 684         Train *trn = new Train(obj, obj.catalogue.get<VehicleType>(n), addr, proto);
 685         load_sub(*trn);
 686 }
 687
 688 void Layout::Loader::turnout(unsigned addr, unsigned path)
 689 {
 690         if(obj.driver)
 691                 obj.driver->set_turnout(addr, path);
 692 }
 693
 694 void Layout::Loader::zone()
 695 {
 696         Zone *zne = new Zone(obj);
 697         load_sub(*zne);
 698 }
 699
 700 } // namespace R2C2