source/libmarklin/train.cpp

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