source/libr2c2/train.cpp

   1 #include <algorithm>
   2 #include <cmath>
   3 #include <msp/core/maputils.h>
   4 #include <msp/core/raii.h>
   5 #include <msp/strings/format.h>
   6 #include <msp/time/units.h>
   7 #include <msp/time/utils.h>
   8 #include "aicontrol.h"
   9 #include "catalogue.h"
  10 #include "driver.h"
  11 #include "layout.h"
  12 #include "route.h"
  13 #include "simplecontroller.h"
  14 #include "speedquantizer.h"
  15 #include "timetable.h"
  16 #include "trackiter.h"
  17 #include "tracktype.h"
  18 #include "train.h"
  19 #include "trainrouter.h"
  20 #include "vehicle.h"
  21 #include "vehicletype.h"
  22 #include "zone.h"
  23
  24 using namespace std;
  25 using namespace Msp;
  26
  27 namespace R2C2 {
  28
  29 Train::Train(Layout &l, const VehicleType &t, unsigned a, const string &p):
  30         layout(l),
  31         loco_type(t),
  32         address(a),
  33         protocol(p),
  34         preceding_train(0),
  35         allocator(*this),
  36         advancing(false),
  37         controller(new SimpleController),
  38         active(false),
  39         current_speed_step(0),
  40         speed_changing(false),
  41         reverse(false),
  42         functions(0),
  43         pure_speed(false),
  44         speed_quantizer(0),
  45         accurate_position(false),
  46         overshoot_dist(false)
  47 {
  48         if(!loco_type.is_locomotive())
  49                 throw invalid_argument("Train::Train");
  50
  51         unsigned speed_steps = layout.get_driver().get_protocol_speed_steps(protocol);
  52         if(speed_steps)
  53                 speed_quantizer = new SpeedQuantizer(speed_steps);
  54
  55         vehicles.push_back(new Vehicle(layout, loco_type));
  56
  57         layout.add_train(*this);
  58
  59         layout.get_driver().add_loco(address, protocol, loco_type);
  60         layout.get_driver().signal_loco_speed.connect(sigc::mem_fun(this, &Train::loco_speed_event));
  61         layout.get_driver().signal_loco_function.connect(sigc::mem_fun(this, &Train::loco_func_event));
  62
  63         layout.signal_block_state_changed.connect(sigc::mem_fun(this, &Train::block_state_changed));
  64
  65         layout.get_driver().signal_halt.connect(sigc::mem_fun(this, &Train::halt_event));
  66
  67         controller->signal_control_changed.connect(sigc::mem_fun(this, &Train::control_changed));
  68 }
  69
  70 Train::~Train()
  71 {
  72         delete controller;
  73         for(vector<Vehicle *>::iterator i=vehicles.begin(); i!=vehicles.end(); ++i)
  74                 delete *i;
  75         layout.remove_train(*this);
  76 }
  77
  78 void Train::set_name(const string &n)
  79 {
  80         name = n;
  81
  82         signal_name_changed.emit(name);
  83 }
  84
  85 void Train::add_vehicle(const VehicleType &vt)
  86 {
  87         Vehicle *veh = new Vehicle(layout, vt);
  88         vehicles.back()->attach_back(*veh);
  89         vehicles.push_back(veh);
  90 }
  91
  92 void Train::remove_vehicle(unsigned i)
  93 {
  94         if(i>=vehicles.size())
  95                 throw out_of_range("Train::remove_vehicle");
  96         if(i==0)
  97                 throw logic_error("can't remove locomotive");
  98         delete vehicles[i];
  99         vehicles.erase(vehicles.begin()+i);
 100         if(i<vehicles.size())
 101                 vehicles[i-1]->attach_back(*vehicles[i]);
 102 }
 103
 104 unsigned Train::get_n_vehicles() const
 105 {
 106         return vehicles.size();
 107 }
 108
 109 Vehicle &Train::get_vehicle(unsigned i)
 110 {
 111         if(i>=vehicles.size())
 112                 throw out_of_range("Train::get_vehicle");
 113         return *vehicles[i];
 114 }
 115
 116 const Vehicle &Train::get_vehicle(unsigned i) const
 117 {
 118         if(i>=vehicles.size())
 119                 throw out_of_range("Train::get_vehicle");
 120         return *vehicles[i];
 121 }
 122
 123 void Train::set_control(const string &n, float v)
 124 {
 125         controller->set_control(n, v);
 126 }
 127
 128 void Train::set_active(bool a)
 129 {
 130         if(a==active)
 131                 return;
 132         if(!a && controller->get_speed())
 133                 throw logic_error("moving");
 134
 135         active = a;
 136         if(active)
 137         {
 138                 stop_timeout = Time::TimeStamp();
 139                 allocator.reserve_more();
 140         }
 141         else
 142                 stop_timeout = Time::now()+2*Time::sec;
 143 }
 144
 145 void Train::set_function(unsigned func, bool state)
 146 {
 147         if(!loco_type.get_functions().count(func))
 148                 throw invalid_argument("Train::set_function");
 149         layout.get_driver().set_loco_function(address, func, state);
 150 }
 151
 152 float Train::get_control(const string &ctrl) const
 153 {
 154         return controller->get_control(ctrl).value;
 155 }
 156
 157 float Train::get_speed() const
 158 {
 159         return controller->get_speed();
 160 }
 161
 162 float Train::get_quantized_speed() const
 163 {
 164         if(speed_quantizer)
 165                 return speed_quantizer->quantize_speed(controller->get_speed());
 166         else
 167                 return controller->get_speed();
 168 }
 169
 170 bool Train::get_function(unsigned func) const
 171 {
 172         return (functions>>func)&1;
 173 }
 174
 175 void Train::add_ai(TrainAI &ai)
 176 {
 177         ais.push_back(&ai);
 178         ai.signal_event.connect(sigc::bind<0>(signal_ai_event, sigc::ref(ai)));
 179 }
 180
 181 void Train::remove_ai(TrainAI &ai)
 182 {
 183         list<TrainAI *>::iterator i = find(ais.begin(), ais.end(), &ai);
 184         if(i!=ais.end())
 185                 ais.erase(i);
 186 }
 187
 188 void Train::ai_message(const TrainAI::Message &msg)
 189 {
 190         for(list<TrainAI *>::iterator i=ais.begin(); i!=ais.end(); ++i)
 191                 (*i)->message(msg);
 192 }
 193
 194 void Train::place(const BlockIter &block)
 195 {
 196         if(!block)
 197                 throw invalid_argument("Train::place");
 198         if(controller->get_speed())
 199                 throw logic_error("moving");
 200
 201         set_active(false);
 202         accurate_position = false;
 203
 204         allocator.start_from(block);
 205
 206         if(reverse)
 207                 vehicles.front()->place(block.reverse().track_iter(), 0, Vehicle::FRONT_BUFFER);
 208         else
 209                 vehicles.back()->place(block.track_iter(), 0, Vehicle::BACK_BUFFER);
 210 }
 211
 212 void Train::unplace()
 213 {
 214         if(controller->get_speed())
 215                 throw logic_error("moving");
 216
 217         allocator.clear();
 218
 219         set_active(false);
 220         accurate_position = false;
 221
 222         for(vector<Vehicle *>::iterator i=vehicles.begin(); i!=vehicles.end(); ++i)
 223                 (*i)->unplace();
 224 }
 225
 226 void Train::stop_at(Block *block)
 227 {
 228         allocator.stop_at(block);
 229         if(active && !block)
 230                 allocator.reserve_more();
 231 }
 232
 233 bool Train::free_block(Block &block)
 234 {
 235         if(get_reserved_distance_until(&block, false)<controller->get_braking_distance()*1.3)
 236                 return false;
 237
 238         return allocator.release_from(block);
 239 }
 240
 241 void Train::free_noncritical_blocks()
 242 {
 243         if(allocator.empty())
 244                 return;
 245
 246         if(controller->get_speed()==0)
 247         {
 248                 allocator.release_noncurrent();
 249                 return;
 250         }
 251
 252         float margin = 10*layout.get_catalogue().get_scale();
 253         float min_dist = controller->get_braking_distance()*1.3+margin;
 254
 255         Vehicle &veh = *(reverse ? vehicles.back() : vehicles.front());
 256
 257         TrackIter track = veh.get_track_iter();
 258         BlockIter block = allocator.first();
 259         const BlockIter &last_cur = allocator.last_current();
 260         const BlockIter &last = allocator.last();
 261         bool in_rsv = false;
 262         while(!block->has_track(*track))
 263         {
 264                 if(&*block==&*last_cur)
 265                         in_rsv = true;
 266                 if(&*block==&*last)
 267                         break;
 268                 block = block.next();
 269         }
 270
 271         float dist = veh.get_offset();
 272         if(reverse)
 273                 track.reverse();
 274         else
 275                 dist = track->get_type().get_path_length(track->get_active_path())-dist;
 276         dist -= veh.get_type().get_length()/2;
 277
 278         bool nsens = 0;
 279         while(1)
 280         {
 281                 track = track.next();
 282
 283                 if(!block->has_track(*track))
 284                 {
 285                         if(&*block==&*last_cur)
 286                                 in_rsv = true;
 287                         if(&*block==&*last)
 288                                 return;
 289                         block = block.next();
 290
 291                         if(dist>min_dist && nsens>0)
 292                         {
 293                                 allocator.release_from(*block);
 294                                 return;
 295                         }
 296
 297                         if(in_rsv && block->get_sensor_id())
 298                                 ++nsens;
 299                 }
 300
 301                 dist += track->get_type().get_path_length(track->get_active_path());
 302         }
 303 }
 304
 305 float Train::get_reserved_distance() const
 306 {
 307         if(allocator.empty())
 308                 return 0;
 309
 310         float margin = 0;
 311         TrackIter next = allocator.last().next().track_iter();
 312         if(next && next->get_type().is_turnout())
 313                 margin = 15*layout.get_catalogue().get_scale();
 314
 315         return max(get_reserved_distance_until(0, false)-margin, 0.0f);
 316 }
 317
 318 void Train::reserve_more()
 319 {
 320         allocator.reserve_more();
 321 }
 322
 323 void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt)
 324 {
 325         if(!active && stop_timeout && t>=stop_timeout)
 326         {
 327                 allocator.release_noncurrent();
 328                 stop_timeout = Time::TimeStamp();
 329         }
 330
 331         Driver &driver = layout.get_driver();
 332
 333         for(list<TrainAI *>::iterator i=ais.begin(); i!=ais.end(); ++i)
 334                 (*i)->tick(t, dt);
 335         controller->tick(dt);
 336         float speed = controller->get_speed();
 337         bool moving = speed>0;
 338
 339         if(controller->get_reverse()!=reverse)
 340         {
 341                 reverse = controller->get_reverse();
 342                 bool r = reverse;
 343                 if(loco_type.get_swap_direction())
 344                         r = !r;
 345                 driver.set_loco_reverse(address, r);
 346
 347                 allocator.reverse();
 348                 if(active)
 349                         allocator.reserve_more();
 350         }
 351
 352         if(speed_quantizer)
 353         {
 354                 unsigned speed_step = speed_quantizer->find_speed_step(speed);
 355                 if(speed_step!=current_speed_step && !speed_changing && !driver.is_halted() && driver.get_power())
 356                 {
 357                         speed_changing = true;
 358                         driver.set_loco_speed(address, speed_step);
 359
 360                         pure_speed = false;
 361                 }
 362
 363                 speed = speed_quantizer->get_speed(current_speed_step);
 364         }
 365
 366         if(moving)
 367         {
 368                 if(!active)
 369                         set_active(true);
 370
 371                 Vehicle &vehicle = *(reverse ? vehicles.back() : vehicles.front());
 372
 373                 float d = speed*(dt/Time::sec);
 374                 if(allocator.is_block_current(vehicle.get_track()->get_block()))
 375                 {
 376                         SetFlag setf(advancing);
 377                         vehicle.advance(reverse ? -d : d);
 378                 }
 379                 else if(accurate_position)
 380                 {
 381                         overshoot_dist += d;
 382                         if(overshoot_dist>40*layout.get_catalogue().get_scale())
 383                         {
 384                                 layout.emergency(name+" has not arrived at sensor");
 385                                 accurate_position = false;
 386                         }
 387                 }
 388         }
 389
 390         if(!allocator.empty() && !allocator.first()->get_sensor_id())
 391         {
 392                 float dist = get_reserved_distance_until(&*allocator.first(), true);
 393
 394                 if(dist>10*layout.get_catalogue().get_scale())
 395                         allocator.release_until(*allocator.first());
 396         }
 397 }
 398
 399 void Train::save(list<DataFile::Statement> &st) const
 400 {
 401         st.push_back((DataFile::Statement("name"), name));
 402
 403         for(vector<Vehicle *>::const_iterator i=vehicles.begin(); i!=vehicles.end(); ++i)
 404                 if(i!=vehicles.begin())
 405                         st.push_back((DataFile::Statement("vehicle"), (*i)->get_type().get_article_number()));
 406
 407         if(speed_quantizer)
 408         {
 409                 DataFile::Statement ss("quantized_speed");
 410                 speed_quantizer->save(ss.sub);
 411                 st.push_back(ss);
 412         }
 413
 414         {
 415                 DataFile::Statement ss("blocks");
 416                 allocator.save(ss.sub);
 417                 st.push_back(ss);
 418         }
 419
 420         // XXX Need more generic way of saving AI state
 421         for(list<TrainAI *>::const_iterator i=ais.begin(); i!=ais.end(); ++i)
 422         {
 423                 if(TrainRouter *router = dynamic_cast<TrainRouter *>(*i))
 424                 {
 425                         DataFile::Statement ss("router");
 426                         router->save(ss.sub);
 427                         st.push_back(ss);
 428                 }
 429                 else if(Timetable *timetable = dynamic_cast<Timetable *>(*i))
 430                 {
 431                         DataFile::Statement ss("timetable");
 432                         timetable->save(ss.sub);
 433                         st.push_back(ss);
 434                 }
 435         }
 436 }
 437
 438 void Train::control_changed(const Controller::Control &ctrl)
 439 {
 440         signal_control_changed.emit(ctrl.name, ctrl.value);
 441 }
 442
 443 void Train::loco_speed_event(unsigned addr, unsigned speed, bool rev)
 444 {
 445         if(addr==address)
 446         {
 447                 current_speed_step = speed;
 448                 bool r = reverse;
 449                 if(loco_type.get_swap_direction())
 450                         r = !r;
 451                 if(rev!=r)
 452                         layout.get_driver().set_loco_reverse(address, r);
 453                 speed_changing = false;
 454                 pure_speed = false;
 455         }
 456 }
 457
 458 void Train::loco_func_event(unsigned addr, unsigned func, bool state)
 459 {
 460         if(addr==address)
 461         {
 462                 if(state)
 463                         functions |= 1<<func;
 464                 else
 465                         functions &= ~(1<<func);
 466
 467                 signal_function_changed.emit(func, state);
 468         }
 469 }
 470
 471 void Train::block_state_changed(Block &block, Block::State state)
 472 {
 473         if(block.get_train()==this && state==Block::MAYBE_ACTIVE)
 474         {
 475                 if(last_entry_block)
 476                 {
 477                         float travel_distance = -1;
 478                         if(pure_speed && speed_quantizer && current_speed_step>0)
 479                                 travel_distance = 0;
 480
 481                         for(BlockIter i=last_entry_block; &*i!=&block; i=i.next())
 482                         {
 483                                 if(i->get_sensor_id())
 484                                         return;
 485                                 if(travel_distance>=0)
 486                                         travel_distance += i->get_path_length(i.entry());
 487                         }
 488
 489                         if(travel_distance>0)
 490                         {
 491                                 float travel_time_secs = (Time::now()-last_entry_time)/Time::sec;
 492
 493                                 if(travel_time_secs>=2)
 494                                         speed_quantizer->learn(current_speed_step, travel_distance/travel_time_secs, travel_time_secs);
 495                         }
 496                 }
 497
 498                 last_entry_block = allocator.iter_for(block);
 499                 last_entry_time = Time::now();
 500                 pure_speed = true;
 501                 accurate_position = true;
 502                 overshoot_dist = 0;
 503
 504                 if(!advancing && vehicles.front()->get_track())
 505                 {
 506                         TrackIter track = last_entry_block.track_iter();
 507                         if(reverse)
 508                         {
 509                                 track = track.flip();
 510                                 vehicles.back()->place(track, 0, Vehicle::BACK_AXLE);
 511                         }
 512                         else
 513                                 vehicles.front()->place(track, 0, Vehicle::FRONT_AXLE);
 514                 }
 515         }
 516 }
 517
 518 void Train::halt_event(bool h)
 519 {
 520         if(h)
 521                 accurate_position = false;
 522 }
 523
 524 float Train::get_reserved_distance_until(const Block *until_block, bool back) const
 525 {
 526         if(allocator.empty())
 527                 return 0;
 528
 529         Vehicle &veh = *(reverse!=back ? vehicles.back() : vehicles.front());
 530         const VehicleType &vtype = veh.get_type();
 531
 532         TrackIter track = veh.get_track_iter();
 533         if(!track)  // XXX Probably unnecessary
 534                 return 0;
 535
 536         const BlockIter &first = allocator.first();
 537         const BlockIter &last = allocator.last();
 538         BlockIter block = first;
 539         while(!block->has_track(*track))
 540         {
 541                 if(&*block==&*last)
 542                         return 0;
 543                 block = block.next();
 544         }
 545         if(&*block==until_block)
 546                 return 0;
 547
 548         if(back)
 549                 block = block.reverse();
 550
 551         float result = veh.get_offset();
 552         if(reverse!=back)
 553                 track = track.reverse();
 554         else
 555                 result = track->get_type().get_path_length(track->get_active_path())-result;
 556         result -= vtype.get_length()/2;
 557
 558         while(1)
 559         {
 560                 track = track.next();
 561                 if(!track)
 562                         break;
 563
 564                 if(!block->has_track(*track))
 565                 {
 566                         if(back)
 567                         {
 568                                 if(&*block==&*first)
 569                                         break;
 570                         }
 571                         else
 572                         {
 573                                 if(&*block==&*last)
 574                                         break;
 575                         }
 576                         block = block.next();
 577
 578                         if(&*block==until_block)
 579                                 break;
 580                 }
 581
 582                 result += track->get_type().get_path_length(track->get_active_path());
 583         }
 584
 585         return result;
 586 }
 587
 588
 589 Train::Loader::Loader(Train &t):
 590         DataFile::ObjectLoader<Train>(t),
 591         prev_block(0),
 592         blocks_valid(true)
 593 {
 594         add("blocks",      &Loader::blocks);
 595         add("name",        &Loader::name);
 596         add("quantized_speed",  &Loader::quantized_speed);
 597         add("router",      &Loader::router);
 598         add("timetable",   &Loader::timetable);
 599         add("vehicle",     &Loader::vehicle);
 600 }
 601
 602 void Train::Loader::finish()
 603 {
 604         if(!obj.allocator.empty())
 605         {
 606                 TrackIter track = obj.allocator.first().track_iter();
 607                 float offset = 2*obj.layout.get_catalogue().get_scale();
 608                 obj.vehicles.back()->place(track, offset, Vehicle::BACK_BUFFER);
 609         }
 610 }
 611
 612 void Train::Loader::blocks()
 613 {
 614         load_sub(obj.allocator);
 615 }
 616
 617 void Train::Loader::name(const string &n)
 618 {
 619         obj.set_name(n);
 620 }
 621
 622 void Train::Loader::quantized_speed()
 623 {
 624         if(obj.speed_quantizer)
 625                 load_sub(*obj.speed_quantizer);
 626 }
 627
 628 void Train::Loader::router()
 629 {
 630         TrainRouter *rtr = new TrainRouter(obj);
 631         load_sub(*rtr);
 632 }
 633
 634 void Train::Loader::timetable()
 635 {
 636         Timetable *ttbl = new Timetable(obj);
 637         load_sub(*ttbl);
 638 }
 639
 640 void Train::Loader::vehicle(ArticleNumber art_nr)
 641 {
 642         const VehicleType &vtype = obj.layout.get_catalogue().get_vehicle(art_nr);
 643         Vehicle *veh = new Vehicle(obj.layout, vtype);
 644         obj.vehicles.back()->attach_back(*veh);
 645         obj.vehicles.push_back(veh);
 646 }
 647
 648 } // namespace R2C2