source/libmarklin/train.cpp

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