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