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 {
  46         clock.set_rate(60);
  47 }
  48
  49 Layout::~Layout()
  50 {
  51         delete driver;
  52         driver = 0;
  53
  54         while(!trains.empty())
  55                 delete trains.begin()->second;
  56         track_chains.del<Route>();
  57         track_chains.del<Zone>();
  58         objects.del<Signal>();
  59         objects.del<Track>();
  60         track_chains.del<Block>();
  61 }
  62
  63 Driver &Layout::get_driver() const
  64 {
  65         if(!driver)
  66                 throw logic_error("!driver");
  67         return *driver;
  68 }
  69
  70 void Layout::add(Object &o)
  71 {
  72         if(objects.insert(o))
  73         {
  74                 try
  75                 {
  76                         signal_object_added.emit(o);
  77                 }
  78                 catch(...)
  79                 {
  80                         objects.erase(o);
  81                         throw;
  82                 }
  83         }
  84 }
  85
  86 void Layout::add(Track &t)
  87 {
  88         if(objects.insert(t))
  89         {
  90                 try
  91                 {
  92                         // Blocks must be recreated first
  93                         create_blocks();
  94                         signal_object_added.emit(t);
  95                 }
  96                 catch(...)
  97                 {
  98                         objects.erase(t);
  99                         create_blocks();
 100                         throw;
 101                 }
 102         }
 103 }
 104
 105 void Layout::add(TrackChain &g)
 106 {
 107         if(track_chains.insert(g))
 108         {
 109                 try
 110                 {
 111                         signal_track_chain_added.emit(g);
 112                 }
 113                 catch(...)
 114                 {
 115                         track_chains.erase(g);
 116                         throw;
 117                 }
 118         }
 119 }
 120
 121 void Layout::add(Block &b)
 122 {
 123         if(track_chains.insert(b))
 124         {
 125                 sigc::connection conn = b.signal_reserved.connect(sigc::bind<0>(signal_block_reserved, sigc::ref(b)));
 126                 try
 127                 {
 128                         signal_track_chain_added.emit(b);
 129                 }
 130                 catch(...)
 131                 {
 132                         track_chains.erase(b);
 133                         conn.disconnect();
 134                         throw;
 135                 }
 136         }
 137 }
 138
 139 void Layout::add(Sensor &s)
 140 {
 141         if(sensors.insert(s))
 142         {
 143                 s.signal_state_changed.connect(sigc::bind<0>(sigc::mem_fun(this, &Layout::sensor_state_changed), sigc::ref(s)));
 144                 s.signal_state_changed.connect(sigc::bind<0>(signal_sensor_state_changed, sigc::ref(s)));
 145         }
 146 }
 147
 148 void Layout::add(BeamGate &g)
 149 {
 150         add(static_cast<Object &>(g));
 151         add(static_cast<Sensor &>(g));
 152 }
 153
 154 template<>
 155 const set<Object *> &Layout::get_all<Object>() const
 156 {
 157         return objects.get();
 158 }
 159
 160 template<>
 161 const set<Track *> &Layout::get_all<Track>() const
 162 {
 163         return objects.get<Track>();
 164 }
 165
 166 template<>
 167 const set<Signal *> &Layout::get_all<Signal>() const
 168 {
 169         return objects.get<Signal>();
 170 }
 171
 172 template<>
 173 const set<Vehicle *> &Layout::get_all<Vehicle>() const
 174 {
 175         return objects.get<Vehicle>();
 176 }
 177
 178 template<>
 179 const set<Terrain *> &Layout::get_all<Terrain>() const
 180 {
 181         return objects.get<Terrain>();
 182 }
 183
 184 template<>
 185 const set<TrackChain *> &Layout::get_all<TrackChain>() const
 186 {
 187         return track_chains.get();
 188 }
 189
 190 template<>
 191 const set<Block *> &Layout::get_all<Block>() const
 192 {
 193         return track_chains.get<Block>();
 194 }
 195
 196 template<>
 197 const set<Route *> &Layout::get_all<Route>() const
 198 {
 199         return track_chains.get<Route>();
 200 }
 201
 202 template<>
 203 const set<Zone *> &Layout::get_all<Zone>() const
 204 {
 205         return track_chains.get<Zone>();
 206 }
 207
 208 template<>
 209 const set<Sensor *> &Layout::get_all<Sensor>() const
 210 {
 211         return sensors.get<Sensor>();
 212 }
 213
 214 template<>
 215 const set<TrackCircuit *> &Layout::get_all<TrackCircuit>() const
 216 {
 217         return sensors.get<TrackCircuit>();
 218 }
 219
 220 template<>
 221 const set<BeamGate *> &Layout::get_all<BeamGate>() const
 222 {
 223         return sensors.get<BeamGate>();
 224 }
 225
 226 template<typename T>
 227 T *Layout::pick(const Ray &ray)
 228 {
 229         const set<Object *> &objs = objects.get();
 230         T *closest = 0;
 231         float distance = -1;
 232         for(set<Object *>::const_iterator i=objs.begin(); i!=objs.end(); ++i)
 233                 if(T *t = dynamic_cast<T *>(*i))
 234                 {
 235                         float d = -1;
 236                         if(t->collide_ray(ray, &d))
 237                                 if(!closest || d<distance)
 238                                 {
 239                                         closest = t;
 240                                         distance = d;
 241                                 }
 242                 }
 243
 244         return closest;
 245 }
 246
 247 template Object *Layout::pick<Object>(const Ray &);
 248 template Track *Layout::pick<Track>(const Ray &);
 249 template Signal *Layout::pick<Signal>(const Ray &);
 250 template Vehicle *Layout::pick<Vehicle>(const Ray &);
 251
 252 void Layout::remove(Object &o)
 253 {
 254         if(objects.erase(o))
 255                 signal_object_removed.emit(o);
 256 }
 257
 258 void Layout::remove(Track &t)
 259 {
 260         if(objects.erase(t))
 261         {
 262                 // Blocks must be recreated first
 263                 create_blocks(t);
 264                 signal_object_removed.emit(t);
 265         }
 266 }
 267
 268 void Layout::remove(TrackChain &g)
 269 {
 270         if(track_chains.erase(g))
 271                 signal_track_chain_removed.emit(g);
 272 }
 273
 274 void Layout::remove(Sensor &s)
 275 {
 276         sensors.erase(s);
 277 }
 278
 279 void Layout::remove(BeamGate &g)
 280 {
 281         remove(static_cast<Object &>(g));
 282         remove(static_cast<Sensor &>(g));
 283 }
 284
 285 unsigned Layout::allocate_turnout_address()
 286 {
 287         set<unsigned> used_addrs;
 288         const set<Track *> &tracks = objects.get<Track>();
 289         for(set<Track *>::const_iterator i=tracks.begin(); i!=tracks.end(); ++i)
 290                 if((*i)->get_type().is_turnout())
 291                         used_addrs.insert((*i)->get_turnout_address());
 292
 293         unsigned result = next_turnout_addr;
 294         while(used_addrs.count(result))
 295                 ++result;
 296         next_turnout_addr = result+1;
 297
 298         return result;
 299 }
 300
 301 Block &Layout::get_block(unsigned id) const
 302 {
 303         const set<Block *> &blocks = track_chains.get<Block>();
 304         for(set<Block *>::const_iterator i=blocks.begin(); i!=blocks.end(); ++i)
 305                 if((*i)->get_id()==id)
 306                         return **i;
 307
 308         throw key_error(id);
 309 }
 310
 311 void Layout::create_blocks()
 312 {
 313         set<Track *> loose_tracks = objects.get<Track>();
 314         const set<Block *> *blocks = &track_chains.get<Block>();
 315         for(set<Block *>::const_iterator i=blocks->begin(); i!=blocks->end(); ++i)
 316         {
 317                 const set<Track *> &btracks = (*i)->get_tracks();
 318                 for(set<Track *>::const_iterator j=btracks.begin(); j!=btracks.end(); ++j)
 319                         loose_tracks.erase(*j);
 320         }
 321
 322         list<Block *> created_blocks;
 323         while(!loose_tracks.empty())
 324         {
 325                 Block *block = new Block(*this, **loose_tracks.begin());
 326                 created_blocks.push_back(block);
 327
 328                 const set<Track *> &btracks = block->get_tracks();
 329                 for(set<Track *>::const_iterator i=btracks.begin(); i!=btracks.end(); ++i)
 330                         loose_tracks.erase(*i);
 331         }
 332
 333         // The previously obtained set has been invalidated by creating new blocks
 334         blocks = &track_chains.get<Block>();
 335         for(list<Block *>::iterator i=created_blocks.begin(); i!=created_blocks.end(); ++i)
 336                 for(set<Block *>::const_iterator j=blocks->begin(); j!=blocks->end(); ++j)
 337                         if(*j!=*i)
 338                                 (*i)->check_link(**j);
 339 }
 340
 341 void Layout::create_blocks(Track &track)
 342 {
 343         /* Must collect the blocks in a set first while all tracks are still
 344         guaranteed to have blocks and to avoid duplicate deletes */
 345         set<Block *> del_blocks;
 346
 347         del_blocks.insert(&track.get_block());
 348
 349         const vector<Track *> &links = track.get_links();
 350         for(vector<Track *>::const_iterator i=links.begin(); i!=links.end(); ++i)
 351                 if(*i)
 352                         del_blocks.insert(&(*i)->get_block());
 353
 354         for(set<Block *>::iterator i=del_blocks.begin(); i!=del_blocks.end(); ++i)
 355                 delete *i;
 356
 357         create_blocks();
 358 }
 359
 360 Route &Layout::get_route(const string &name) const
 361 {
 362         const set<Route *> &routes = track_chains.get<Route>();
 363         for(set<Route *>::const_iterator i=routes.begin(); i!=routes.end(); ++i)
 364                 if((*i)->get_name()==name)
 365                         return **i;
 366         throw key_error(name);
 367 }
 368
 369 void Layout::update_routes()
 370 {
 371         const set<Route *> &routes = track_chains.get<Route>();
 372         for(set<Route *>::iterator i=routes.begin(); i!=routes.end(); ++i)
 373                 (*i)->update_turnouts();
 374 }
 375
 376 Layout::ZoneArray Layout::get_zones(const string &group) const
 377 {
 378         ZoneArray result;
 379         const set<Zone *> &zones = track_chains.get<Zone>();
 380         for(set<Zone *>::const_iterator i=zones.begin(); i!=zones.end(); ++i)
 381                 if((*i)->get_group()==group)
 382                         result.push_back(*i);
 383
 384         sort(result.begin(), result.end(), zone_order);
 385
 386         return result;
 387 }
 388
 389 Zone &Layout::get_zone(const string &group, unsigned num) const
 390 {
 391         const set<Zone *> &zones = track_chains.get<Zone>();
 392         for(set<Zone *>::const_iterator i=zones.begin(); i!=zones.end(); ++i)
 393                 if((*i)->get_group()==group && (*i)->get_number()==num)
 394                         return **i;
 395
 396         throw key_error(format("%s %d", group, num));
 397 }
 398
 399 void Layout::add_train(Train &t)
 400 {
 401         insert_unique(trains, t.get_address(), &t);
 402         signal_train_added.emit(t);
 403 }
 404
 405 Train &Layout::get_train(unsigned addr) const
 406 {
 407         return *get_item(trains, addr);
 408 }
 409
 410 void Layout::remove_train(Train &t)
 411 {
 412         if(trains.erase(t.get_address()))
 413                 signal_train_removed.emit(t);
 414 }
 415
 416 void Layout::tick(float speed)
 417 {
 418         if(driver)
 419                 driver->tick();
 420
 421         Time::TimeStamp t = Time::now();
 422         Time::TimeDelta dt;
 423         if(last_tick)
 424                 dt = t-last_tick;
 425         last_tick = t;
 426
 427         dt *= speed;
 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
 565 template<typename B>
 566 Layout::Storage<B>::~Storage()
 567 {
 568         while(!base.empty())
 569                 delete *base.begin();
 570 }
 571
 572 template<typename B>
 573 bool Layout::Storage<B>::insert(B &b)
 574 {
 575         bool result = base.insert(&b).second;
 576         if(result)
 577                 caches.clear();
 578         return result;
 579 }
 580
 581 template<typename B>
 582 bool Layout::Storage<B>::erase(B &b)
 583 {
 584         bool result = base.erase(&b);
 585         if(result)
 586                 caches.clear();
 587         return result;
 588 }
 589
 590 template<typename B>
 591 template<typename T>
 592 const set<T *> &Layout::Storage<B>::get() const
 593 {
 594         for(list<Variant>::const_iterator i=caches.begin(); i!=caches.end(); ++i)
 595                 if(i->check_type<set<T *> >())
 596                         return i->value<set<T *> >();
 597
 598         caches.push_back(set<T *>());
 599         set<T *> &result = caches.back().value<set<T *> >();
 600         for(typename set<B *>::const_iterator i=base.begin(); i!=base.end(); ++i)
 601                 if(T *t = dynamic_cast<T *>(*i))
 602                         result.insert(t);
 603
 604         return result;
 605 }
 606
 607 template<typename B>
 608 template<typename T>
 609 void Layout::Storage<B>::del()
 610 {
 611         set<T *> ts = get<T>();
 612         for(typename set<T *>::iterator i=ts.begin(); i!=ts.end(); ++i)
 613                 if(base.count(*i))
 614                         delete *i;
 615 }
 616
 617
 618 Layout::Loader::Loader(Layout &l):
 619         DataFile::ObjectLoader<Layout>(l)
 620 {
 621         add("base",  &Layout::base);
 622         add("beamgate", &Loader::beamgate);
 623         add("clock", &Loader::clock);
 624         add("route", &Loader::route);
 625         add("signal", &Loader::signal);
 626         add("terrain", &Loader::terrain);
 627         add("track", &Loader::track);
 628         add("train", &Loader::train);
 629         add("turnout", &Loader::turnout);
 630         add("zone",  &Loader::zone);
 631
 632         // Deprecated
 633         add("turnout", &Loader::turnout2);
 634 }
 635
 636 void Layout::Loader::beamgate()
 637 {
 638         BeamGate *gate = new BeamGate(obj);
 639         load_sub(*gate);
 640 }
 641
 642 void Layout::Loader::clock()
 643 {
 644         load_sub(obj.clock);
 645 }
 646
 647 void Layout::Loader::route()
 648 {
 649         Route *rte = new Route(obj);
 650         load_sub(*rte);
 651 }
 652
 653 void Layout::Loader::signal(const string &n)
 654 {
 655         Signal *sig = new Signal(obj, obj.catalogue.get<SignalType>(n));
 656         load_sub(*sig);
 657 }
 658
 659 void Layout::Loader::terrain(const string &n)
 660 {
 661         Terrain *ter = new Terrain(obj, obj.catalogue.get<TerrainType>(n));
 662         load_sub(*ter);
 663 }
 664
 665 void Layout::Loader::track(const string &n)
 666 {
 667         Track *trk = new Track(obj, obj.catalogue.get<TrackType>(n));
 668         load_sub(*trk);
 669         const set<Track *> &tracks = obj.objects.get<Track>();
 670         for(set<Track *>::const_iterator i=tracks.begin(); i!=tracks.end(); ++i)
 671                 if(*i!=trk)
 672                         trk->link_to(**i);
 673 }
 674
 675 void Layout::Loader::train(const string &n, unsigned addr, const std::string &proto)
 676 {
 677         Train *trn = new Train(obj, obj.catalogue.get<VehicleType>(n), addr, proto);
 678         load_sub(*trn);
 679 }
 680
 681 void Layout::Loader::turnout(unsigned addr)
 682 {
 683         /* Kinda slow, but maintaining a map of turnouts seems more trouble than
 684         it's worth at this point. */
 685         const set<Track *> &tracks = obj.objects.get<Track>();
 686         for(set<Track *>::const_iterator i=tracks.begin(); i!=tracks.end(); ++i)
 687                 if((*i)->get_turnout_address()==addr)
 688                 {
 689                         load_sub(**i);
 690                         return;
 691                 }
 692
 693         throw key_error(addr);
 694 }
 695
 696 void Layout::Loader::turnout2(unsigned addr, unsigned path)
 697 {
 698         // XXX Discrepancy between ids and addresses
 699         if(obj.driver)
 700                 obj.driver->set_turnout(addr, path);
 701 }
 702
 703 void Layout::Loader::zone()
 704 {
 705         Zone *zne = new Zone(obj);
 706         load_sub(*zne);
 707 }
 708
 709 } // namespace R2C2