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