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