DirectNeighborsPerception.java
package org.opentrafficsim.road.gtu.lane.perception.categories;
import java.util.Collection;
import java.util.Comparator;
import java.util.ConcurrentModificationException;
import java.util.HashMap;
import java.util.Iterator;
import java.util.LinkedHashSet;
import java.util.Map;
import java.util.Set;
import java.util.SortedMap;
import java.util.SortedSet;
import java.util.TreeMap;
import java.util.TreeSet;
import org.djunits.value.vdouble.scalar.Length;
import org.opentrafficsim.base.TimeStampedObject;
import org.opentrafficsim.base.parameters.ParameterException;
import org.opentrafficsim.base.parameters.ParameterTypeLength;
import org.opentrafficsim.base.parameters.ParameterTypes;
import org.opentrafficsim.core.gtu.GTUException;
import org.opentrafficsim.core.gtu.RelativePosition;
import org.opentrafficsim.core.gtu.Try;
import org.opentrafficsim.core.network.LateralDirectionality;
import org.opentrafficsim.core.network.NetworkException;
import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
import org.opentrafficsim.road.gtu.lane.perception.DownstreamNeighborsIterable;
import org.opentrafficsim.road.gtu.lane.perception.LanePerception;
import org.opentrafficsim.road.gtu.lane.perception.LaneStructureRecord;
import org.opentrafficsim.road.gtu.lane.perception.PerceptionCollectable;
import org.opentrafficsim.road.gtu.lane.perception.RelativeLane;
import org.opentrafficsim.road.gtu.lane.perception.UpstreamNeighborsIterable;
import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayGTU;
import nl.tudelft.simulation.language.Throw;
/**
* Perception of surrounding traffic on the own road, i.e. without crossing traffic.
* <p>
* Copyright (c) 2013-2018 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
* BSD-style license. See <a href="http://opentrafficsim.org/docs/current/license.html">OpenTrafficSim License</a>.
* <p>
* @version $Revision$, $LastChangedDate$, by $Author$, initial version Jul 22, 2016 <br>
* @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
* @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
* @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
*/
public class DirectNeighborsPerception extends LaneBasedAbstractPerceptionCategory implements NeighborsPerception
{
/** */
private static final long serialVersionUID = 20160811L;
/** Look ahead parameter type. */
protected static final ParameterTypeLength LOOKAHEAD = ParameterTypes.LOOKAHEAD;
/** Look back parameter type. */
protected static final ParameterTypeLength LOOKBACK = ParameterTypes.LOOKBACK;
/** Set of followers per relative lane. */
private final Map<RelativeLane, TimeStampedObject<PerceptionCollectable<HeadwayGTU, LaneBasedGTU>>> followers =
new HashMap<>();
/** Set of leaders per relative lane. */
private final Map<RelativeLane, TimeStampedObject<PerceptionCollectable<HeadwayGTU, LaneBasedGTU>>> leaders =
new HashMap<>();
/** Set of first followers per lane upstream of merge per lateral direction, i.e. in the left or right lane. */
private final Map<LateralDirectionality, TimeStampedObject<SortedSet<HeadwayGTU>>> firstFollowers = new HashMap<>();
/** Set of first leaders per lane downstream of split per lateral direction, i.e. in the left or right lane. */
private final Map<LateralDirectionality, TimeStampedObject<SortedSet<HeadwayGTU>>> firstLeaders = new HashMap<>();
/** Whether a GTU is alongside per lateral direction, i.e. in the left or right lane. */
private final Map<LateralDirectionality, TimeStampedObject<Boolean>> gtuAlongside = new HashMap<>();
/** Headway GTU type that should be used. */
private final HeadwayGtuType headwayGtuType;
/**
* @param perception perception
* @param headwayGtuType type of headway gtu to generate
*/
public DirectNeighborsPerception(final LanePerception perception, final HeadwayGtuType headwayGtuType)
{
super(perception);
this.headwayGtuType = headwayGtuType;
}
/** {@inheritDoc} */
@Override
public final void updateAll() throws GTUException, NetworkException, ParameterException
{
this.firstLeaders.clear();
this.firstFollowers.clear();
this.gtuAlongside.clear();
if (getPerception().getLaneStructure().getExtendedCrossSection().contains(RelativeLane.LEFT))
{
updateFirstLeaders(LateralDirectionality.LEFT);
updateFirstFollowers(LateralDirectionality.LEFT);
updateGtuAlongside(LateralDirectionality.LEFT);
}
if (getPerception().getLaneStructure().getExtendedCrossSection().contains(RelativeLane.RIGHT))
{
updateFirstLeaders(LateralDirectionality.RIGHT);
updateFirstFollowers(LateralDirectionality.RIGHT);
updateGtuAlongside(LateralDirectionality.RIGHT);
}
this.leaders.clear();
this.followers.clear();
for (RelativeLane lane : getPerception().getLaneStructure().getExtendedCrossSection())
{
updateLeaders(lane);
updateFollowers(lane);
}
}
/** {@inheritDoc} */
@Override
public final void updateFirstLeaders(final LateralDirectionality lat)
throws ParameterException, GTUException, NetworkException
{
checkLateralDirectionality(lat);
SortedSet<HeadwayGTU> headwaySet =
new SortedNeighborsSet(getFirstDownstreamGTUs(lat, RelativePosition.FRONT, RelativePosition.REAR),
this.headwayGtuType, getGtu(), true);
this.firstLeaders.put(lat, new TimeStampedObject<>(headwaySet, getTimestamp()));
}
/** {@inheritDoc} */
@Override
public final void updateFirstFollowers(final LateralDirectionality lat)
throws GTUException, ParameterException, NetworkException
{
checkLateralDirectionality(lat);
SortedSet<HeadwayGTU> headwaySet = new SortedNeighborsSet(
getFirstUpstreamGTUs(lat, RelativePosition.REAR, RelativePosition.FRONT), this.headwayGtuType, getGtu(), false);
this.firstFollowers.put(lat, new TimeStampedObject<>(headwaySet, getTimestamp()));
}
/** {@inheritDoc} */
@Override
public final void updateGtuAlongside(final LateralDirectionality lat) throws GTUException, ParameterException
{
checkLateralDirectionality(lat);
// check if any GTU is downstream of the rear, within the vehicle length
SortedSet<DistanceGTU> headwaySet = getFirstDownstreamGTUs(lat, RelativePosition.REAR, RelativePosition.FRONT);
if (!headwaySet.isEmpty() && headwaySet.first().getDistance().le0())
{
this.gtuAlongside.put(lat, new TimeStampedObject<>(true, getTimestamp()));
return;
}
// check if any GTU is upstream of the front, within the vehicle length
headwaySet = getFirstUpstreamGTUs(lat, RelativePosition.FRONT, RelativePosition.REAR);
if (!headwaySet.isEmpty() && headwaySet.first().getDistance().le0())
{
this.gtuAlongside.put(lat, new TimeStampedObject<>(true, getTimestamp()));
return;
}
// no such GTU
this.gtuAlongside.put(lat, new TimeStampedObject<>(false, getTimestamp()));
}
/**
* Returns a set of first leaders per branch, relative to given relative position. Helper method to find first leaders and
* GTU's alongside.
* @param lat LEFT or RIGHT
* @param egoRelativePosition position of GTU to start search from
* @param otherRelativePosition position of other GTU
* @return set of first leaders per branch
* @throws GTUException if the GTU was not initialized
* @throws ParameterException if a parameter was not present or out of bounds
*/
private SortedSet<DistanceGTU> getFirstDownstreamGTUs(final LateralDirectionality lat,
final RelativePosition.TYPE egoRelativePosition, final RelativePosition.TYPE otherRelativePosition)
throws GTUException, ParameterException
{
SortedSet<DistanceGTU> headwaySet = new TreeSet<>();
Set<LaneStructureRecord> currentSet = new LinkedHashSet<>();
Set<LaneStructureRecord> nextSet = new LinkedHashSet<>();
LaneStructureRecord record = getPerception().getLaneStructure().getFirstRecord(new RelativeLane(lat, 1));
Length dxSearch = getGtu().getRelativePositions().get(egoRelativePosition).getDx();
Length dxHeadway = getGtu().getFront().getDx();
branchUpstream(record, dxSearch, currentSet);
// move downstream over branches as long as no vehicles are found
while (!currentSet.isEmpty())
{
Iterator<LaneStructureRecord> iterator = currentSet.iterator();
while (iterator.hasNext())
{
record = iterator.next();
/*-
* _ _ _ ______________________ _ _ _
* _|___ |
* find any vehicle downstream of this point on lane A | |__o__| A |
* _ _ _ ___________|_______|__ _ _ _
* (--------) negative distance
*/
LaneBasedGTU down = record.getLane().getGtuAhead(record.getStartDistance().neg().plus(dxSearch),
record.getDirection(), otherRelativePosition, getTimestamp());
if (down != null)
{
// GTU found, add to set
// headwaySet.add(this.headwayGtuType.createHeadwayGtu(getGtu(), down,
// record.getStartDistance().plus(down.position(record.getLane(), down.getRear())).minus(dxHeadway),
// true));
headwaySet.add(new DistanceGTU(down,
record.getStartDistance().plus(down.position(record.getLane(), down.getRear())).minus(dxHeadway)));
}
else
{
// no GTU found, search on next lanes in next loop and maintain cumulative length
for (LaneStructureRecord next : record.getNext())
{
nextSet.add(next);
}
}
}
currentSet = nextSet;
nextSet = new LinkedHashSet<>();
}
return headwaySet;
}
/**
* Returns a set of lanes to start from for a downstream search, upstream of the reference lane if the tail is before this
* lane.
* @param record start record
* @param dx distance between reference point and point to search from
* @param set set of lanes that is recursively built up, starting with the reference record
*/
private void branchUpstream(final LaneStructureRecord record, final Length dx, final Set<LaneStructureRecord> set)
{
Length pos = record.getStartDistance().neg().minus(dx);
if (pos.lt0() && !record.getPrev().isEmpty())
{
for (LaneStructureRecord prev : record.getPrev())
{
branchUpstream(prev, dx, set);
}
}
else
{
set.add(record);
}
}
/**
* Returns a set of first followers per branch, relative to given relative position. Helper method to find first followers
* and GTU's alongside.
* @param lat LEFT or RIGHT
* @param egoRelativePosition position of GTU to start search from
* @param otherRelativePosition position of other GTU
* @return set of first followers per branch
* @throws GTUException if the GTU was not initialized
* @throws ParameterException if a parameter was not present or out of bounds
*/
private SortedSet<DistanceGTU> getFirstUpstreamGTUs(final LateralDirectionality lat,
final RelativePosition.TYPE egoRelativePosition, final RelativePosition.TYPE otherRelativePosition)
throws GTUException, ParameterException
{
SortedSet<DistanceGTU> headwaySet = new TreeSet<>();
Set<LaneStructureRecord> currentSet = new LinkedHashSet<>();
Set<LaneStructureRecord> prevSet = new LinkedHashSet<>();
LaneStructureRecord record = getPerception().getLaneStructure().getFirstRecord(new RelativeLane(lat, 1));
Length dxSearch = getGtu().getRelativePositions().get(egoRelativePosition).getDx();
Length dxHeadway = getGtu().getRear().getDx();
branchDownstream(record, dxSearch, currentSet);
// move upstream over branches as long as no vehicles are found
while (!currentSet.isEmpty())
{
Iterator<LaneStructureRecord> iterator = currentSet.iterator();
while (iterator.hasNext())
{
record = iterator.next();
/*-
* _ _ _ ______________________ _ _ _
* | ___|_
* | A |__o__| | find any upstream of this point on lane A
* _ _ _ __|_______|___________ _ _ _
* (----------------) distance
*/
LaneBasedGTU up = record.getLane().getGtuBehind(record.getStartDistance().neg().plus(dxSearch),
record.getDirection(), otherRelativePosition, getTimestamp());
if (up != null)
{
// GTU found, add to set
// headwaySet.add(this.headwayGtuType.createHeadwayGtu(getGtu(), up,
// record.getStartDistance().neg().minus(up.position(record.getLane(), up.getFront())).plus(dxHeadway),
// false));
headwaySet.add(new DistanceGTU(up, record.getStartDistance().neg()
.minus(up.position(record.getLane(), up.getFront())).plus(dxHeadway)));
}
else
{
// no GTU found, search on next lanes in next loop and maintain cumulative length
for (LaneStructureRecord prev : record.getPrev())
{
prevSet.add(prev);
}
}
}
currentSet = prevSet;
prevSet = new LinkedHashSet<>();
}
return headwaySet;
}
/**
* Returns a set of lanes to start from for an upstream search, downstream of the reference lane if the front is after this
* lane.
* @param record start record
* @param dx distance between reference point and point to search from
* @param set set of lanes that is recursively built up, starting with the reference record
*/
private void branchDownstream(final LaneStructureRecord record, final Length dx, final Set<LaneStructureRecord> set)
{
Length pos = record.getStartDistance().neg().plus(dx);
if (pos.gt(record.getLane().getLength()))
{
for (LaneStructureRecord next : record.getNext())
{
branchDownstream(next, dx, set);
}
}
else
{
set.add(record);
}
}
/** {@inheritDoc} */
@Override
public final void updateLeaders(final RelativeLane lane) throws ParameterException, GTUException, NetworkException
{
Throw.whenNull(lane, "Lane may not be null.");
LaneStructureRecord record = getPerception().getLaneStructure().getFirstRecord(lane);
Length pos = record.getStartDistance().neg();
pos = record.getDirection().isPlus() ? pos.plus(getGtu().getFront().getDx()) : pos.minus(getGtu().getFront().getDx());
boolean ignoreIfUpstream = true;
PerceptionCollectable<HeadwayGTU, LaneBasedGTU> it = new DownstreamNeighborsIterable(getGtu(), record,
Length.max(Length.ZERO, pos), getGtu().getParameters().getParameter(LOOKAHEAD), getGtu().getFront(),
this.headwayGtuType, getGtu(), lane, ignoreIfUpstream);
this.leaders.put(lane, new TimeStampedObject<>(it, getTimestamp()));
}
/** {@inheritDoc} */
@Override
public final void updateFollowers(final RelativeLane lane) throws GTUException, NetworkException, ParameterException
{
Throw.whenNull(lane, "Lane may not be null.");
LaneStructureRecord record = getPerception().getLaneStructure().getFirstRecord(lane);
Length pos = record.getStartDistance().neg();
pos = record.getDirection().isPlus() ? pos.plus(getGtu().getFront().getDx()) : pos.minus(getGtu().getFront().getDx());
PerceptionCollectable<HeadwayGTU, LaneBasedGTU> it =
new UpstreamNeighborsIterable(getGtu(), record, Length.max(Length.ZERO, pos),
getGtu().getParameters().getParameter(LOOKBACK), getGtu().getRear(), this.headwayGtuType, lane);
this.followers.put(lane, new TimeStampedObject<>(it, getTimestamp()));
}
/** {@inheritDoc} */
@Override
public final SortedSet<HeadwayGTU> getFirstLeaders(final LateralDirectionality lat)
throws ParameterException, NullPointerException, IllegalArgumentException
{
checkLateralDirectionality(lat);
return this.firstLeaders.get(lat).getObject();
}
/** {@inheritDoc} */
@Override
public final SortedSet<HeadwayGTU> getFirstFollowers(final LateralDirectionality lat)
throws ParameterException, NullPointerException, IllegalArgumentException
{
checkLateralDirectionality(lat);
return getObjectOrNull(this.firstFollowers.get(lat));
}
/** {@inheritDoc} */
@Override
public final boolean isGtuAlongside(final LateralDirectionality lat)
throws ParameterException, NullPointerException, IllegalArgumentException
{
checkLateralDirectionality(lat);
return getObjectOrNull(this.gtuAlongside.get(lat));
}
/** {@inheritDoc} */
@Override
public final PerceptionCollectable<HeadwayGTU, LaneBasedGTU> getLeaders(final RelativeLane lane)
{
return getObjectOrNull(this.leaders.get(lane));
}
/** {@inheritDoc} */
@Override
public final PerceptionCollectable<HeadwayGTU, LaneBasedGTU> getFollowers(final RelativeLane lane)
{
return getObjectOrNull(this.followers.get(lane));
}
/**
* Set of leaders on a lane, which is usually 0 or 1, but possibly more in case of a downstream split with no intermediate
* GTU. This is shown below. Suppose A needs to go straight. If A considers a lane change to the left, both GTUs B (who's
* tail ~ is still on the straight lane) and C need to be considered for whether it's safe to do so. In case of multiple
* splits close to one another, the returned set may contain even more than 2 leaders. Leaders are sorted by headway value.
*
* <pre>
* | |
* _________/B/_____
* _ _?_ _ _~_ _C_ _
* _ _A_ _ _ _ _ _ _
* _________________
* </pre>
*
* @param lat LEFT or RIGHT
* @return list of followers on a lane
* @throws ParameterException if parameter is not defined
* @throws NullPointerException if {@code lat} is {@code null}
* @throws IllegalArgumentException if {@code lat} is {@code NONE}
*/
public final TimeStampedObject<SortedSet<HeadwayGTU>> getTimeStampedFirstLeaders(final LateralDirectionality lat)
throws ParameterException, NullPointerException, IllegalArgumentException
{
checkLateralDirectionality(lat);
return this.firstLeaders.get(lat);
}
/**
* Set of followers on a lane, which is usually 0 or 1, but possibly more in case of an upstream merge with no intermediate
* GTU. This is shown below. If A considers a lane change to the left, both GTUs B and C need to be considered for whether
* it's safe to do so. In case of multiple merges close to one another, the returned set may contain even more than 2
* followers. Followers are sorted by tailway value.
*
* <pre>
* | |
* |C|
* ________\ \______
* _ _B_|_ _ _ _ _?_
* _ _ _|_ _ _ _ _A_
* _____|___________
* </pre>
*
* @param lat LEFT or RIGHT
* @return list of followers on a lane
* @throws ParameterException if parameter is not defined
* @throws NullPointerException if {@code lat} is {@code null}
* @throws IllegalArgumentException if {@code lat} is {@code NONE}
*/
public final TimeStampedObject<SortedSet<HeadwayGTU>> getTimeStampedFirstFollowers(final LateralDirectionality lat)
throws ParameterException, NullPointerException, IllegalArgumentException
{
checkLateralDirectionality(lat);
return this.firstFollowers.get(lat);
}
/**
* Whether there is a GTU alongside, i.e. with overlap, in an adjacent lane.
* @param lat LEFT or RIGHT
* @return whether there is a GTU alongside, i.e. with overlap, in an adjacent lane
* @throws ParameterException if parameter is not defined
* @throws NullPointerException if {@code lat} is {@code null}
* @throws IllegalArgumentException if {@code lat} is {@code NONE}
*/
public final TimeStampedObject<Boolean> isGtuAlongsideTimeStamped(final LateralDirectionality lat)
throws ParameterException, NullPointerException, IllegalArgumentException
{
checkLateralDirectionality(lat);
return this.gtuAlongside.get(lat);
}
/**
* Set of leaders on a lane, including adjacent GTU's who's FRONT is ahead of the own vehicle FRONT. Leaders are sorted by
* headway value.
* @param lane relative lateral lane
* @return set of leaders on a lane, including adjacent GTU's who's FRONT is ahead of the own vehicle FRONT
*/
public final TimeStampedObject<PerceptionCollectable<HeadwayGTU, LaneBasedGTU>> getTimeStampedLeaders(
final RelativeLane lane)
{
return this.leaders.get(lane);
}
/**
* Set of followers on a lane, including adjacent GTU's who's REAR is back of the own vehicle REAR. Follower are are sorted
* by tailway value.
* @param lane relative lateral lane
* @return set of followers on a lane, including adjacent GTU's who's REAR is back of the own vehicle REAR
*/
public final TimeStampedObject<PerceptionCollectable<HeadwayGTU, LaneBasedGTU>> getTimeStampedFollowers(
final RelativeLane lane)
{
return this.followers.get(lane);
}
/**
* Checks that lateral directionality is either left or right and an existing lane.
* @param lat LEFT or RIGHT
* @throws ParameterException if parameter is not defined
* @throws NullPointerException if {@code lat} is {@code null}
* @throws IllegalArgumentException if {@code lat} is {@code NONE}
*/
private void checkLateralDirectionality(final LateralDirectionality lat)
throws ParameterException, NullPointerException, IllegalArgumentException
{
// TODO not use this check when synchronizing or cooperating
Throw.whenNull(lat, "Lateral directionality may not be null.");
Throw.when(lat.equals(LateralDirectionality.NONE), IllegalArgumentException.class,
"Lateral directionality may not be NONE.");
Throw.when(
(lat.equals(LateralDirectionality.LEFT)
&& !getPerception().getLaneStructure().getExtendedCrossSection().contains(RelativeLane.LEFT))
|| (lat.equals(LateralDirectionality.RIGHT)
&& !getPerception().getLaneStructure().getExtendedCrossSection().contains(RelativeLane.RIGHT)),
IllegalArgumentException.class, "Lateral directionality may only point to an existing adjacent lane.");
}
/** {@inheritDoc} */
@Override
public final String toString()
{
return "DirectNeighborsPesrception";
}
/**
* GTU at a distance, as preliminary info towards perceiving it. For instance, as a set from a search algorithm.
* <p>
* Copyright (c) 2013-2018 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved.
* <br>
* BSD-style license. See <a href="http://opentrafficsim.org/node/13">OpenTrafficSim License</a>.
* <p>
* @version $Revision$, $LastChangedDate$, by $Author$, initial version 22 apr. 2018 <br>
* @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
* @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
* @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
*/
private class DistanceGTU implements Comparable<DistanceGTU>
{
/** GTU. */
private LaneBasedGTU gtu;
/** Distance. */
private Length distance;
/**
* Constructor.
* @param gtu LaneBasedGTU; GTU
* @param distance Length; distance
*/
DistanceGTU(final LaneBasedGTU gtu, final Length distance)
{
this.gtu = gtu;
this.distance = distance;
}
/**
* Returns the GTU.
* @return LaneBasedGTU; GTU
*/
public LaneBasedGTU getGTU()
{
return this.gtu;
}
/**
* Returns the distance.
* @return Length; distance
*/
public Length getDistance()
{
return this.distance;
}
/** {@inheritDoc} */
@Override
public int compareTo(final DistanceGTU o)
{
return this.distance.compareTo(o.distance);
}
}
/**
* Translation from a set of {@code DistanceGTU}'s, to a sorted set of {@code HeadwayGTU}'s. This bridges the gap between a
* raw network search, and the perceived result.
* <p>
* Copyright (c) 2013-2018 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved.
* <br>
* BSD-style license. See <a href="http://opentrafficsim.org/node/13">OpenTrafficSim License</a>.
* <p>
* @version $Revision$, $LastChangedDate$, by $Author$, initial version 22 apr. 2018 <br>
* @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
* @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
* @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
*/
private static class SortedNeighborsSet implements SortedSet<HeadwayGTU>
{
/** Base set of GTU's at distance. */
private final SortedSet<DistanceGTU> base;
/** Headway type for perceived GTU's. */
private final HeadwayGtuType headwayGtuType;
/** Perceiving GTU. */
private final LaneBasedGTU perceivingGtu;
/** Whether the GTU's are downstream. */
private final boolean downstream;
/** Contains all GTU's preceived so far, to prevent re-perception. */
private final SortedMap<String, HeadwayGTU> all = new TreeMap<>();
/**
* Constructor.
* @param base SortedSet; base set of GTU's at distance
* @param headwayGtuType HeadwayGtuType; headway type for perceived GTU's
* @param perceivingGtu LaneBasedGTU; perceiving GTU
* @param downstream boolean; whether the GTU's are downstream
*/
SortedNeighborsSet(final SortedSet<DistanceGTU> base, final HeadwayGtuType headwayGtuType,
final LaneBasedGTU perceivingGtu, final boolean downstream)
{
this.base = base;
this.headwayGtuType = headwayGtuType;
this.perceivingGtu = perceivingGtu;
this.downstream = downstream;
}
/** {@inheritDoc} */
@Override
public int size()
{
return this.base.size();
}
/** {@inheritDoc} */
@Override
public boolean isEmpty()
{
return this.base.isEmpty();
}
/**
* Make sure all GTU are available in perceived for. Helper method.
*/
private void getAll()
{
Iterator<HeadwayGTU> it = iterator();
while (it.hasNext())
{
@SuppressWarnings("unused")
HeadwayGTU gtu = it.next(); // iterator creates all HeadwayGTU's
}
}
/** {@inheritDoc} */
@Override
public boolean contains(final Object o)
{
getAll();
return this.all.containsValue(o);
}
/** {@inheritDoc} */
@Override
public Iterator<HeadwayGTU> iterator()
{
return new Iterator<HeadwayGTU>()
{
@SuppressWarnings("synthetic-access")
private Iterator<DistanceGTU> it = SortedNeighborsSet.this.base.iterator();
@Override
public boolean hasNext()
{
return this.it.hasNext();
}
@SuppressWarnings("synthetic-access")
@Override
public HeadwayGTU next()
{
DistanceGTU next = this.it.next();
if (next == null)
{
throw new ConcurrentModificationException();
}
HeadwayGTU out = SortedNeighborsSet.this.all.get(next.getGTU().getId());
if (out == null)
{
out = Try.assign(() -> SortedNeighborsSet.this.headwayGtuType.createHeadwayGtu(
SortedNeighborsSet.this.perceivingGtu, next.getGTU(), next.getDistance(),
SortedNeighborsSet.this.downstream), "Exception while perceiving a neighbor.");
SortedNeighborsSet.this.all.put(next.getGTU().getId(), out);
}
return out;
}
};
}
/** {@inheritDoc} */
@Override
public Object[] toArray()
{
getAll();
return this.all.values().toArray();
}
/** {@inheritDoc} */
@Override
public <T> T[] toArray(final T[] a)
{
getAll();
return this.all.values().toArray(a);
}
/** {@inheritDoc} */
@Override
public boolean add(final HeadwayGTU e)
{
throw new UnsupportedOperationException();
}
/** {@inheritDoc} */
@Override
public boolean remove(final Object o)
{
throw new UnsupportedOperationException();
}
/** {@inheritDoc} */
@Override
public boolean containsAll(final Collection<?> c)
{
getAll();
return this.all.values().containsAll(c);
}
/** {@inheritDoc} */
@Override
public boolean addAll(final Collection<? extends HeadwayGTU> c)
{
throw new UnsupportedOperationException();
}
/** {@inheritDoc} */
@Override
public boolean retainAll(final Collection<?> c)
{
throw new UnsupportedOperationException();
}
/** {@inheritDoc} */
@Override
public boolean removeAll(final Collection<?> c)
{
throw new UnsupportedOperationException();
}
/** {@inheritDoc} */
@Override
public void clear()
{
throw new UnsupportedOperationException();
}
/** {@inheritDoc} */
@Override
public Comparator<? super HeadwayGTU> comparator()
{
return null;
}
/**
* Helper method for sub-lists to find distance-GTU from the perceived GTU.
* @param element HeadwayGTU; perceived GTU
* @return DistanceGTU; pertaining to given GTU
*/
private DistanceGTU getGTU(final HeadwayGTU element)
{
for (DistanceGTU distanceGtu : this.base)
{
if (distanceGtu.getGTU().getId().equals(element.getId()))
{
return distanceGtu;
}
}
throw new IllegalArgumentException("GTU used to obtain a subset is not in the set.");
}
/** {@inheritDoc} */
@Override
public SortedSet<HeadwayGTU> subSet(final HeadwayGTU fromElement, final HeadwayGTU toElement)
{
return new SortedNeighborsSet(this.base.subSet(getGTU(fromElement), getGTU(toElement)), this.headwayGtuType,
this.perceivingGtu, this.downstream);
}
/** {@inheritDoc} */
@Override
public SortedSet<HeadwayGTU> headSet(final HeadwayGTU toElement)
{
return new SortedNeighborsSet(this.base.headSet(getGTU(toElement)), this.headwayGtuType, this.perceivingGtu,
this.downstream);
}
/** {@inheritDoc} */
@Override
public SortedSet<HeadwayGTU> tailSet(final HeadwayGTU fromElement)
{
return new SortedNeighborsSet(this.base.tailSet(getGTU(fromElement)), this.headwayGtuType, this.perceivingGtu,
this.downstream);
}
/** {@inheritDoc} */
@Override
public HeadwayGTU first()
{
return iterator().next();
}
/** {@inheritDoc} */
@Override
public HeadwayGTU last()
{
getAll();
return this.all.get(this.all.lastKey());
}
}
}