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