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