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 void Train::unplace()
 301 {
 302         if(controller->get_speed())
 303                 throw InvalidState("Must be stopped before unplacing");
 304
 305         release_blocks(rsv_blocks);
 306         release_blocks(cur_blocks);
 307
 308         set_active(false);
 309         accurate_position = false;
 310
 311         for(vector<Vehicle *>::iterator i=vehicles.begin(); i!=vehicles.end(); ++i)
 312                 (*i)->unplace();
 313
 314         set_status("Unplaced");
 315 }
 316
 317 bool Train::free_block(Block &block)
 318 {
 319         float margin = 10*layout.get_catalogue().get_scale();
 320         if(get_reserved_distance_until(&block, false)<controller->get_braking_distance()*1.3+margin)
 321                 return false;
 322
 323         unsigned nsens = 0;
 324         for(list<BlockRef>::iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
 325         {
 326                 if(i->block==&block)
 327                 {
 328                         if(nsens<1)
 329                                 return false;
 330                         release_blocks(rsv_blocks, i, rsv_blocks.end());
 331                         return true;
 332                 }
 333                 else if(i->block->get_sensor_id())
 334                         ++nsens;
 335         }
 336
 337         return false;
 338 }
 339
 340 int Train::get_entry_to_block(Block &block) const
 341 {
 342         for(list<BlockRef>::const_iterator i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
 343                 if(i->block==&block)
 344                         return i->entry;
 345         for(list<BlockRef>::const_iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
 346                 if(i->block==&block)
 347                         return i->entry;
 348         return -1;
 349 }
 350
 351 float Train::get_reserved_distance() const
 352 {
 353         return get_reserved_distance_until(0, false);
 354 }
 355
 356 void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt)
 357 {
 358         if(!active && stop_timeout && t>=stop_timeout)
 359         {
 360                 release_blocks(rsv_blocks);
 361                 end_of_route = false;
 362                 stop_timeout = Time::TimeStamp();
 363         }
 364
 365         Driver &driver = layout.get_driver();
 366
 367         if(timetable)
 368                 timetable->tick(t);
 369         controller->tick(dt);
 370         float speed = controller->get_speed();
 371         unsigned speed_notch = find_speed(speed);
 372
 373         if(controller->get_reverse()!=reverse)
 374         {
 375                 reverse = controller->get_reverse();
 376                 driver.set_loco_reverse(address, reverse);
 377
 378                 release_blocks(rsv_blocks);
 379                 reverse_blocks(cur_blocks);
 380
 381                 reserve_more();
 382         }
 383         if(speed_notch!=current_speed && !speed_changing && !driver.is_halted() && driver.get_power())
 384         {
 385                 speed_changing = true;
 386                 driver.set_loco_speed(address, speed_notch);
 387
 388                 pure_speed = false;
 389
 390                 if(speed_notch)
 391                         set_status(format("Traveling %d kmh", get_travel_speed()));
 392                 else
 393                         set_status("Waiting");
 394         }
 395
 396         if(speed)
 397         {
 398                 if(!active)
 399                         set_active(true);
 400
 401                 Vehicle &vehicle = *(reverse ? vehicles.back() : vehicles.front());
 402                 Track *track = vehicle.get_track();
 403
 404                 bool ok = false;
 405                 for(list<BlockRef>::const_iterator i=cur_blocks.begin(); (!ok && i!=cur_blocks.end()); ++i)
 406                         ok = i->block->get_tracks().count(track);
 407
 408                 float d = get_real_speed(current_speed)*(dt/Time::sec);
 409                 if(ok)
 410                 {
 411                         SetFlag setf(advancing);
 412                         vehicle.advance(reverse ? -d : d);
 413                 }
 414                 else if(accurate_position)
 415                 {
 416                         overshoot_dist += d;
 417                         if(overshoot_dist>40*layout.get_catalogue().get_scale())
 418                         {
 419                                 layout.emergency(name+" has not arrived at sensor");
 420                                 accurate_position = false;
 421                         }
 422                 }
 423         }
 424         else if(end_of_route && rsv_blocks.empty())
 425         {
 426                 set_active(false);
 427                 signal_arrived.emit();
 428                 set_route(0);
 429         }
 430
 431         if(!cur_blocks.empty() && !cur_blocks.front().block->get_sensor_id())
 432         {
 433                 float dist = get_reserved_distance_until(cur_blocks.front().block, true);
 434
 435                 if(dist>10*layout.get_catalogue().get_scale())
 436                 {
 437                         cur_blocks.front().block->reserve(0);
 438                         cur_blocks.erase(cur_blocks.begin());
 439                 }
 440         }
 441 }
 442
 443 void Train::save(list<DataFile::Statement> &st) const
 444 {
 445         st.push_back((DataFile::Statement("name"), name));
 446
 447         st.push_back((DataFile::Statement("priority"), priority));
 448
 449         for(vector<Vehicle *>::const_iterator i=vehicles.begin(); i!=vehicles.end(); ++i)
 450                 if(i!=vehicles.begin())
 451                         st.push_back((DataFile::Statement("vehicle"), (*i)->get_type().get_article_number()));
 452
 453         for(unsigned i=0; i<=14; ++i)
 454                 if(real_speed[i].weight)
 455                         st.push_back((DataFile::Statement("real_speed"), i, real_speed[i].speed, real_speed[i].weight));
 456
 457         if(!cur_blocks.empty())
 458         {
 459                 list<BlockRef> blocks = cur_blocks;
 460                 if(reverse)
 461                         reverse_blocks(blocks);
 462
 463                 Block *prev = blocks.front().block->get_endpoints()[blocks.front().entry].link;
 464                 st.push_back((DataFile::Statement("block_hint"), prev->get_id()));
 465
 466                 for(list<BlockRef>::const_iterator i=blocks.begin(); i!=blocks.end(); ++i)
 467                         st.push_back((DataFile::Statement("block"), i->block->get_id()));
 468         }
 469
 470         if(route)
 471         {
 472                 if(!route->is_temporary())
 473                         st.push_back((DataFile::Statement("route"), route->get_name()));
 474                 else if(next_route && !next_route->is_temporary())
 475                         st.push_back((DataFile::Statement("route"), next_route->get_name()));
 476         }
 477
 478         if(timetable)
 479         {
 480                 DataFile::Statement ss("timetable");
 481                 timetable->save(ss.sub);
 482                 st.push_back(ss);
 483         }
 484 }
 485
 486 void Train::control_changed(const Controller::Control &ctrl)
 487 {
 488         signal_control_changed.emit(ctrl.name, ctrl.value);
 489 }
 490
 491 void Train::loco_speed_event(unsigned addr, unsigned speed, bool)
 492 {
 493         if(addr==address)
 494         {
 495                 current_speed = speed;
 496                 speed_changing = false;
 497                 pure_speed = false;
 498         }
 499 }
 500
 501 void Train::loco_func_event(unsigned addr, unsigned func, bool state)
 502 {
 503         if(addr==address || (addr==address+1 && loco_type.get_max_function()>4))
 504         {
 505                 if(addr==address+1)
 506                         func += 4;
 507                 if(state)
 508                         functions |= 1<<func;
 509                 else
 510                         functions &= ~(1<<func);
 511
 512                 signal_function_changed.emit(func, state);
 513         }
 514 }
 515
 516 void Train::sensor_event(unsigned addr, bool state)
 517 {
 518         if(state)
 519         {
 520                 // Find the first sensor block from our reserved blocks that isn't this sensor
 521                 list<BlockRef>::iterator i;
 522                 unsigned result = 0;
 523                 for(i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
 524                         if(i->block->get_sensor_id())
 525                         {
 526                                 if(i->block->get_sensor_id()!=addr)
 527                                 {
 528                                         if(result==0)
 529                                                 result = 2;
 530                                         else if(result==1)
 531                                                 break;
 532                                 }
 533                                 else if(result==0)
 534                                         result = 1;
 535                                 else if(result==2)
 536                                         result = 3;
 537                         }
 538
 539                 if(result==1 && i!=rsv_blocks.begin())
 540                 {
 541                         // Compute speed and update related state
 542                         float travel_time_secs = (Time::now()-last_entry_time)/Time::sec;
 543
 544                         if(pure_speed)
 545                         {
 546                                 if(current_speed)
 547                                 {
 548                                         RealSpeed &rs = real_speed[current_speed];
 549                                         rs.add(travel_dist/travel_time_secs, travel_time_secs);
 550                                 }
 551                                 set_status(format("Traveling %d kmh", get_travel_speed()));
 552                         }
 553
 554                         travel_dist = 0;
 555                         float block_len;
 556                         for(list<BlockRef>::iterator j=rsv_blocks.begin(); j!=i; ++j)
 557                         {
 558                                 j->block->traverse(j->entry, &block_len);
 559                                 travel_dist += block_len;
 560
 561                                 if(j->block->get_sensor_id()==addr && !advancing)
 562                                 {
 563                                         const Block::Endpoint &bep = j->block->get_endpoints()[j->entry];
 564                                         if(reverse)
 565                                         {
 566                                                 Track *track = bep.track->get_link(bep.track_ep);
 567                                                 unsigned ep = track->get_endpoint_by_link(*bep.track);
 568                                                 vehicles.back()->place(*track, ep, 0, Vehicle::BACK_AXLE);
 569                                         }
 570                                         else
 571                                                 vehicles.front()->place(*bep.track, bep.track_ep, 0, Vehicle::FRONT_AXLE);
 572                                 }
 573                         }
 574                         last_entry_time = Time::now();
 575                         pure_speed = true;
 576                         accurate_position = true;
 577                         overshoot_dist = 0;
 578
 579                         // Check if we've reached the next route
 580                         if(next_route)
 581                         {
 582                                 const set<const Track *> &rtracks = next_route->get_tracks();
 583                                 for(list<BlockRef>::iterator j=rsv_blocks.begin(); j!=i; ++j)
 584                                         if(rtracks.count(j->block->get_endpoints()[j->entry].track))
 585                                         {
 586                                                 route = next_route;
 587                                                 next_route = 0;
 588                                                 // XXX Exceptions?
 589                                                 signal_route_changed.emit(route);
 590                                                 break;
 591                                         }
 592                         }
 593
 594                         // Move blocks up to the next sensor to our current blocks
 595                         cur_blocks.splice(cur_blocks.end(), rsv_blocks, rsv_blocks.begin(), i);
 596
 597                         // Try to get more blocks if we're moving
 598                         if(active)
 599                                 reserve_more();
 600                 }
 601                 else if(result==3)
 602                         layout.emergency("Sensor for "+name+" triggered out of order");
 603         }
 604         else
 605         {
 606                 const Vehicle &veh = *(reverse ? vehicles.front() : vehicles.back());
 607
 608                 // Find the first sensor in our current blocks that's still active
 609                 list<BlockRef>::iterator end = cur_blocks.begin();
 610                 for(list<BlockRef>::iterator i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
 611                 {
 612                         if(i->block->get_tracks().count(veh.get_track()))
 613                                 break;
 614                         if(i->block->get_sensor_id())
 615                         {
 616                                 if(layout.get_driver().get_sensor(i->block->get_sensor_id()))
 617                                         break;
 618                                 else
 619                                 {
 620                                         end = i;
 621                                         ++end;
 622                                 }
 623                         }
 624                 }
 625
 626                 if(end!=cur_blocks.begin() && end!=cur_blocks.end())
 627                         // Free blocks up to the last inactive sensor
 628                         release_blocks(cur_blocks, cur_blocks.begin(), end);
 629         }
 630 }
 631
 632 void Train::turnout_event(unsigned addr, bool)
 633 {
 634         if(pending_block)
 635         {
 636                 unsigned pending_addr = pending_block->get_turnout_id();
 637                 bool double_addr = (*pending_block->get_tracks().begin())->get_type().is_double_address();
 638                 if(addr==pending_addr || (double_addr && addr==pending_addr+1))
 639                 {
 640                         if(reserving)
 641                                 pending_block = 0;
 642                         else
 643                                 reserve_more();
 644                 }
 645         }
 646 }
 647
 648 void Train::halt_event(bool h)
 649 {
 650         if(h)
 651                 accurate_position = false;
 652 }
 653
 654 void Train::block_reserved(const Block &block, const Train *train)
 655 {
 656         if(&block==pending_block && !train && !reserving)
 657                 reserve_more();
 658 }
 659
 660 unsigned Train::reserve_more()
 661 {
 662         if(!active)
 663                 return 0;
 664
 665         BlockRef *start = 0;
 666         if(!rsv_blocks.empty())
 667                 start = &rsv_blocks.back();
 668         else if(!cur_blocks.empty())
 669                 start = &cur_blocks.back();
 670         if(!start)
 671                 return 0;
 672
 673         pending_block = 0;
 674
 675         // See how many sensor blocks and how much track we already have
 676         unsigned nsens = 0;
 677         float dist = 0;
 678         for(list<BlockRef>::const_iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
 679         {
 680                 if(i->block->get_sensor_id())
 681                         ++nsens;
 682                 if(nsens>0)
 683                 {
 684                         float length = 0;
 685                         i->block->traverse(i->entry, &length);
 686                         dist += length;
 687                 }
 688         }
 689
 690         if(end_of_route)
 691                 return nsens;
 692
 693         const Route *cur_route = 0;
 694         if(route)
 695         {
 696                 const set<Track *> &tracks = start->block->get_tracks();
 697                 for(set<Track *>::const_iterator i=tracks.begin(); (cur_route!=route && i!=tracks.end()); ++i)
 698                 {
 699                         if(route->get_tracks().count(*i))
 700                                 cur_route = route;
 701                         else if(next_route && next_route->get_tracks().count(*i))
 702                                 cur_route = next_route;
 703                 }
 704         }
 705
 706         float approach_margin = 50*layout.get_catalogue().get_scale();
 707         float min_dist = controller->get_braking_distance()*1.3+approach_margin*2;
 708
 709         BlockRef *last = start;
 710         BlockRef *good = start;
 711         unsigned good_sens = nsens;
 712         float good_dist = dist;
 713         Train *blocking_train = 0;
 714         std::list<BlockRef> contested_blocks;
 715
 716         SetFlag setf(reserving);
 717
 718         while(good_sens<3 || good_dist<min_dist || !contested_blocks.empty())
 719         {
 720                 // Traverse to the next block
 721                 float length = 0;
 722                 unsigned exit = last->block->traverse(last->entry, cur_route, &length);
 723                 Block *link = last->block->get_link(exit);
 724                 if(!link)
 725                         break;
 726
 727                 int entry = link->get_endpoint_by_link(*last->block);
 728                 if(entry<0)
 729                         throw LogicError("Block links are inconsistent!");
 730
 731                 const Block::Endpoint &entry_ep = link->get_endpoints()[entry];
 732
 733                 if(cur_route)
 734                 {
 735                         if(cur_route!=next_route && next_route && next_route->get_tracks().count(entry_ep.track))
 736                                 cur_route = next_route;
 737                         else if(!cur_route->get_tracks().count(entry_ep.track))
 738                         {
 739                                 // Keep the blocks if we arrived at the end of the route
 740                                 if(!blocking_train)
 741                                 {
 742                                         good = last;
 743                                         good_sens = nsens;
 744                                         good_dist = dist;
 745                                         end_of_route = true;
 746                                 }
 747                                 break;
 748                         }
 749                 }
 750                 else if(route && route->get_tracks().count(entry_ep.track))
 751                         cur_route = route;
 752
 753                 if(link->get_endpoints().size()<2)
 754                 {
 755                         if(!blocking_train)
 756                         {
 757                                 good = last;
 758                                 good_sens = nsens;
 759                                 good_dist = dist;
 760                         }
 761                         break;
 762                 }
 763
 764                 if(blocking_train)
 765                 {
 766                         if(link->get_train()!=blocking_train)
 767                         {
 768                                 // XXX is it possible that this won't free all the blocks we want?
 769                                 if(blocking_train->free_block(*contested_blocks.back().block))
 770                                 {
 771                                         // Roll back and start actually reserving the blocks
 772                                         last = &rsv_blocks.back();
 773                                         if(blocking_train->get_priority()==priority)
 774                                                 blocking_train->yield_to(*this);
 775                                         blocking_train = 0;
 776                                         continue;
 777                                 }
 778                                 else
 779                                 {
 780                                         pending_block = contested_blocks.front().block;
 781                                         break;
 782                                 }
 783                         }
 784                         else
 785                         {
 786                                 contested_blocks.push_back(BlockRef(link, entry));
 787                                 last = &contested_blocks.back();
 788                                 continue;
 789                         }
 790                 }
 791
 792                 bool reserved = link->reserve(this);
 793                 if(!reserved)
 794                 {
 795                         /* We've found another train.  If it wants to exit the block from the
 796                         same endpoint we're trying to enter from or the other way around,
 797                         treat it as coming towards us.  Otherwise treat it as going in the
 798                         same direction. */
 799                         Train *other_train = link->get_train();
 800                         int other_entry = other_train->get_entry_to_block(*link);
 801                         if(other_entry<0)
 802                                 throw LogicError("Block reservation inconsistency");
 803
 804                         int other_prio = other_train->get_priority();
 805
 806                         bool entry_conflict = (static_cast<unsigned>(entry)==link->traverse(other_entry));
 807                         bool exit_conflict = (link->traverse(entry)==static_cast<unsigned>(other_entry));
 808                         if(!entry_conflict && !exit_conflict)
 809                         {
 810                                 /* Same direction, keep the blocks we got so far and wait for the
 811                                 other train to pass */
 812                                 good = last;
 813                                 good_sens = nsens;
 814                                 good_dist = dist;
 815
 816                                 // Ask a lesser priority train to free the block for us
 817                                 if(other_train->get_priority()<priority)
 818                                         if(other_train->free_block(*link))
 819                                                 reserved = link->reserve(this);
 820                         }
 821                         else if(other_prio<priority || (other_prio==priority && other_train!=yielding_to && entry_conflict))
 822                         {
 823                                 /* A lesser priority train is coming at us, we must ask it to free
 824                                 enough blocks to get clear of it to avoid a potential deadlock */
 825                                 blocking_train = other_train;
 826                                 contested_blocks.clear();
 827                                 contested_blocks.push_back(BlockRef(link, entry));
 828                                 last = &contested_blocks.back();
 829                                 continue;
 830                         }
 831
 832                         if(!reserved)
 833                         {
 834                                 pending_block = link;
 835                                 break;
 836                         }
 837                 }
 838
 839                 if(link->get_turnout_id())
 840                 {
 841                         const Endpoint &track_ep = entry_ep.track->get_type().get_endpoints()[entry_ep.track_ep];
 842
 843                         // Keep the blocks reserved so far, as either us or the other train can diverge
 844                         good = last;
 845                         good_sens = nsens;
 846                         good_dist = dist;
 847
 848                         // Figure out what path we'd like to take on the turnout
 849                         int path = -1;
 850                         if(cur_route)
 851                                 path = cur_route->get_turnout(link->get_turnout_id());
 852                         if(path<0)
 853                                 path = entry_ep.track->get_active_path();
 854                         if(!((track_ep.paths>>path)&1))
 855                         {
 856                                 for(unsigned i=0; track_ep.paths>>i; ++i)
 857                                         if((track_ep.paths>>i)&1)
 858                                                 path = i;
 859                         }
 860
 861                         if(path!=static_cast<int>(entry_ep.track->get_active_path()))
 862                         {
 863                                 // The turnout is set to wrong path - switch and wait for it
 864                                 pending_block = link;
 865                                 entry_ep.track->set_active_path(path);
 866                                 if(pending_block)
 867                                 {
 868                                         link->reserve(0);
 869                                         break;
 870                                 }
 871                         }
 872                 }
 873
 874                 if(!contested_blocks.empty() && contested_blocks.front().block==link)
 875                         contested_blocks.pop_front();
 876
 877                 rsv_blocks.push_back(BlockRef(link, entry));
 878                 last = &rsv_blocks.back();
 879                 if(last->block->get_sensor_id())
 880                         ++nsens;
 881                 if(nsens>0)
 882                         dist += length;
 883         }
 884
 885         // Unreserve blocks that were not good
 886         while(!rsv_blocks.empty() && &rsv_blocks.back()!=good)
 887         {
 888                 rsv_blocks.back().block->reserve(0);
 889                 rsv_blocks.erase(--rsv_blocks.end());
 890         }
 891
 892         if(!rsv_blocks.empty() && &rsv_blocks.back()!=start)
 893                 // We got some new blocks, so no longer need to yield
 894                 yielding_to = 0;
 895
 896         // Make any sensorless blocks at the beginning immediately current
 897         list<BlockRef>::iterator i;
 898         for(i=rsv_blocks.begin(); (i!=rsv_blocks.end() && !i->block->get_sensor_id()); ++i) ;
 899         if(i!=rsv_blocks.begin())
 900                 cur_blocks.splice(cur_blocks.end(), rsv_blocks, rsv_blocks.begin(), i);
 901
 902         return good_sens;
 903 }
 904
 905 float Train::get_reserved_distance_until(const Block *until_block, bool back) const
 906 {
 907         if(cur_blocks.empty())
 908                 return 0;
 909
 910         Vehicle &veh = *(reverse!=back ? vehicles.back() : vehicles.front());
 911         const VehicleType &vtype = veh.get_type();
 912
 913         Track *track = veh.get_track();
 914         if(!track)
 915                 return 0;
 916
 917         list<BlockRef>::const_iterator block = cur_blocks.begin();
 918         while(block!=rsv_blocks.end() && !block->block->get_tracks().count(track))
 919         {
 920                 ++block;
 921                 if(block==cur_blocks.end())
 922                 {
 923                         if(back)
 924                                 return 0;
 925                         block = rsv_blocks.begin();
 926                 }
 927         }
 928         if(block==rsv_blocks.end() || block->block==until_block)
 929                 return 0;
 930
 931         unsigned entry = veh.get_entry();
 932
 933         float result = veh.get_offset();
 934         if(reverse!=back)
 935                 entry = track->traverse(entry);
 936         else
 937                 result = track->get_type().get_path_length(track->get_active_path())-result;
 938         result -= vtype.get_length()/2;
 939
 940         while(1)
 941         {
 942                 if(track->get_type().get_endpoints().size()<2)
 943                         break;
 944
 945                 Track *next = track->get_link(track->traverse(entry));
 946
 947                 if(!block->block->get_tracks().count(next))
 948                 {
 949                         if(back)
 950                         {
 951                                 if(block==cur_blocks.begin())
 952                                         break;
 953                                 --block;
 954                         }
 955                         else
 956                         {
 957                                 ++block;
 958                                 if(block==cur_blocks.end())
 959                                         block = rsv_blocks.begin();
 960                                 if(block==rsv_blocks.end())
 961                                         break;
 962                         }
 963
 964                         if(block->block==until_block)
 965                                 break;
 966                 }
 967
 968                 entry = next->get_endpoint_by_link(*track);
 969                 track = next;
 970
 971                 result += track->get_type().get_path_length(track->get_active_path());
 972         }
 973
 974         return result;
 975 }
 976
 977 float Train::get_real_speed(unsigned i) const
 978 {
 979         if(real_speed[i].weight)
 980                 return real_speed[i].speed;
 981
 982         unsigned low;
 983         unsigned high;
 984         for(low=i; low>0; --low)
 985                 if(real_speed[low].weight)
 986                         break;
 987         for(high=i; high<14; ++high)
 988                 if(real_speed[high].weight)
 989                         break;
 990
 991         if(real_speed[high].weight)
 992         {
 993                 if(real_speed[low].weight)
 994                 {
 995                         float f = float(i-low)/(high-low);
 996                         return real_speed[low].speed*(1-f)+real_speed[high].speed*f;
 997                 }
 998                 else
 999                         return real_speed[high].speed*float(i)/high;
1000         }
1001         else if(real_speed[low].weight)
1002                 return real_speed[low].speed*float(i)/low;
1003         else
1004                 return 0;
1005 }
1006
1007 unsigned Train::find_speed(float real) const
1008 {
1009         if(real<=real_speed[0].speed)
1010                 return 0;
1011
1012         unsigned low = 0;
1013         unsigned high = 0;
1014         unsigned last = 0;
1015         for(unsigned i=0; (!high && i<=14); ++i)
1016                 if(real_speed[i].weight)
1017                 {
1018                         last = i;
1019                         if(real_speed[i].speed<real)
1020                                 low = i;
1021                         else
1022                                 high = i;
1023                 }
1024         if(!high)
1025         {
1026                 if(!low)
1027                 {
1028                         if(real)
1029                                 return 3;
1030                         else
1031                                 return 0;
1032                 }
1033                 return min(min(static_cast<unsigned>(low*real/real_speed[low].speed), 14U), last+3);
1034         }
1035
1036         float f = (real-real_speed[low].speed)/(real_speed[high].speed-real_speed[low].speed);
1037         return static_cast<unsigned>(low*(1-f)+high*f+0.5);
1038 }
1039
1040 float Train::get_travel_speed() const
1041 {
1042         float speed = get_real_speed(current_speed);
1043         float scale = layout.get_catalogue().get_scale();
1044         return static_cast<int>(round(speed/scale*3.6/5))*5;
1045 }
1046
1047 void Train::set_status(const string &s)
1048 {
1049         status = s;
1050         signal_status_changed.emit(s);
1051 }
1052
1053 void Train::release_blocks(list<BlockRef> &blocks)
1054 {
1055         release_blocks(blocks, blocks.begin(), blocks.end());
1056 }
1057
1058 void Train::release_blocks(list<BlockRef> &blocks, list<BlockRef>::iterator begin, list<BlockRef>::iterator end)
1059 {
1060         while(begin!=end)
1061         {
1062                 Block *block = begin->block;
1063                 blocks.erase(begin++);
1064                 block->reserve(0);
1065         }
1066 }
1067
1068 void Train::reverse_blocks(list<BlockRef> &blocks) const
1069 {
1070         blocks.reverse();
1071         for(list<BlockRef>::iterator i=blocks.begin(); i!=blocks.end(); ++i)
1072                 i->entry = i->block->traverse(i->entry);
1073 }
1074
1075
1076 Train::BlockRef::BlockRef(Block *b, unsigned e):
1077         block(b),
1078         entry(e)
1079 { }
1080
1081 Train::BlockRef Train::BlockRef::next() const
1082 {
1083         Block *blk = block->get_endpoints()[block->traverse(entry)].link;
1084         if(!blk)
1085                 throw InvalidState("At end of line");
1086
1087         int ep = blk->get_endpoint_by_link(*block);
1088         if(ep<0)
1089                 throw LogicError("Block links are inconsistent");
1090
1091         return BlockRef(blk, ep);
1092 }
1093
1094
1095 Train::RealSpeed::RealSpeed():
1096         speed(0),
1097         weight(0)
1098 { }
1099
1100 void Train::RealSpeed::add(float s, float w)
1101 {
1102         speed = (speed*weight+s*w)/(weight+w);
1103         weight = min(weight+w, 300.0f);
1104 }
1105
1106
1107 Train::Loader::Loader(Train &t):
1108         DataFile::BasicLoader<Train>(t),
1109         prev_block(0),
1110         blocks_valid(true)
1111 {
1112         add("block",       &Loader::block);
1113         add("block_hint",  &Loader::block_hint);
1114         add("name",        &Loader::name);
1115         add("priority",    &Train::priority);
1116         add("real_speed",  &Loader::real_speed);
1117         add("route",       &Loader::route);
1118         add("timetable",   &Loader::timetable);
1119         add("vehicle",     &Loader::vehicle);
1120 }
1121
1122 void Train::Loader::finish()
1123 {
1124         if(!obj.cur_blocks.empty())
1125         {
1126                 const BlockRef &blkref = obj.cur_blocks.front();
1127                 const Block::Endpoint &bep = blkref.block->get_endpoints()[blkref.entry];
1128                 float offset = 2*obj.layout.get_catalogue().get_scale();
1129                 obj.vehicles.back()->place(*bep.track, bep.track_ep, offset, Vehicle::BACK_BUFFER);
1130
1131                 obj.set_status("Stopped");
1132         }
1133 }
1134
1135 void Train::Loader::block(unsigned id)
1136 {
1137         if(!blocks_valid)
1138                 return;
1139
1140         Block *blk;
1141         try
1142         {
1143                 blk = &obj.layout.get_block(id);
1144         }
1145         catch(const KeyError &)
1146         {
1147                 blocks_valid = false;
1148                 return;
1149         }
1150
1151         int entry = -1;
1152         if(prev_block)
1153                 entry = blk->get_endpoint_by_link(*prev_block);
1154         if(entry<0)
1155                 entry = 0;
1156
1157         blk->reserve(&obj);
1158         obj.cur_blocks.push_back(BlockRef(blk, entry));
1159
1160         if(blk->get_sensor_id())
1161                 obj.layout.get_driver().set_sensor(blk->get_sensor_id(), true);
1162
1163         prev_block = blk;
1164 }
1165
1166 void Train::Loader::block_hint(unsigned id)
1167 {
1168         try
1169         {
1170                 prev_block = &obj.layout.get_block(id);
1171         }
1172         catch(const KeyError &)
1173         {
1174                 blocks_valid = false;
1175         }
1176 }
1177
1178 void Train::Loader::name(const string &n)
1179 {
1180         obj.set_name(n);
1181 }
1182
1183 void Train::Loader::real_speed(unsigned i, float speed, float weight)
1184 {
1185         obj.real_speed[i].speed = speed;
1186         obj.real_speed[i].weight = weight;
1187 }
1188
1189 void Train::Loader::route(const string &n)
1190 {
1191         obj.set_route(&obj.layout.get_route(n));
1192 }
1193
1194 void Train::Loader::timetable()
1195 {
1196         if(obj.timetable)
1197                 throw InvalidState("A timetable has already been loaded");
1198
1199         obj.timetable = new Timetable(obj);
1200         load_sub(*obj.timetable);
1201 }
1202
1203 void Train::Loader::vehicle(unsigned n)
1204 {
1205         const VehicleType &vtype = obj.layout.get_catalogue().get_vehicle(n);
1206         Vehicle *veh = new Vehicle(obj.layout, vtype);
1207         obj.vehicles.back()->attach_back(*veh);
1208         obj.vehicles.push_back(veh);
1209 }
1210
1211 } // namespace Marklin