source/libr2c2/train.cpp

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