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