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