source/libr2c2/train.cpp

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