source/libmarklin/train.cpp

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