+
+ int entry = link->get_endpoint_by_link(*last->block);
+ if(entry<0)
+ throw LogicError("Block links are inconsistent!");
+
+ const Block::Endpoint &entry_ep = link->get_endpoints()[entry];
+
+ if(cur_route)
+ {
+ if(cur_route!=next_route && next_route && next_route->get_tracks().count(entry_ep.track))
+ cur_route = next_route;
+ else if(!cur_route->get_tracks().count(entry_ep.track))
+ {
+ // Keep the blocks if we arrived at the end of the route
+ if(!blocking_train)
+ {
+ good = last;
+ good_sens = nsens;
+ good_dist = dist;
+ end_of_route = true;
+ }
+ break;
+ }
+ }
+ else if(route && route->get_tracks().count(entry_ep.track))
+ cur_route = route;
+
+ if(link->get_endpoints().size()<2)
+ {
+ if(!blocking_train)
+ {
+ good = last;
+ good_sens = nsens;
+ good_dist = dist;
+ }
+ break;
+ }
+
+ if(blocking_train)
+ {
+ if(link->get_train()!=blocking_train)
+ {
+ // XXX is it possible that this won't free all the blocks we want?
+ if(blocking_train->free_block(*contested_blocks.back().block))
+ {
+ // Roll back and start actually reserving the blocks
+ last = &rsv_blocks.back();
+ if(blocking_train->get_priority()==priority)
+ blocking_train->yield_to(*this);
+ blocking_train = 0;
+ continue;
+ }
+ else
+ {
+ pending_block = contested_blocks.front().block;
+ break;
+ }
+ }
+ else
+ {
+ contested_blocks.push_back(BlockRef(link, entry));
+ last = &contested_blocks.back();
+ continue;
+ }
+ }
+
+ bool reserved = link->reserve(this);
+ if(!reserved)
+ {
+ /* We've found another train. If it wants to exit the block from the
+ same endpoint we're trying to enter from or the other way around,
+ treat it as coming towards us. Otherwise treat it as going in the
+ same direction. */
+ Train *other_train = link->get_train();
+ int other_entry = other_train->get_entry_to_block(*link);
+ if(other_entry<0)
+ throw LogicError("Block reservation inconsistency");
+
+ int other_prio = other_train->get_priority();
+
+ bool entry_conflict = (static_cast<unsigned>(entry)==link->traverse(other_entry));
+ bool exit_conflict = (link->traverse(entry)==static_cast<unsigned>(other_entry));
+ if(!entry_conflict && !exit_conflict)
+ {
+ /* Same direction, keep the blocks we got so far and wait for the
+ other train to pass */
+ good = last;
+ good_sens = nsens;
+ good_dist = dist;
+
+ // Ask a lesser priority train to free the block for us
+ if(other_train->get_priority()<priority)
+ if(other_train->free_block(*link))
+ reserved = link->reserve(this);
+ }
+ else if(other_prio<priority || (other_prio==priority && other_train!=yielding_to && entry_conflict))
+ {
+ /* A lesser priority train is coming at us, we must ask it to free
+ enough blocks to get clear of it to avoid a potential deadlock */
+ blocking_train = other_train;
+ contested_blocks.clear();
+ contested_blocks.push_back(BlockRef(link, entry));
+ last = &contested_blocks.back();
+ continue;
+ }
+
+ if(!reserved)
+ {
+ pending_block = link;
+ break;
+ }
+ }
+
+ if(link->get_turnout_id())
+ {
+ const Endpoint &track_ep = entry_ep.track->get_type().get_endpoints()[entry_ep.track_ep];
+
+ // Keep the blocks reserved so far, as either us or the other train can diverge
+ good = last;
+ good_sens = nsens;
+ good_dist = dist;
+
+ // Figure out what path we'd like to take on the turnout
+ int path = -1;
+ if(cur_route)
+ path = cur_route->get_turnout(link->get_turnout_id());
+ if(path<0)
+ path = entry_ep.track->get_active_path();
+ if(!((track_ep.paths>>path)&1))
+ {
+ for(unsigned i=0; track_ep.paths>>i; ++i)
+ if((track_ep.paths>>i)&1)
+ path = i;
+ }
+
+ if(path!=static_cast<int>(entry_ep.track->get_active_path()))
+ {
+ // The turnout is set to wrong path - switch and wait for it
+ pending_block = link;
+ entry_ep.track->set_active_path(path);
+ if(pending_block)
+ {
+ link->reserve(0);
+ break;
+ }
+ }
+ }
+
+ if(!contested_blocks.empty() && contested_blocks.front().block==link)
+ contested_blocks.pop_front();
+
+ rsv_blocks.push_back(BlockRef(link, entry));
+ last = &rsv_blocks.back();
+ if(last->block->get_sensor_id())
+ ++nsens;
+ if(nsens>0)
+ dist += length;
+ }
+
+ // Unreserve blocks that were not good
+ while(!rsv_blocks.empty() && &rsv_blocks.back()!=good)
+ {
+ rsv_blocks.back().block->reserve(0);
+ rsv_blocks.erase(--rsv_blocks.end());
+ }
+
+ if(!rsv_blocks.empty() && &rsv_blocks.back()!=start)
+ // We got some new blocks, so no longer need to yield
+ yielding_to = 0;
+
+ // Make any sensorless blocks at the beginning immediately current
+ list<BlockRef>::iterator i;
+ for(i=rsv_blocks.begin(); (i!=rsv_blocks.end() && !i->block->get_sensor_id()); ++i) ;
+ if(i!=rsv_blocks.begin())
+ cur_blocks.splice(cur_blocks.end(), rsv_blocks, rsv_blocks.begin(), i);
+
+ return good_sens;
+}
+
+float Train::get_reserved_distance_until(const Block *until_block, bool back) const
+{
+ if(cur_blocks.empty())
+ return 0;
+
+ Vehicle &veh = *(reverse!=back ? vehicles.back() : vehicles.front());
+ const VehicleType &vtype = veh.get_type();
+
+ Track *track = veh.get_track();
+ if(!track)
+ return 0;
+
+ list<BlockRef>::const_iterator block = cur_blocks.begin();
+ while(block!=rsv_blocks.end() && !block->block->get_tracks().count(track))
+ {
+ ++block;
+ if(block==cur_blocks.end())
+ {
+ if(back)
+ return 0;
+ block = rsv_blocks.begin();
+ }
+ }
+ if(block==rsv_blocks.end() || block->block==until_block)
+ return 0;
+
+ unsigned entry = veh.get_entry();
+
+ float result = veh.get_offset();
+ if(reverse!=back)
+ entry = track->traverse(entry);
+ else
+ result = track->get_type().get_path_length(track->get_active_path())-result;
+ result -= vtype.get_length()/2;
+
+ while(1)
+ {
+ if(track->get_type().get_endpoints().size()<2)
+ break;
+
+ Track *next = track->get_link(track->traverse(entry));
+
+ if(!block->block->get_tracks().count(next))
+ {
+ if(back)
+ {
+ if(block==cur_blocks.begin())
+ break;
+ --block;
+ }
+ else
+ {
+ ++block;
+ if(block==cur_blocks.end())
+ block = rsv_blocks.begin();
+ if(block==rsv_blocks.end())
+ break;
+ }
+
+ if(block->block==until_block)
+ break;
+ }
+
+ entry = next->get_endpoint_by_link(*track);
+ track = next;
+
+ result += track->get_type().get_path_length(track->get_active_path());