}
    
    /**************************************************************************************************************************/
    /**************************************************** HEADWAY ALGORITHMS **************************************************/
    /**************************************************************************************************************************/

    /**
     * Determine which GTU is in front of this GTU. This method looks in all lanes where this GTU is registered, and not further
     * than the value of the given maxDistance. The minimum headway is returned of all Lanes where the GTU is registered. When
     * no GTU is found within the given maxDistance, a HeadwayGTU with <b>null</b> as the gtuId and maxDistance as the distance
     * is returned. The search will extend into successive lanes if the maxDistance is larger than the remaining length on the
     * lane. When Lanes (or underlying CrossSectionLinks) diverge, a route planner may be used to determine which kinks and
     * lanes to take into account and which ones not. When the Lanes (or underlying CrossSectionLinks) converge, "parallel"
     * traffic is not taken into account in the headway calculation. Instead, gap acceptance algorithms or their equivalent
     * should guide the merging behavior.<br>
     * <b>Note:</b> Headway is the net headway and calculated on a front-to-back basis.
     * @param maxDistance the maximum distance to look for the nearest GTU; positive values search forwards; negative values
     *            search backwards
     * @return HeadwayGTU; the headway and the GTU information
     * @throws GTUException when there is an error with the next lanes in the network.
     * @throws NetworkException when there is a problem with the route planner
     */
    private Headway forwardHeadway(final Length maxDistance) throws GTUException, NetworkException
    {
        LanePathInfo lpi = getLanePathInfo();
        return forwardHeadway(lpi, maxDistance);
    }

    /**
     * Determine which GTU is in front of this GTU. This method uses a given lanePathInfo to look forward, but not further than
     * the value of the given maxDistance. The minimum headway is returned of all Lanes where the GTU is registered. When no GTU
     * is found within the given maxDistance, a HeadwayGTU with <b>null</b> as the gtuId and maxDistance as the distance is
     * returned. The search will extend into successive lanes if the maxDistance is larger than the remaining length on the
     * lane. When Lanes (or underlying CrossSectionLinks) diverge, a route planner may be used to determine which kinks and
     * lanes to take into account and which ones not. When the Lanes (or underlying CrossSectionLinks) converge, "parallel"
     * traffic is not taken into account in the headway calculation. Instead, gap acceptance algorithms or their equivalent
     * should guide the merging behavior.<br>
     * <b>Note:</b> Headway is the net headway and calculated on a front-to-back basis.
     * @param lpi the lanePathInfo object that informs the headway algorithm in which lanes to look, and from which position on
     *            the first lane.
     * @param maxDistance the maximum distance to look for the nearest GTU; positive values search forwards; negative values
     *            search backwards
     * @return HeadwayGTU; the headway and the GTU information
     * @throws GTUException when there is an error with the next lanes in the network.
     * @throws NetworkException when there is a problem with the route planner
     */
    private Headway forwardHeadway(final LanePathInfo lpi, final Length maxDistance) throws GTUException, NetworkException
    {
        Throw.when(maxDistance.le(Length.ZERO), GTUException.class, "forwardHeadway: maxDistance should be positive");

        int ldIndex = 0;
        LaneDirection ld = lpi.getReferenceLaneDirection();
        double gtuPosFrontSI = lpi.getReferencePosition().si;
        if (lpi.getReferenceLaneDirection().getDirection().isPlus())
        {
            gtuPosFrontSI += getGtu().getFront().getDx().si;
        }
        else
        {
            gtuPosFrontSI -= getGtu().getFront().getDx().si;
        }

        // TODO end of lanepath

        while ((gtuPosFrontSI > ld.getLane().getLength().si || gtuPosFrontSI < 0.0)
            && ldIndex < lpi.getLaneDirectionList().size() - 1)
        {
            ldIndex++;
            if (ld.getDirection().isPlus()) // e.g. 1005 on length of lane = 1000
            {
                if (lpi.getLaneDirectionList().get(ldIndex).getDirection().isPlus())
                {
                    gtuPosFrontSI -= ld.getLane().getLength().si;
                }
                else
                {
                    gtuPosFrontSI = lpi.getLaneDirectionList().get(ldIndex).getLane().getLength().si - gtuPosFrontSI;
                }
                ld = lpi.getLaneDirectionList().get(ldIndex);
            }
            else
            // e.g. -5 on lane of whatever length
            {
                if (lpi.getLaneDirectionList().get(ldIndex).getDirection().isPlus())
                {
                    gtuPosFrontSI += ld.getLane().getLength().si;
                }
                else
                {
                    gtuPosFrontSI += lpi.getLaneDirectionList().get(ldIndex).getLane().getLength().si;
                }
                ld = lpi.getLaneDirectionList().get(ldIndex);
            }
        }

        double maxDistanceSI = maxDistance.si;
        Time time = getGtu().getSimulator().getSimulatorTime().getTime();

        // look forward based on the provided lanePathInfo.
        Headway closest = headwayLane(ld, gtuPosFrontSI, 0.0, time);
        if (closest != null)
        {
            if (closest.getDistance().si > maxDistanceSI)
            {
                return new HeadwayDistance(maxDistanceSI);
            }
            return closest;
        }
        double cumDistSI = ld.getDirection().isPlus() ? ld.getLane().getLength().si - gtuPosFrontSI : gtuPosFrontSI;
        for (int i = ldIndex + 1; i < lpi.getLaneDirectionList().size(); i++)
        {
            ld = lpi.getLaneDirectionList().get(i);
            closest = headwayLane(ld, ld.getDirection().isPlus() ? 0.0 : ld.getLane().getLength().si, cumDistSI, time);
            if (closest != null)
            {
                if (closest.getDistance().si > maxDistanceSI)
                {
                    return new HeadwayDistance(maxDistanceSI);
                }
                return closest;
            }
            cumDistSI += ld.getLane().getLength().si;
        }
        return new HeadwayDistance(maxDistanceSI);
    }

    /**
     * Determine the positive headway on a lane, or null if no GTU can be found on this lane.
     * @param laneDirection the lane and direction to look
     * @param startPosSI the start position to look from in meters
     * @param cumDistSI the cumulative distance that has already been observed on other lanes
     * @param now the current time to determine the GTU positions on the lane
     * @return the HeadwayGTU, containing information on a GTU that is ahead of the given start position, or null if no GTU can
     *         be found on this lane
     * @throws GTUException when the GTUs ahead on the lane cannot be determined
     */
    private Headway headwayLane(final LaneDirection laneDirection, final double startPosSI, final double cumDistSI,
        final Time now) throws GTUException
    {
        Lane lane = laneDirection.getLane();
        LaneBasedGTU laneBasedGTU =
            lane.getGtuAhead(new Length(startPosSI, LengthUnit.SI), laneDirection.getDirection(), RelativePosition.REAR, now);
        if (laneBasedGTU == null)
        {
            return null;
        }
        double distanceSI = Math.abs(laneBasedGTU.position(lane, laneBasedGTU.getRear()).si - startPosSI);
        return new HeadwayGTUSimple(laneBasedGTU.getId(), laneBasedGTU.getGTUType(), new Length(cumDistSI + distanceSI,
            LengthUnit.SI), laneBasedGTU.getLength(), laneBasedGTU.getSpeed(), laneBasedGTU.getAcceleration());
    }

    /**
     * Determine which GTU is behind this GTU. This method looks in all lanes where this GTU is registered, and not further back
     * than the absolute value of the given maxDistance. The minimum net headway is returned of all Lanes where the GTU is
     * registered. When no GTU is found within the given maxDistance, <b>null</b> is returned. The search will extend into
     * successive lanes if the maxDistance is larger than the remaining length on the lane. When Lanes (or underlying
     * CrossSectionLinks) diverge, the headway algorithms have to look at multiple Lanes and return the minimum headway in each
     * of the Lanes. When the Lanes (or underlying CrossSectionLinks) converge, "parallel" traffic is not taken into account in
     * the headway calculation. Instead, gap acceptance algorithms or their equivalent should guide the merging behavior.<br>
     * <b>Note:</b> Headway is the net headway and calculated on a back-to-front basis.
     * @param maxDistance the maximum distance to look for the nearest GTU; it should have a negative value to search backwards
     * @return HeadwayGTU; the headway and the GTU information
     * @throws GTUException when there is an error with the next lanes in the network.
     * @throws NetworkException when there is a problem with the route planner
     */
    private Headway backwardHeadway(final Length maxDistance) throws GTUException, NetworkException
    {
        Throw.when(maxDistance.ge(Length.ZERO), GTUException.class, "backwardHeadway: maxDistance should be negative");
        Time time = getGtu().getSimulator().getSimulatorTime().getTime();
        double maxDistanceSI = maxDistance.si;
        Headway foundHeadway = new HeadwayDistance(-maxDistanceSI);
        for (Lane lane : getGtu().positions(getGtu().getRear()).keySet())
        {
            Headway closest =
                headwayRecursiveBackwardSI(lane, getGtu().getLanes().get(lane), getGtu().position(lane, getGtu().getRear(),
                    time).getSI(), 0.0, -maxDistanceSI, time);
            if (closest.getDistance().si < -maxDistanceSI && closest.getDistance().si < -foundHeadway.getDistance().si)
            {
                foundHeadway = closest;
            }
        }
        if (foundHeadway instanceof AbstractHeadwayGTU)
        {
            return new HeadwayGTUSimple(foundHeadway.getId(), ((AbstractHeadwayGTU) foundHeadway).getGtuType(), foundHeadway
                .getDistance().multiplyBy(-1.0), foundHeadway.getLength(), foundHeadway.getSpeed(), null);
        }
        if (foundHeadway instanceof HeadwayDistance)
        {
            return new HeadwayDistance(foundHeadway.getDistance().multiplyBy(-1.0));
        }
        // TODO allow observation of other objects as well.
        throw new GTUException("backwardHeadway not implemented yet for other object types than GTU");
    }

    /**
     * Calculate the minimum headway, possibly on subsequent lanes, in backward direction (so between our back, and the other
     * GTU's front). Note: this method returns a POSITIVE number.
     * @param lane the lane where we are looking right now
     * @param direction the direction we are driving on that lane
     * @param lanePositionSI from which position on this lane do we start measuring? This is the current position of the rear of
     *            the GTU when we measure in the lane where the original GTU is positioned, and lane.getLength() for each
     *            subsequent lane.
     * @param cumDistanceSI the distance we have already covered searching on previous lanes. Note: This is a POSITIVE number.
     * @param maxDistanceSI the maximum distance to look for in SI units; stays the same in subsequent calls. Note: this is a
     *            POSITIVE number.
     * @param when the current or future time for which to calculate the headway
     * @return the headway in SI units when we have found the GTU, or a null GTU with a distance of Double.MAX_VALUE meters when
     *         no other GTU could not be found within maxDistanceSI meters
     * @throws GTUException when there is a problem with the geometry of the network
     */
    private Headway headwayRecursiveBackwardSI(final Lane lane, final GTUDirectionality direction,
        final double lanePositionSI, final double cumDistanceSI, final double maxDistanceSI, final Time when)
        throws GTUException
    {
        LaneBasedGTU otherGTU =
            lane.getGtuBehind(new Length(lanePositionSI, LengthUnit.SI), direction, RelativePosition.FRONT, when);
        if (otherGTU != null)
        {
            double distanceM = cumDistanceSI + lanePositionSI - otherGTU.position(lane, otherGTU.getFront(), when).getSI();
            if (distanceM > 0 && distanceM <= maxDistanceSI)
            {
                return new HeadwayGTUSimple(otherGTU.getId(), otherGTU.getGTUType(), new Length(distanceM, LengthUnit.SI),
                    otherGTU.getLength(), otherGTU.getSpeed(), null);
            }
            return new HeadwayDistance(Double.MAX_VALUE);
        }

        // Continue search on predecessor lanes.
        if (cumDistanceSI + lanePositionSI < maxDistanceSI)
        {
            // is there a predecessor link?
            if (lane.prevLanes(getGtu().getGTUType()).size() > 0)
            {
                Headway foundMaxGTUDistanceSI = new HeadwayDistance(Double.MAX_VALUE);
                for (Lane prevLane : lane.prevLanes(getGtu().getGTUType()).keySet())
                {
                    // What is behind us is INDEPENDENT of the followed route!
                    double traveledDistanceSI = cumDistanceSI + lanePositionSI;
                    // WRONG - adapt method to forward perception method!
                    Headway closest =
                        headwayRecursiveBackwardSI(prevLane, direction, prevLane.getLength().getSI(), traveledDistanceSI,
                            maxDistanceSI, when);
                    if (closest.getDistance().si < maxDistanceSI
                        && closest.getDistance().si < foundMaxGTUDistanceSI.getDistance().si)
                    {
                        foundMaxGTUDistanceSI = closest;
                    }
                }
                return foundMaxGTUDistanceSI;
            }
        }

        // No other GTU was not on one of the current lanes or their successors.
        return new HeadwayDistance(Double.MAX_VALUE);
    }

    /**************************************************************************************************************************/
    /************************************************ ADJACENT LANE TRAFFIC ***************************************************/
    /**************************************************************************************************************************/

    /**
     * Determine which GTUs are parallel with us on another lane, based on fractional positions. <br>
     * Note: When the GTU that calls the method is also registered on the given lane, it is excluded from the return set.
     * @param lane the lane to look for parallel (partial or full overlapping) GTUs.
     * @param when the future time for which to calculate the headway
     * @return the set of GTUs parallel to us on the other lane (partial overlap counts as parallel), based on fractional
     *         positions, or an empty set when no GTUs were found.
     * @throws GTUException when the vehicle's route is inconclusive, when vehicles are not registered correctly on their lanes,
     *             or when the given lane is not parallel to one of the lanes where we are registered.
     */
    private Collection<Headway> parallel(final Lane lane, final Time when) throws GTUException
    {
        Collection<Headway> headwayCollection = new LinkedHashSet<>();

        sample.addData(gtu.getSpeed());
    }

    /**
     * Set up a JFreeChart axis.
     * @param valueAxis ValueAxis; the axis to set up
     * @param axis Axis; the Axis that provides the data to setup the ValueAxis
     */
    private static void configureAxis(final ValueAxis valueAxis, final Axis axis)
    {
        valueAxis.setLabel("\u2192 " + axis.getName());
        valueAxis.setRange(axis.getMinimumValue().getInUnit(), axis.getMaximumValue().getInUnit());
    }

    /**
     * Redraw this TrajectoryGraph (after the underlying data has been changed, or to change axes).
     */
    public final void reGraph()
    {
        NumberAxis numberAxis = new NumberAxis();
        configureAxis(numberAxis, this.xAxis);
        this.chartPanel.getXYPlot().setDomainAxis(numberAxis);
        this.chartPanel.getPlot().axisChanged(new AxisChangeEvent(numberAxis));
        numberAxis = new NumberAxis();
        configureAxis(numberAxis, this.yAxis);
        this.chartPanel.getXYPlot().setRangeAxis(numberAxis);
        this.chartPanel.getPlot().axisChanged(new AxisChangeEvent(numberAxis));
        notifyListeners(new DatasetChangeEvent(this, null)); // This guess work actually works!
    }

    /**
     * Notify interested parties of an event affecting this TrajectoryPlot.
     * @param event DatasetChangedEvent
     */
    private void notifyListeners(final DatasetChangeEvent event)
    {
        for (DatasetChangeListener dcl : this.listenerList.getListeners(DatasetChangeListener.class))
        {
            dcl.datasetChanged(event);
        }
    }

    /** {@inheritDoc} */
    @Override
    public final int getSeriesCount()
    {
        return 1;
    }

    /** {@inheritDoc} */
    @Override
    public final Comparable<Integer> getSeriesKey(final int series)
    {
        return series;
    }

    /** {@inheritDoc} */
    @SuppressWarnings("rawtypes")
    @Override
    public final int indexOf(final Comparable seriesKey)
    {
        if (seriesKey instanceof Integer)
        {
            return (Integer) seriesKey;
        }
        return -1;
    }

    /** {@inheritDoc} */
    @Override
    public final void addChangeListener(final DatasetChangeListener listener)
    {
        this.listenerList.add(DatasetChangeListener.class, listener);
    }

    /** {@inheritDoc} */
    @Override
    public final void removeChangeListener(final DatasetChangeListener listener)
    {
        this.listenerList.remove(DatasetChangeListener.class, listener);
    }

    /** {@inheritDoc} */
    @Override
    public final DatasetGroup getGroup()
    {
        return this.datasetGroup;
    }

    /** {@inheritDoc} */
    @Override
    public final void setGroup(final DatasetGroup group)
    {
        this.datasetGroup = group;
    }

    /** {@inheritDoc} */
    @Override
    public final DomainOrder getDomainOrder()
    {
        return DomainOrder.ASCENDING;
    }

    /** {@inheritDoc} */
    @Override
    public final int getItemCount(final int series)
    {
        return this.samples.size();
    }

    /**
     * Retrieve a value from the recorded samples.
     * @param item Integer; the rank number of the sample
     * @param axis Axis; the axis that determines which quantity to retrieve
     * @return Double; the requested value, or Double.NaN if the sample does not (yet) exist
     */
    private Double getSample(final int item, final Axis axis)
    {
        if (item >= this.samples.size())
        {
            return Double.NaN;
        }
        double result = this.samples.get(item).getValue(axis);
        /*-
        System.out.println(String.format("getSample(item=%d, axis=%s) returns %f", item, axis.name,
                result));
         */
        return result;
    }

    /** {@inheritDoc} */
    @Override
    public final Number getX(final int series, final int item)
    {
        return getXValue(series, item);
    }

    /** {@inheritDoc} */
    @Override
    public final double getXValue(final int series, final int item)
    {
        return getSample(item, this.xAxis);
    }

    /** {@inheritDoc} */
    @Override
    public final Number getY(final int series, final int item)
    {
        return getYValue(series, item);
    }

    /** {@inheritDoc} */
    @Override
    public final double getYValue(final int series, final int item)
    {
        return getSample(item, this.yAxis);
    }

    /** {@inheritDoc} */
    @Override

            for (Lane adjacentLane : getAccessibleAdjacentLanes(directionality).get(lane))
            {
                Headway follower =
                    headwayRecursiveBackwardSI(adjacentLane, getGtu().getLanes().get(lane), getGtu().projectedPosition(
                        adjacentLane, getGtu().getRear(), when).getSI(), 0.0, -maximumReverseHeadway.getSI(), when);
                if (follower instanceof AbstractHeadwayGTU)
                {
                    boolean found = false;
                    for (Headway headway : result)
                    {
                        if (headway.getId().equals(follower.getId()))
                        {
                            found = true;
                        }
                    }
                    if (!found)
                    {
                        result.add(new HeadwayGTUSimple(follower.getId(), ((AbstractHeadwayGTU) follower).getGtuType(),
                            follower.getDistance().multiplyBy(-1.0), follower.getLength(), follower.getSpeed(), null));
                    }
                }
                else if (follower instanceof HeadwayDistance) // always add for potential lane drop
                {
                    result.add(new HeadwayDistance(follower.getDistance().multiplyBy(-1.0)));
                }
                else
                {
                    throw new GTUException(
                        "collectNeighborLaneTraffic not yet suited to observe obstacles on neighboring lanes");
                }
            }
        }
        return result;
    }

    /**
     * Find a lanePathInfo left or right of the current LanePath.
     * @param adjacentLane the start adjacent lane for which we calculate the LanePathInfo
     * @param direction LateralDirectionality; either <cite>LateralDirectionality.LEFT</cite>, or
     *            <cite>LateralDirectionality.RIGHT</cite>
     * @param when DoubleScalar.Abs&lt;TimeUnit&gt;; the (current) time
     * @return the adjacent LanePathInfo
     * @throws GTUException when the GTU was not initialized yet.
     * @throws NetworkException when the speed limit for a GTU type cannot be retrieved from the network.
     * @throws ParameterException in case of a parameter problem
     */
    private LanePathInfo buildLanePathInfoAdjacent(final Lane adjacentLane, final LateralDirectionality direction,
        final Time when) throws GTUException, NetworkException, ParameterException
    {
        if (this.lanePathInfo == null || this.lanePathInfo.getTimestamp().ne(when))
        {
            updateLanePathInfo();
        }
        LanePathInfo lpi = getLanePathInfo();
        List<LaneDirection> laneDirectionList = new ArrayList<>();
        laneDirectionList.add(new LaneDirection(adjacentLane, lpi.getReferenceLaneDirection().getDirection()));
        Length referencePosition = getGtu().projectedPosition(adjacentLane, getGtu().getReference(), when);
        for (int i = 1; i < lpi.getLaneDirectionList().size(); i++)
        {
            LaneDirection ld = lpi.getLaneDirectionList().get(i);
            Set<Lane> accessibleLanes = ld.getLane().accessibleAdjacentLanes(direction, getGtu().getGTUType());
            Lane adjLane = null;
            for (Lane lane : accessibleLanes)
            {
                if (lane.getParentLink().equals(ld.getLane().getParentLink()))
                {
                    adjLane = lane;
                }
            }
            if (adjLane == null)
            {
                break;
            }
            laneDirectionList.add(new LaneDirection(adjLane, ld.getDirection()));
        }
        return new LanePathInfo(null, laneDirectionList, referencePosition);
    }

        Collection<Headway> headwayCollection = new LinkedHashSet<>();
        for (Lane l : getGtu().getLanes().keySet())
        {
            // only take lanes that we can compare based on a shared design line
            if (l.getParentLink().equals(lane.getParentLink()))
            {
                // compare based on fractional positions.
                double posFractionRef = getGtu().fractionalPosition(l, getGtu().getReference(), when);
                double posFractionFront =
                    Math.max(0.0, posFractionRef + getGtu().getFront().getDx().si / lane.getLength().si);
                double posFractionRear = Math.min(1.0, posFractionRef + getGtu().getRear().getDx().si / lane.getLength().si);
                // double posFractionFront = Math.max(0.0, this.gtu.fractionalPosition(l, this.gtu.getFront(), when));
                // double posFractionRear = Math.min(1.0, this.gtu.fractionalPosition(l, this.gtu.getRear(), when));
                double posMin = Math.min(posFractionFront, posFractionRear);
                double posMax = Math.max(posFractionFront, posFractionRear);
                for (LaneBasedGTU otherGTU : lane.getGtuList())
                {
                    if (!otherGTU.equals(this)) // TODO
                    {
                        /*- cater for: *-----*         *-----*       *-----*       *----------*
                         *                *-----*    *----*      *------------*       *-----*
                         * where the GTUs can each drive in two directions (!)
                         */
                        double gtuFractionRef = otherGTU.fractionalPosition(lane, otherGTU.getReference(), when);
                        double gtuFractionFront =
                            Math.max(0.0, gtuFractionRef + otherGTU.getFront().getDx().si / lane.getLength().si);
                        double gtuFractionRear =
                            Math.min(1.0, gtuFractionRef + otherGTU.getRear().getDx().si / lane.getLength().si);
                        double gtuMin = Math.min(gtuFractionFront, gtuFractionRear);
                        double gtuMax = Math.max(gtuFractionFront, gtuFractionRear);
                        // TODO calculate real overlaps
                        Length overlapFront = new Length(1.0, LengthUnit.SI);
                        Length overlap = new Length(1.0, LengthUnit.SI);
                        Length overlapRear = new Length(1.0, LengthUnit.SI);
                        if ((gtuMin >= posMin && gtuMin <= posMax) || (gtuMax >= posMin && gtuMax <= posMax)
                            || (posMin >= gtuMin && posMin <= gtuMax) || (posMax >= gtuMin && posMax <= gtuMax))
                        {
                            headwayCollection.add(new HeadwayGTUSimple(otherGTU.getId(), otherGTU.getGTUType(),
                                overlapFront, overlap, overlapRear, otherGTU.getLength(), otherGTU.getSpeed(), otherGTU
                                    .getAcceleration()));
                        }
                    }
                }
            }
        }
        return headwayCollection;
    }

    /**
     * Determine which GTUs are parallel with us in a certain lateral direction, based on fractional positions. <br>
     * Note 1: This method will look to the adjacent lanes of all lanes where the vehicle has been registered.<br>
     * Note 2: When the GTU that calls the method is also registered on the given lane, it is excluded from the return set.
     * @param lateralDirection the direction of the adjacent lane(s) to look for parallel (partial or full overlapping) GTUs.
     * @param when the future time for which to calculate the headway
     * @return the set of GTUs parallel to us on other lane(s) in the given direction (partial overlap counts as parallel),
     *         based on fractional positions, or an empty set when no GTUs were found.
     * @throws GTUException when the vehicle's route is inconclusive, when vehicles are not registered correctly on their lanes,
     *             or when there are no lanes parallel to one of the lanes where we are registered in the given direction.
     */
    private Collection<Headway> parallel(final LateralDirectionality lateralDirection, final Time when) throws GTUException
    {
        Collection<Headway> gtuSet = new LinkedHashSet<>();

    }
    
    /**************************************************************************************************************************/
    /**************************************************** HEADWAY ALGORITHMS **************************************************/
    /**************************************************************************************************************************/

    /**
     * Determine which GTU is in front of this GTU. This method looks in all lanes where this GTU is registered, and not further
     * than the value of the given maxDistance. The minimum headway is returned of all Lanes where the GTU is registered. When
     * no GTU is found within the given maxDistance, a HeadwayGTU with <b>null</b> as the gtuId and maxDistance as the distance
     * is returned. The search will extend into successive lanes if the maxDistance is larger than the remaining length on the
     * lane. When Lanes (or underlying CrossSectionLinks) diverge, a route planner may be used to determine which kinks and
     * lanes to take into account and which ones not. When the Lanes (or underlying CrossSectionLinks) converge, "parallel"
     * traffic is not taken into account in the headway calculation. Instead, gap acceptance algorithms or their equivalent
     * should guide the merging behavior.<br>
     * <b>Note:</b> Headway is the net headway and calculated on a front-to-back basis.
     * @param maxDistance the maximum distance to look for the nearest GTU; positive values search forwards; negative values
     *            search backwards
     * @return HeadwayGTU; the headway and the GTU information
     * @throws GTUException when there is an error with the next lanes in the network.
     * @throws NetworkException when there is a problem with the route planner
     */
    private Headway forwardHeadway(final Length maxDistance) throws GTUException, NetworkException
    {
        LanePathInfo lpi = getLanePathInfo();
        return forwardHeadway(lpi, maxDistance);
    }

    /**
     * Determine which GTU is in front of this GTU. This method uses a given lanePathInfo to look forward, but not further than
     * the value of the given maxDistance. The minimum headway is returned of all Lanes where the GTU is registered. When no GTU
     * is found within the given maxDistance, a HeadwayGTU with <b>null</b> as the gtuId and maxDistance as the distance is
     * returned. The search will extend into successive lanes if the maxDistance is larger than the remaining length on the
     * lane. When Lanes (or underlying CrossSectionLinks) diverge, a route planner may be used to determine which kinks and
     * lanes to take into account and which ones not. When the Lanes (or underlying CrossSectionLinks) converge, "parallel"
     * traffic is not taken into account in the headway calculation. Instead, gap acceptance algorithms or their equivalent
     * should guide the merging behavior.<br>
     * <b>Note:</b> Headway is the net headway and calculated on a front-to-back basis.
     * @param lpi the lanePathInfo object that informs the headway algorithm in which lanes to look, and from which position on
     *            the first lane.
     * @param maxDistance the maximum distance to look for the nearest GTU; positive values search forwards; negative values
     *            search backwards
     * @return HeadwayGTU; the headway and the GTU information
     * @throws GTUException when there is an error with the next lanes in the network.
     * @throws NetworkException when there is a problem with the route planner
     */
    private Headway forwardHeadway(final LanePathInfo lpi, final Length maxDistance) throws GTUException, NetworkException
    {
        Throw.when(maxDistance.le(Length.ZERO), GTUException.class, "forwardHeadway: maxDistance should be positive");

        int ldIndex = 0;
        LaneDirection ld = lpi.getReferenceLaneDirection();
        double gtuPosFrontSI = lpi.getReferencePosition().si;
        if (lpi.getReferenceLaneDirection().getDirection().isPlus())
        {
            gtuPosFrontSI += getGtu().getFront().getDx().si;
        }
        else
        {
            gtuPosFrontSI -= getGtu().getFront().getDx().si;
        }

        // TODO end of lanepath

        while ((gtuPosFrontSI > ld.getLane().getLength().si || gtuPosFrontSI < 0.0)
            && ldIndex < lpi.getLaneDirectionList().size() - 1)
        {
            ldIndex++;
            if (ld.getDirection().isPlus()) // e.g. 1005 on length of lane = 1000
            {
                if (lpi.getLaneDirectionList().get(ldIndex).getDirection().isPlus())
                {
                    gtuPosFrontSI -= ld.getLane().getLength().si;
                }
                else
                {
                    gtuPosFrontSI = lpi.getLaneDirectionList().get(ldIndex).getLane().getLength().si - gtuPosFrontSI;
                }
                ld = lpi.getLaneDirectionList().get(ldIndex);
            }
            else
            // e.g. -5 on lane of whatever length
            {
                if (lpi.getLaneDirectionList().get(ldIndex).getDirection().isPlus())
                {
                    gtuPosFrontSI += ld.getLane().getLength().si;
                }
                else
                {
                    gtuPosFrontSI += lpi.getLaneDirectionList().get(ldIndex).getLane().getLength().si;
                }
                ld = lpi.getLaneDirectionList().get(ldIndex);
            }
        }

                        }
                    }
                }
            }

            // Condition, if we have just changed lane, let's not change immediately again.
            // TODO make direction dependent!
            if (getGtu().getSimulator().getSimulatorTime().getTime().lt(this.earliestNexLaneChangeTime))
            {
                return currentLanePlan(laneBasedGTU, startTime, locationAtStartTime, lanePathInfo);
            }

            // Step 2. Do we want to change lanes to the left because of our predecessor on the current lane?
            // does the lane left of us [TODO: driving direction] bring us to our destination as well?
            Set<Lane> leftLanes =
                perception.getPerceptionCategory(DefaultAlexander.class).getAccessibleAdjacentLanesLeft().get(
                    lanePathInfo.getReferenceLane());
            if (nextSplitInfo.isSplit())
            {
                leftLanes.retainAll(nextSplitInfo.getCorrectCurrentLanes());
            }
            if (!leftLanes.isEmpty() && laneBasedGTU.getSpeed().si > 4.0) // XXX we are driving...
            {
                perception.getPerceptionCategory(DefaultAlexander.class).updateBackwardHeadway();
                perception.getPerceptionCategory(DefaultAlexander.class).updateParallelHeadwaysLeft();
                perception.getPerceptionCategory(DefaultAlexander.class).updateNeighboringHeadwaysLeft();
                if (perception.getPerceptionCategory(DefaultAlexander.class).getParallelHeadwaysLeft().isEmpty())
                {
                    Collection<Headway> sameLaneTraffic = new HashSet<>();
                    // TODO should it be getObjectType().isGtu() or !getObjectType().isDistanceOnly() ?
                    if (perception.getPerceptionCategory(DefaultAlexander.class).getForwardHeadway() != null
                        && perception.getPerceptionCategory(DefaultAlexander.class).getForwardHeadway().getObjectType()
                            .isGtu())
                    {
                        sameLaneTraffic.add(perception.getPerceptionCategory(DefaultAlexander.class).getForwardHeadway());
                    }
                    if (perception.getPerceptionCategory(DefaultAlexander.class).getBackwardHeadway() != null
                        && perception.getPerceptionCategory(DefaultAlexander.class).getBackwardHeadway().getObjectType()
                            .isGtu())
                    {
                        sameLaneTraffic.add(perception.getPerceptionCategory(DefaultAlexander.class).getBackwardHeadway());
                    }
                    DirectedLaneChangeModel dlcm = new DirectedAltruistic(getPerception());
                    DirectedLaneMovementStep dlms =
                        dlcm.computeLaneChangeAndAcceleration(laneBasedGTU, LateralDirectionality.LEFT, sameLaneTraffic,
                            perception.getPerceptionCategory(DefaultAlexander.class).getNeighboringHeadwaysLeft(),

    }

    /** {@inheritDoc} */
    @Override
    public final Comparable<Integer> getSeriesKey(final int series)
    {
        return series;
    }

    /** {@inheritDoc} */
    @SuppressWarnings("rawtypes")
    @Override
    public final int indexOf(final Comparable seriesKey)
    {
        if (seriesKey instanceof Integer)
        {
            return (Integer) seriesKey;
        }
        return -1;
    }

    /** {@inheritDoc} */
    @Override
    public final void addChangeListener(final DatasetChangeListener listener)
    {
        this.listenerList.add(DatasetChangeListener.class, listener);
    }

    /** {@inheritDoc} */
    @Override
    public final void removeChangeListener(final DatasetChangeListener listener)
    {
        this.listenerList.remove(DatasetChangeListener.class, listener);
    }

    /** {@inheritDoc} */
    @Override
    public final DatasetGroup getGroup()
    {
        return this.datasetGroup;
    }

    /** {@inheritDoc} */
    @Override
    public final void setGroup(final DatasetGroup group)
    {
        this.datasetGroup = group;
    }

    /** {@inheritDoc} */
    @Override
    public final DomainOrder getDomainOrder()
    {
        return DomainOrder.ASCENDING;
    }

    /** {@inheritDoc} */
    @Override
    public final int getItemCount(final int series)
    {
        return this.samples.size();

                    FourStop xmlModel = new FourStop();
                    // 1 hour simulation run for testing
                    xmlModel.buildAnimator(new Time(0.0, TimeUnit.SECOND), new Duration(0.0, TimeUnit.SECOND),
                        new Duration(60.0, TimeUnit.MINUTE), new ArrayList<AbstractProperty<?>>(), null, true);
                }
                catch (SimRuntimeException | NamingException | OTSSimulationException | PropertyException exception)
                {
                    exception.printStackTrace();
                }
            }
        });
    }

    /** {@inheritDoc} */
    @Override
    public final String shortName()
    {
        return "TestXMLModel";
    }

    /** {@inheritDoc} */
    @Override
    public final String description()
    {
        return "TestXMLModel";
    }

    /** {@inheritDoc} */
    @Override
    public final void stopTimersThreads()
    {
        super.stopTimersThreads();
    }

    /** {@inheritDoc} */
    @Override
    protected final JPanel makeCharts()
    {
        return null;
    }

    /** {@inheritDoc} */
    @Override
    protected final OTSModelInterface makeModel(final GTUColorer colorer)
    {
        return new TestXMLModel();
    }

    /** {@inheritDoc} */
    @Override
    protected final Double makeAnimationRectangle()
    {
        return new Rectangle2D.Double(-1000, -1000, 2000, 2000);

    private boolean canChange(final LaneBasedGTU gtu, final LanePerception perception, final LanePathInfo lanePathInfo,
        final LateralDirectionality direction) throws GTUException, NetworkException, ParameterException,
        OperationalPlanException
    {
        Collection<Headway> otherLaneTraffic;
        perception.getPerceptionCategory(DefaultAlexander.class).updateForwardHeadway();
        perception.getPerceptionCategory(DefaultAlexander.class).updateBackwardHeadway();
        if (direction.isLeft())
        {
            perception.getPerceptionCategory(DefaultAlexander.class).updateParallelHeadwaysLeft();
            perception.getPerceptionCategory(DefaultAlexander.class).updateNeighboringHeadwaysLeft();
            otherLaneTraffic = perception.getPerceptionCategory(DefaultAlexander.class).getNeighboringHeadwaysLeft();
        }
        else if (direction.isRight())
        {
            perception.getPerceptionCategory(DefaultAlexander.class).updateParallelHeadwaysRight();
            perception.getPerceptionCategory(DefaultAlexander.class).updateNeighboringHeadwaysRight();
            otherLaneTraffic = perception.getPerceptionCategory(DefaultAlexander.class).getNeighboringHeadwaysRight();
        }
        else
        {
            throw new GTUException("Lateral direction is neither LEFT nor RIGHT during a lane change");

    private Length laneDrop(final LaneBasedGTU gtu, final LateralDirectionality direction) throws NetworkException,
        GTUException, OperationalPlanException
    {
        Lane lane = null;
        Length longitudinalPosition = null;
        Map<Lane, Length> positions = gtu.positions(RelativePosition.REFERENCE_POSITION);
        if (null == direction)
        {
            for (Lane l : gtu.getLanes().keySet())
            {
                if (l.getLaneType().isCompatible(gtu.getGTUType()))
                {
                    lane = l;
                }
            }
            if (null == lane)
            {
                throw new NetworkException(this + " is not on any compatible lane");
            }
            longitudinalPosition = positions.get(lane);
        }
        else
        {
            lane = positions.keySet().iterator().next();
            longitudinalPosition = positions.get(lane);
            lane =
                getPerception().getPerceptionCategory(DefaultAlexander.class).bestAccessibleAdjacentLane(lane, direction,
                    longitudinalPosition); // XXX

                if (perception.getPerceptionCategory(DefaultAlexander.class).getParallelHeadwaysLeft().isEmpty())
                {
                    Collection<Headway> sameLaneTraffic = new HashSet<>();
                    // TODO should it be getObjectType().isGtu() or !getObjectType().isDistanceOnly() ?
                    if (perception.getPerceptionCategory(DefaultAlexander.class).getForwardHeadway() != null
                        && perception.getPerceptionCategory(DefaultAlexander.class).getForwardHeadway().getObjectType()
                            .isGtu())
                    {
                        sameLaneTraffic.add(perception.getPerceptionCategory(DefaultAlexander.class).getForwardHeadway());
                    }
                    if (perception.getPerceptionCategory(DefaultAlexander.class).getBackwardHeadway() != null
                        && perception.getPerceptionCategory(DefaultAlexander.class).getBackwardHeadway().getObjectType()
                            .isGtu())
                    {
                        sameLaneTraffic.add(perception.getPerceptionCategory(DefaultAlexander.class).getBackwardHeadway());
                    }
                    DirectedLaneChangeModel dlcm = new DirectedAltruistic(getPerception());
                    DirectedLaneMovementStep dlms =
                        dlcm.computeLaneChangeAndAcceleration(laneBasedGTU, LateralDirectionality.LEFT, sameLaneTraffic,

                if (lcmr.getGfmr().getAcceleration().si < 1E-6
                    && laneBasedGTU.getSpeed().si < OperationalPlan.DRIFTING_SPEED_SI)
                {
                    // TODO Make a 100% lateral move from standing still...
                    return new OperationalPlan(getGtu(), locationAtStartTime, startTime, duration);
                }

                // TODO make a gradual lateral move
                OTSLine3D path = lanePathInfo.getPath();
                List<Segment> operationalPlanSegmentList = new ArrayList<>();
                if (lcmr.getGfmr().getAcceleration().si == 0.0)
                {
                    Segment segment = new OperationalPlan.SpeedSegment(duration);
                    operationalPlanSegmentList.add(segment);
                }
                else
                {
                    Segment segment = new OperationalPlan.AccelerationSegment(duration, lcmr.getGfmr().getAcceleration());
                    operationalPlanSegmentList.add(segment);
                }
                OperationalPlan op =
                    new OperationalPlan(getGtu(), path, startTime, getGtu().getSpeed(), operationalPlanSegmentList);
                return op;
            }

    }

    /** {@inheritDoc} */
    @Override
    public final String getLongName()
    {
        return String.format("%s (a=%.1fm/s\u00b2, b=%.1fm/s\u00b2, s0=%.1fm, tSafe=%.1fs, delta=%.2f)", getName(), this.a
            .getSI(), this.b.getSI(), this.s0.getSI(), this.tSafe.getSI(), this.delta);
    }

    // The following is inherited from CarFollowingModel

    /** {@inheritDoc} */
    @Override
    public final Speed desiredSpeed(final BehavioralCharacteristics behavioralCharacteristics, final SpeedLimitInfo speedInfo)
        throws ParameterException
    {
        return null;
    }

    /** {@inheritDoc} */
    @Override
    public final Length desiredHeadway(final BehavioralCharacteristics behavioralCharacteristics, final Speed speed)
        throws ParameterException
    {
        return null;
    }

    /** {@inheritDoc} */
    @Override
    public final Acceleration followingAcceleration(final BehavioralCharacteristics behavioralCharacteristics,
        final Speed speed, final SpeedLimitInfo speedInfo, final SortedMap<Length, Speed> leaders) throws ParameterException
    {
        return null;
    }

    /** {@inheritDoc} */
    @Override
    public final String toString()
    {
        return "IDMOld [s0=" + this.s0 + ", a=" + this.a + ", b=" + this.b + ", tSafe=" + this.tSafe + ", stepSize="

        else if (ocStr.startsWith("LEFTSET"))
        {
            int lset1 = ocStr.indexOf('[') + 1;
            int rset1 = ocStr.indexOf(']', lset1);
            int lset2 = ocStr.indexOf('[', ocStr.indexOf("OVERTAKE")) + 1;
            int rset2 = ocStr.indexOf(']', lset2);
            if (lset1 == -1 || rset1 == -1 || rset1 - lset1 < 3 || lset2 == -1 || rset2 == -1 || rset2 - lset2 < 3)
            {
                throw new NetworkException("Sets in overtaking conditions string: '" + ocStr + "' not coded right");
            }
            Set<GTUType> overtakingGTUs = parseGTUTypeSet(ocStr.substring(lset1, rset1), parser);
            Set<GTUType> overtakenGTUs = parseGTUTypeSet(ocStr.substring(lset2, rset2), parser);
            if (ocStr.contains("RIGHTSPEED"))

        for (int i = 0; i < path.size(); i++)
        {
            Lane lane = path.get(i);
            lane.addListener(this, Lane.GTU_ADD_EVENT, true);
            lane.addListener(this, Lane.GTU_REMOVE_EVENT, true);
            try
            {
                // register the current GTUs on the lanes (if any) for statistics sampling.
                for (LaneBasedGTU gtu : lane.getGtuList())
                {
                    notify(new TimedEvent<OTSSimTimeDouble>(Lane.GTU_ADD_EVENT, lane, new Object[] { gtu.getId(), gtu },
                            gtu.getSimulator().getSimulatorTime()));
                }
            }
            catch (RemoteException exception)
            {
                exception.printStackTrace();
            }
            cumulativeLength += lane.getLength().getSI();
            endLengths[i] = cumulativeLength;
        }

                    Collection<Headway> sameLaneTraffic = new HashSet<>();
                    // TODO should it be getObjectType().isGtu() or !getObjectType().isDistanceOnly() ?
                    if (perception.getPerceptionCategory(DefaultAlexander.class).getForwardHeadway() != null
                        && perception.getPerceptionCategory(DefaultAlexander.class).getForwardHeadway().getObjectType()
                            .isGtu())
                    {
                        sameLaneTraffic.add(perception.getPerceptionCategory(DefaultAlexander.class).getForwardHeadway());
                    }
                    if (perception.getPerceptionCategory(DefaultAlexander.class).getBackwardHeadway() != null
                        && perception.getPerceptionCategory(DefaultAlexander.class).getBackwardHeadway().getObjectType()
                            .isGtu())
                    {
                        sameLaneTraffic.add(perception.getPerceptionCategory(DefaultAlexander.class).getBackwardHeadway());
                    }
                    DirectedLaneChangeModel dlcm = new DirectedAltruistic(getPerception());

            if (lastGtuDir.equals(GTUDirectionality.DIR_PLUS))
            {
                if (lastLink.getEndNode().equals(link.getStartNode()))
                {
                    // -----> O ----->, GTU moves ---->
                    lastGtuDir = GTUDirectionality.DIR_PLUS;
                    lastNode = (OTSNode) lastLink.getEndNode();
                }
                else
                {
                    // -----> O <-----, GTU moves ---->
                    lastGtuDir = GTUDirectionality.DIR_MINUS;
                    lastNode = (OTSNode) lastLink.getEndNode();
                }
            }
            else
            {
                if (lastLink.getStartNode().equals(link.getStartNode()))
                {
                    // <----- O ----->, GTU moves ---->
                    lastNode = (OTSNode) lastLink.getStartNode();
                    lastGtuDir = GTUDirectionality.DIR_PLUS;
                }
                else
                {
                    // <----- O <-----, GTU moves ---->
                    lastNode = (OTSNode) lastLink.getStartNode();
                    lastGtuDir = GTUDirectionality.DIR_MINUS;
                }
            }
            lastLink = links.iterator().next();

    {
        Time timestamp = getTimestamp();
        if (this.accessibleAdjacentLanesLeft == null || !timestamp.equals(this.accessibleAdjacentLanesLeft.getTimestamp()))
        {
            updateAccessibleAdjacentLanesLeft();
        }

        if (this.parallelHeadwaysLeft == null || !timestamp.equals(this.parallelHeadwaysLeft.getTimestamp()))
        {
            updateParallelHeadwaysLeft();
        }

        // for the accessible lanes, see who is ahead of us and in front of us
        Length maximumForwardHeadway = getGtu().getBehavioralCharacteristics().getParameter(ParameterTypes.LOOKAHEAD);
        Length maximumReverseHeadway = getGtu().getBehavioralCharacteristics().getParameter(ParameterTypes.LOOKBACKOLD);
        this.neighboringHeadwaysLeft =
            new TimeStampedObject<>(collectNeighborLaneTraffic(LateralDirectionality.LEFT, timestamp, maximumForwardHeadway,
                maximumReverseHeadway), timestamp);
    }

    {
        Time timestamp = getTimestamp();
        if (this.accessibleAdjacentLanesRight == null || !timestamp.equals(this.accessibleAdjacentLanesRight.getTimestamp()))
        {
            updateAccessibleAdjacentLanesRight();
        }

        if (this.parallelHeadwaysRight == null || !timestamp.equals(this.parallelHeadwaysRight.getTimestamp()))
        {
            updateParallelHeadwaysRight();
        }

        // for the accessible lanes, see who is ahead of us and in front of us
        Length maximumForwardHeadway = getGtu().getBehavioralCharacteristics().getParameter(ParameterTypes.LOOKAHEAD);
        Length maximumReverseHeadway = getGtu().getBehavioralCharacteristics().getParameter(ParameterTypes.LOOKBACKOLD);
        this.neighboringHeadwaysRight =
            new TimeStampedObject<>(collectNeighborLaneTraffic(LateralDirectionality.RIGHT, timestamp,
                maximumForwardHeadway, maximumReverseHeadway), timestamp);
    }

        final CrossSectionLink link = makeLink(name, from, to, intermediatePoints, direction);
        Lane[] result = new Lane[laneCount];
        Length width = new Length(4.0, LengthUnit.METER);
        for (int laneIndex = 0; laneIndex < laneCount; laneIndex++)
        {
            // Be ware! LEFT is lateral positive, RIGHT is lateral negative.
            Length latPosAtStart =
                new Length((-0.5 - laneIndex - laneOffsetAtStart) * width.getSI(), LengthUnit.SI);
            Length latPosAtEnd =
                new Length((-0.5 - laneIndex - laneOffsetAtEnd) * width.getSI(), LengthUnit.SI);
            result[laneIndex] =
                makeLane(link, "lane." + laneIndex, laneType, latPosAtStart, latPosAtEnd, width, speedLimit, simulator,
                    direction);
        }
        return result;
    }

    public final void updateParallelHeadwaysLeft() throws GTUException
    {
        Time timestamp = getTimestamp();
        if (this.accessibleAdjacentLanesLeft == null || !timestamp.equals(this.accessibleAdjacentLanesLeft.getTimestamp()))
        {
            updateAccessibleAdjacentLanesLeft();
        }
        Set<Headway> parallelHeadwaySet = new HashSet<>();
        for (Lane lane : this.accessibleAdjacentLanesLeft.getObject().keySet())
        {
            for (Lane adjacentLane : this.accessibleAdjacentLanesLeft.getObject().get(lane))
            {
                parallelHeadwaySet.addAll(parallel(adjacentLane, timestamp));
            }
        }
        this.parallelHeadwaysLeft = new TimeStampedObject<>(parallelHeadwaySet, timestamp);
    }

    public final void updateParallelHeadwaysRight() throws GTUException
    {
        Time timestamp = getTimestamp();
        if (this.accessibleAdjacentLanesRight == null || !timestamp.equals(this.accessibleAdjacentLanesRight.getTimestamp()))
        {
            updateAccessibleAdjacentLanesRight();
        }
        Set<Headway> parallelHeadwaySet = new HashSet<>();
        for (Lane lane : this.accessibleAdjacentLanesRight.getObject().keySet())
        {
            for (Lane adjacentLane : this.accessibleAdjacentLanesRight.getObject().get(lane))
            {
                parallelHeadwaySet.addAll(parallel(adjacentLane, timestamp));
            }
        }
        this.parallelHeadwaysRight = new TimeStampedObject<>(parallelHeadwaySet, timestamp);
    }