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                 if(i==cur_blocks_end && !(*i)->get_sensor_id())
1109                         ++cur_blocks_end;
1110         }
1111 }
1112
1113 float Train::get_reserved_distance_until(const Block *until_block, bool back) const
1114 {
1115         if(blocks.empty())
1116                 return 0;
1117
1118         Vehicle &veh = *(reverse!=back ? vehicles.back() : vehicles.front());
1119         const VehicleType &vtype = veh.get_type();
1120
1121         TrackIter track(veh.get_track(), veh.get_entry());
1122         if(!track)  // XXX Probably unnecessary
1123                 return 0;
1124
1125         BlockList::const_iterator block = blocks.begin();
1126         while(block!=clear_blocks_end && !(*block)->has_track(*track))
1127                 ++block;
1128         if(block==clear_blocks_end || &**block==until_block)
1129                 return 0;
1130
1131         float result = veh.get_offset();
1132         if(reverse!=back)
1133                 track = track.reverse();
1134         else
1135                 result = track->get_type().get_path_length(track->get_active_path())-result;
1136         result -= vtype.get_length()/2;
1137
1138         while(1)
1139         {
1140                 track = track.next();
1141                 if(!track)
1142                         break;
1143
1144                 if(!(*block)->has_track(*track))
1145                 {
1146                         if(back)
1147                         {
1148                                 if(block==blocks.begin())
1149                                         break;
1150                                 --block;
1151                         }
1152                         else
1153                         {
1154                                 ++block;
1155                                 if(block==clear_blocks_end)
1156                                         break;
1157                         }
1158
1159                         if(&**block==until_block)
1160                                 break;
1161                 }
1162
1163                 result += track->get_type().get_path_length(track->get_active_path());
1164         }
1165
1166         return result;
1167 }
1168
1169 void Train::release_blocks()
1170 {
1171         release_blocks(blocks.begin(), blocks.end());
1172 }
1173
1174 void Train::release_blocks(BlockList::iterator begin, BlockList::iterator end)
1175 {
1176         while(begin!=end)
1177         {
1178                 if(begin==cur_blocks_end)
1179                         cur_blocks_end = end;
1180                 if(begin==clear_blocks_end)
1181                         clear_blocks_end = end;
1182
1183                 Block &block = **begin;
1184                 blocks.erase(begin++);
1185                 block.reserve(0);
1186
1187                 if(begin==blocks.end())
1188                         end_of_route = false;
1189         }
1190 }
1191
1192 void Train::reverse_blocks(BlockList &blks) const
1193 {
1194         blks.reverse();
1195         for(BlockList::iterator i=blks.begin(); i!=blks.end(); ++i)
1196                 *i = i->reverse();
1197 }
1198
1199 bool Train::advance_route(list<RouteRef>::iterator &iter, Track &track)
1200 {
1201         while(iter!=routes.end() && !iter->route->has_track(track))
1202                 ++iter;
1203         if(iter==routes.end())
1204                 return false;
1205
1206         list<RouteRef>::iterator next = iter;
1207         ++next;
1208         if(next!=routes.end() && next->diversion && next->route->has_track(track))
1209                 iter = next;
1210
1211         return true;
1212 }
1213
1214 Route *Train::create_lead_route(Route *lead, const Route *target)
1215 {
1216         if(!lead)
1217         {
1218                 lead = new Route(layout);
1219                 lead->set_name("Lead");
1220                 lead->set_temporary(true);
1221         }
1222
1223         set<Track *> tracks;
1224         for(BlockList::iterator i=blocks.begin(); i!=blocks.end(); ++i)
1225         {
1226                 const set<Track *> &btracks = (*i)->get_tracks();
1227                 for(set<Track *>::const_iterator j=btracks.begin(); j!=btracks.end(); ++j)
1228                         if(!target || !target->has_track(**j))
1229                                 tracks.insert(*j);
1230         }
1231
1232         lead->add_tracks(tracks);
1233
1234         return lead;
1235 }
1236
1237 bool Train::is_valid_diversion(const Route &diversion, const TrackIter &from)
1238 {
1239         float diversion_len = 0;
1240         TrackLoopIter track1 = from;
1241         while(diversion.has_track(*track1))
1242         {
1243                 unsigned path = diversion.get_path(*track1);
1244                 diversion_len += track1->get_type().get_path_length(path);
1245
1246                 track1 = track1.next(path);
1247
1248                 if(!track1 || track1.looped())
1249                         return false;
1250         }
1251
1252         list<RouteRef>::iterator route = routes.begin();
1253         if(!advance_route(route, *from))
1254                 return false;
1255
1256         float route_len = 0;
1257         TrackLoopIter track2 = from;
1258         while(1)
1259         {
1260                 unsigned path = route->route->get_path(*track2);
1261                 route_len += track2->get_type().get_path_length(path);
1262
1263                 bool ok = (track2!=from && diversion.has_track(*track2));
1264
1265                 track2 = track2.next(path);
1266                 if(!track2)
1267                         return false;
1268
1269                 if(ok)
1270                         break;
1271
1272                 if(track2.looped())
1273                         return false;
1274
1275                 if(!advance_route(route, *track2))
1276                         return false;
1277         }
1278
1279         // Must end up at the same place through both routes
1280         if(track2!=track1)
1281                 return false;
1282
1283         return diversion_len<route_len*1.2;
1284 }
1285
1286
1287 Train::RouteRef::RouteRef(const Route *r, unsigned d):
1288         route(r),
1289         diversion(d)
1290 { }
1291
1292
1293 Train::Loader::Loader(Train &t):
1294         DataFile::ObjectLoader<Train>(t),
1295         prev_block(0),
1296         blocks_valid(true)
1297 {
1298         add("block",       &Loader::block);
1299         add("block_hint",  &Loader::block_hint);
1300         add("name",        &Loader::name);
1301         add("priority",    &Train::priority);
1302         add("quantized_speed",  &Loader::quantized_speed);
1303         add("route",       &Loader::route);
1304         add("timetable",   &Loader::timetable);
1305         add("vehicle",     &Loader::vehicle);
1306 }
1307
1308 void Train::Loader::finish()
1309 {
1310         if(!obj.blocks.empty())
1311         {
1312                 TrackIter track = obj.blocks.front().track_iter();
1313                 float offset = 2*obj.layout.get_catalogue().get_scale();
1314                 obj.vehicles.back()->place(*track, track.entry(), offset, Vehicle::BACK_BUFFER);
1315         }
1316 }
1317
1318 void Train::Loader::block(unsigned id)
1319 {
1320         if(!blocks_valid)
1321                 return;
1322
1323         Block *blk;
1324         try
1325         {
1326                 blk = &obj.layout.get_block(id);
1327         }
1328         catch(const key_error &)
1329         {
1330                 blocks_valid = false;
1331                 return;
1332         }
1333
1334         int entry = -1;
1335         if(prev_block)
1336                 entry = blk->get_endpoint_by_link(*prev_block);
1337         if(entry<0)
1338                 entry = 0;
1339
1340         blk->reserve(&obj);
1341         obj.blocks.push_back(BlockIter(blk, entry));
1342
1343         if(blk->get_sensor_id())
1344                 obj.layout.get_driver().set_sensor(blk->get_sensor_id(), true);
1345
1346         prev_block = blk;
1347 }
1348
1349 void Train::Loader::block_hint(unsigned id)
1350 {
1351         try
1352         {
1353                 prev_block = &obj.layout.get_block(id);
1354         }
1355         catch(const key_error &)
1356         {
1357                 blocks_valid = false;
1358         }
1359 }
1360
1361 void Train::Loader::name(const string &n)
1362 {
1363         obj.set_name(n);
1364 }
1365
1366 void Train::Loader::quantized_speed()
1367 {
1368         if(obj.speed_quantizer)
1369                 load_sub(*obj.speed_quantizer);
1370 }
1371
1372 void Train::Loader::route(const string &n)
1373 {
1374         obj.set_route(&obj.layout.get_route(n));
1375 }
1376
1377 void Train::Loader::timetable()
1378 {
1379         Timetable *ttbl = new Timetable(obj);
1380         load_sub(*ttbl);
1381 }
1382
1383 void Train::Loader::vehicle(ArticleNumber art_nr)
1384 {
1385         const VehicleType &vtype = obj.layout.get_catalogue().get_vehicle(art_nr);
1386         Vehicle *veh = new Vehicle(obj.layout, vtype);
1387         obj.vehicles.back()->attach_back(*veh);
1388         obj.vehicles.push_back(veh);
1389 }
1390
1391 } // namespace R2C2