source/libr2c2/train.cpp

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