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