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                 unsigned tid = track->get_turnout_id();
 305                 track = track.next(tid ? route->route->get_turnout(tid) : 0);
 306
 307                 if(!track || track.looped())
 308                         return false;
 309         }
 310
 311         TrackIter track = TrackIter(&from, entry).next(path);
 312         if(!track)
 313                 return false;
 314
 315         set<Track *> tracks;
 316         for(list<RouteRef>::iterator i=routes.begin(); i!=routes.end(); ++i)
 317                 tracks.insert(i->route->get_tracks().begin(), i->route->get_tracks().end());
 318         RefPtr<Route> diversion = Route::find(track, tracks);
 319         if(!diversion)
 320                 return false;
 321
 322         diversion->set_name("Diversion");
 323         diversion->add_track(from);
 324         diversion->set_turnout(from.get_turnout_id(), path);
 325
 326         if(!is_valid_diversion(*diversion, TrackIter(&from, entry)))
 327                 return false;
 328
 329         // Follow the diversion route until we get back to the original route
 330         list<RouteRef>::iterator end = routes.end();
 331         while(1)
 332         {
 333                 for(list<RouteRef>::iterator i=route; (end==routes.end() && i!=routes.end()); ++i)
 334                         if(i->route->has_track(*track))
 335                                 end = i;
 336
 337                 if(end!=routes.end())
 338                         break;
 339                 else if(!diversion->has_track(*track))
 340                         throw LogicError("Pathfinder returned a bad route");
 341
 342                 unsigned tid = track->get_turnout_id();
 343                 track = track.next(tid ? diversion->get_turnout(tid) : 0);
 344         }
 345
 346         if(route==end)
 347                 // We are rejoining the same route we diverted from, duplicate it
 348                 routes.insert(end, *route);
 349         else
 350         {
 351                 ++route;
 352                 routes.erase(route, end);
 353         }
 354         routes.insert(end, RouteRef(diversion.release(), from.get_turnout_id()));
 355
 356         return true;
 357 }
 358
 359 const Route *Train::get_route() const
 360 {
 361         if(routes.empty())
 362                 return 0;
 363         return routes.front().route;
 364 }
 365
 366 void Train::place(Block &block, unsigned entry)
 367 {
 368         if(controller->get_speed())
 369                 throw InvalidState("Must be stopped before placing");
 370
 371         release_blocks(rsv_blocks);
 372         release_blocks(cur_blocks);
 373
 374         set_active(false);
 375         accurate_position = false;
 376
 377         if(!block.reserve(this))
 378         {
 379                 set_status("Unplaced");
 380                 return;
 381         }
 382
 383         cur_blocks.push_back(BlockIter(&block, entry));
 384         if(reverse)
 385         {
 386                 TrackIter track = BlockIter(&block, entry).reverse().track_iter();
 387                 vehicles.front()->place(*track, track.entry(), 0, Vehicle::FRONT_BUFFER);
 388         }
 389         else
 390         {
 391                 const Block::Endpoint &bep = block.get_endpoints()[entry];
 392                 vehicles.back()->place(*bep.track, bep.track_ep, 0, Vehicle::BACK_BUFFER);
 393         }
 394 }
 395
 396 void Train::unplace()
 397 {
 398         if(controller->get_speed())
 399                 throw InvalidState("Must be stopped before unplacing");
 400
 401         release_blocks(rsv_blocks);
 402         release_blocks(cur_blocks);
 403
 404         set_active(false);
 405         accurate_position = false;
 406
 407         for(vector<Vehicle *>::iterator i=vehicles.begin(); i!=vehicles.end(); ++i)
 408                 (*i)->unplace();
 409
 410         set_status("Unplaced");
 411 }
 412
 413 bool Train::free_block(Block &block)
 414 {
 415         float margin = 10*layout.get_catalogue().get_scale();
 416         if(get_reserved_distance_until(&block, false)<controller->get_braking_distance()*1.3+margin)
 417                 return false;
 418
 419         unsigned nsens = 0;
 420         for(BlockList::iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
 421         {
 422                 if(i->block()==&block)
 423                 {
 424                         if(nsens<1)
 425                                 return false;
 426                         release_blocks(rsv_blocks, i, rsv_blocks.end());
 427                         return true;
 428                 }
 429                 else if((*i)->get_sensor_id())
 430                         ++nsens;
 431         }
 432
 433         return false;
 434 }
 435
 436 void Train::free_noncritical_blocks()
 437 {
 438         if(cur_blocks.empty() || rsv_blocks.empty())
 439                 return;
 440
 441         if(controller->get_speed()==0)
 442         {
 443                 release_blocks(rsv_blocks);
 444                 return;
 445         }
 446
 447         float margin = 10*layout.get_catalogue().get_scale();
 448         float min_dist = controller->get_braking_distance()*1.3+margin;
 449
 450         Vehicle &veh = *(reverse ? vehicles.back() : vehicles.front());
 451
 452         TrackIter track(veh.get_track(), veh.get_entry());
 453         BlockList::iterator block = cur_blocks.begin();
 454         bool in_rsv = false;
 455         while(block!=rsv_blocks.end() && !(*block)->has_track(*track))
 456         {
 457                 ++block;
 458                 if(block==cur_blocks.end())
 459                 {
 460                         block = rsv_blocks.begin();
 461                         in_rsv = true;
 462                 }
 463         }
 464
 465         float dist = veh.get_offset();
 466         if(reverse)
 467                 track.reverse();
 468         else
 469                 dist = track->get_type().get_path_length(track->get_active_path())-dist;
 470         dist -= veh.get_type().get_length()/2;
 471
 472         bool nsens = 0;
 473         while(1)
 474         {
 475                 track = track.next();
 476
 477                 if(!(*block)->has_track(*track))
 478                 {
 479                         ++block;
 480                         if(block==cur_blocks.end())
 481                         {
 482                                 block = rsv_blocks.begin();
 483                                 in_rsv = true;
 484                         }
 485                         if(block==rsv_blocks.end())
 486                                 return;
 487
 488                         if(dist>min_dist && nsens>0)
 489                         {
 490                                 release_blocks(rsv_blocks, block, rsv_blocks.end());
 491                                 return;
 492                         }
 493
 494                         if(in_rsv && (*block)->get_sensor_id())
 495                                 ++nsens;
 496                 }
 497
 498                 dist += track->get_type().get_path_length(track->get_active_path());
 499         }
 500 }
 501
 502 int Train::get_entry_to_block(Block &block) const
 503 {
 504         for(BlockList::const_iterator i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
 505                 if(i->block()==&block)
 506                         return i->entry();
 507         for(BlockList::const_iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
 508                 if(i->block()==&block)
 509                         return i->entry();
 510         return -1;
 511 }
 512
 513 float Train::get_reserved_distance() const
 514 {
 515         return get_reserved_distance_until(0, false);
 516 }
 517
 518 void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt)
 519 {
 520         if(!active && stop_timeout && t>=stop_timeout)
 521         {
 522                 release_blocks(rsv_blocks);
 523                 end_of_route = false;
 524                 stop_timeout = Time::TimeStamp();
 525         }
 526
 527         Driver &driver = layout.get_driver();
 528
 529         if(timetable)
 530                 timetable->tick(t);
 531         controller->tick(dt);
 532         float speed = controller->get_speed();
 533         unsigned speed_notch = find_speed(speed);
 534
 535         if(controller->get_reverse()!=reverse)
 536         {
 537                 reverse = controller->get_reverse();
 538                 driver.set_loco_reverse(address, reverse);
 539
 540                 release_blocks(rsv_blocks);
 541                 reverse_blocks(cur_blocks);
 542
 543                 reserve_more();
 544         }
 545         if(speed_notch!=current_speed && !speed_changing && !driver.is_halted() && driver.get_power())
 546         {
 547                 speed_changing = true;
 548                 driver.set_loco_speed(address, speed_notch);
 549
 550                 pure_speed = false;
 551
 552                 if(speed_notch)
 553                         set_status(format("Traveling %d kmh", get_travel_speed()));
 554                 else
 555                         set_status("Waiting");
 556         }
 557
 558         if(speed)
 559         {
 560                 if(!active)
 561                         set_active(true);
 562
 563                 Vehicle &vehicle = *(reverse ? vehicles.back() : vehicles.front());
 564                 Track *track = vehicle.get_track();
 565
 566                 bool ok = false;
 567                 for(BlockList::const_iterator i=cur_blocks.begin(); (!ok && i!=cur_blocks.end()); ++i)
 568                         ok = (*i)->has_track(*track);
 569
 570                 float d = get_real_speed(current_speed)*(dt/Time::sec);
 571                 if(ok)
 572                 {
 573                         SetFlag setf(advancing);
 574                         vehicle.advance(reverse ? -d : d);
 575                 }
 576                 else if(accurate_position)
 577                 {
 578                         overshoot_dist += d;
 579                         if(overshoot_dist>40*layout.get_catalogue().get_scale())
 580                         {
 581                                 layout.emergency(name+" has not arrived at sensor");
 582                                 accurate_position = false;
 583                         }
 584                 }
 585         }
 586         else if(end_of_route && rsv_blocks.empty())
 587         {
 588                 set_active(false);
 589                 signal_arrived.emit();
 590                 set_route(0);
 591         }
 592
 593         if(!cur_blocks.empty() && !cur_blocks.front()->get_sensor_id())
 594         {
 595                 float dist = get_reserved_distance_until(&*cur_blocks.front(), true);
 596
 597                 if(dist>10*layout.get_catalogue().get_scale())
 598                 {
 599                         cur_blocks.front()->reserve(0);
 600                         cur_blocks.pop_front();
 601                 }
 602         }
 603 }
 604
 605 void Train::save(list<DataFile::Statement> &st) const
 606 {
 607         st.push_back((DataFile::Statement("name"), name));
 608
 609         st.push_back((DataFile::Statement("priority"), priority));
 610
 611         for(vector<Vehicle *>::const_iterator i=vehicles.begin(); i!=vehicles.end(); ++i)
 612                 if(i!=vehicles.begin())
 613                         st.push_back((DataFile::Statement("vehicle"), (*i)->get_type().get_article_number()));
 614
 615         for(unsigned i=0; i<=14; ++i)
 616                 if(real_speed[i].weight)
 617                         st.push_back((DataFile::Statement("real_speed"), i, real_speed[i].speed, real_speed[i].weight));
 618
 619         if(!cur_blocks.empty())
 620         {
 621                 BlockList blocks = cur_blocks;
 622                 if(reverse)
 623                         reverse_blocks(blocks);
 624
 625                 BlockIter prev = blocks.front().flip();
 626                 st.push_back((DataFile::Statement("block_hint"), prev->get_id()));
 627
 628                 for(BlockList::const_iterator i=blocks.begin(); i!=blocks.end(); ++i)
 629                         st.push_back((DataFile::Statement("block"), (*i)->get_id()));
 630         }
 631
 632         if(!routes.empty())
 633         {
 634                 list<RouteRef>::const_iterator i = routes.begin();
 635                 for(; (i!=routes.end() && i->route->is_temporary()); ++i) ;
 636                 if(i!=routes.end())
 637                         st.push_back((DataFile::Statement("route"), i->route->get_name()));
 638         }
 639
 640         if(timetable)
 641         {
 642                 DataFile::Statement ss("timetable");
 643                 timetable->save(ss.sub);
 644                 st.push_back(ss);
 645         }
 646 }
 647
 648 void Train::control_changed(const Controller::Control &ctrl)
 649 {
 650         signal_control_changed.emit(ctrl.name, ctrl.value);
 651 }
 652
 653 void Train::loco_speed_event(unsigned addr, unsigned speed, bool)
 654 {
 655         if(addr==address)
 656         {
 657                 current_speed = speed;
 658                 speed_changing = false;
 659                 pure_speed = false;
 660         }
 661 }
 662
 663 void Train::loco_func_event(unsigned addr, unsigned func, bool state)
 664 {
 665         if(addr==address || (addr==address+1 && loco_type.get_max_function()>4))
 666         {
 667                 if(addr==address+1)
 668                         func += 4;
 669                 if(state)
 670                         functions |= 1<<func;
 671                 else
 672                         functions &= ~(1<<func);
 673
 674                 signal_function_changed.emit(func, state);
 675         }
 676 }
 677
 678 void Train::sensor_event(unsigned addr, bool state)
 679 {
 680         if(state)
 681         {
 682                 // Find the first sensor block from our reserved blocks that isn't this sensor
 683                 BlockList::iterator i;
 684                 unsigned result = 0;
 685                 for(i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
 686                         if((*i)->get_sensor_id())
 687                         {
 688                                 if((*i)->get_sensor_id()!=addr)
 689                                 {
 690                                         if(result==0)
 691                                                 result = 2;
 692                                         else if(result==1)
 693                                                 break;
 694                                 }
 695                                 else if(result==0)
 696                                         result = 1;
 697                                 else if(result==2)
 698                                         result = 3;
 699                         }
 700
 701                 if(result==1 && i!=rsv_blocks.begin())
 702                 {
 703                         // Compute speed and update related state
 704                         float travel_time_secs = (Time::now()-last_entry_time)/Time::sec;
 705
 706                         if(pure_speed)
 707                         {
 708                                 if(current_speed)
 709                                 {
 710                                         RealSpeed &rs = real_speed[current_speed];
 711                                         rs.add(travel_dist/travel_time_secs, travel_time_secs);
 712                                 }
 713                                 set_status(format("Traveling %d kmh", get_travel_speed()));
 714                         }
 715
 716                         travel_dist = 0;
 717                         for(BlockList::iterator j=rsv_blocks.begin(); j!=i; ++j)
 718                         {
 719                                 travel_dist += (*j)->get_path_length(j->entry());
 720
 721                                 if((*j)->get_sensor_id()==addr && !advancing)
 722                                 {
 723                                         TrackIter track = j->track_iter();
 724                                         if(reverse)
 725                                         {
 726                                                 track = track.flip();
 727                                                 vehicles.back()->place(*track, track.entry(), 0, Vehicle::BACK_AXLE);
 728                                         }
 729                                         else
 730                                                 vehicles.front()->place(*track, track.entry(), 0, Vehicle::FRONT_AXLE);
 731                                 }
 732                         }
 733                         last_entry_time = Time::now();
 734                         pure_speed = true;
 735                         accurate_position = true;
 736                         overshoot_dist = 0;
 737
 738                         // Check if we've reached the next route
 739                         if(routes.size()>1)
 740                         {
 741                                 const set<Track *> &rtracks = (++routes.begin())->route->get_tracks();
 742                                 for(BlockList::iterator j=rsv_blocks.begin(); j!=i; ++j)
 743                                         if(rtracks.count((*j)->get_endpoints()[j->entry()].track))
 744                                         {
 745                                                 routes.pop_front();
 746                                                 // XXX Exceptions?
 747                                                 signal_route_changed.emit(routes.front().route);
 748                                                 break;
 749                                         }
 750                         }
 751
 752                         // Move blocks up to the next sensor to our current blocks
 753                         cur_blocks.splice(cur_blocks.end(), rsv_blocks, rsv_blocks.begin(), i);
 754
 755                         // Try to get more blocks if we're moving
 756                         if(active)
 757                                 reserve_more();
 758                 }
 759                 else if(result==3)
 760                         layout.emergency("Sensor for "+name+" triggered out of order");
 761         }
 762         else
 763         {
 764                 const Vehicle &veh = *(reverse ? vehicles.front() : vehicles.back());
 765
 766                 // Find the first sensor in our current blocks that's still active
 767                 BlockList::iterator end = cur_blocks.begin();
 768                 for(BlockList::iterator i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
 769                 {
 770                         if((*i)->has_track(*veh.get_track()))
 771                                 break;
 772                         if((*i)->get_sensor_id())
 773                         {
 774                                 if(layout.get_driver().get_sensor((*i)->get_sensor_id()))
 775                                         break;
 776                                 else
 777                                 {
 778                                         end = i;
 779                                         ++end;
 780                                 }
 781                         }
 782                 }
 783
 784                 if(end!=cur_blocks.begin() && end!=cur_blocks.end())
 785                         // Free blocks up to the last inactive sensor
 786                         release_blocks(cur_blocks, cur_blocks.begin(), end);
 787         }
 788 }
 789
 790 void Train::turnout_event(unsigned addr, bool)
 791 {
 792         if(pending_block && (!pending_block->get_train() || pending_block->get_train()==this))
 793         {
 794                 unsigned pending_addr = pending_block->get_turnout_id();
 795                 bool double_addr = (*pending_block->get_tracks().begin())->get_type().is_double_address();
 796                 if(addr==pending_addr || (double_addr && addr==pending_addr+1))
 797                 {
 798                         if(reserving)
 799                                 pending_block = 0;
 800                         else
 801                                 reserve_more();
 802                 }
 803         }
 804 }
 805
 806 void Train::halt_event(bool h)
 807 {
 808         if(h)
 809                 accurate_position = false;
 810 }
 811
 812 void Train::block_reserved(const Block &block, const Train *train)
 813 {
 814         if(&block==pending_block && !train && !reserving)
 815                 reserve_more();
 816 }
 817
 818 unsigned Train::reserve_more()
 819 {
 820         if(!active)
 821                 return 0;
 822
 823         BlockIter start;
 824         if(!rsv_blocks.empty())
 825                 start = rsv_blocks.back();
 826         else if(!cur_blocks.empty())
 827                 start = cur_blocks.back();
 828         else
 829                 return 0;
 830
 831         pending_block = 0;
 832
 833         // See how many sensor blocks and how much track we already have
 834         unsigned nsens = 0;
 835         float dist = 0;
 836         for(BlockList::const_iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
 837         {
 838                 if((*i)->get_sensor_id())
 839                         ++nsens;
 840                 if(nsens>0)
 841                         dist += (*i)->get_path_length(i->entry());
 842         }
 843
 844         if(end_of_route)
 845                 return nsens;
 846
 847         list<RouteRef>::iterator cur_route = routes.begin();
 848         advance_route(cur_route, *start.track_iter());
 849
 850         float approach_margin = 50*layout.get_catalogue().get_scale();
 851         float min_dist = controller->get_braking_distance()*1.3+approach_margin*2;
 852
 853         BlockIter block = start;
 854         BlockIter good = start;
 855         Track *divert_track = 0;
 856         bool try_divert = false;
 857         unsigned good_sens = nsens;
 858         float good_dist = dist;
 859         Train *blocking_train = 0;
 860         BlockList contested_blocks;
 861
 862         SetFlag setf(reserving);
 863
 864         while(good_sens<3 || good_dist<min_dist || !contested_blocks.empty())
 865         {
 866                 BlockIter last = block;
 867                 block = block.next(cur_route!=routes.end() ? cur_route->route : 0);
 868                 if(!block)
 869                         break;
 870
 871                 TrackIter track = block.track_iter();
 872
 873                 if(cur_route!=routes.end())
 874                 {
 875                         if(!advance_route(cur_route, *track))
 876                         {
 877                                 // Keep the blocks if we arrived at the end of the route
 878                                 if(!blocking_train)
 879                                 {
 880                                         good = last;
 881                                         good_sens = nsens;
 882                                         good_dist = dist;
 883                                         end_of_route = true;
 884                                 }
 885                                 break;
 886                         }
 887                 }
 888                 else if(!routes.empty() && routes.front().route->has_track(*track))
 889                         cur_route = routes.begin();
 890
 891                 if(block->get_endpoints().size()<2)
 892                 {
 893                         if(!blocking_train)
 894                         {
 895                                 good = last;
 896                                 good_sens = nsens;
 897                                 good_dist = dist;
 898                         }
 899                         break;
 900                 }
 901
 902                 if(blocking_train)
 903                 {
 904                         if(block->get_train()!=blocking_train)
 905                         {
 906                                 if(blocking_train->free_block(*contested_blocks.back()))
 907                                 {
 908                                         // Roll back and start actually reserving the blocks
 909                                         block = rsv_blocks.back();
 910                                         cur_route = routes.begin();
 911                                         advance_route(cur_route, *block.track_iter().track());
 912                                         if(blocking_train->get_priority()==priority)
 913                                                 blocking_train->yield_to(*this);
 914                                         blocking_train = 0;
 915                                         continue;
 916                                 }
 917                                 else
 918                                 {
 919                                         yield_to(*blocking_train);
 920                                         pending_block = contested_blocks.front().block();
 921                                         try_divert = divert_track;
 922                                         break;
 923                                 }
 924                         }
 925                         else
 926                         {
 927                                 contested_blocks.push_back(block);
 928                                 continue;
 929                         }
 930                 }
 931
 932                 bool reserved = block->reserve(this);
 933                 if(!reserved)
 934                 {
 935                         /* We've found another train.  If it wants to exit the block from the
 936                         same endpoint we're trying to enter from or the other way around,
 937                         treat it as coming towards us.  Otherwise treat it as going in the
 938                         same direction. */
 939                         Train *other_train = block->get_train();
 940                         int other_entry = other_train->get_entry_to_block(*block);
 941                         if(other_entry<0)
 942                                 throw LogicError("Block reservation inconsistency");
 943
 944                         unsigned exit = block.reverse().entry();
 945                         unsigned other_exit = BlockIter(block.block(), other_entry).reverse().entry();
 946                         bool entry_conflict = (block.entry()==other_exit);
 947                         bool exit_conflict = (exit==static_cast<unsigned>(other_entry));
 948                         if(!entry_conflict && !last->get_turnout_id())
 949                         {
 950                                 /* The other train is not coming to the blocks we're holding, so we
 951                                 can keep them. */
 952                                 good = last;
 953                                 good_sens = nsens;
 954                                 good_dist = dist;
 955                         }
 956
 957                         int other_prio = other_train->get_priority();
 958
 959                         if(!entry_conflict && !exit_conflict && other_prio<priority)
 960                         {
 961                                 /* Ask a lesser priority train going to the same direction to free
 962                                 the block for us */
 963                                 if(other_train->free_block(*block))
 964                                         reserved = block->reserve(this);
 965                         }
 966                         else if(other_train!=yielding_to && (other_prio<priority || (other_prio==priority && entry_conflict)))
 967                         {
 968                                 /* A lesser priority train is coming at us, we must ask it to free
 969                                 enough blocks to get clear of it to avoid a potential deadlock */
 970                                 blocking_train = other_train;
 971                                 contested_blocks.clear();
 972                                 contested_blocks.push_back(block);
 973                                 continue;
 974                         }
 975                         else if(divert_track && (entry_conflict || exit_conflict))
 976                                 // We are blocked, but there's a diversion possibility
 977                                 try_divert = true;
 978
 979                         if(!reserved)
 980                         {
 981                                 pending_block = &*block;
 982                                 break;
 983                         }
 984                 }
 985
 986                 if(block->get_turnout_id())
 987                 {
 988                         const TrackType::Endpoint &track_ep = track->get_type().get_endpoints()[track.entry()];
 989                         bool multiple_paths = (track_ep.paths&(track_ep.paths-1));
 990
 991                         if(multiple_paths || !last->get_turnout_id())
 992                         {
 993                                 /* We can keep the blocks reserved so far if we are facing the
 994                                 points or if there was no turnout immediately before this one.
 995                                 With multiple successive turnouts (as is common in crossovers) it's
 996                                 best to hold at one we can divert from. */
 997                                 good = last;
 998                                 good_sens = nsens;
 999                                 good_dist = dist;
1000                         }
1001
1002                         // Figure out what path we'd like to take on the turnout
1003                         int path = -1;
1004                         for(list<RouteRef>::iterator i=cur_route; (path<0 && i!=routes.end()); ++i)
1005                                 path = i->route->get_turnout(block->get_turnout_id());
1006                         if(path<0)
1007                                 path = track->get_active_path();
1008                         if(!(track_ep.paths&(1<<path)))
1009                         {
1010                                 for(unsigned i=0; track_ep.paths>>i; ++i)
1011                                         if(track_ep.paths&(1<<i))
1012                                                 path = i;
1013                         }
1014
1015                         if(path!=static_cast<int>(track->get_active_path()))
1016                         {
1017                                 // The turnout is set to wrong path - switch and wait for it
1018                                 pending_block = &*block;
1019                                 track->set_active_path(path);
1020                                 if(pending_block)
1021                                 {
1022                                         block->reserve(0);
1023                                         break;
1024                                 }
1025                         }
1026
1027                         if(multiple_paths && cur_route!=routes.end() && cur_route->diversion!=block->get_turnout_id())
1028                                 /* There's multiple paths to be taken and we are on a route - take
1029                                 note of the diversion possibility */
1030                                 divert_track = &*track;
1031                 }
1032
1033                 if(!contested_blocks.empty() && contested_blocks.front()==block)
1034                         contested_blocks.pop_front();
1035
1036                 rsv_blocks.push_back(block);
1037                 if(block->get_sensor_id())
1038                         ++nsens;
1039                 if(nsens>0)
1040                         dist += block->get_path_length(block.entry());
1041         }
1042
1043         // Unreserve blocks that were not good
1044         while(!rsv_blocks.empty() && rsv_blocks.back()!=good)
1045         {
1046                 rsv_blocks.back()->reserve(0);
1047                 rsv_blocks.pop_back();
1048         }
1049
1050         if(!rsv_blocks.empty() && rsv_blocks.back()!=start)
1051                 // We got some new blocks, so no longer need to yield
1052                 yielding_to = 0;
1053
1054         // Make any sensorless blocks at the beginning immediately current
1055         BlockList::iterator i;
1056         for(i=rsv_blocks.begin(); (i!=rsv_blocks.end() && !(*i)->get_sensor_id()); ++i) ;
1057         if(i!=rsv_blocks.begin())
1058                 cur_blocks.splice(cur_blocks.end(), rsv_blocks, rsv_blocks.begin(), i);
1059
1060         if(try_divert && divert(*divert_track))
1061                 return reserve_more();
1062
1063         return good_sens;
1064 }
1065
1066 float Train::get_reserved_distance_until(const Block *until_block, bool back) const
1067 {
1068         if(cur_blocks.empty())
1069                 return 0;
1070
1071         Vehicle &veh = *(reverse!=back ? vehicles.back() : vehicles.front());
1072         const VehicleType &vtype = veh.get_type();
1073
1074         TrackIter track(veh.get_track(), veh.get_entry());
1075         if(!track)
1076                 return 0;
1077
1078         BlockList::const_iterator block = cur_blocks.begin();
1079         while(block!=rsv_blocks.end() && !(*block)->has_track(*track))
1080         {
1081                 ++block;
1082                 if(block==cur_blocks.end())
1083                 {
1084                         if(back)
1085                                 return 0;
1086                         block = rsv_blocks.begin();
1087                 }
1088         }
1089         if(block==rsv_blocks.end() || &**block==until_block)
1090                 return 0;
1091
1092         float result = veh.get_offset();
1093         if(reverse!=back)
1094                 track = track.reverse();
1095         else
1096                 result = track->get_type().get_path_length(track->get_active_path())-result;
1097         result -= vtype.get_length()/2;
1098
1099         while(1)
1100         {
1101                 track = track.next();
1102                 if(!track)
1103                         break;
1104
1105                 if(!(*block)->has_track(*track))
1106                 {
1107                         if(back)
1108                         {
1109                                 if(block==cur_blocks.begin())
1110                                         break;
1111                                 --block;
1112                         }
1113                         else
1114                         {
1115                                 ++block;
1116                                 if(block==cur_blocks.end())
1117                                         block = rsv_blocks.begin();
1118                                 if(block==rsv_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(BlockList &blocks)
1209 {
1210         release_blocks(blocks, blocks.begin(), blocks.end());
1211 }
1212
1213 void Train::release_blocks(BlockList &blocks, BlockList::iterator begin, BlockList::iterator end)
1214 {
1215         while(begin!=end)
1216         {
1217                 Block &block = **begin;
1218                 blocks.erase(begin++);
1219                 block.reserve(0);
1220         }
1221 }
1222
1223 void Train::reverse_blocks(BlockList &blocks) const
1224 {
1225         blocks.reverse();
1226         for(BlockList::iterator i=blocks.begin(); i!=blocks.end(); ++i)
1227                 *i = i->reverse();
1228 }
1229
1230 bool Train::advance_route(list<RouteRef>::iterator &iter, Track &track)
1231 {
1232         while(iter!=routes.end() && !iter->route->has_track(track))
1233                 ++iter;
1234         if(iter==routes.end())
1235                 return false;
1236
1237         list<RouteRef>::iterator next = iter;
1238         ++next;
1239         if(next!=routes.end() && next->diversion && next->route->has_track(track))
1240                 iter = next;
1241
1242         return true;
1243 }
1244
1245 Route *Train::create_lead_route(Route *lead, const Route *target)
1246 {
1247         if(!lead)
1248         {
1249                 lead = new Route(layout);
1250                 lead->set_name("Lead");
1251                 lead->set_temporary(true);
1252         }
1253
1254         set<Track *> tracks;
1255         for(BlockList::iterator i=cur_blocks.begin(); i!=rsv_blocks.end(); )
1256         {
1257                 const set<Track *> &btracks = (*i)->get_tracks();
1258                 for(set<Track *>::const_iterator j=btracks.begin(); j!=btracks.end(); ++j)
1259                         if(!target || !target->has_track(**j))
1260                                 tracks.insert(*j);
1261
1262                 if(++i==cur_blocks.end())
1263                         i = rsv_blocks.begin();
1264         }
1265
1266         lead->add_tracks(tracks);
1267
1268         return lead;
1269 }
1270
1271 bool Train::is_valid_diversion(const Route &diversion, const TrackIter &from)
1272 {
1273         float diversion_len = 0;
1274         TrackLoopIter track1 = from;
1275         while(diversion.has_track(*track1))
1276         {
1277                 unsigned tid = track1->get_turnout_id();
1278                 unsigned path = (tid ? diversion.get_turnout(tid) : 0);
1279                 diversion_len += track1->get_type().get_path_length(path);
1280
1281                 track1 = track1.next(path);
1282
1283                 if(track1.looped())
1284                         return false;
1285         }
1286
1287         list<RouteRef>::iterator route = routes.begin();
1288         if(!advance_route(route, *from))
1289                 return false;
1290
1291         float route_len = 0;
1292         TrackLoopIter track2 = from;
1293         while(1)
1294         {
1295                 unsigned tid = track2->get_turnout_id();
1296                 unsigned path = (tid ? route->route->get_turnout(tid) : 0);
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.cur_blocks.empty())
1357         {
1358                 TrackIter track = obj.cur_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.cur_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