source/libr2c2/train.cpp

   1 #include <algorithm>
   2 #include <cmath>
   3 #include <msp/core/maputils.h>
   4 #include <msp/strings/format.h>
   5 #include <msp/time/units.h>
   6 #include <msp/time/utils.h>
   7 #include "aicontrol.h"
   8 #include "catalogue.h"
   9 #include "driver.h"
  10 #include "layout.h"
  11 #include "route.h"
  12 #include "simplecontroller.h"
  13 #include "speedquantizer.h"
  14 #include "timetable.h"
  15 #include "trackiter.h"
  16 #include "tracktype.h"
  17 #include "train.h"
  18 #include "vehicle.h"
  19 #include "vehicletype.h"
  20 #include "zone.h"
  21
  22 using namespace std;
  23 using namespace Msp;
  24
  25 namespace {
  26
  27 struct SetFlag
  28 {
  29         bool &flag;
  30
  31         SetFlag(bool &f): flag(f) { flag = true; }
  32         ~SetFlag() { flag = false; }
  33 };
  34
  35 }
  36
  37
  38 namespace R2C2 {
  39
  40 Train::Train(Layout &l, const VehicleType &t, unsigned a, const string &p):
  41         layout(l),
  42         loco_type(t),
  43         address(a),
  44         protocol(p),
  45         priority(0),
  46         yielding_to(0),
  47         preceding_train(0),
  48         cur_blocks_end(blocks.end()),
  49         clear_blocks_end(blocks.end()),
  50         pending_block(0),
  51         reserving(false),
  52         advancing(false),
  53         controller(new SimpleController),
  54         active(false),
  55         current_speed_step(0),
  56         speed_changing(false),
  57         reverse(false),
  58         functions(0),
  59         end_of_route(false),
  60         travel_dist(0),
  61         pure_speed(false),
  62         speed_quantizer(0),
  63         accurate_position(false),
  64         overshoot_dist(false)
  65 {
  66         if(!loco_type.is_locomotive())
  67                 throw invalid_argument("Train::Train");
  68
  69         unsigned speed_steps = layout.get_driver().get_protocol_speed_steps(protocol);
  70         if(speed_steps)
  71                 speed_quantizer = new SpeedQuantizer(speed_steps);
  72
  73         vehicles.push_back(new Vehicle(layout, loco_type));
  74
  75         layout.add_train(*this);
  76
  77         layout.get_driver().add_loco(address, protocol, loco_type);
  78         layout.get_driver().signal_loco_speed.connect(sigc::mem_fun(this, &Train::loco_speed_event));
  79         layout.get_driver().signal_loco_function.connect(sigc::mem_fun(this, &Train::loco_func_event));
  80
  81         layout.signal_block_reserved.connect(sigc::mem_fun(this, &Train::block_reserved));
  82         layout.signal_block_state_changed.connect(sigc::mem_fun(this, &Train::block_state_changed));
  83
  84         layout.get_driver().signal_halt.connect(sigc::mem_fun(this, &Train::halt_event));
  85
  86         const set<Track *> &tracks = layout.get_tracks();
  87         for(set<Track *>::const_iterator i=tracks.begin(); i!=tracks.end(); ++i)
  88                 if((*i)->get_turnout_id())
  89                         (*i)->signal_path_changed.connect(sigc::hide(sigc::bind(sigc::mem_fun(this, &Train::turnout_path_changed), sigc::ref(**i))));
  90
  91         controller->signal_control_changed.connect(sigc::mem_fun(this, &Train::control_changed));
  92 }
  93
  94 Train::~Train()
  95 {
  96         delete controller;
  97         for(vector<Vehicle *>::iterator i=vehicles.begin(); i!=vehicles.end(); ++i)
  98                 delete *i;
  99         layout.remove_train(*this);
 100 }
 101
 102 void Train::set_name(const string &n)
 103 {
 104         name = n;
 105
 106         signal_name_changed.emit(name);
 107 }
 108
 109 void Train::set_priority(int p)
 110 {
 111         priority = p;
 112 }
 113
 114 void Train::yield_to(const Train &t)
 115 {
 116         yielding_to = &t;
 117 }
 118
 119 void Train::add_vehicle(const VehicleType &vt)
 120 {
 121         Vehicle *veh = new Vehicle(layout, vt);
 122         vehicles.back()->attach_back(*veh);
 123         vehicles.push_back(veh);
 124 }
 125
 126 void Train::remove_vehicle(unsigned i)
 127 {
 128         if(i>=vehicles.size())
 129                 throw out_of_range("Train::remove_vehicle");
 130         if(i==0)
 131                 throw logic_error("can't remove locomotive");
 132         delete vehicles[i];
 133         vehicles.erase(vehicles.begin()+i);
 134         if(i<vehicles.size())
 135                 vehicles[i-1]->attach_back(*vehicles[i]);
 136 }
 137
 138 unsigned Train::get_n_vehicles() const
 139 {
 140         return vehicles.size();
 141 }
 142
 143 Vehicle &Train::get_vehicle(unsigned i)
 144 {
 145         if(i>=vehicles.size())
 146                 throw out_of_range("Train::get_vehicle");
 147         return *vehicles[i];
 148 }
 149
 150 const Vehicle &Train::get_vehicle(unsigned i) const
 151 {
 152         if(i>=vehicles.size())
 153                 throw out_of_range("Train::get_vehicle");
 154         return *vehicles[i];
 155 }
 156
 157 void Train::set_control(const string &n, float v)
 158 {
 159         controller->set_control(n, v);
 160 }
 161
 162 void Train::set_active(bool a)
 163 {
 164         if(a==active)
 165                 return;
 166         if(!a && controller->get_speed())
 167                 throw logic_error("moving");
 168
 169         active = a;
 170         if(active)
 171         {
 172                 stop_timeout = Time::TimeStamp();
 173                 reserve_more();
 174         }
 175         else
 176                 stop_timeout = Time::now()+2*Time::sec;
 177 }
 178
 179 void Train::set_function(unsigned func, bool state)
 180 {
 181         if(!loco_type.get_functions().count(func))
 182                 throw invalid_argument("Train::set_function");
 183         layout.get_driver().set_loco_function(address, func, state);
 184 }
 185
 186 float Train::get_control(const string &ctrl) const
 187 {
 188         return controller->get_control(ctrl).value;
 189 }
 190
 191 float Train::get_speed() const
 192 {
 193         return controller->get_speed();
 194 }
 195
 196 float Train::get_quantized_speed() const
 197 {
 198         if(speed_quantizer)
 199                 return speed_quantizer->quantize_speed(controller->get_speed());
 200         else
 201                 return controller->get_speed();
 202 }
 203
 204 bool Train::get_function(unsigned func) const
 205 {
 206         return (functions>>func)&1;
 207 }
 208
 209 void Train::add_ai(TrainAI &ai)
 210 {
 211         ais.push_back(&ai);
 212         ai.signal_event.connect(sigc::bind<0>(signal_ai_event, sigc::ref(ai)));
 213 }
 214
 215 void Train::remove_ai(TrainAI &ai)
 216 {
 217         list<TrainAI *>::iterator i = find(ais.begin(), ais.end(), &ai);
 218         if(i!=ais.end())
 219                 ais.erase(i);
 220 }
 221
 222 TrainAI *Train::get_tagged_ai(const string &tag) const
 223 {
 224         for(list<TrainAI *>::const_iterator i=ais.begin(); i!=ais.end(); ++i)
 225                 if((*i)->get_tag()==tag)
 226                         return *i;
 227
 228         return 0;
 229 }
 230
 231 void Train::ai_message(const TrainAI::Message &msg)
 232 {
 233         for(list<TrainAI *>::iterator i=ais.begin(); i!=ais.end(); ++i)
 234                 (*i)->message(msg);
 235 }
 236
 237 bool Train::set_route(const Route *r)
 238 {
 239         free_noncritical_blocks();
 240
 241         Route *lead = 0;
 242         if(r && !blocks.empty())
 243         {
 244                 TrackIter first = blocks.front().track_iter();
 245                 TrackIter next = blocks.back().next().track_iter();
 246                 if(!r->has_track(*next))
 247                 {
 248                         lead = Route::find(next, *r);
 249                         if(!lead)
 250                                 return false;
 251                         create_lead_route(lead, lead);
 252                         routes.push_front(lead);
 253                 }
 254                 else if(!r->has_track(*first))
 255                         lead = create_lead_route(0, r);
 256         }
 257
 258         routes.clear();
 259         if(lead)
 260                 routes.push_back(lead);
 261         if(r)
 262                 routes.push_back(r);
 263         end_of_route = false;
 264
 265         reserve_more();
 266
 267         signal_route_changed.emit(get_route());
 268
 269         return true;
 270 }
 271
 272 bool Train::go_to(Track &to)
 273 {
 274         for(BlockList::const_iterator i=blocks.begin(); i!=cur_blocks_end; ++i)
 275                 if((*i)->has_track(to))
 276                 {
 277                         signal_arrived.emit();
 278                         return set_route(0);
 279                 }
 280
 281         free_noncritical_blocks();
 282
 283         TrackIter next = blocks.back().next().track_iter();
 284
 285         Route *route = Route::find(next, to);
 286         if(!route)
 287                 return false;
 288         create_lead_route(route, route);
 289         return set_route(route);
 290 }
 291
 292 bool Train::go_to(const Zone &to)
 293 {
 294         set<Track *> tracks;
 295         for(BlockList::const_iterator i=blocks.begin(); i!=blocks.end(); ++i)
 296                 tracks.insert((*i)->get_tracks().begin(), (*i)->get_tracks().end());
 297
 298         const Zone::TrackSet &ztracks = to.get_tracks();
 299         unsigned union_size = 0;
 300         for(Zone::TrackSet::const_iterator i=ztracks.begin(); i!=ztracks.end(); ++i)
 301                 union_size += tracks.count(*i);
 302
 303         if(union_size==tracks.size() || union_size==ztracks.size())
 304         {
 305                 signal_arrived.emit();
 306                 return set_route(0);
 307         }
 308
 309         free_noncritical_blocks();
 310
 311         TrackIter next = blocks.back().next().track_iter();
 312
 313         Route *route = Route::find(next, to);
 314         if(!route)
 315                 return false;
 316         create_lead_route(route, route);
 317         route->add_tracks(ztracks);
 318         return set_route(route);
 319 }
 320
 321 bool Train::divert(Track &from)
 322 {
 323         if(!from.get_turnout_id())
 324                 throw invalid_argument("Train::divert");
 325         if(routes.empty())
 326                 return false;
 327
 328         unsigned path = 0;
 329         unsigned entry = 0;
 330         list<RouteRef>::iterator route = routes.begin();
 331
 332         // Follow our routes to find out where we're entering the turnout
 333         for(TrackLoopIter track = blocks.front().track_iter();;)
 334         {
 335                 if(!advance_route(route, *track))
 336                         return false;
 337
 338                 if(&*track==&from)
 339                 {
 340                         Block &block = track->get_block();
 341                         if(block.get_train()==this && !free_block(block))
 342                                 return false;
 343
 344                         int route_path = route->route->get_turnout(from.get_turnout_id());
 345
 346                         // Check that more than one path is available
 347                         unsigned ep_paths = track.endpoint().paths;
 348                         if(!(ep_paths&(ep_paths-1)))
 349                                 return false;
 350
 351                         // Choose some other path
 352                         for(int i=0; ep_paths>>i; ++i)
 353                                 if((ep_paths&(1<<i)) && i!=route_path)
 354                                 {
 355                                         path = i;
 356                                         break;
 357                                 }
 358
 359                         entry = track.entry();
 360                         break;
 361                 }
 362
 363                 track = track.next(route->route->get_path(*track));
 364
 365                 if(!track || track.looped())
 366                         return false;
 367         }
 368
 369         TrackIter track = TrackIter(&from, entry).next(path);
 370         if(!track)
 371                 return false;
 372
 373         set<Track *> tracks;
 374         for(list<RouteRef>::iterator i=routes.begin(); i!=routes.end(); ++i)
 375                 tracks.insert(i->route->get_tracks().begin(), i->route->get_tracks().end());
 376         RefPtr<Route> diversion = Route::find(track, tracks);
 377         if(!diversion)
 378                 return false;
 379
 380         diversion->set_name("Diversion");
 381         diversion->add_track(from);
 382         diversion->set_turnout(from.get_turnout_id(), path);
 383
 384         if(!is_valid_diversion(*diversion, TrackIter(&from, entry)))
 385                 return false;
 386
 387         // Follow the diversion route until we get back to the original route
 388         list<RouteRef>::iterator end = routes.end();
 389         while(1)
 390         {
 391                 for(list<RouteRef>::iterator i=route; (end==routes.end() && i!=routes.end()); ++i)
 392                         if(i->route->has_track(*track))
 393                                 end = i;
 394
 395                 if(end!=routes.end())
 396                         break;
 397                 else if(!diversion->has_track(*track))
 398                         throw logic_error("bad diversion");
 399
 400                 track = track.next(diversion->get_path(*track));
 401         }
 402
 403         if(route==end)
 404                 // We are rejoining the same route we diverted from, duplicate it
 405                 routes.insert(end, *route);
 406         else
 407         {
 408                 ++route;
 409                 routes.erase(route, end);
 410         }
 411         routes.insert(end, RouteRef(diversion.release(), from.get_turnout_id()));
 412
 413         return true;
 414 }
 415
 416 const Route *Train::get_route() const
 417 {
 418         if(routes.empty())
 419                 return 0;
 420         return routes.front().route;
 421 }
 422
 423 void Train::place(Block &block, unsigned entry)
 424 {
 425         if(controller->get_speed())
 426                 throw logic_error("moving");
 427
 428         release_blocks();
 429
 430         set_active(false);
 431         accurate_position = false;
 432
 433         if(!block.reserve(this))
 434                 return;
 435
 436         blocks.push_back(BlockIter(&block, entry));
 437         if(reverse)
 438         {
 439                 TrackIter track = BlockIter(&block, entry).reverse().track_iter();
 440                 vehicles.front()->place(*track, track.entry(), 0, Vehicle::FRONT_BUFFER);
 441         }
 442         else
 443         {
 444                 const Block::Endpoint &bep = block.get_endpoint(entry);
 445                 vehicles.back()->place(*bep.track, bep.track_ep, 0, Vehicle::BACK_BUFFER);
 446         }
 447 }
 448
 449 void Train::unplace()
 450 {
 451         if(controller->get_speed())
 452                 throw logic_error("moving");
 453
 454         release_blocks();
 455
 456         set_active(false);
 457         accurate_position = false;
 458
 459         for(vector<Vehicle *>::iterator i=vehicles.begin(); i!=vehicles.end(); ++i)
 460                 (*i)->unplace();
 461 }
 462
 463 bool Train::free_block(Block &block)
 464 {
 465         if(get_reserved_distance_until(&block, false)<controller->get_braking_distance()*1.3)
 466                 return false;
 467
 468         unsigned nsens = 0;
 469         for(BlockList::iterator i=cur_blocks_end; i!=blocks.end(); ++i)
 470         {
 471                 if(i->block()==&block)
 472                 {
 473                         if(nsens<1)
 474                                 return false;
 475                         release_blocks(i, blocks.end());
 476                         return true;
 477                 }
 478                 else if((*i)->get_sensor_id())
 479                         ++nsens;
 480         }
 481
 482         return false;
 483 }
 484
 485 void Train::free_noncritical_blocks()
 486 {
 487         if(blocks.empty())
 488                 return;
 489
 490         if(controller->get_speed()==0)
 491         {
 492                 release_blocks(cur_blocks_end, blocks.end());
 493                 return;
 494         }
 495
 496         float margin = 10*layout.get_catalogue().get_scale();
 497         float min_dist = controller->get_braking_distance()*1.3+margin;
 498
 499         Vehicle &veh = *(reverse ? vehicles.back() : vehicles.front());
 500
 501         TrackIter track(veh.get_track(), veh.get_entry());
 502         BlockList::iterator block = blocks.begin();
 503         bool in_rsv = false;
 504         while(block!=blocks.end() && !(*block)->has_track(*track))
 505         {
 506                 ++block;
 507                 if(block==cur_blocks_end)
 508                         in_rsv = true;
 509         }
 510
 511         float dist = veh.get_offset();
 512         if(reverse)
 513                 track.reverse();
 514         else
 515                 dist = track->get_type().get_path_length(track->get_active_path())-dist;
 516         dist -= veh.get_type().get_length()/2;
 517
 518         bool nsens = 0;
 519         while(1)
 520         {
 521                 track = track.next();
 522
 523                 if(!(*block)->has_track(*track))
 524                 {
 525                         ++block;
 526                         if(block==cur_blocks_end)
 527                                 in_rsv = true;
 528                         if(block==blocks.end())
 529                                 return;
 530
 531                         if(dist>min_dist && nsens>0)
 532                         {
 533                                 release_blocks(block, blocks.end());
 534                                 return;
 535                         }
 536
 537                         if(in_rsv && (*block)->get_sensor_id())
 538                                 ++nsens;
 539                 }
 540
 541                 dist += track->get_type().get_path_length(track->get_active_path());
 542         }
 543 }
 544
 545 int Train::get_entry_to_block(const Block &block) const
 546 {
 547         for(BlockList::const_iterator i=blocks.begin(); i!=blocks.end(); ++i)
 548                 if(i->block()==&block)
 549                         return i->entry();
 550         return -1;
 551 }
 552
 553 float Train::get_reserved_distance() const
 554 {
 555         if(blocks.empty())
 556                 return 0;
 557
 558         float margin = 0;
 559         TrackIter next = blocks.back().next().track_iter();
 560         if(next && next->get_type().is_turnout())
 561                 margin = 15*layout.get_catalogue().get_scale();
 562
 563         return max(get_reserved_distance_until(0, false)-margin, 0.0f);
 564 }
 565
 566 void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt)
 567 {
 568         if(!active && stop_timeout && t>=stop_timeout)
 569         {
 570                 release_blocks(cur_blocks_end, blocks.end());
 571                 stop_timeout = Time::TimeStamp();
 572         }
 573
 574         Driver &driver = layout.get_driver();
 575
 576         for(list<TrainAI *>::iterator i=ais.begin(); i!=ais.end(); ++i)
 577                 (*i)->tick(t, dt);
 578         controller->tick(dt);
 579         float speed = controller->get_speed();
 580         bool moving = speed>0;
 581
 582         if(controller->get_reverse()!=reverse)
 583         {
 584                 reverse = controller->get_reverse();
 585                 bool r = reverse;
 586                 if(loco_type.get_swap_direction())
 587                         r = !r;
 588                 driver.set_loco_reverse(address, r);
 589
 590                 release_blocks(cur_blocks_end, blocks.end());
 591                 reverse_blocks(blocks);
 592
 593                 reserve_more();
 594         }
 595
 596         if(speed_quantizer)
 597         {
 598                 unsigned speed_step = speed_quantizer->find_speed_step(speed);
 599                 if(speed_step!=current_speed_step && !speed_changing && !driver.is_halted() && driver.get_power())
 600                 {
 601                         speed_changing = true;
 602                         driver.set_loco_speed(address, speed_step);
 603
 604                         pure_speed = false;
 605                 }
 606
 607                 speed = speed_quantizer->get_speed(current_speed_step);
 608         }
 609
 610         if(moving)
 611         {
 612                 if(!active)
 613                         set_active(true);
 614
 615                 Vehicle &vehicle = *(reverse ? vehicles.back() : vehicles.front());
 616                 Track *track = vehicle.get_track();
 617
 618                 bool ok = false;
 619                 for(BlockList::const_iterator i=blocks.begin(); (!ok && i!=cur_blocks_end); ++i)
 620                         ok = (*i)->has_track(*track);
 621
 622                 float d = speed*(dt/Time::sec);
 623                 if(ok)
 624                 {
 625                         SetFlag setf(advancing);
 626                         vehicle.advance(reverse ? -d : d);
 627                 }
 628                 else if(accurate_position)
 629                 {
 630                         overshoot_dist += d;
 631                         if(overshoot_dist>40*layout.get_catalogue().get_scale())
 632                         {
 633                                 layout.emergency(name+" has not arrived at sensor");
 634                                 accurate_position = false;
 635                         }
 636                 }
 637         }
 638         else if(end_of_route && cur_blocks_end==blocks.end())
 639         {
 640                 set_active(false);
 641                 signal_arrived.emit();
 642                 set_route(0);
 643         }
 644
 645         if(!blocks.empty() && !blocks.front()->get_sensor_id())
 646         {
 647                 float dist = get_reserved_distance_until(&*blocks.front(), true);
 648
 649                 if(dist>10*layout.get_catalogue().get_scale())
 650                 {
 651                         blocks.front()->reserve(0);
 652                         blocks.pop_front();
 653                 }
 654         }
 655 }
 656
 657 void Train::save(list<DataFile::Statement> &st) const
 658 {
 659         st.push_back((DataFile::Statement("name"), name));
 660
 661         st.push_back((DataFile::Statement("priority"), priority));
 662
 663         for(vector<Vehicle *>::const_iterator i=vehicles.begin(); i!=vehicles.end(); ++i)
 664                 if(i!=vehicles.begin())
 665                         st.push_back((DataFile::Statement("vehicle"), (*i)->get_type().get_article_number()));
 666
 667         if(speed_quantizer)
 668         {
 669                 DataFile::Statement ss("quantized_speed");
 670                 speed_quantizer->save(ss.sub);
 671                 st.push_back(ss);
 672         }
 673
 674         if(!blocks.empty() && cur_blocks_end!=blocks.begin())
 675         {
 676                 BlockList blks(blocks.begin(), BlockList::const_iterator(cur_blocks_end));
 677                 if(reverse)
 678                         reverse_blocks(blks);
 679
 680                 BlockIter prev = blks.front().flip();
 681                 st.push_back((DataFile::Statement("block_hint"), prev->get_id()));
 682
 683                 for(BlockList::const_iterator i=blks.begin(); i!=blks.end(); ++i)
 684                         st.push_back((DataFile::Statement("block"), (*i)->get_id()));
 685         }
 686
 687         if(!routes.empty())
 688         {
 689                 list<RouteRef>::const_iterator i = routes.begin();
 690                 for(; (i!=routes.end() && i->route->is_temporary()); ++i) ;
 691                 if(i!=routes.end())
 692                         st.push_back((DataFile::Statement("route"), i->route->get_name()));
 693         }
 694
 695         // XXX Need more generic way of saving AI state
 696         for(list<TrainAI *>::const_iterator i=ais.begin(); i!=ais.end(); ++i)
 697                 if(Timetable *timetable = dynamic_cast<Timetable *>(*i))
 698                 {
 699                         DataFile::Statement ss("timetable");
 700                         timetable->save(ss.sub);
 701                         st.push_back(ss);
 702                 }
 703 }
 704
 705 void Train::control_changed(const Controller::Control &ctrl)
 706 {
 707         signal_control_changed.emit(ctrl.name, ctrl.value);
 708 }
 709
 710 void Train::loco_speed_event(unsigned addr, unsigned speed, bool rev)
 711 {
 712         if(addr==address)
 713         {
 714                 current_speed_step = speed;
 715                 bool r = reverse;
 716                 if(loco_type.get_swap_direction())
 717                         r = !r;
 718                 if(rev!=r)
 719                         layout.get_driver().set_loco_reverse(address, r);
 720                 speed_changing = false;
 721                 pure_speed = false;
 722         }
 723 }
 724
 725 void Train::loco_func_event(unsigned addr, unsigned func, bool state)
 726 {
 727         if(addr==address)
 728         {
 729                 if(state)
 730                         functions |= 1<<func;
 731                 else
 732                         functions &= ~(1<<func);
 733
 734                 signal_function_changed.emit(func, state);
 735         }
 736 }
 737
 738 void Train::block_state_changed(Block &block, Block::State state)
 739 {
 740         if(state==Block::MAYBE_ACTIVE)
 741         {
 742                 // Find the first sensor block from our reserved blocks that isn't this sensor
 743                 BlockList::iterator end;
 744                 unsigned result = 0;
 745                 for(end=cur_blocks_end; end!=blocks.end(); ++end)
 746                         if((*end)->get_sensor_id())
 747                         {
 748                                 if(&**end!=&block)
 749                                 {
 750                                         if(result==0)
 751                                                 result = 2;
 752                                         else if(result==1)
 753                                                 break;
 754                                 }
 755                                 else if(result==0)
 756                                         result = 1;
 757                                 else if(result==2)
 758                                         result = 3;
 759                         }
 760
 761                 if(result==1)
 762                 {
 763                         // Compute speed and update related state
 764                         float travel_time_secs = (Time::now()-last_entry_time)/Time::sec;
 765
 766                         if(pure_speed && speed_quantizer && current_speed_step>0 && travel_time_secs>=2)
 767                                 speed_quantizer->learn(current_speed_step, travel_dist/travel_time_secs, travel_time_secs);
 768
 769                         travel_dist = 0;
 770                         for(BlockList::iterator j=cur_blocks_end; j!=end; ++j)
 771                         {
 772                                 travel_dist += (*j)->get_path_length(j->entry());
 773
 774                                 if(&**j==&block && !advancing)
 775                                 {
 776                                         TrackIter track = j->track_iter();
 777                                         if(reverse)
 778                                         {
 779                                                 track = track.flip();
 780                                                 vehicles.back()->place(*track, track.entry(), 0, Vehicle::BACK_AXLE);
 781                                         }
 782                                         else
 783                                                 vehicles.front()->place(*track, track.entry(), 0, Vehicle::FRONT_AXLE);
 784                                 }
 785                         }
 786                         last_entry_time = Time::now();
 787                         pure_speed = true;
 788                         accurate_position = true;
 789                         overshoot_dist = 0;
 790
 791                         // Check if we've reached the next route
 792                         if(routes.size()>1)
 793                         {
 794                                 const Route &route = *(++routes.begin())->route;
 795                                 for(BlockList::iterator j=cur_blocks_end; j!=end; ++j)
 796                                         if(route.has_track(*j->track_iter()))
 797                                         {
 798                                                 routes.pop_front();
 799                                                 // XXX Exceptions?
 800                                                 signal_route_changed.emit(routes.front().route);
 801                                                 break;
 802                                         }
 803                         }
 804
 805                         // Move blocks up to the next sensor to our current blocks
 806                         for(BlockList::iterator j=cur_blocks_end; j!=end; ++j)
 807                                 signal_advanced.emit(**j);
 808                         cur_blocks_end = end;
 809
 810                         // Try to get more blocks if we're moving
 811                         if(active)
 812                                 reserve_more();
 813                 }
 814                 else if(result==3)
 815                         layout.emergency("Sensor for "+name+" triggered out of order");
 816         }
 817         else if(state==Block::INACTIVE)
 818         {
 819                 const Vehicle &veh = *(reverse ? vehicles.front() : vehicles.back());
 820
 821                 // Find the first sensor in our current blocks that's still active
 822                 BlockList::iterator end = blocks.begin();
 823                 for(BlockList::iterator i=blocks.begin(); i!=cur_blocks_end; ++i)
 824                 {
 825                         if((*i)->has_track(*veh.get_track()))
 826                                 break;
 827                         if((*i)->get_sensor_id())
 828                         {
 829                                 if(layout.get_driver().get_sensor((*i)->get_sensor_id()))
 830                                         break;
 831                                 else
 832                                 {
 833                                         end = i;
 834                                         ++end;
 835                                 }
 836                         }
 837                 }
 838
 839                 if(end!=blocks.begin() && end!=cur_blocks_end)
 840                         // Free blocks up to the last inactive sensor
 841                         release_blocks(blocks.begin(), end);
 842         }
 843 }
 844
 845 void Train::turnout_path_changed(Track &track)
 846 {
 847         for(list<BlockIter>::iterator i=blocks.begin(); i!=blocks.end(); ++i)
 848                 if((*i)->get_turnout_id()==track.get_turnout_id() && !reserving)
 849                         check_turnout_paths(false);
 850 }
 851
 852 void Train::halt_event(bool h)
 853 {
 854         if(h)
 855                 accurate_position = false;
 856 }
 857
 858 void Train::block_reserved(const Block &block, const Train *train)
 859 {
 860         if(&block==pending_block && !train && !reserving)
 861                 reserve_more();
 862 }
 863
 864 void Train::reserve_more()
 865 {
 866         if(!active || blocks.empty() || end_of_route)
 867                 return;
 868
 869         BlockIter start = blocks.back();
 870
 871         pending_block = 0;
 872         preceding_train = 0;
 873
 874         // See how many sensor blocks and how much track we already have
 875         unsigned nsens = 0;
 876         float dist = 0;
 877         for(BlockList::const_iterator i=cur_blocks_end; i!=blocks.end(); ++i)
 878         {
 879                 if((*i)->get_sensor_id())
 880                         ++nsens;
 881                 if(nsens>0)
 882                         dist += (*i)->get_path_length(i->entry());
 883         }
 884
 885         list<RouteRef>::iterator cur_route = routes.begin();
 886         advance_route(cur_route, *start.track_iter());
 887
 888         float approach_margin = 50*layout.get_catalogue().get_scale();
 889         float min_dist = controller->get_braking_distance()*1.3+approach_margin*2;
 890
 891         BlockIter block = start;
 892         list<BlockIter>::iterator good_end = blocks.end();
 893         Track *divert_track = 0;
 894         bool try_divert = false;
 895         Train *blocking_train = 0;
 896         BlockList contested_blocks;
 897
 898         SetFlag setf(reserving);
 899
 900         while(1)
 901         {
 902                 BlockIter last = block;
 903                 block = block.next(cur_route!=routes.end() ? cur_route->route : 0);
 904                 if(!block || block->get_endpoints().size()<2)
 905                 {
 906                         if(!blocking_train)
 907                         {
 908                                 good_end = blocks.end();
 909                                 end_of_route = true;
 910                         }
 911                         break;
 912                 }
 913
 914                 TrackIter track = block.track_iter();
 915
 916                 if(cur_route!=routes.end())
 917                 {
 918                         if(!advance_route(cur_route, *track))
 919                         {
 920                                 // Keep the blocks if we arrived at the end of the route
 921                                 if(!blocking_train)
 922                                 {
 923                                         good_end = blocks.end();
 924                                         end_of_route = true;
 925                                 }
 926                                 break;
 927                         }
 928                 }
 929
 930                 if(block->get_turnout_id() && !last->get_turnout_id())
 931                 {
 932                         /* We can keep the blocks if we arrive at a turnout from a non-turnout
 933                         block.  Having a turnout block as our last reserved block is not good
 934                         as it would limit our diversion possibilities for little benefit. */
 935                         good_end = blocks.end();
 936                         if(nsens>=3 && dist>=min_dist)
 937                                 break;
 938                 }
 939
 940                 if(blocking_train)
 941                 {
 942                         if(block->get_train()!=blocking_train)
 943                         {
 944                                 if(blocking_train->free_block(*contested_blocks.back()))
 945                                 {
 946                                         // Roll back and start actually reserving the blocks
 947                                         block = blocks.back();
 948                                         cur_route = routes.begin();
 949                                         advance_route(cur_route, *block.track_iter().track());
 950                                         if(blocking_train->get_priority()==priority)
 951                                                 blocking_train->yield_to(*this);
 952                                         blocking_train = 0;
 953                                         continue;
 954                                 }
 955                                 else
 956                                 {
 957                                         yield_to(*blocking_train);
 958                                         pending_block = contested_blocks.front().block();
 959                                         try_divert = divert_track;
 960                                         break;
 961                                 }
 962                         }
 963                         else
 964                         {
 965                                 contested_blocks.push_back(block);
 966                                 continue;
 967                         }
 968                 }
 969
 970                 bool reserved = block->reserve(this);
 971                 if(!reserved)
 972                 {
 973                         /* We've found another train.  If it wants to exit the block from the
 974                         same endpoint we're trying to enter from or the other way around,
 975                         treat it as coming towards us.  Otherwise treat it as going in the
 976                         same direction. */
 977                         Train *other_train = block->get_train();
 978                         int other_entry = other_train->get_entry_to_block(*block);
 979                         if(other_entry<0)
 980                                 throw logic_error("block reservation inconsistency");
 981
 982                         unsigned exit = block.reverse().entry();
 983                         unsigned other_exit = BlockIter(block.block(), other_entry).reverse().entry();
 984                         bool entry_conflict = (block.entry()==other_exit);
 985                         bool exit_conflict = (exit==static_cast<unsigned>(other_entry));
 986                         if(!entry_conflict && !last->get_turnout_id())
 987                         {
 988                                 /* The other train is not coming to the blocks we're holding, so we
 989                                 can keep them. */
 990                                 good_end = blocks.end();
 991
 992                                 if(static_cast<unsigned>(other_entry)==block.entry())
 993                                         preceding_train = other_train;
 994                         }
 995
 996                         int other_prio = other_train->get_priority();
 997
 998                         if(!entry_conflict && !exit_conflict && other_prio<priority)
 999                         {
1000                                 /* Ask a lesser priority train going to the same direction to free
1001                                 the block for us */
1002                                 if(other_train->free_block(*block))
1003                                         reserved = block->reserve(this);
1004                         }
1005                         else if(other_train!=yielding_to && (other_prio<priority || (other_prio==priority && entry_conflict)))
1006                         {
1007                                 /* A lesser priority train is coming at us, we must ask it to free
1008                                 enough blocks to get clear of it to avoid a potential deadlock */
1009                                 blocking_train = other_train;
1010                                 contested_blocks.clear();
1011                                 contested_blocks.push_back(block);
1012                                 continue;
1013                         }
1014                         else if(divert_track && (entry_conflict || exit_conflict || !other_train->is_active()))
1015                                 // We are blocked, but there's a diversion possibility
1016                                 try_divert = true;
1017
1018                         if(!reserved)
1019                         {
1020                                 pending_block = &*block;
1021                                 break;
1022                         }
1023                 }
1024
1025                 if(block->get_turnout_id())
1026                 {
1027                         const TrackType::Endpoint &track_ep = track.endpoint();
1028                         bool multiple_paths = (track_ep.paths&(track_ep.paths-1));
1029
1030                         if(multiple_paths && cur_route!=routes.end() && cur_route->diversion!=block->get_turnout_id())
1031                                 /* There's multiple paths to be taken and we are on a route - take
1032                                 note of the diversion possibility */
1033                                 divert_track = &*track;
1034                 }
1035
1036                 if(!contested_blocks.empty() && contested_blocks.front()==block)
1037                         contested_blocks.pop_front();
1038
1039                 blocks.push_back(block);
1040
1041                 if(cur_blocks_end==blocks.end())
1042                         --cur_blocks_end;
1043                 if(clear_blocks_end==blocks.end())
1044                         --clear_blocks_end;
1045                 if(good_end==blocks.end())
1046                         --good_end;
1047
1048                 if(block->get_sensor_id())
1049                         ++nsens;
1050                 if(nsens>0)
1051                         dist += block->get_path_length(block.entry());
1052         }
1053
1054         // Unreserve blocks that were not good
1055         release_blocks(good_end, blocks.end());
1056
1057         if(blocks.back()!=start)
1058                 // We got some new blocks, so no longer need to yield
1059                 yielding_to = 0;
1060
1061         check_turnout_paths(true);
1062
1063         // Make any sensorless blocks at the beginning immediately current
1064         while(cur_blocks_end!=clear_blocks_end && !(*cur_blocks_end)->get_sensor_id())
1065                 ++cur_blocks_end;
1066
1067         if(try_divert && divert(*divert_track))
1068                 reserve_more();
1069 }
1070
1071 void Train::check_turnout_paths(bool set)
1072 {
1073         if(clear_blocks_end==blocks.end())
1074                 return;
1075
1076         for(list<BlockIter>::iterator i=clear_blocks_end; i!=blocks.end(); ++i)
1077         {
1078                 if((*i)->get_turnout_id())
1079                 {
1080                         TrackIter track = i->track_iter();
1081                         const TrackType::Endpoint &track_ep = track.endpoint();
1082
1083                         unsigned path = 0;
1084                         list<BlockIter>::iterator j = i;
1085                         if(++j!=blocks.end())
1086                         {
1087                                 TrackIter rev = j->track_iter().flip();
1088                                 unsigned mask = rev.endpoint().paths&track_ep.paths;
1089                                 for(path=0; mask>1; mask>>=1, ++path) ;
1090                         }
1091                         else
1092                                 return;
1093
1094                         if(path!=track->get_active_path())
1095                         {
1096                                 if(set)
1097                                         track->set_active_path(path);
1098
1099                                 /* Check again, in case the driver was able to service the request
1100                                 instantly */
1101                                 if(!set || path!=track->get_active_path())
1102                                         continue;
1103                         }
1104                 }
1105
1106                 if(i==clear_blocks_end)
1107                         ++clear_blocks_end;
1108         }
1109 }
1110
1111 float Train::get_reserved_distance_until(const Block *until_block, bool back) const
1112 {
1113         if(blocks.empty())
1114                 return 0;
1115
1116         Vehicle &veh = *(reverse!=back ? vehicles.back() : vehicles.front());
1117         const VehicleType &vtype = veh.get_type();
1118
1119         TrackIter track(veh.get_track(), veh.get_entry());
1120         if(!track)  // XXX Probably unnecessary
1121                 return 0;
1122
1123         BlockList::const_iterator block = blocks.begin();
1124         while(block!=clear_blocks_end && !(*block)->has_track(*track))
1125                 ++block;
1126         if(block==clear_blocks_end || &**block==until_block)
1127                 return 0;
1128
1129         float result = veh.get_offset();
1130         if(reverse!=back)
1131                 track = track.reverse();
1132         else
1133                 result = track->get_type().get_path_length(track->get_active_path())-result;
1134         result -= vtype.get_length()/2;
1135
1136         while(1)
1137         {
1138                 track = track.next();
1139                 if(!track)
1140                         break;
1141
1142                 if(!(*block)->has_track(*track))
1143                 {
1144                         if(back)
1145                         {
1146                                 if(block==blocks.begin())
1147                                         break;
1148                                 --block;
1149                         }
1150                         else
1151                         {
1152                                 ++block;
1153                                 if(block==clear_blocks_end)
1154                                         break;
1155                         }
1156
1157                         if(&**block==until_block)
1158                                 break;
1159                 }
1160
1161                 result += track->get_type().get_path_length(track->get_active_path());
1162         }
1163
1164         return result;
1165 }
1166
1167 void Train::release_blocks()
1168 {
1169         release_blocks(blocks.begin(), blocks.end());
1170 }
1171
1172 void Train::release_blocks(BlockList::iterator begin, BlockList::iterator end)
1173 {
1174         while(begin!=end)
1175         {
1176                 if(begin==cur_blocks_end)
1177                         cur_blocks_end = end;
1178                 if(begin==clear_blocks_end)
1179                         clear_blocks_end = end;
1180
1181                 Block &block = **begin;
1182                 blocks.erase(begin++);
1183                 block.reserve(0);
1184
1185                 if(begin==blocks.end())
1186                         end_of_route = false;
1187         }
1188 }
1189
1190 void Train::reverse_blocks(BlockList &blks) const
1191 {
1192         blks.reverse();
1193         for(BlockList::iterator i=blks.begin(); i!=blks.end(); ++i)
1194                 *i = i->reverse();
1195 }
1196
1197 bool Train::advance_route(list<RouteRef>::iterator &iter, Track &track)
1198 {
1199         while(iter!=routes.end() && !iter->route->has_track(track))
1200                 ++iter;
1201         if(iter==routes.end())
1202                 return false;
1203
1204         list<RouteRef>::iterator next = iter;
1205         ++next;
1206         if(next!=routes.end() && next->diversion && next->route->has_track(track))
1207                 iter = next;
1208
1209         return true;
1210 }
1211
1212 Route *Train::create_lead_route(Route *lead, const Route *target)
1213 {
1214         if(!lead)
1215         {
1216                 lead = new Route(layout);
1217                 lead->set_name("Lead");
1218                 lead->set_temporary(true);
1219         }
1220
1221         set<Track *> tracks;
1222         for(BlockList::iterator i=blocks.begin(); i!=blocks.end(); ++i)
1223         {
1224                 const set<Track *> &btracks = (*i)->get_tracks();
1225                 for(set<Track *>::const_iterator j=btracks.begin(); j!=btracks.end(); ++j)
1226                         if(!target || !target->has_track(**j))
1227                                 tracks.insert(*j);
1228         }
1229
1230         lead->add_tracks(tracks);
1231
1232         return lead;
1233 }
1234
1235 bool Train::is_valid_diversion(const Route &diversion, const TrackIter &from)
1236 {
1237         float diversion_len = 0;
1238         TrackLoopIter track1 = from;
1239         while(diversion.has_track(*track1))
1240         {
1241                 unsigned path = diversion.get_path(*track1);
1242                 diversion_len += track1->get_type().get_path_length(path);
1243
1244                 track1 = track1.next(path);
1245
1246                 if(!track1 || track1.looped())
1247                         return false;
1248         }
1249
1250         list<RouteRef>::iterator route = routes.begin();
1251         if(!advance_route(route, *from))
1252                 return false;
1253
1254         float route_len = 0;
1255         TrackLoopIter track2 = from;
1256         while(1)
1257         {
1258                 unsigned path = route->route->get_path(*track2);
1259                 route_len += track2->get_type().get_path_length(path);
1260
1261                 bool ok = (track2!=from && diversion.has_track(*track2));
1262
1263                 track2 = track2.next(path);
1264                 if(!track2)
1265                         return false;
1266
1267                 if(ok)
1268                         break;
1269
1270                 if(track2.looped())
1271                         return false;
1272
1273                 if(!advance_route(route, *track2))
1274                         return false;
1275         }
1276
1277         // Must end up at the same place through both routes
1278         if(track2!=track1)
1279                 return false;
1280
1281         return diversion_len<route_len*1.2;
1282 }
1283
1284
1285 Train::RouteRef::RouteRef(const Route *r, unsigned d):
1286         route(r),
1287         diversion(d)
1288 { }
1289
1290
1291 Train::Loader::Loader(Train &t):
1292         DataFile::ObjectLoader<Train>(t),
1293         prev_block(0),
1294         blocks_valid(true)
1295 {
1296         add("block",       &Loader::block);
1297         add("block_hint",  &Loader::block_hint);
1298         add("name",        &Loader::name);
1299         add("priority",    &Train::priority);
1300         add("quantized_speed",  &Loader::quantized_speed);
1301         add("route",       &Loader::route);
1302         add("timetable",   &Loader::timetable);
1303         add("vehicle",     &Loader::vehicle);
1304 }
1305
1306 void Train::Loader::finish()
1307 {
1308         if(!obj.blocks.empty())
1309         {
1310                 TrackIter track = obj.blocks.front().track_iter();
1311                 float offset = 2*obj.layout.get_catalogue().get_scale();
1312                 obj.vehicles.back()->place(*track, track.entry(), offset, Vehicle::BACK_BUFFER);
1313         }
1314 }
1315
1316 void Train::Loader::block(unsigned id)
1317 {
1318         if(!blocks_valid)
1319                 return;
1320
1321         Block *blk;
1322         try
1323         {
1324                 blk = &obj.layout.get_block(id);
1325         }
1326         catch(const key_error &)
1327         {
1328                 blocks_valid = false;
1329                 return;
1330         }
1331
1332         int entry = -1;
1333         if(prev_block)
1334                 entry = blk->get_endpoint_by_link(*prev_block);
1335         if(entry<0)
1336                 entry = 0;
1337
1338         blk->reserve(&obj);
1339         obj.blocks.push_back(BlockIter(blk, entry));
1340
1341         if(blk->get_sensor_id())
1342                 obj.layout.get_driver().set_sensor(blk->get_sensor_id(), true);
1343
1344         prev_block = blk;
1345 }
1346
1347 void Train::Loader::block_hint(unsigned id)
1348 {
1349         try
1350         {
1351                 prev_block = &obj.layout.get_block(id);
1352         }
1353         catch(const key_error &)
1354         {
1355                 blocks_valid = false;
1356         }
1357 }
1358
1359 void Train::Loader::name(const string &n)
1360 {
1361         obj.set_name(n);
1362 }
1363
1364 void Train::Loader::quantized_speed()
1365 {
1366         if(obj.speed_quantizer)
1367                 load_sub(*obj.speed_quantizer);
1368 }
1369
1370 void Train::Loader::route(const string &n)
1371 {
1372         obj.set_route(&obj.layout.get_route(n));
1373 }
1374
1375 void Train::Loader::timetable()
1376 {
1377         Timetable *ttbl = new Timetable(obj);
1378         load_sub(*ttbl);
1379 }
1380
1381 void Train::Loader::vehicle(ArticleNumber art_nr)
1382 {
1383         const VehicleType &vtype = obj.layout.get_catalogue().get_vehicle(art_nr);
1384         Vehicle *veh = new Vehicle(obj.layout, vtype);
1385         obj.vehicles.back()->attach_back(*veh);
1386         obj.vehicles.push_back(veh);
1387 }
1388
1389 } // namespace R2C2