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