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 "tracktype.h"
  20 #include "train.h"
  21 #include "vehicle.h"
  22 #include "vehicletype.h"
  23
  24 using namespace std;
  25 using namespace Msp;
  26
  27 namespace {
  28
  29 struct SetFlag
  30 {
  31         bool &flag;
  32
  33         SetFlag(bool &f): flag(f) { flag = true; }
  34         ~SetFlag() { flag = false; }
  35 };
  36
  37 }
  38
  39
  40 namespace Marklin {
  41
  42 Train::Train(Layout &l, const VehicleType &t, unsigned a):
  43         layout(l),
  44         loco_type(t),
  45         address(a),
  46         priority(0),
  47         yielding_to(0),
  48         pending_block(0),
  49         reserving(false),
  50         advancing(false),
  51         controller(new AIControl(*this, new SimpleController)),
  52         timetable(0),
  53         active(false),
  54         current_speed(0),
  55         speed_changing(false),
  56         reverse(false),
  57         functions(0),
  58         route(0),
  59         next_route(0),
  60         end_of_route(false),
  61         status("Unplaced"),
  62         travel_dist(0),
  63         pure_speed(false),
  64         real_speed(15),
  65         accurate_position(false),
  66         overshoot_dist(false)
  67 {
  68         if(!loco_type.is_locomotive())
  69                 throw InvalidParameterValue("Initial vehicle must be a locomotive");
  70
  71         vehicles.push_back(new Vehicle(layout, loco_type));
  72
  73         layout.add_train(*this);
  74
  75         layout.get_driver().add_loco(address);
  76         layout.get_driver().signal_loco_speed.connect(sigc::mem_fun(this, &Train::loco_speed_event));
  77         layout.get_driver().signal_loco_function.connect(sigc::mem_fun(this, &Train::loco_func_event));
  78
  79         layout.signal_block_reserved.connect(sigc::mem_fun(this, &Train::block_reserved));
  80         layout.get_driver().signal_sensor.connect(sigc::mem_fun(this, &Train::sensor_event));
  81         layout.get_driver().signal_turnout.connect(sigc::mem_fun(this, &Train::turnout_event));
  82
  83         layout.get_driver().signal_halt.connect(sigc::mem_fun(this, &Train::halt_event));
  84
  85         controller->signal_control_changed.connect(sigc::mem_fun(this, &Train::control_changed));
  86 }
  87
  88 Train::~Train()
  89 {
  90         delete controller;
  91         delete timetable;
  92         for(vector<Vehicle *>::iterator i=vehicles.begin(); i!=vehicles.end(); ++i)
  93                 delete *i;
  94         layout.remove_train(*this);
  95 }
  96
  97 void Train::set_name(const string &n)
  98 {
  99         name = n;
 100
 101         signal_name_changed.emit(name);
 102 }
 103
 104 void Train::set_priority(int p)
 105 {
 106         priority = p;
 107 }
 108
 109 void Train::yield_to(const Train &t)
 110 {
 111         yielding_to = &t;
 112 }
 113
 114 void Train::add_vehicle(const VehicleType &vt)
 115 {
 116         Vehicle *veh = new Vehicle(layout, vt);
 117         vehicles.back()->attach_back(*veh);
 118         vehicles.push_back(veh);
 119 }
 120
 121 void Train::remove_vehicle(unsigned i)
 122 {
 123         if(i>=vehicles.size())
 124                 throw InvalidParameterValue("Vehicle index out of range");
 125         if(i==0)
 126                 throw InvalidParameterValue("Can't remove the locomotive");
 127         delete vehicles[i];
 128         vehicles.erase(vehicles.begin()+i);
 129         if(i<vehicles.size())
 130                 vehicles[i-1]->attach_back(*vehicles[i]);
 131 }
 132
 133 unsigned Train::get_n_vehicles() const
 134 {
 135         return vehicles.size();
 136 }
 137
 138 Vehicle &Train::get_vehicle(unsigned i)
 139 {
 140         if(i>=vehicles.size())
 141                 throw InvalidParameterValue("Vehicle index out of range");
 142         return *vehicles[i];
 143 }
 144
 145 const Vehicle &Train::get_vehicle(unsigned i) const
 146 {
 147         if(i>=vehicles.size())
 148                 throw InvalidParameterValue("Vehicle index out of range");
 149         return *vehicles[i];
 150 }
 151
 152 void Train::set_control(const string &n, float v)
 153 {
 154         controller->set_control(n, v);
 155 }
 156
 157 void Train::set_active(bool a)
 158 {
 159         if(a==active)
 160                 return;
 161         if(!a && controller->get_speed())
 162                 throw InvalidState("Can't deactivate while moving");
 163
 164         active = a;
 165         if(active)
 166         {
 167                 stop_timeout = Time::TimeStamp();
 168                 reserve_more();
 169         }
 170         else
 171         {
 172                 stop_timeout = Time::now()+2*Time::sec;
 173                 set_status("Stopped");
 174         }
 175 }
 176
 177 void Train::set_function(unsigned func, bool state)
 178 {
 179         if(!loco_type.get_functions().count(func))
 180                 throw InvalidParameterValue("Invalid function");
 181         if(func<5)
 182                 layout.get_driver().set_loco_function(address, func, state);
 183         else
 184                 layout.get_driver().set_loco_function(address+1, func-4, state);
 185 }
 186
 187 float Train::get_control(const string &ctrl) const
 188 {
 189         return controller->get_control(ctrl).value;
 190 }
 191
 192 float Train::get_speed() const
 193 {
 194         return controller->get_speed();
 195 }
 196
 197 bool Train::get_function(unsigned func) const
 198 {
 199         return (functions>>func)&1;
 200 }
 201
 202 void Train::set_timetable(Timetable *tt)
 203 {
 204         delete timetable;
 205         timetable = tt;
 206 }
 207
 208 void Train::set_route(const Route *r)
 209 {
 210         if(!rsv_blocks.empty())
 211         {
 212                 for(list<BlockRef>::iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
 213                         if(i->block->get_sensor_id())
 214                         {
 215                                 release_blocks(rsv_blocks, ++i, rsv_blocks.end());
 216                                 break;
 217                         }
 218         }
 219
 220         route = r;
 221         next_route = 0;
 222         end_of_route = false;
 223
 224         if(route && !cur_blocks.empty())
 225         {
 226                 BlockRef &last = (rsv_blocks.empty() ? cur_blocks.back() : rsv_blocks.back());
 227                 BlockRef next = last.next();
 228                 const Block::Endpoint &ep = next.block->get_endpoints()[next.entry];
 229                 if(!route->get_tracks().count(ep.track))
 230                 {
 231                         next_route = route;
 232                         route = Route::find(*ep.track, ep.track_ep, *next_route);
 233                 }
 234         }
 235
 236         reserve_more();
 237
 238         signal_route_changed.emit(route);
 239 }
 240
 241 void Train::go_to(const Track &to)
 242 {
 243         for(list<BlockRef>::const_iterator i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
 244                 if(i->block->get_tracks().count(const_cast<Track *>(&to)))
 245                 {
 246                         signal_arrived.emit();
 247                         set_route(0);
 248                         return;
 249                 }
 250
 251         BlockRef *last = 0;
 252         if(rsv_blocks.empty())
 253                 last = &cur_blocks.back();
 254         else
 255         {
 256                 for(list<BlockRef>::iterator i=rsv_blocks.begin(); (i!=rsv_blocks.end() && !last); ++i)
 257                         if(i->block->get_sensor_id())
 258                                 last = &*i;
 259         }
 260
 261         BlockRef next = last->next();
 262         const Block::Endpoint &ep = next.block->get_endpoints()[next.entry];
 263
 264         set_route(Route::find(*ep.track, ep.track_ep, to));
 265 }
 266
 267 void Train::place(Block &block, unsigned entry)
 268 {
 269         if(controller->get_speed())
 270                 throw InvalidState("Must be stopped before placing");
 271
 272         release_blocks(rsv_blocks);
 273         release_blocks(cur_blocks);
 274
 275         set_active(false);
 276         accurate_position = false;
 277
 278         if(!block.reserve(this))
 279         {
 280                 set_status("Unplaced");
 281                 return;
 282         }
 283
 284         cur_blocks.push_back(BlockRef(&block, entry));
 285         if(reverse)
 286         {
 287                 unsigned exit = block.traverse(entry);
 288                 const Block::Endpoint &bep = block.get_endpoints()[exit];
 289                 Track *track = bep.track->get_link(bep.track_ep);
 290                 unsigned ep = track->get_endpoint_by_link(*bep.track);
 291                 vehicles.front()->place(track, ep, 0, Vehicle::FRONT_BUFFER);
 292         }
 293         else
 294         {
 295                 const Block::Endpoint &bep = block.get_endpoints()[entry];
 296                 vehicles.back()->place(bep.track, bep.track_ep, 0, Vehicle::BACK_BUFFER);
 297         }
 298 }
 299
 300 bool Train::free_block(Block &block)
 301 {
 302         unsigned nsens = 0;
 303         for(list<BlockRef>::iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
 304         {
 305                 if(i->block==&block)
 306                 {
 307                         if(nsens<1)
 308                                 return false;
 309                         release_blocks(rsv_blocks, i, rsv_blocks.end());
 310                         return true;
 311                 }
 312                 else if(i->block->get_sensor_id())
 313                         ++nsens;
 314         }
 315
 316         return false;
 317 }
 318
 319 int Train::get_entry_to_block(Block &block) const
 320 {
 321         for(list<BlockRef>::const_iterator i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
 322                 if(i->block==&block)
 323                         return i->entry;
 324         for(list<BlockRef>::const_iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
 325                 if(i->block==&block)
 326                         return i->entry;
 327         return -1;
 328 }
 329
 330 float Train::get_reserved_distance() const
 331 {
 332         if(cur_blocks.empty())
 333                 return 0;
 334
 335         Vehicle &veh = *(reverse ? vehicles.back() : vehicles.front());
 336         const VehicleType &vtype = veh.get_type();
 337
 338         Track *track = veh.get_track();
 339         if(!track)
 340                 return 0;
 341         unsigned entry = veh.get_entry();
 342
 343         float result = -vtype.get_length()/2;
 344         if(reverse)
 345         {
 346                 entry = track->traverse(entry);
 347                 result += veh.get_offset();
 348         }
 349         else
 350                 result -= veh.get_offset();
 351
 352         bool first = true;
 353         list<BlockRef>::const_iterator block = cur_blocks.begin();
 354         while(1)
 355         {
 356                 if(!first || !reverse)
 357                         result += track->get_type().get_path_length(track->get_active_path());
 358                 first = false;
 359
 360                 if(track->get_type().get_endpoints().size()<2)
 361                         return result;
 362
 363                 unsigned exit = track->traverse(entry);
 364                 Track *next = track->get_link(exit);
 365
 366                 while(!block->block->get_tracks().count(next))
 367                 {
 368                         ++block;
 369                         if(block==cur_blocks.end())
 370                                 block = rsv_blocks.begin();
 371                         if(block==rsv_blocks.end())
 372                                 return result;
 373                 }
 374
 375                 entry = next->get_endpoint_by_link(*track);
 376                 track = next;
 377         }
 378 }
 379
 380 void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt)
 381 {
 382         if(!active && stop_timeout && t>=stop_timeout)
 383         {
 384                 release_blocks(rsv_blocks);
 385                 end_of_route = false;
 386                 stop_timeout = Time::TimeStamp();
 387         }
 388
 389         Driver &driver = layout.get_driver();
 390
 391         if(timetable)
 392                 timetable->tick(t);
 393         controller->tick(dt);
 394         float speed = controller->get_speed();
 395         unsigned speed_notch = find_speed(speed);
 396
 397         if(controller->get_reverse()!=reverse)
 398         {
 399                 reverse = controller->get_reverse();
 400                 driver.set_loco_reverse(address, reverse);
 401
 402                 release_blocks(rsv_blocks);
 403                 reverse_blocks(cur_blocks);
 404
 405                 reserve_more();
 406         }
 407         if(speed_notch!=current_speed && !speed_changing && !driver.is_halted() && driver.get_power())
 408         {
 409                 speed_changing = true;
 410                 driver.set_loco_speed(address, speed_notch);
 411
 412                 pure_speed = false;
 413
 414                 if(speed_notch)
 415                         set_status(format("Traveling %d kmh", get_travel_speed()));
 416                 else
 417                         set_status("Waiting");
 418         }
 419
 420         if(speed)
 421         {
 422                 if(!active)
 423                         set_active(true);
 424
 425                 Vehicle &vehicle = *(reverse ? vehicles.back() : vehicles.front());
 426                 Track *track = vehicle.get_track();
 427
 428                 bool ok = false;
 429                 for(list<BlockRef>::const_iterator i=cur_blocks.begin(); (!ok && i!=cur_blocks.end()); ++i)
 430                         ok = i->block->get_tracks().count(track);
 431
 432                 float d = get_real_speed(current_speed)*(dt/Time::sec);
 433                 if(ok)
 434                 {
 435                         SetFlag setf(advancing);
 436                         vehicle.advance(reverse ? -d : d);
 437                 }
 438                 else if(accurate_position)
 439                 {
 440                         overshoot_dist += d;
 441                         if(overshoot_dist>40*layout.get_catalogue().get_scale())
 442                         {
 443                                 layout.emergency(name+" has not arrived at sensor");
 444                                 accurate_position = false;
 445                         }
 446                 }
 447         }
 448         else if(end_of_route && rsv_blocks.empty())
 449         {
 450                 signal_arrived.emit();
 451                 set_route(0);
 452         }
 453
 454         if(!cur_blocks.empty() && !cur_blocks.front().block->get_sensor_id())
 455         {
 456                 Vehicle &veh = *(reverse ? vehicles.front() : vehicles.back());
 457
 458                 list<BlockRef>::iterator i = cur_blocks.begin();
 459                 const Block::Endpoint &bep = i->block->get_endpoints()[i->entry];
 460
 461                 Track *track = bep.track;
 462                 unsigned entry = bep.track_ep;
 463
 464                 bool found = false;
 465                 float dist = veh.get_offset();
 466                 if(reverse)
 467                         dist = veh.get_track()->get_type().get_path_length(veh.get_track()->get_active_path())-dist;
 468                 dist -= veh.get_type().get_length()/2;
 469                 while(1)
 470                 {
 471                         if(track==veh.get_track())
 472                         {
 473                                 found = true;
 474                                 break;
 475                         }
 476
 477                         if(i!=cur_blocks.begin())
 478                         {
 479                                 float path_len = track->get_type().get_path_length(track->get_active_path());
 480                                 dist += path_len;
 481                         }
 482
 483                         unsigned exit = track->traverse(entry);
 484                         Track *next = track->get_link(exit);
 485                         entry = next->get_endpoint_by_link(*track);
 486                         track = next;
 487
 488                         if(!i->block->get_tracks().count(track))
 489                         {
 490                                 ++i;
 491                                 if(i==cur_blocks.end())
 492                                         break;
 493                         }
 494                 }
 495
 496                 if(found && i!=cur_blocks.begin() && dist>10*layout.get_catalogue().get_scale())
 497                 {
 498                         cur_blocks.front().block->reserve(0);
 499                         cur_blocks.erase(cur_blocks.begin());
 500                 }
 501         }
 502 }
 503
 504 void Train::save(list<DataFile::Statement> &st) const
 505 {
 506         st.push_back((DataFile::Statement("name"), name));
 507
 508         st.push_back((DataFile::Statement("priority"), priority));
 509
 510         for(vector<Vehicle *>::const_iterator i=vehicles.begin(); i!=vehicles.end(); ++i)
 511                 if(i!=vehicles.begin())
 512                         st.push_back((DataFile::Statement("vehicle"), (*i)->get_type().get_article_number()));
 513
 514         for(unsigned i=0; i<=14; ++i)
 515                 if(real_speed[i].weight)
 516                         st.push_back((DataFile::Statement("real_speed"), i, real_speed[i].speed, real_speed[i].weight));
 517
 518         if(!cur_blocks.empty())
 519         {
 520                 list<BlockRef> blocks = cur_blocks;
 521                 if(reverse)
 522                         reverse_blocks(blocks);
 523
 524                 Block *prev = blocks.front().block->get_endpoints()[blocks.front().entry].link;
 525                 st.push_back((DataFile::Statement("block_hint"), prev->get_id()));
 526
 527                 for(list<BlockRef>::const_iterator i=blocks.begin(); i!=blocks.end(); ++i)
 528                         st.push_back((DataFile::Statement("block"), i->block->get_id()));
 529         }
 530
 531         if(route)
 532         {
 533                 if(!route->is_temporary())
 534                         st.push_back((DataFile::Statement("route"), route->get_name()));
 535                 else if(next_route && !next_route->is_temporary())
 536                         st.push_back((DataFile::Statement("route"), next_route->get_name()));
 537         }
 538
 539         if(timetable)
 540         {
 541                 DataFile::Statement ss("timetable");
 542                 timetable->save(ss.sub);
 543                 st.push_back(ss);
 544         }
 545 }
 546
 547 void Train::control_changed(const Controller::Control &ctrl)
 548 {
 549         signal_control_changed.emit(ctrl.name, ctrl.value);
 550 }
 551
 552 void Train::loco_speed_event(unsigned addr, unsigned speed, bool)
 553 {
 554         if(addr==address)
 555         {
 556                 current_speed = speed;
 557                 speed_changing = false;
 558                 pure_speed = false;
 559         }
 560 }
 561
 562 void Train::loco_func_event(unsigned addr, unsigned func, bool state)
 563 {
 564         if(addr==address || (addr==address+1 && loco_type.get_max_function()>4))
 565         {
 566                 if(addr==address+1)
 567                         func += 4;
 568                 if(state)
 569                         functions |= 1<<func;
 570                 else
 571                         functions &= ~(1<<func);
 572
 573                 signal_function_changed.emit(func, state);
 574         }
 575 }
 576
 577 void Train::sensor_event(unsigned addr, bool state)
 578 {
 579         if(state)
 580         {
 581                 // Find the first sensor block from our reserved blocks that isn't this sensor
 582                 list<BlockRef>::iterator i;
 583                 unsigned result = 0;
 584                 for(i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
 585                         if(i->block->get_sensor_id())
 586                         {
 587                                 if(i->block->get_sensor_id()!=addr)
 588                                 {
 589                                         if(result==0)
 590                                                 result = 2;
 591                                         else if(result==1)
 592                                                 break;
 593                                 }
 594                                 else if(result==0)
 595                                         result = 1;
 596                                 else if(result==2)
 597                                         result = 3;
 598                         }
 599
 600                 if(result==1 && i!=rsv_blocks.begin())
 601                 {
 602                         // Compute speed and update related state
 603                         float travel_time_secs = (Time::now()-last_entry_time)/Time::sec;
 604
 605                         if(pure_speed)
 606                         {
 607                                 if(current_speed)
 608                                 {
 609                                         RealSpeed &rs = real_speed[current_speed];
 610                                         rs.add(travel_dist/travel_time_secs, travel_time_secs);
 611                                 }
 612                                 set_status(format("Traveling %d kmh", get_travel_speed()));
 613                         }
 614
 615                         travel_dist = 0;
 616                         float block_len;
 617                         for(list<BlockRef>::iterator j=rsv_blocks.begin(); j!=i; ++j)
 618                         {
 619                                 j->block->traverse(j->entry, &block_len);
 620                                 travel_dist += block_len;
 621
 622                                 if(j->block->get_sensor_id()==addr && !advancing)
 623                                 {
 624                                         const Block::Endpoint &bep = j->block->get_endpoints()[j->entry];
 625                                         if(reverse)
 626                                         {
 627                                                 Track *track = bep.track->get_link(bep.track_ep);
 628                                                 unsigned ep = track->get_endpoint_by_link(*bep.track);
 629                                                 vehicles.back()->place(track, ep, 0, Vehicle::BACK_AXLE);
 630                                         }
 631                                         else
 632                                                 vehicles.front()->place(bep.track, bep.track_ep, 0, Vehicle::FRONT_AXLE);
 633                                 }
 634                         }
 635                         last_entry_time = Time::now();
 636                         pure_speed = true;
 637                         accurate_position = true;
 638                         overshoot_dist = 0;
 639
 640                         // Check if we've reached the next route
 641                         if(next_route)
 642                         {
 643                                 const set<const Track *> &rtracks = next_route->get_tracks();
 644                                 for(list<BlockRef>::iterator j=rsv_blocks.begin(); j!=i; ++j)
 645                                         if(rtracks.count(j->block->get_endpoints()[j->entry].track))
 646                                         {
 647                                                 route = next_route;
 648                                                 next_route = 0;
 649                                                 // XXX Exceptions?
 650                                                 signal_route_changed.emit(route);
 651                                                 break;
 652                                         }
 653                         }
 654
 655                         // Move blocks up to the next sensor to our current blocks
 656                         cur_blocks.splice(cur_blocks.end(), rsv_blocks, rsv_blocks.begin(), i);
 657
 658                         // Try to get more blocks if we're moving
 659                         if(active)
 660                                 reserve_more();
 661                 }
 662                 else if(result==3)
 663                         layout.emergency("Sensor for "+name+" triggered out of order");
 664         }
 665         else
 666         {
 667                 // Find the first sensor in our current blocks that's still active
 668                 list<BlockRef>::iterator end = cur_blocks.begin();
 669                 for(list<BlockRef>::iterator i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
 670                         if(i->block->get_sensor_id())
 671                         {
 672                                 if(layout.get_driver().get_sensor(i->block->get_sensor_id()))
 673                                         break;
 674                                 else
 675                                 {
 676                                         end = i;
 677                                         ++end;
 678                                 }
 679                         }
 680
 681                 if(end!=cur_blocks.begin() && end!=cur_blocks.end())
 682                         // Free blocks up to the last inactive sensor
 683                         release_blocks(cur_blocks, cur_blocks.begin(), end);
 684         }
 685 }
 686
 687 void Train::turnout_event(unsigned addr, bool)
 688 {
 689         if(pending_block)
 690         {
 691                 unsigned pending_addr = pending_block->get_turnout_id();
 692                 bool double_addr = (*pending_block->get_tracks().begin())->get_type().is_double_address();
 693                 if(addr==pending_addr || (double_addr && addr==pending_addr+1))
 694                 {
 695                         if(reserving)
 696                                 pending_block = 0;
 697                         else
 698                                 reserve_more();
 699                 }
 700         }
 701 }
 702
 703 void Train::halt_event(bool h)
 704 {
 705         if(h)
 706                 accurate_position = false;
 707 }
 708
 709 void Train::block_reserved(const Block &block, const Train *train)
 710 {
 711         if(&block==pending_block && !train)
 712                 reserve_more();
 713 }
 714
 715 unsigned Train::reserve_more()
 716 {
 717         if(!active)
 718                 return 0;
 719
 720         BlockRef *start = 0;
 721         if(!rsv_blocks.empty())
 722                 start = &rsv_blocks.back();
 723         else if(!cur_blocks.empty())
 724                 start = &cur_blocks.back();
 725         if(!start)
 726                 return 0;
 727
 728         pending_block = 0;
 729
 730         // See how many sensor blocks we already have
 731         unsigned nsens = 0;
 732         for(list<BlockRef>::const_iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
 733                 if(i->block->get_sensor_id())
 734                         ++nsens;
 735
 736         if(end_of_route)
 737                 return nsens;
 738
 739         const Route *cur_route = 0;
 740         if(route)
 741         {
 742                 const set<Track *> &tracks = start->block->get_tracks();
 743                 for(set<Track *>::const_iterator i=tracks.begin(); (cur_route!=route && i!=tracks.end()); ++i)
 744                 {
 745                         if(route->get_tracks().count(*i))
 746                                 cur_route = route;
 747                         else if(next_route && next_route->get_tracks().count(*i))
 748                                 cur_route = next_route;
 749                 }
 750         }
 751
 752         SetFlag setf(reserving);
 753
 754         BlockRef *last = start;
 755         BlockRef *good = start;
 756         unsigned good_sens = nsens;
 757         Train *blocking_train = 0;
 758         std::list<BlockRef> contested_blocks;
 759         while(good_sens<3 || !contested_blocks.empty())
 760         {
 761                 // Traverse to the next block
 762                 unsigned exit = last->block->traverse(last->entry, cur_route);
 763                 Block *link = last->block->get_link(exit);
 764                 if(!link)
 765                         break;
 766
 767                 int entry = link->get_endpoint_by_link(*last->block);
 768                 if(entry<0)
 769                         throw LogicError("Block links are inconsistent!");
 770
 771                 const Block::Endpoint &entry_ep = link->get_endpoints()[entry];
 772
 773                 if(cur_route)
 774                 {
 775                         if(cur_route!=next_route && next_route && next_route->get_tracks().count(entry_ep.track))
 776                                 cur_route = next_route;
 777                         else if(!cur_route->get_tracks().count(entry_ep.track))
 778                         {
 779                                 // Keep the blocks if we arrived at the end of the route
 780                                 if(!blocking_train)
 781                                 {
 782                                         good = last;
 783                                         good_sens = nsens;
 784                                         end_of_route = true;
 785                                 }
 786                                 break;
 787                         }
 788                 }
 789                 else if(route && route->get_tracks().count(entry_ep.track))
 790                         cur_route = route;
 791
 792                 if(link->get_endpoints().size()<2)
 793                 {
 794                         if(!blocking_train)
 795                         {
 796                                 good = last;
 797                                 good_sens = nsens;
 798                         }
 799                         break;
 800                 }
 801
 802                 if(blocking_train)
 803                 {
 804                         if(link->get_train()!=blocking_train)
 805                         {
 806                                 // XXX is it possible that this won't free all the blocks we want?
 807                                 if(blocking_train->free_block(*contested_blocks.back().block))
 808                                 {
 809                                         // Roll back and start actually reserving the blocks
 810                                         last = &rsv_blocks.back();
 811                                         if(blocking_train->get_priority()==priority)
 812                                                 blocking_train->yield_to(*this);
 813                                         blocking_train = 0;
 814                                         continue;
 815                                 }
 816                                 else
 817                                 {
 818                                         pending_block = contested_blocks.front().block;
 819                                         break;
 820                                 }
 821                         }
 822                         else
 823                         {
 824                                 contested_blocks.push_back(BlockRef(link, entry));
 825                                 last = &contested_blocks.back();
 826                                 continue;
 827                         }
 828                 }
 829
 830                 bool reserved = link->reserve(this);
 831                 if(!reserved)
 832                 {
 833                         /* We've found another train.  If it wants to exit the block from the
 834                         same endpoint we're trying to enter from or the other way around,
 835                         treat it as coming towards us.  Otherwise treat it as going in the
 836                         same direction. */
 837                         Train *other_train = link->get_train();
 838                         int other_entry = other_train->get_entry_to_block(*link);
 839                         if(other_entry<0)
 840                                 throw LogicError("Block reservation inconsistency");
 841
 842                         int other_prio = other_train->get_priority();
 843
 844                         bool entry_conflict = (static_cast<unsigned>(entry)==link->traverse(other_entry));
 845                         bool exit_conflict = (link->traverse(entry)==static_cast<unsigned>(other_entry));
 846                         if(!entry_conflict && !exit_conflict)
 847                         {
 848                                 /* Same direction, keep the blocks we got so far and wait for the
 849                                 other train to pass */
 850                                 good = last;
 851                                 good_sens = nsens;
 852
 853                                 // Ask a lesser priority train to free the block for us
 854                                 if(other_train->get_priority()<priority)
 855                                         if(other_train->free_block(*link))
 856                                                 reserved = link->reserve(this);
 857                         }
 858                         else if(other_prio<priority || (other_prio==priority && other_train!=yielding_to && entry_conflict))
 859                         {
 860                                 /* A lesser priority train is coming at us, we must ask it to free
 861                                 enough blocks to get clear of it to avoid a potential deadlock */
 862                                 blocking_train = other_train;
 863                                 contested_blocks.clear();
 864                                 contested_blocks.push_back(BlockRef(link, entry));
 865                                 last = &contested_blocks.back();
 866                                 continue;
 867                         }
 868
 869                         if(!reserved)
 870                         {
 871                                 pending_block = link;
 872                                 break;
 873                         }
 874                 }
 875
 876                 if(link->get_turnout_id())
 877                 {
 878                         const Endpoint &track_ep = entry_ep.track->get_type().get_endpoints()[entry_ep.track_ep];
 879
 880                         // Keep the blocks reserved so far, as either us or the other train can diverge
 881                         good = last;
 882                         good_sens = nsens;
 883
 884                         // Figure out what path we'd like to take on the turnout
 885                         int path = -1;
 886                         if(cur_route)
 887                                 path = cur_route->get_turnout(link->get_turnout_id());
 888                         if(path<0)
 889                                 path = entry_ep.track->get_active_path();
 890                         if(!((track_ep.paths>>path)&1))
 891                         {
 892                                 for(unsigned i=0; track_ep.paths>>i; ++i)
 893                                         if((track_ep.paths>>i)&1)
 894                                                 path = i;
 895                         }
 896
 897                         if(path!=static_cast<int>(entry_ep.track->get_active_path()))
 898                         {
 899                                 // The turnout is set to wrong path - switch and wait for it
 900                                 pending_block = link;
 901                                 entry_ep.track->set_active_path(path);
 902                                 if(pending_block)
 903                                 {
 904                                         link->reserve(0);
 905                                         break;
 906                                 }
 907                         }
 908                 }
 909
 910                 if(!contested_blocks.empty() && contested_blocks.front().block==link)
 911                         contested_blocks.pop_front();
 912
 913                 rsv_blocks.push_back(BlockRef(link, entry));
 914                 last = &rsv_blocks.back();
 915                 if(last->block->get_sensor_id())
 916                         ++nsens;
 917         }
 918
 919         // Unreserve blocks that were not good
 920         while(!rsv_blocks.empty() && &rsv_blocks.back()!=good)
 921         {
 922                 rsv_blocks.back().block->reserve(0);
 923                 rsv_blocks.erase(--rsv_blocks.end());
 924         }
 925
 926         if(!rsv_blocks.empty() && &rsv_blocks.back()!=start)
 927                 // We got some new blocks, so no longer need to yield
 928                 yielding_to = 0;
 929
 930         // Make any sensorless blocks at the beginning immediately current
 931         list<BlockRef>::iterator i;
 932         for(i=rsv_blocks.begin(); (i!=rsv_blocks.end() && !i->block->get_sensor_id()); ++i) ;
 933         if(i!=rsv_blocks.begin())
 934                 cur_blocks.splice(cur_blocks.end(), rsv_blocks, rsv_blocks.begin(), i);
 935
 936         return good_sens;
 937 }
 938
 939 float Train::get_real_speed(unsigned i) const
 940 {
 941         if(real_speed[i].weight)
 942                 return real_speed[i].speed;
 943
 944         unsigned low;
 945         unsigned high;
 946         for(low=i; low>0; --low)
 947                 if(real_speed[low].weight)
 948                         break;
 949         for(high=i; high<14; ++high)
 950                 if(real_speed[high].weight)
 951                         break;
 952
 953         if(real_speed[high].weight)
 954         {
 955                 if(real_speed[low].weight)
 956                 {
 957                         float f = float(i-low)/(high-low);
 958                         return real_speed[low].speed*(1-f)+real_speed[high].speed*f;
 959                 }
 960                 else
 961                         return real_speed[high].speed*float(i)/high;
 962         }
 963         else if(real_speed[low].weight)
 964                 return real_speed[low].speed*float(i)/low;
 965         else
 966                 return 0;
 967 }
 968
 969 unsigned Train::find_speed(float real) const
 970 {
 971         if(real<=real_speed[0].speed)
 972                 return 0;
 973
 974         unsigned low = 0;
 975         unsigned high = 0;
 976         for(unsigned i=0; (!high && i<=14); ++i)
 977                 if(real_speed[i].weight)
 978                 {
 979                         if(real_speed[i].speed<real)
 980                                 low = i;
 981                         else
 982                                 high = i;
 983                 }
 984         if(!high)
 985         {
 986                 if(!low)
 987                 {
 988                         if(real)
 989                                 return 3;
 990                         else
 991                                 return 0;
 992                 }
 993                 return min(static_cast<unsigned>(low*real/real_speed[low].speed), 14U);
 994         }
 995
 996         float f = (real-real_speed[low].speed)/(real_speed[high].speed-real_speed[low].speed);
 997         return static_cast<unsigned>(low*(1-f)+high*f+0.5);
 998 }
 999
1000 float Train::get_travel_speed() const
1001 {
1002         float speed = get_real_speed(current_speed);
1003         float scale = layout.get_catalogue().get_scale();
1004         return static_cast<int>(round(speed/scale*3.6/5))*5;
1005 }
1006
1007 void Train::set_status(const string &s)
1008 {
1009         status = s;
1010         signal_status_changed.emit(s);
1011 }
1012
1013 void Train::release_blocks(list<BlockRef> &blocks)
1014 {
1015         release_blocks(blocks, blocks.begin(), blocks.end());
1016 }
1017
1018 void Train::release_blocks(list<BlockRef> &blocks, list<BlockRef>::iterator begin, list<BlockRef>::iterator end)
1019 {
1020         while(begin!=end)
1021         {
1022                 Block *block = begin->block;
1023                 blocks.erase(begin++);
1024                 block->reserve(0);
1025         }
1026 }
1027
1028 void Train::reverse_blocks(list<BlockRef> &blocks) const
1029 {
1030         blocks.reverse();
1031         for(list<BlockRef>::iterator i=blocks.begin(); i!=blocks.end(); ++i)
1032                 i->entry = i->block->traverse(i->entry);
1033 }
1034
1035
1036 Train::BlockRef::BlockRef(Block *b, unsigned e):
1037         block(b),
1038         entry(e)
1039 { }
1040
1041 Train::BlockRef Train::BlockRef::next() const
1042 {
1043         Block *blk = block->get_endpoints()[block->traverse(entry)].link;
1044         if(!blk)
1045                 throw InvalidState("At end of line");
1046
1047         int ep = blk->get_endpoint_by_link(*block);
1048         if(ep<0)
1049                 throw LogicError("Block links are inconsistent");
1050
1051         return BlockRef(blk, ep);
1052 }
1053
1054
1055 Train::RealSpeed::RealSpeed():
1056         speed(0),
1057         weight(0)
1058 { }
1059
1060 void Train::RealSpeed::add(float s, float w)
1061 {
1062         speed = (speed*weight+s*w)/(weight+w);
1063         weight = min(weight+w, 300.0f);
1064 }
1065
1066
1067 Train::Loader::Loader(Train &t):
1068         DataFile::BasicLoader<Train>(t),
1069         prev_block(0),
1070         blocks_valid(true)
1071 {
1072         add("block",       &Loader::block);
1073         add("block_hint",  &Loader::block_hint);
1074         add("name",        &Loader::name);
1075         add("priority",    &Train::priority);
1076         add("real_speed",  &Loader::real_speed);
1077         add("route",       &Loader::route);
1078         add("timetable",   &Loader::timetable);
1079         add("vehicle",     &Loader::vehicle);
1080 }
1081
1082 void Train::Loader::finish()
1083 {
1084         if(!obj.cur_blocks.empty())
1085         {
1086                 const BlockRef &blkref = obj.cur_blocks.front();
1087                 const Block::Endpoint &bep = blkref.block->get_endpoints()[blkref.entry];
1088                 obj.vehicles.back()->place(bep.track, bep.track_ep, 0, Vehicle::BACK_BUFFER);
1089
1090                 obj.set_status("Stopped");
1091         }
1092 }
1093
1094 void Train::Loader::block(unsigned id)
1095 {
1096         if(!blocks_valid)
1097                 return;
1098
1099         Block *blk;
1100         try
1101         {
1102                 blk = &obj.layout.get_block(id);
1103         }
1104         catch(const KeyError &)
1105         {
1106                 blocks_valid = false;
1107                 return;
1108         }
1109
1110         int entry = -1;
1111         if(prev_block)
1112                 entry = blk->get_endpoint_by_link(*prev_block);
1113         if(entry<0)
1114                 entry = 0;
1115
1116         blk->reserve(&obj);
1117         obj.cur_blocks.push_back(BlockRef(blk, entry));
1118
1119         if(blk->get_sensor_id())
1120                 obj.layout.get_driver().set_sensor(blk->get_sensor_id(), true);
1121
1122         prev_block = blk;
1123 }
1124
1125 void Train::Loader::block_hint(unsigned id)
1126 {
1127         try
1128         {
1129                 prev_block = &obj.layout.get_block(id);
1130         }
1131         catch(const KeyError &)
1132         {
1133                 blocks_valid = false;
1134         }
1135 }
1136
1137 void Train::Loader::name(const string &n)
1138 {
1139         obj.set_name(n);
1140 }
1141
1142 void Train::Loader::real_speed(unsigned i, float speed, float weight)
1143 {
1144         obj.real_speed[i].speed = speed;
1145         obj.real_speed[i].weight = weight;
1146 }
1147
1148 void Train::Loader::route(const string &n)
1149 {
1150         obj.set_route(&obj.layout.get_route(n));
1151 }
1152
1153 void Train::Loader::timetable()
1154 {
1155         if(obj.timetable)
1156                 throw InvalidState("A timetable has already been loaded");
1157
1158         obj.timetable = new Timetable(obj);
1159         load_sub(*obj.timetable);
1160 }
1161
1162 void Train::Loader::vehicle(unsigned n)
1163 {
1164         const VehicleType &vtype = obj.layout.get_catalogue().get_vehicle(n);
1165         Vehicle *veh = new Vehicle(obj.layout, vtype);
1166         obj.vehicles.back()->attach_back(*veh);
1167         obj.vehicles.push_back(veh);
1168 }
1169
1170 } // namespace Marklin