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