source/libr2c2/train.cpp

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