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