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