source/libmarklin/train.cpp

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