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                         cur_blocks_end = end;
 786
 787                         // Try to get more blocks if we're moving
 788                         if(active)
 789                                 reserve_more();
 790                 }
 791                 else if(result==3)
 792                         layout.emergency("Sensor for "+name+" triggered out of order");
 793         }
 794         else
 795         {
 796                 const Vehicle &veh = *(reverse ? vehicles.front() : vehicles.back());
 797
 798                 // Find the first sensor in our current blocks that's still active
 799                 BlockList::iterator end = blocks.begin();
 800                 for(BlockList::iterator i=blocks.begin(); i!=cur_blocks_end; ++i)
 801                 {
 802                         if((*i)->has_track(*veh.get_track()))
 803                                 break;
 804                         if((*i)->get_sensor_id())
 805                         {
 806                                 if(layout.get_driver().get_sensor((*i)->get_sensor_id()))
 807                                         break;
 808                                 else
 809                                 {
 810                                         end = i;
 811                                         ++end;
 812                                 }
 813                         }
 814                 }
 815
 816                 if(end!=blocks.begin() && end!=cur_blocks_end)
 817                         // Free blocks up to the last inactive sensor
 818                         release_blocks(blocks.begin(), end);
 819         }
 820 }
 821
 822 void Train::turnout_path_changed(Track &track)
 823 {
 824         for(list<BlockIter>::iterator i=blocks.begin(); i!=blocks.end(); ++i)
 825                 if((*i)->get_turnout_id()==track.get_turnout_id() && !reserving)
 826                         check_turnout_paths(false);
 827 }
 828
 829 void Train::halt_event(bool h)
 830 {
 831         if(h)
 832                 accurate_position = false;
 833 }
 834
 835 void Train::block_reserved(const Block &block, const Train *train)
 836 {
 837         if(&block==pending_block && !train && !reserving)
 838                 reserve_more();
 839 }
 840
 841 void Train::reserve_more()
 842 {
 843         if(!active || blocks.empty() || end_of_route)
 844                 return;
 845
 846         BlockIter start = blocks.back();
 847
 848         pending_block = 0;
 849         preceding_train = 0;
 850
 851         // See how many sensor blocks and how much track we already have
 852         unsigned nsens = 0;
 853         float dist = 0;
 854         for(BlockList::const_iterator i=cur_blocks_end; i!=blocks.end(); ++i)
 855         {
 856                 if((*i)->get_sensor_id())
 857                         ++nsens;
 858                 if(nsens>0)
 859                         dist += (*i)->get_path_length(i->entry());
 860         }
 861
 862         list<RouteRef>::iterator cur_route = routes.begin();
 863         advance_route(cur_route, *start.track_iter());
 864
 865         float approach_margin = 50*layout.get_catalogue().get_scale();
 866         float min_dist = controller->get_braking_distance()*1.3+approach_margin*2;
 867
 868         BlockIter block = start;
 869         list<BlockIter>::iterator good_end = blocks.end();
 870         Track *divert_track = 0;
 871         bool try_divert = false;
 872         Train *blocking_train = 0;
 873         BlockList contested_blocks;
 874
 875         SetFlag setf(reserving);
 876
 877         while(1)
 878         {
 879                 BlockIter last = block;
 880                 block = block.next(cur_route!=routes.end() ? cur_route->route : 0);
 881                 if(!block || block->get_endpoints().size()<2)
 882                 {
 883                         if(!blocking_train)
 884                         {
 885                                 good_end = blocks.end();
 886                                 end_of_route = true;
 887                         }
 888                         break;
 889                 }
 890
 891                 TrackIter track = block.track_iter();
 892
 893                 if(cur_route!=routes.end())
 894                 {
 895                         if(!advance_route(cur_route, *track))
 896                         {
 897                                 // Keep the blocks if we arrived at the end of the route
 898                                 if(!blocking_train)
 899                                 {
 900                                         good_end = blocks.end();
 901                                         end_of_route = true;
 902                                 }
 903                                 break;
 904                         }
 905                 }
 906
 907                 if(block->get_turnout_id() && !last->get_turnout_id())
 908                 {
 909                         /* We can keep the blocks if we arrive at a turnout from a non-turnout
 910                         block.  Having a turnout block as our last reserved block is not good
 911                         as it would limit our diversion possibilities for little benefit. */
 912                         good_end = blocks.end();
 913                         if(nsens>=3 && dist>=min_dist)
 914                                 break;
 915                 }
 916
 917                 if(blocking_train)
 918                 {
 919                         if(block->get_train()!=blocking_train)
 920                         {
 921                                 if(blocking_train->free_block(*contested_blocks.back()))
 922                                 {
 923                                         // Roll back and start actually reserving the blocks
 924                                         block = blocks.back();
 925                                         cur_route = routes.begin();
 926                                         advance_route(cur_route, *block.track_iter().track());
 927                                         if(blocking_train->get_priority()==priority)
 928                                                 blocking_train->yield_to(*this);
 929                                         blocking_train = 0;
 930                                         continue;
 931                                 }
 932                                 else
 933                                 {
 934                                         yield_to(*blocking_train);
 935                                         pending_block = contested_blocks.front().block();
 936                                         try_divert = divert_track;
 937                                         break;
 938                                 }
 939                         }
 940                         else
 941                         {
 942                                 contested_blocks.push_back(block);
 943                                 continue;
 944                         }
 945                 }
 946
 947                 bool reserved = block->reserve(this);
 948                 if(!reserved)
 949                 {
 950                         /* We've found another train.  If it wants to exit the block from the
 951                         same endpoint we're trying to enter from or the other way around,
 952                         treat it as coming towards us.  Otherwise treat it as going in the
 953                         same direction. */
 954                         Train *other_train = block->get_train();
 955                         int other_entry = other_train->get_entry_to_block(*block);
 956                         if(other_entry<0)
 957                                 throw LogicError("Block reservation inconsistency");
 958
 959                         unsigned exit = block.reverse().entry();
 960                         unsigned other_exit = BlockIter(block.block(), other_entry).reverse().entry();
 961                         bool entry_conflict = (block.entry()==other_exit);
 962                         bool exit_conflict = (exit==static_cast<unsigned>(other_entry));
 963                         if(!entry_conflict && !last->get_turnout_id())
 964                         {
 965                                 /* The other train is not coming to the blocks we're holding, so we
 966                                 can keep them. */
 967                                 good_end = blocks.end();
 968
 969                                 if(static_cast<unsigned>(other_entry)==block.entry())
 970                                         preceding_train = other_train;
 971                         }
 972
 973                         int other_prio = other_train->get_priority();
 974
 975                         if(!entry_conflict && !exit_conflict && other_prio<priority)
 976                         {
 977                                 /* Ask a lesser priority train going to the same direction to free
 978                                 the block for us */
 979                                 if(other_train->free_block(*block))
 980                                         reserved = block->reserve(this);
 981                         }
 982                         else if(other_train!=yielding_to && (other_prio<priority || (other_prio==priority && entry_conflict)))
 983                         {
 984                                 /* A lesser priority train is coming at us, we must ask it to free
 985                                 enough blocks to get clear of it to avoid a potential deadlock */
 986                                 blocking_train = other_train;
 987                                 contested_blocks.clear();
 988                                 contested_blocks.push_back(block);
 989                                 continue;
 990                         }
 991                         else if(divert_track && (entry_conflict || exit_conflict || !other_train->is_active()))
 992                                 // We are blocked, but there's a diversion possibility
 993                                 try_divert = true;
 994
 995                         if(!reserved)
 996                         {
 997                                 pending_block = &*block;
 998                                 break;
 999                         }
1000                 }
1001
1002                 if(block->get_turnout_id())
1003                 {
1004                         const TrackType::Endpoint &track_ep = track.endpoint();
1005                         bool multiple_paths = (track_ep.paths&(track_ep.paths-1));
1006
1007                         if(multiple_paths && cur_route!=routes.end() && cur_route->diversion!=block->get_turnout_id())
1008                                 /* There's multiple paths to be taken and we are on a route - take
1009                                 note of the diversion possibility */
1010                                 divert_track = &*track;
1011                 }
1012
1013                 if(!contested_blocks.empty() && contested_blocks.front()==block)
1014                         contested_blocks.pop_front();
1015
1016                 blocks.push_back(block);
1017
1018                 if(cur_blocks_end==blocks.end())
1019                         --cur_blocks_end;
1020                 if(clear_blocks_end==blocks.end())
1021                         --clear_blocks_end;
1022                 if(good_end==blocks.end())
1023                         --good_end;
1024
1025                 if(block->get_sensor_id())
1026                         ++nsens;
1027                 if(nsens>0)
1028                         dist += block->get_path_length(block.entry());
1029         }
1030
1031         // Unreserve blocks that were not good
1032         release_blocks(good_end, blocks.end());
1033
1034         if(blocks.back()!=start)
1035                 // We got some new blocks, so no longer need to yield
1036                 yielding_to = 0;
1037
1038         check_turnout_paths(true);
1039
1040         // Make any sensorless blocks at the beginning immediately current
1041         while(cur_blocks_end!=clear_blocks_end && !(*cur_blocks_end)->get_sensor_id())
1042                 ++cur_blocks_end;
1043
1044         if(try_divert && divert(*divert_track))
1045                 reserve_more();
1046 }
1047
1048 void Train::check_turnout_paths(bool set)
1049 {
1050         if(clear_blocks_end==blocks.end())
1051                 return;
1052
1053         for(list<BlockIter>::iterator i=clear_blocks_end; i!=blocks.end(); ++i)
1054         {
1055                 if((*i)->get_turnout_id())
1056                 {
1057                         TrackIter track = i->track_iter();
1058                         const TrackType::Endpoint &track_ep = track.endpoint();
1059
1060                         unsigned path = 0;
1061                         list<BlockIter>::iterator j = i;
1062                         if(++j!=blocks.end())
1063                         {
1064                                 TrackIter rev = j->track_iter().flip();
1065                                 unsigned mask = rev.endpoint().paths&track_ep.paths;
1066                                 for(path=0; mask>1; mask>>=1, ++path) ;
1067                         }
1068                         else
1069                                 return;
1070
1071                         if(path!=track->get_active_path())
1072                         {
1073                                 if(set)
1074                                         track->set_active_path(path);
1075
1076                                 /* Check again, in case the driver was able to service the request
1077                                 instantly */
1078                                 if(!set || path!=track->get_active_path())
1079                                         continue;
1080                         }
1081                 }
1082
1083                 if(i==clear_blocks_end)
1084                         ++clear_blocks_end;
1085         }
1086 }
1087
1088 float Train::get_reserved_distance_until(const Block *until_block, bool back) const
1089 {
1090         if(blocks.empty())
1091                 return 0;
1092
1093         Vehicle &veh = *(reverse!=back ? vehicles.back() : vehicles.front());
1094         const VehicleType &vtype = veh.get_type();
1095
1096         TrackIter track(veh.get_track(), veh.get_entry());
1097         if(!track)  // XXX Probably unnecessary
1098                 return 0;
1099
1100         BlockList::const_iterator block = blocks.begin();
1101         while(block!=clear_blocks_end && !(*block)->has_track(*track))
1102                 ++block;
1103         if(block==clear_blocks_end || &**block==until_block)
1104                 return 0;
1105
1106         float result = veh.get_offset();
1107         if(reverse!=back)
1108                 track = track.reverse();
1109         else
1110                 result = track->get_type().get_path_length(track->get_active_path())-result;
1111         result -= vtype.get_length()/2;
1112
1113         while(1)
1114         {
1115                 track = track.next();
1116                 if(!track)
1117                         break;
1118
1119                 if(!(*block)->has_track(*track))
1120                 {
1121                         if(back)
1122                         {
1123                                 if(block==blocks.begin())
1124                                         break;
1125                                 --block;
1126                         }
1127                         else
1128                         {
1129                                 ++block;
1130                                 if(block==clear_blocks_end)
1131                                         break;
1132                         }
1133
1134                         if(&**block==until_block)
1135                                 break;
1136                 }
1137
1138                 result += track->get_type().get_path_length(track->get_active_path());
1139         }
1140
1141         return result;
1142 }
1143
1144 float Train::get_real_speed(unsigned i) const
1145 {
1146         if(i==0)
1147                 return 0;
1148         if(real_speed[i].weight)
1149                 return real_speed[i].speed;
1150
1151         unsigned low;
1152         unsigned high;
1153         for(low=i; low>0; --low)
1154                 if(real_speed[low].weight)
1155                         break;
1156         for(high=i; high+1<real_speed.size(); ++high)
1157                 if(real_speed[high].weight)
1158                         break;
1159
1160         if(real_speed[high].weight)
1161         {
1162                 if(real_speed[low].weight)
1163                 {
1164                         float f = float(i-low)/(high-low);
1165                         return real_speed[low].speed*(1-f)+real_speed[high].speed*f;
1166                 }
1167                 else
1168                         return real_speed[high].speed*float(i)/high;
1169         }
1170         else if(real_speed[low].weight)
1171                 return real_speed[low].speed*float(i)/low;
1172         else
1173                 return 0;
1174 }
1175
1176 unsigned Train::find_speed_step(float real) const
1177 {
1178         if(real_speed.size()<=1)
1179                 return 0;
1180         if(real<=real_speed[1].speed*0.5)
1181                 return 0;
1182
1183         unsigned low = 0;
1184         unsigned high = 0;
1185         unsigned last = 0;
1186         for(unsigned i=0; (!high && i<real_speed.size()); ++i)
1187                 if(real_speed[i].weight)
1188                 {
1189                         last = i;
1190                         if(real_speed[i].speed>=real)
1191                                 high = i;
1192                         else if(real_speed[i].speed>real_speed[low].speed)
1193                                 low = i;
1194                 }
1195         if(!high)
1196         {
1197                 unsigned limit = real_speed.size()/5;
1198                 if(!low)
1199                 {
1200                         if(real)
1201                                 return limit;
1202                         else
1203                                 return 0;
1204                 }
1205                 return min(min(static_cast<unsigned>(low*real/real_speed[low].speed), real_speed.size()-1), last+limit);
1206         }
1207
1208         float f = (real-real_speed[low].speed)/(real_speed[high].speed-real_speed[low].speed);
1209         return static_cast<unsigned>(low*(1-f)+high*f+0.5);
1210 }
1211
1212 float Train::get_travel_speed() const
1213 {
1214         float speed = get_real_speed(current_speed_step);
1215         float scale = layout.get_catalogue().get_scale();
1216         return static_cast<int>(round(speed/scale*3.6/5))*5;
1217 }
1218
1219 void Train::set_status(const string &s)
1220 {
1221         status = s;
1222         signal_status_changed.emit(s);
1223 }
1224
1225 void Train::release_blocks()
1226 {
1227         release_blocks(blocks.begin(), blocks.end());
1228 }
1229
1230 void Train::release_blocks(BlockList::iterator begin, BlockList::iterator end)
1231 {
1232         while(begin!=end)
1233         {
1234                 if(begin==cur_blocks_end)
1235                         cur_blocks_end = end;
1236                 if(begin==clear_blocks_end)
1237                         clear_blocks_end = end;
1238
1239                 Block &block = **begin;
1240                 blocks.erase(begin++);
1241                 block.reserve(0);
1242
1243                 if(begin==blocks.end())
1244                         end_of_route = false;
1245         }
1246 }
1247
1248 void Train::reverse_blocks(BlockList &blks) const
1249 {
1250         blks.reverse();
1251         for(BlockList::iterator i=blks.begin(); i!=blks.end(); ++i)
1252                 *i = i->reverse();
1253 }
1254
1255 bool Train::advance_route(list<RouteRef>::iterator &iter, Track &track)
1256 {
1257         while(iter!=routes.end() && !iter->route->has_track(track))
1258                 ++iter;
1259         if(iter==routes.end())
1260                 return false;
1261
1262         list<RouteRef>::iterator next = iter;
1263         ++next;
1264         if(next!=routes.end() && next->diversion && next->route->has_track(track))
1265                 iter = next;
1266
1267         return true;
1268 }
1269
1270 Route *Train::create_lead_route(Route *lead, const Route *target)
1271 {
1272         if(!lead)
1273         {
1274                 lead = new Route(layout);
1275                 lead->set_name("Lead");
1276                 lead->set_temporary(true);
1277         }
1278
1279         set<Track *> tracks;
1280         for(BlockList::iterator i=blocks.begin(); i!=blocks.end(); ++i)
1281         {
1282                 const set<Track *> &btracks = (*i)->get_tracks();
1283                 for(set<Track *>::const_iterator j=btracks.begin(); j!=btracks.end(); ++j)
1284                         if(!target || !target->has_track(**j))
1285                                 tracks.insert(*j);
1286         }
1287
1288         lead->add_tracks(tracks);
1289
1290         return lead;
1291 }
1292
1293 bool Train::is_valid_diversion(const Route &diversion, const TrackIter &from)
1294 {
1295         float diversion_len = 0;
1296         TrackLoopIter track1 = from;
1297         while(diversion.has_track(*track1))
1298         {
1299                 unsigned path = diversion.get_path(*track1);
1300                 diversion_len += track1->get_type().get_path_length(path);
1301
1302                 track1 = track1.next(path);
1303
1304                 if(!track1 || track1.looped())
1305                         return false;
1306         }
1307
1308         list<RouteRef>::iterator route = routes.begin();
1309         if(!advance_route(route, *from))
1310                 return false;
1311
1312         float route_len = 0;
1313         TrackLoopIter track2 = from;
1314         while(1)
1315         {
1316                 unsigned path = route->route->get_path(*track2);
1317                 route_len += track2->get_type().get_path_length(path);
1318
1319                 bool ok = (track2!=from && diversion.has_track(*track2));
1320
1321                 track2 = track2.next(path);
1322                 if(!track2)
1323                         return false;
1324
1325                 if(ok)
1326                         break;
1327
1328                 if(track2.looped())
1329                         return false;
1330
1331                 if(!advance_route(route, *track2))
1332                         return false;
1333         }
1334
1335         // Must end up at the same place through both routes
1336         if(track2!=track1)
1337                 return false;
1338
1339         return diversion_len<route_len*1.2;
1340 }
1341
1342
1343 Train::RouteRef::RouteRef(const Route *r, unsigned d):
1344         route(r),
1345         diversion(d)
1346 { }
1347
1348
1349 Train::RealSpeed::RealSpeed():
1350         speed(0),
1351         weight(0)
1352 { }
1353
1354 void Train::RealSpeed::add(float s, float w)
1355 {
1356         speed = (speed*weight+s*w)/(weight+w);
1357         weight = min(weight+w, 300.0f);
1358 }
1359
1360
1361 Train::Loader::Loader(Train &t):
1362         DataFile::BasicLoader<Train>(t),
1363         prev_block(0),
1364         blocks_valid(true)
1365 {
1366         add("block",       &Loader::block);
1367         add("block_hint",  &Loader::block_hint);
1368         add("name",        &Loader::name);
1369         add("priority",    &Train::priority);
1370         add("real_speed",  &Loader::real_speed);
1371         add("route",       &Loader::route);
1372         add("timetable",   &Loader::timetable);
1373         add("vehicle",     &Loader::vehicle);
1374 }
1375
1376 void Train::Loader::finish()
1377 {
1378         if(!obj.blocks.empty())
1379         {
1380                 TrackIter track = obj.blocks.front().track_iter();
1381                 float offset = 2*obj.layout.get_catalogue().get_scale();
1382                 obj.vehicles.back()->place(*track, track.entry(), offset, Vehicle::BACK_BUFFER);
1383
1384                 obj.set_status("Stopped");
1385         }
1386 }
1387
1388 void Train::Loader::block(unsigned id)
1389 {
1390         if(!blocks_valid)
1391                 return;
1392
1393         Block *blk;
1394         try
1395         {
1396                 blk = &obj.layout.get_block(id);
1397         }
1398         catch(const KeyError &)
1399         {
1400                 blocks_valid = false;
1401                 return;
1402         }
1403
1404         int entry = -1;
1405         if(prev_block)
1406                 entry = blk->get_endpoint_by_link(*prev_block);
1407         if(entry<0)
1408                 entry = 0;
1409
1410         blk->reserve(&obj);
1411         obj.blocks.push_back(BlockIter(blk, entry));
1412
1413         if(blk->get_sensor_id())
1414                 obj.layout.get_driver().set_sensor(blk->get_sensor_id(), true);
1415
1416         prev_block = blk;
1417 }
1418
1419 void Train::Loader::block_hint(unsigned id)
1420 {
1421         try
1422         {
1423                 prev_block = &obj.layout.get_block(id);
1424         }
1425         catch(const KeyError &)
1426         {
1427                 blocks_valid = false;
1428         }
1429 }
1430
1431 void Train::Loader::name(const string &n)
1432 {
1433         obj.set_name(n);
1434 }
1435
1436 void Train::Loader::real_speed(unsigned i, float speed, float weight)
1437 {
1438         if(i>=obj.real_speed.size())
1439                 return;
1440         obj.real_speed[i].speed = speed;
1441         obj.real_speed[i].weight = weight;
1442 }
1443
1444 void Train::Loader::route(const string &n)
1445 {
1446         obj.set_route(&obj.layout.get_route(n));
1447 }
1448
1449 void Train::Loader::timetable()
1450 {
1451         if(obj.timetable)
1452                 throw InvalidState("A timetable has already been loaded");
1453
1454         obj.timetable = new Timetable(obj);
1455         load_sub(*obj.timetable);
1456 }
1457
1458 void Train::Loader::vehicle(ArticleNumber art_nr)
1459 {
1460         const VehicleType &vtype = obj.layout.get_catalogue().get_vehicle(art_nr);
1461         Vehicle *veh = new Vehicle(obj.layout, vtype);
1462         obj.vehicles.back()->attach_back(*veh);
1463         obj.vehicles.push_back(veh);
1464 }
1465
1466 } // namespace R2C2