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