AbstractLaneBasedGTU.java

package org.opentrafficsim.core.gtu.lane;

import java.rmi.RemoteException;
import java.util.ArrayList;
import java.util.Collection;
import java.util.HashMap;
import java.util.HashSet;
import java.util.List;
import java.util.Map;
import java.util.Set;

import javax.media.j3d.Bounds;
import javax.naming.NamingException;
import javax.vecmath.Point3d;

import nl.tudelft.simulation.dsol.SimRuntimeException;
import nl.tudelft.simulation.language.d3.BoundingBox;
import nl.tudelft.simulation.language.d3.DirectedPoint;

import org.opentrafficsim.core.dsol.OTSDEVSSimulatorInterface;
import org.opentrafficsim.core.gtu.AbstractGTU;
import org.opentrafficsim.core.gtu.GTUException;
import org.opentrafficsim.core.gtu.GTUType;
import org.opentrafficsim.core.gtu.RelativePosition;
import org.opentrafficsim.core.gtu.following.AccelerationStep;
import org.opentrafficsim.core.gtu.following.GTUFollowingModel;
import org.opentrafficsim.core.gtu.following.HeadwayGTU;
import org.opentrafficsim.core.gtu.lane.changing.LaneChangeModel;
import org.opentrafficsim.core.gtu.lane.changing.LaneMovementStep;
import org.opentrafficsim.core.network.LateralDirectionality;
import org.opentrafficsim.core.network.Link;
import org.opentrafficsim.core.network.NetworkException;
import org.opentrafficsim.core.network.lane.CrossSectionElement;
import org.opentrafficsim.core.network.lane.CrossSectionLink;
import org.opentrafficsim.core.network.lane.Lane;
import org.opentrafficsim.core.unit.AccelerationUnit;
import org.opentrafficsim.core.unit.LengthUnit;
import org.opentrafficsim.core.unit.SpeedUnit;
import org.opentrafficsim.core.unit.TimeUnit;
import org.opentrafficsim.core.value.conversions.Calc;
import org.opentrafficsim.core.value.vdouble.scalar.DoubleScalar;
import org.opentrafficsim.core.value.vdouble.scalar.DoubleScalar.Rel;

import com.vividsolutions.jts.geom.Coordinate;
import com.vividsolutions.jts.geom.LineString;
import com.vividsolutions.jts.linearref.LengthIndexedLine;

/**
 * <p>
 * Copyright (c) 2013-2014 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 Oct 22, 2014 <br>
 * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
 * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
 * @param <ID> The type of ID, e.g., String or Integer
 */
public abstract class AbstractLaneBasedGTU<ID> extends AbstractGTU<ID> implements LaneBasedGTU<ID>
{
    /** */
    private static final long serialVersionUID = 20140822L;

    /** Time of last evaluation. */
    private DoubleScalar.Abs<TimeUnit> lastEvaluationTime;

    /** Time of next evaluation. */
    private DoubleScalar.Abs<TimeUnit> nextEvaluationTime;

    /**
     * Fractional longitudinal positions of the reference point of the GTU on one or more links at the lastEvaluationTime.
     * Because the reference point of the GTU might not be on all the links the GTU is registered on, the fractional
     * longitudinal positions can be more than one, or less than zero.
     */
    private final Map<Link<?, ?>, Double> fractionalLinkPositions = new HashMap<>();

    /**
     * The lanes the GTU is registered on. Each lane has to have its link registered in the fractionalLinkPositions as well to
     * keep consistency. Each link from the fractionalLinkPositions can have one or more Lanes on which the vehicle is
     * registered. This is a list to improve reproducibility: The 'oldest' lanes on which the vehicle is registered are at the
     * front of the list, the later ones more to the back.
     */
    private final List<Lane> lanes = new ArrayList<>();

    /**
     * @return lanes.
     */
    public final List<Lane> getLanes()
    {
        return this.lanes;
    }

    /** Speed at lastEvaluationTime. */
    private DoubleScalar.Abs<SpeedUnit> speed;

    /** lateral velocity at lastEvaluationTime. */
    private DoubleScalar.Abs<SpeedUnit> lateralVelocity;

    /** acceleration (negative values indicate deceleration) at the lastEvaluationTime. */
    private DoubleScalar.Abs<AccelerationUnit> acceleration = new DoubleScalar.Abs<AccelerationUnit>(0,
        AccelerationUnit.METER_PER_SECOND_2);

    /** CarFollowingModel used by this GTU. */
    private final GTUFollowingModel gtuFollowingModel;

    /** LaneChangeModel used by this GTU. */
    private final LaneChangeModel laneChangeModel;

    /**
     * Construct a Lane Based GTU.
     * @param id the id of the GTU, could be String or Integer
     * @param gtuType the type of GTU, e.g. TruckType, CarType, BusType
     * @param gtuFollowingModel the following model, including a reference to the simulator.
     * @param laneChangeModel LaneChangeModel; the lane change model
     * @param initialLongitudinalPositions the initial positions of the car on one or more lanes
     * @param initialSpeed the initial speed of the car on the lane
     * @param simulator to initialize the move method and to get the current time
     * @throws RemoteException when the simulator cannot be reached
     * @throws NetworkException when the GTU cannot be placed on the given lane
     * @throws SimRuntimeException when the move method cannot be scheduled
     */
    public AbstractLaneBasedGTU(final ID id, final GTUType<?> gtuType, final GTUFollowingModel gtuFollowingModel,
        final LaneChangeModel laneChangeModel, final Map<Lane, DoubleScalar.Rel<LengthUnit>> initialLongitudinalPositions,
        final DoubleScalar.Abs<SpeedUnit> initialSpeed, final OTSDEVSSimulatorInterface simulator) throws RemoteException,
        NetworkException, SimRuntimeException
    {
        super(id, gtuType);
        this.gtuFollowingModel = gtuFollowingModel;
        this.laneChangeModel = laneChangeModel;
        this.lateralVelocity = new DoubleScalar.Abs<SpeedUnit>(0.0, SpeedUnit.METER_PER_SECOND);

        // register the GTU on the lanes
        for (Lane lane : initialLongitudinalPositions.keySet())
        {
            this.lanes.add(lane);
            this.fractionalLinkPositions.put(lane.getParentLink(), lane.fraction(initialLongitudinalPositions.get(lane)));
            lane.addGTU(this, initialLongitudinalPositions.get(lane));
        }

        // Duplicate the other arguments as these are modified in this class and may be re-used by the caller
        this.lastEvaluationTime = new DoubleScalar.Abs<TimeUnit>(simulator.getSimulatorTime().get());
        this.speed = new DoubleScalar.Abs<SpeedUnit>(initialSpeed);
        this.nextEvaluationTime = new DoubleScalar.Abs<TimeUnit>(simulator.getSimulatorTime().get());

        // start the movement of the GTU
        simulator.scheduleEventNow(this, this, "move", null);
    }

    /** {@inheritDoc} */
    @Override
    public final DoubleScalar.Abs<SpeedUnit> getLongitudinalVelocity(final DoubleScalar.Abs<TimeUnit> when)
    {
        DoubleScalar.Rel<TimeUnit> dT = DoubleScalar.minus(when, this.lastEvaluationTime).immutable();
        return DoubleScalar.plus(this.speed, Calc.accelerationTimesTime(this.getAcceleration(when), dT)).immutable();
    }

    /** {@inheritDoc} */
    @Override
    public final DoubleScalar.Abs<SpeedUnit> getLongitudinalVelocity() throws RemoteException
    {
        return getLongitudinalVelocity(getSimulator().getSimulatorTime().get());
    }

    /** {@inheritDoc} */
    @Override
    public final DoubleScalar.Abs<TimeUnit> getLastEvaluationTime()
    {
        return new DoubleScalar.Abs<TimeUnit>(this.lastEvaluationTime);
    }

    /** {@inheritDoc} */
    @Override
    public final DoubleScalar.Abs<TimeUnit> getNextEvaluationTime()
    {
        return this.nextEvaluationTime;
    }

    /** {@inheritDoc} */
    @Override
    public final DoubleScalar.Abs<AccelerationUnit> getAcceleration(final DoubleScalar.Abs<TimeUnit> when)
    {
        // Currently the acceleration is independent of when; it is constant during the evaluation interval
        return new DoubleScalar.Abs<AccelerationUnit>(this.acceleration);
    }

    /** {@inheritDoc} */
    @Override
    public final DoubleScalar.Abs<AccelerationUnit> getAcceleration() throws RemoteException
    {
        return getAcceleration(getSimulator().getSimulatorTime().get());
    }

    /** {@inheritDoc} */
    @Override
    public final DoubleScalar.Abs<SpeedUnit> getLateralVelocity()
    {
        return new DoubleScalar.Abs<SpeedUnit>(this.lateralVelocity);
    }

    @Override
    public final void addFrontToSubsequentLane(final Lane lane) throws RemoteException, NetworkException
    {
        Lane prevLane = this.lanes.get(0); // TODO exceptions en zo.
        double positionPrevTimeStepSI = position(prevLane, getReference(), this.lastEvaluationTime).getSI();
        double positionNowSI = position(prevLane, getReference()).getSI();
        DoubleScalar.Rel<LengthUnit> position =
            new DoubleScalar.Rel<>(positionPrevTimeStepSI - positionNowSI
                - (getFront().getDx().getSI() - getReference().getDx().getSI()), LengthUnit.SI);
        addLane(lane, position);
    }

    /** {@inheritDoc} */
    @Override
    public final void addLane(final Lane lane, final DoubleScalar.Rel<LengthUnit> position) throws NetworkException
    {
        if (this.lanes.contains(lane))
        {
            throw new NetworkException("GTU " + toString() + " is already registered on this lane: " + lane);
        }
        // if the GTU is already registered on a lane of the same link, do not change its fractional position, as this
        // might lead to a "jump".
        if (!this.fractionalLinkPositions.containsKey(lane.getParentLink()))
        {
            this.fractionalLinkPositions.put(lane.getParentLink(), lane.fraction(position));
        }
        this.lanes.add(lane);
    }

    /** {@inheritDoc} */
    @Override
    public final void removeLane(final Lane lane)
    {
        this.lanes.remove(lane);
        // check of there are any lanes for this link left. If not, remove the link.
        boolean found = false;
        for (Lane l : this.lanes)
        {
            if (l.getParentLink().equals(lane.getParentLink()))
            {
                found = true;
            }
        }
        if (!found)
        {
            this.fractionalLinkPositions.remove(lane.getParentLink());
        }
    }

    /**
     * Set the new state.
     * @param cfmr AccelerationStep; the new state of this GTU
     * @throws RemoteException when simulator time could not be retrieved or sensor trigger scheduling fails.
     * @throws NetworkException when the vehicle is not on the given lane.
     * @throws SimRuntimeException when sensor trigger(s) cannot be scheduled on the simulator.
     */
    private void setState(final AccelerationStep cfmr) throws RemoteException, NetworkException, SimRuntimeException
    {
        if (cfmr.getAcceleration().getSI() < -9999)
        {
            System.out.println("Problem");
        }
        // GTUs move based on their fractional position to stay aligned when registered in parallel lanes.
        // The "oldest" lane of parallel lanes takes preference when updating the fractional position.
        // So we work from back to front.
        for (int i = this.lanes.size() - 1; i >= 0; i--)
        {
            Lane lane = this.lanes.get(i);
            this.fractionalLinkPositions.put(lane.getParentLink(), lane.fraction(position(lane, getReference(),
                this.nextEvaluationTime)));
        }
        // Compute and set the current speed using the "old" nextEvaluationTime and acceleration
        this.speed = getLongitudinalVelocity(this.nextEvaluationTime);
        // Update lastEvaluationTime and then set the new nextEvaluationTime
        this.lastEvaluationTime = this.nextEvaluationTime;
        this.nextEvaluationTime = cfmr.getValidUntil();
        this.acceleration = cfmr.getAcceleration();

        // // Does our front reference point enter new lane(s) during the next time step? If so, add to our lane list!
        // // Note: the trigger at the start of the lane will add the vehicle to that lane at the exact right time.
        // List<Lane> lanesToCheck = new ArrayList<Lane>(this.lanes);
        // while (!lanesToCheck.isEmpty())
        // {
        // Lane lane = lanesToCheck.remove(0);
        // double frontPosSI = position(lane, getFront(), this.lastEvaluationTime).getSI();
        // // TODO speed this up by using SI units, caching, etc.
        // if (lane.fractionSI(frontPosSI) <= 1.0
        // && lane.fraction(position(lane, getFront(), this.nextEvaluationTime)) > 1.0)
        // {
        // for (Lane nextLane : lane.nextLanes())
        // {
        // // Only follow links on the Route if there is a Route, and we haven't added this lane already
        // if (!this.lanes.contains(nextLane)
        // && (this.getRoute() == null || this.getRoute() != null
        // && this.getRoute().containsLink(nextLane.getParentLink())))
        // {
        // this.lanes.add(nextLane);
        // if (!this.fractionalLinkPositions.containsKey(nextLane.getParentLink()))
        // {
        // double positionSI = frontPosSI - lane.getLength().getSI() - getFront().getDx().getSI();
        // this.fractionalLinkPositions.put(nextLane.getParentLink(), nextLane.fractionSI(positionSI));
        // }
        // lanesToCheck.add(nextLane);
        // }
        // }
        // }
        // }
        // Execute all samplers
        for (Lane lane : this.lanes)
        {
            lane.sample(this);
        }
        // System.out.println("setState: " + cfmr + " " + this + " next evaluation is " + cfmr.getValidUntil());
    }

    /**
     * @throws RemoteException RemoteException
     * @throws NamingException on ???
     * @throws NetworkException on network inconsistency
     * @throws SimRuntimeException on not being able to reschedule the move() method.
     */
    protected final void move() throws RemoteException, NamingException, NetworkException, GTUException, SimRuntimeException
    {
//         if (getId().toString().equals("41")) // && getSimulator().getSimulatorTime().get().getSI() > 7.9)
//         {
//         System.out.println("Debug me: " + this);
//         }

        // Sanity check
        if (getSimulator().getSimulatorTime().get().getSI() != getNextEvaluationTime().getSI())
        {
            throw new Error("move called at wrong time: expected time " + getNextEvaluationTime() + " simulator time is : "
                + getSimulator().getSimulatorTime().get());
        }
        // only carry out move() if we still have lane(s) to drive on.
        // Note: a (Sink) trigger can have 'destroyed' us between the previous evaluation step and this one.
        if (this.lanes.isEmpty())
        {
            return;
        }
        DoubleScalar.Rel<LengthUnit> maximumForwardHeadway = new DoubleScalar.Rel<LengthUnit>(500.0, LengthUnit.METER);
        // TODO 500?
        DoubleScalar.Rel<LengthUnit> maximumReverseHeadway = new DoubleScalar.Rel<LengthUnit>(-200.0, LengthUnit.METER);
        // TODO 200?
        DoubleScalar.Abs<SpeedUnit> speedLimit = new DoubleScalar.Abs<SpeedUnit>(100.0, SpeedUnit.KM_PER_HOUR);
        // TODO should be the local speed limit and based on the maximum lane speed and the maximum GTU speed
        if (null != this.laneChangeModel)
        {
            Collection<HeadwayGTU> sameLaneTraffic = new ArrayList<HeadwayGTU>();
            HeadwayGTU leader = headway(maximumForwardHeadway);
            if (null != leader.getOtherGTU())
            {
                sameLaneTraffic.add(leader);
            }
            HeadwayGTU follower = headway(maximumReverseHeadway);
            if (null != follower.getOtherGTU())
            {
                sameLaneTraffic.add(new HeadwayGTU(follower.getOtherGTU(), -follower.getDistanceSI()));
            }
            DoubleScalar.Abs<TimeUnit> now = getSimulator().getSimulatorTime().get();
            Collection<HeadwayGTU> leftLaneTraffic =
                collectNeighborLaneTraffic(LateralDirectionality.LEFT, now, maximumForwardHeadway, maximumReverseHeadway);
            Collection<HeadwayGTU> rightLaneTraffic =
                collectNeighborLaneTraffic(LateralDirectionality.RIGHT, now, maximumForwardHeadway, maximumReverseHeadway);
            LaneMovementStep lcmr =
                this.laneChangeModel.computeLaneChangeAndAcceleration(this, sameLaneTraffic, rightLaneTraffic,
                    leftLaneTraffic, speedLimit, new DoubleScalar.Rel<AccelerationUnit>(0.3,
                        AccelerationUnit.METER_PER_SECOND_2), new DoubleScalar.Rel<AccelerationUnit>(0.1,
                        AccelerationUnit.METER_PER_SECOND_2), new DoubleScalar.Rel<AccelerationUnit>(-0.3,
                        AccelerationUnit.METER_PER_SECOND_2));
            // First move this GTU forward (to its current location)
            setState(lcmr.getGfmr());

            // Then change onto laterally adjacent lane(s) if the LaneMovementStep indicates a lane change

            if (lcmr.getLaneChange() != null)
            {
                synchronized (this.lanes)
                {
                    // TODO: make lane changes gradual (not instantaneous; like now)
                    Collection<Lane> oldLaneSet = new ArrayList<Lane>(this.lanes);
                    Collection<Lane> newLaneSet = adjacentLanes(lcmr.getLaneChange());
                    // Remove this GTU from all of the Lanes that it is on and remember the fractional position on each one
                    Map<Lane, Double> oldFractionalPositions = new HashMap<Lane, Double>();
                    for (Lane l : this.lanes)
                    {
                        oldFractionalPositions.put(l, fractionalPosition(l, getReference(), getLastEvaluationTime()));
                        this.fractionalLinkPositions.remove(l.getParentLink());
                    }
                    for (Lane l : oldFractionalPositions.keySet())
                    {
                        l.removeGTU(this);
                        removeLane(l);
                    }
                    ArrayList<Lane> replacementLanes = new ArrayList<Lane>(); // for DEBUG
                    // Add this GTU to the lanes in newLaneSet
                    // This could be rewritten to be more efficient.
                    for (Lane newLane : newLaneSet)
                    {
                        Double fractionalPosition = null;
                        // find ONE lane in oldLaneSet that has l as neighbor
                        for (Lane oldLane : oldLaneSet)
                        {
                            if (oldLane.accessibleAdjacentLanes(lcmr.getLaneChange(), getGTUType()).contains(newLane))
                            {
                                fractionalPosition = oldFractionalPositions.get(oldLane);
                                break;
                            }
                        }
                        if (null == fractionalPosition)
                        {
                            throw new Error("Program error: Cannot find an oldLane that has newLane " + newLane + " as "
                                + lcmr.getLaneChange() + " neighbor");
                        }
                        newLane.addGTU(this, fractionalPosition);
                        addLane(newLane, (Rel<LengthUnit>) newLane.getLength().mutable().multiply(fractionalPosition)
                            .immutable());
                        replacementLanes.add(newLane);
                    }
                    System.out.println("GTU " + this + " changed lanes from: " + oldLaneSet + " to " + replacementLanes);
                    checkConsistency();
                }
            }
            // }
            else
            {
                // throw new GTUException("All LaneBasedGTUs should have a LaneChangeModel");
                /*- TODO the rest of this method should disappear; all GTUs should have a LaneChangeModel
                HeadwayGTU leader = headway(maximumForwardHeadway);
                AccelerationStep as =
                    null != leader.getOtherGTU() ? getGTUFollowingModel().computeAcceleration(this, leader.getOtherGTU().getLateralVelocity(),
                        leader.getDistance(), speedLimit) : new AccelerationStep(new DoubleScalar.Abs<AccelerationUnit>(0,
                        AccelerationUnit.SI), DoubleScalar.plus(getSimulator().getSimulatorTime().get(),
                        getGTUFollowingModel().getStepSize()).immutable());
                setState(as);
                 */
            }
            // Schedule all sensor triggers that are going to happen until the next evaluation time.
            scheduleTriggers();
            getSimulator().scheduleEventAbs(this.getNextEvaluationTime(), this, this, "move", null);
        }
    }

    /**
     * Verify that all the lanes registered in this GTU have this GTU registered as well and vice versa.
     */
    private void checkConsistency()
    {
        for (Lane l : this.lanes)
        {
            if (!this.fractionalLinkPositions.containsKey(l.getParentLink()))
            {
                System.err.println("GTU " + this + "XXXXXXXX");
            }
        }
        for (Link<?, ?> csl : this.fractionalLinkPositions.keySet())
        {
            boolean found = false;
            for (Lane l : this.lanes)
            {
                if (l.getParentLink().equals(csl))
                {
                    found = true;
                    break;
                }
            }
            if (!found)
            {
                System.err.println("GTU " + this + "YYYYYYYYY");
            }
        }
    }

    /**
     * Schedule the triggers for this GTU during the new time period.
     * @throws NetworkException on network inconsistency
     * @throws RemoteException on communications failure
     * @throws SimRuntimeException should never happen
     */
    private void scheduleTriggers() throws NetworkException, RemoteException, SimRuntimeException
    {
        // Does our front reference point enter new lane(s) during the next time step?
        // Note: the trigger at the start of the lane will add the vehicle to that lane at the exact right time.
        for (Lane lane : this.lanes)
        {
            double frontPosSI = position(lane, getFront(), this.lastEvaluationTime).getSI();
            double frontPosAtNextEvalSI = position(lane, getFront(), this.nextEvaluationTime).getSI();
            double remainingDistanceSI = lane.getLength().getSI() - frontPosSI;
            //double excessNextLaneSI = frontPosAtNextEvalSI - lane.getLength().getSI();
            double moveSI = frontPosAtNextEvalSI - frontPosSI;
            if (lane.fractionSI(frontPosSI) <= 1.0 && lane.fractionSI(frontPosAtNextEvalSI) > 1.0)
            {
                for (Lane nextLane : lane.nextLanes())
                {
                    // Only follow links on the Route if there is a Route, and we haven't added this lane already
                    if (!this.lanes.contains(nextLane)
                        && (this.getRoute() == null || this.getRoute() != null
                            && this.getRoute().containsLink(nextLane.getParentLink())))
                    {
                        nextLane.scheduleTriggers(this, -remainingDistanceSI - getFront().getDx().getSI(), moveSI);
                    }
                }
            }
            // also schedule any triggers for the current lane(s)
            lane.scheduleTriggers(this, lane.positionSI(this.fractionalLinkPositions.get(lane.getParentLink())), moveSI);
        }

        // for (Lane lane : triggerLanes)
        // {
        // double dt = this.nextEvaluationTime.getSI() - this.getLastEvaluationTime().getSI();
        //
        // double frontPosSI = position(lane, getFront(), this.lastEvaluationTime).getSI();
        // double positionSI = frontPosSI - lane.getLength().getSI() - getFront().getDx().getSI();
        // fractionalLinkPosition = lane.fractionSI(positionSI);
        // }
    }

    /**
     * Collect relevant traffic in adjacent lanes. Parallel traffic is included with headway equal to Double.NaN.
     * @param directionality LateralDirectionality; either <cite>LateralDirectionality.LEFT</cite>, or
     *            <cite>LateralDirectionality.RIGHT</cite>
     * @param when DoubleScalar.Abs&lt;TimeUnit&gt;; the (current) time
     * @param maximumForwardHeadway DoubleScalar.Rel&lt;LengthUnit&gt;; the maximum forward search distance
     * @param maximumReverseHeadway DoubleScalar.Rel&lt;LengthUnit&gt;; the maximum reverse search distance
     * @return Collection&lt;LaneBasedGTU&lt;?&gt;&gt;;
     * @throws RemoteException on communications failure
     * @throws NetworkException on network inconsistency
     */
    private Collection<HeadwayGTU> collectNeighborLaneTraffic(final LateralDirectionality directionality,
        final DoubleScalar.Abs<TimeUnit> when, final DoubleScalar.Rel<LengthUnit> maximumForwardHeadway,
        final DoubleScalar.Rel<LengthUnit> maximumReverseHeadway) throws RemoteException, NetworkException
    {
        Collection<HeadwayGTU> result = new HashSet<HeadwayGTU>();
        for (LaneBasedGTU<?> p : parallel(directionality, when))
        {
            result.add(new HeadwayGTU(p, Double.NaN));
        }
        for (Lane adjacentLane : adjacentLanes(directionality))
        {
            HeadwayGTU leader = headway(adjacentLane, maximumForwardHeadway);
            if (null != leader.getOtherGTU() && !result.contains(leader))
            {
                result.add(leader);
            }
            HeadwayGTU follower = headway(adjacentLane, maximumReverseHeadway);
            if (null != follower.getOtherGTU() && !result.contains(follower))
            {
                result.add(new HeadwayGTU(follower.getOtherGTU(), -follower.getDistanceSI()));
            }
        }
        return result;
    }

    /** {@inheritDoc} */
    @Override
    public final Map<Lane, DoubleScalar.Rel<LengthUnit>> positions(final RelativePosition relativePosition)
        throws NetworkException, RemoteException
    {
        return positions(relativePosition, getSimulator().getSimulatorTime().get());
    }

    /** {@inheritDoc} */
    @Override
    public final Map<Lane, DoubleScalar.Rel<LengthUnit>> positions(final RelativePosition relativePosition,
        final DoubleScalar.Abs<TimeUnit> when) throws NetworkException
    {
        Map<Lane, DoubleScalar.Rel<LengthUnit>> positions = new HashMap<>();
        for (Lane lane : this.lanes)
        {
            positions.put(lane, position(lane, relativePosition, when));
        }
        return positions;
    }

    /** {@inheritDoc} */
    @Override
    public final DoubleScalar.Rel<LengthUnit> position(final Lane lane, final RelativePosition relativePosition)
        throws NetworkException, RemoteException
    {
        return position(lane, relativePosition, getSimulator().getSimulatorTime().get());
    }

    /** {@inheritDoc} */
    public final DoubleScalar.Rel<LengthUnit> projectedPosition(final Lane projectionLane,
        final RelativePosition relativePosition, final DoubleScalar.Abs<TimeUnit> when) throws NetworkException
    {
        CrossSectionLink<?, ?> link = projectionLane.getParentLink();
        for (CrossSectionElement cse : link.getCrossSectionElementList())
        {
            if (cse instanceof Lane)
            {
                Lane cseLane = (Lane) cse;
                if (this.lanes.contains(cseLane))
                {
                    double fractionalPosition = fractionalPosition(cseLane, relativePosition, when);
                    return new DoubleScalar.Rel<LengthUnit>(projectionLane.getLength().getSI() * fractionalPosition,
                        LengthUnit.SI);
                }
            }
        }
        throw new NetworkException("GTU " + this + " is not on any lane of Link " + link);
    }

    /** {@inheritDoc} */
    @Override
    public final DoubleScalar.Rel<LengthUnit> position(final Lane lane, final RelativePosition relativePosition,
        final DoubleScalar.Abs<TimeUnit> when) throws NetworkException
    {
        if (null == lane)
        {
            throw new NetworkException("lane is null");
        }
        if (!this.lanes.contains(lane))
        {
            throw new NetworkException("GTU is not on lane " + lane.toString());
        }
        if (!this.fractionalLinkPositions.containsKey(lane.getParentLink()))
        {
            throw new NetworkException("GTU does not have a fractional position on " + lane.toString());
        }
        DoubleScalar.Rel<LengthUnit> longitudinalPosition =
            lane.position(this.fractionalLinkPositions.get(lane.getParentLink()));
        if (longitudinalPosition == null)
        {
            throw new NetworkException("GetPosition: GTU " + toString() + " not in lane " + lane);
        }
        DoubleScalar.Rel<TimeUnit> dT = DoubleScalar.minus(when, this.lastEvaluationTime).immutable();
        DoubleScalar.Rel<LengthUnit> loc =
            DoubleScalar.plus(
                DoubleScalar.plus(DoubleScalar.plus(longitudinalPosition, Calc.speedTimesTime(this.speed, dT)).immutable(),
                    Calc.accelerationTimesTimeSquaredDiv2(this.getAcceleration(when), dT)).immutable(),
                relativePosition.getDx()).immutable();
        if (Double.isNaN(loc.getSI()))
        {
            System.out.println("loc is NaN");
        }
        return loc;
    }

    /** {@inheritDoc} */
    @Override
    public final Map<Lane, Double> fractionalPositions(final RelativePosition relativePosition) throws NetworkException,
        RemoteException
    {
        return fractionalPositions(relativePosition, getSimulator().getSimulatorTime().get());
    }

    /** {@inheritDoc} */
    @Override
    public final Map<Lane, Double> fractionalPositions(final RelativePosition relativePosition,
        final DoubleScalar.Abs<TimeUnit> when) throws NetworkException
    {
        Map<Lane, Double> positions = new HashMap<>();
        for (Lane lane : this.lanes)
        {
            positions.put(lane, fractionalPosition(lane, relativePosition, when));
        }
        return positions;
    }

    /** {@inheritDoc} */
    @Override
    public final double fractionalPosition(final Lane lane, final RelativePosition relativePosition,
        final DoubleScalar.Abs<TimeUnit> when) throws NetworkException
    {
        return position(lane, relativePosition, when).getSI() / lane.getLength().getSI();
    }

    /** {@inheritDoc} */
    @Override
    public final double fractionalPosition(final Lane lane, final RelativePosition relativePosition)
        throws NetworkException, RemoteException
    {
        return position(lane, relativePosition).getSI() / lane.getLength().getSI();
    }

    /**
     * Calculate the minimum headway, possibly on subsequent lanes, in forward direction.
     * @param lane the lane where we are looking right now
     * @param lanePositionSI from which position on this lane do we start measuring? This is the current position of the GTU
     *            when we measure in the lane where the original GTU is positioned, and 0.0 for each subsequent lane
     * @param cumDistanceSI the distance we have already covered searching on previous lanes
     * @param maxDistanceSI the maximum distance to look for in SI units; stays the same in subsequent calls
     * @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 RemoteException when the simulation time cannot be retrieved
     * @throws NetworkException when there is a problem with the geometry of the network
     */
    private HeadwayGTU headwayRecursiveForwardSI(final Lane lane, final double lanePositionSI, final double cumDistanceSI,
        final double maxDistanceSI, final DoubleScalar.Abs<TimeUnit> when) throws RemoteException, NetworkException
    {
        LaneBasedGTU<?> otherGTU =
            lane.getGtuAfter(new DoubleScalar.Rel<LengthUnit>(lanePositionSI, LengthUnit.METER), RelativePosition.REAR, when);
        if (otherGTU != null)
        {
            double distanceM = cumDistanceSI + otherGTU.position(lane, otherGTU.getRear(), when).getSI() - lanePositionSI;
            if (distanceM > 0 && distanceM <= maxDistanceSI)
            {
                return new HeadwayGTU(otherGTU, distanceM);
            }
            return new HeadwayGTU(null, Double.MAX_VALUE);
        }

        // Continue search on successor lanes.
        if (cumDistanceSI + lane.getLength().getSI() - lanePositionSI < maxDistanceSI)
        {
            // is there a successor link?
            Set<Lane> nextLanes = lane.nextLanes();
            if (nextLanes.size() > 0)
            {
                HeadwayGTU foundMaxGTUDistanceSI = new HeadwayGTU(null, Double.MAX_VALUE);
                for (Lane nextLane : nextLanes)
                {
                    // Only follow links on the Route if there is a Route
                    if (this.getRoute() == null || this.getRoute() != null
                        && this.getRoute().containsLink(lane.getParentLink()))
                    {
                        double traveledDistanceSI = cumDistanceSI + lane.getLength().getSI() - lanePositionSI;
                        HeadwayGTU closest =
                            headwayRecursiveForwardSI(nextLane, 0.0, traveledDistanceSI, maxDistanceSI, when);
                        if (closest.getDistanceSI() < maxDistanceSI
                            && closest.getDistanceSI() < foundMaxGTUDistanceSI.getDistanceSI())
                        {
                            foundMaxGTUDistanceSI = closest;
                        }
                    }
                }
                return foundMaxGTUDistanceSI;
            }
        }

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

    /**
     * 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 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 RemoteException when the simulation time cannot be retrieved
     * @throws NetworkException when there is a problem with the geometry of the network
     */
    private HeadwayGTU headwayRecursiveBackwardSI(final Lane lane, final double lanePositionSI, final double cumDistanceSI,
        final double maxDistanceSI, final DoubleScalar.Abs<TimeUnit> when) throws RemoteException, NetworkException
    {
        LaneBasedGTU<?> otherGTU =
            lane.getGtuBefore(new DoubleScalar.Rel<LengthUnit>(lanePositionSI, LengthUnit.METER), RelativePosition.FRONT,
                when);
        if (otherGTU != null)
        {
            double distanceM = cumDistanceSI + lanePositionSI - otherGTU.position(lane, otherGTU.getFront(), when).getSI();
            if (distanceM > 0 && distanceM <= maxDistanceSI)
            {
                return new HeadwayGTU(otherGTU, distanceM);
            }
            return new HeadwayGTU(null, Double.MAX_VALUE);
        }

        // Continue search on predecessor lanes.
        if (cumDistanceSI + lanePositionSI < maxDistanceSI)
        {
            // is there a predecessor link?
            Set<Lane> prevLanes = lane.prevLanes();
            if (prevLanes.size() > 0)
            {
                HeadwayGTU foundMaxGTUDistanceSI = new HeadwayGTU(null, Double.MAX_VALUE);
                for (Lane prevLane : prevLanes)
                {
                    // What is behind us is INDEPENDENT of the followed route!
                    double traveledDistanceSI = cumDistanceSI + lanePositionSI;
                    HeadwayGTU closest =
                        headwayRecursiveBackwardSI(prevLane, prevLane.getLength().getSI(), traveledDistanceSI,
                            maxDistanceSI, when);
                    if (closest.getDistanceSI() < maxDistanceSI
                        && closest.getDistanceSI() < foundMaxGTUDistanceSI.getDistanceSI())
                    {
                        foundMaxGTUDistanceSI = closest;
                    }
                }
                return foundMaxGTUDistanceSI;
            }
        }

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

    /**
     * @param maxDistanceSI the maximum distance to look for in SI units
     * @return the nearest GTU and the net headway to this GTU 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 RemoteException when the simulation time cannot be retrieved
     * @throws NetworkException when there is a problem with the geometry of the network
     */
    private HeadwayGTU headwayGTUSI(final double maxDistanceSI) throws RemoteException, NetworkException
    {
        DoubleScalar.Abs<TimeUnit> when = getSimulator().getSimulatorTime().get();
        HeadwayGTU foundMaxGTUDistanceSI = new HeadwayGTU(null, Double.MAX_VALUE);
        // search for the closest GTU on all current lanes we are registered on.
        if (maxDistanceSI > 0.0)
        {
            // look forward.
            for (Lane lane : positions(getFront()).keySet())
            {
                HeadwayGTU closest =
                    headwayRecursiveForwardSI(lane, this.position(lane, this.getFront(), when).getSI(), 0.0, maxDistanceSI,
                        when);
                if (closest.getDistanceSI() < maxDistanceSI
                    && closest.getDistanceSI() < foundMaxGTUDistanceSI.getDistanceSI())
                {
                    foundMaxGTUDistanceSI = closest;
                }
            }
        }
        else
        {
            // look backward.
            for (Lane lane : positions(getRear()).keySet())
            {
                HeadwayGTU closest =
                    headwayRecursiveBackwardSI(lane, this.position(lane, this.getRear(), when).getSI(), 0.0, -maxDistanceSI,
                        when);
                if (closest.getDistanceSI() < -maxDistanceSI
                    && closest.getDistanceSI() < foundMaxGTUDistanceSI.getDistanceSI())
                {
                    foundMaxGTUDistanceSI = closest;
                }
            }
        }
        return foundMaxGTUDistanceSI;
    }

    /** {@inheritDoc} */
    @Override
    public final HeadwayGTU headway(final DoubleScalar.Rel<LengthUnit> maxDistance) throws RemoteException, NetworkException
    {
        return headwayGTUSI(maxDistance.getSI());
    }

    /** {@inheritDoc} */
    @Override
    public final HeadwayGTU headway(final Lane lane, final DoubleScalar.Rel<LengthUnit> maxDistance) throws RemoteException,
        NetworkException
    {
        DoubleScalar.Abs<TimeUnit> when = getSimulator().getSimulatorTime().get();
        if (maxDistance.getSI() > 0.0)
        {
            return headwayRecursiveForwardSI(lane, this.projectedPosition(lane, this.getFront(), when).getSI(), 0.0,
                maxDistance.getSI(), when);
        }
        else
        {
            return headwayRecursiveBackwardSI(lane, this.projectedPosition(lane, this.getRear(), when).getSI(), 0.0,
                -maxDistance.getSI(), when);
        }
    }

    /**
     * Calculate the headway to a GTU, possibly on subsequent lanes, in forward direction.
     * @param lane the lane where we are looking right now
     * @param lanePositionSI from which position on this lane do we start measuring? This is the current position of the (front
     *            of the) GTU when we measure in the lane where the original GTU is positioned, and 0.0 for each subsequent lane
     * @param otherGTU the GTU to which the headway must be returned
     * @param cumDistanceSI the distance we have already covered searching on previous lanes
     * @param maxDistanceSI the maximum distance to look for; stays the same in subsequent calls
     * @param when the future time for which to calculate the headway
     * @return the headway in SI units when we have found the GTU, or Double.MAX_VALUE when the otherGTU could not be found
     *         within maxDistanceSI
     * @throws RemoteException when the simulation time cannot be retrieved
     * @throws NetworkException when there is a problem with the geometry of the network
     */
    private double headwayRecursiveForwardSI(final Lane lane, final double lanePositionSI, final LaneBasedGTU<?> otherGTU,
        final double cumDistanceSI, final double maxDistanceSI, final DoubleScalar.Abs<TimeUnit> when)
        throws RemoteException, NetworkException
    {
        if (lane.getGtuList().contains(otherGTU))
        {
            double distanceM = cumDistanceSI + otherGTU.position(lane, otherGTU.getRear(), when).getSI() - lanePositionSI;
            if (distanceM > 0 && distanceM <= maxDistanceSI)
            {
                return distanceM;
            }
            return Double.MAX_VALUE;
        }

        // Continue search on successor lanes.
        if (cumDistanceSI + lane.getLength().getSI() - lanePositionSI < maxDistanceSI)
        {
            // is there a successor link?
            Set<Lane> nextLanes = lane.nextLanes();
            if (nextLanes.size() > 0)
            {
                for (Lane nextLane : nextLanes)
                {
                    // Only follow links on the Route if there is a Route
                    if (this.getRoute() == null || this.getRoute() != null
                        && this.getRoute().containsLink(lane.getParentLink()))
                    {
                        double traveledDistanceSI = cumDistanceSI + lane.getLength().getSI() - lanePositionSI;
                        double headwaySuccessor =
                            headwayRecursiveForwardSI(nextLane, 0.0, otherGTU, traveledDistanceSI, maxDistanceSI, when);
                        if (headwaySuccessor < maxDistanceSI)
                        {
                            return headwaySuccessor;
                        }
                    }
                }
            }
        }

        // The otherGTU was not on one of the current lanes or their successors.
        return Double.MAX_VALUE;
    }

    /**
     * Calculate the headway to a GTU, possibly on subsequent lanes, in backward direction.
     * @param lane the lane where we are looking right now
     * @param lanePositionSI from which position on this lane do we start measuring? This is the current position of the (back
     *            of) the GTU when we measure in the lane where the original GTU is positioned, and the length of the lane for
     *            each subsequent lane
     * @param otherGTU the GTU to which the headway must be returned
     * @param cumDistanceSI the distance we have already covered searching on previous lanes, as a POSITIVE number
     * @param maxDistanceSI the maximum distance to look for; stays the same in subsequent calls, as a POSITIVE number
     * @param when the future time for which to calculate the headway
     * @return the headway in SI units when we have found the GTU, or Double.MAX_VALUE when the otherGTU could not be found
     *         within maxDistanceSI
     * @throws RemoteException when the simulation time cannot be retrieved
     * @throws NetworkException when there is a problem with the geometry of the network
     */
    private double headwayRecursiveBackwardSI(final Lane lane, final double lanePositionSI, final LaneBasedGTU<?> otherGTU,
        final double cumDistanceSI, final double maxDistanceSI, final DoubleScalar.Abs<TimeUnit> when)
        throws RemoteException, NetworkException
    {
        if (lane.getGtuList().contains(otherGTU))
        {
            double distanceM = cumDistanceSI + lanePositionSI - otherGTU.position(lane, otherGTU.getFront(), when).getSI();
            if (distanceM > 0 && distanceM <= maxDistanceSI)
            {
                return distanceM;
            }
            return Double.MAX_VALUE;
        }

        // Continue search on successor lanes.
        if (cumDistanceSI + lanePositionSI < maxDistanceSI)
        {
            // is there a successor link?
            Set<Lane> prevLanes = lane.prevLanes();
            if (prevLanes.size() > 0)
            {
                for (Lane prevLane : prevLanes)
                {
                    // Routes are NOT IMPORTANT when we look backward.
                    double traveledDistanceSI = cumDistanceSI + lanePositionSI;
                    double headwayPredecessor =
                        headwayRecursiveForwardSI(prevLane, prevLane.getLength().getSI(), otherGTU, traveledDistanceSI,
                            maxDistanceSI, when);
                    if (headwayPredecessor < maxDistanceSI)
                    {
                        return headwayPredecessor;
                    }
                }
            }
        }

        // The otherGTU was not on one of the current lanes or their successors.
        return Double.MAX_VALUE;
    }

    /** {@inheritDoc} */
    @Override
    public final Set<LaneBasedGTU<?>> parallel(final Lane lane, final DoubleScalar.Abs<TimeUnit> when)
        throws RemoteException, NetworkException
    {
        Set<LaneBasedGTU<?>> gtuSet = new HashSet<LaneBasedGTU<?>>();
        for (Lane l : this.lanes)
        {
            // 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 posFractionFront = Math.max(0.0, this.fractionalPosition(l, getFront(), when));
                double posFractionRear = Math.min(1.0, this.fractionalPosition(l, getRear(), when));
                for (LaneBasedGTU<?> gtu : lane.getGtuList())
                {
                    if (!gtu.equals(this))
                    {
                        double gtuFractionFront = Math.max(0.0, gtu.fractionalPosition(lane, gtu.getFront(), when));
                        double gtuFractionRear = Math.min(1.0, gtu.fractionalPosition(lane, gtu.getRear(), when));
                        if (gtuFractionFront >= posFractionRear && gtuFractionRear <= posFractionFront)
                        {
                            gtuSet.add(gtu);
                        }
                    }
                }
            }
        }
        return gtuSet;
    }

    /**
     * Build a set of Lanes that is adjacent to the lanes that this GTU is in, in the specified lateral direction.
     * @param lateralDirection LateralDirectionality; the lateral direction.
     * @return Set&lt;Lane&gt;
     */
    private Set<Lane> adjacentLanes(final LateralDirectionality lateralDirection)
    {
        Set<Lane> result = new HashSet<Lane>();
        for (Lane lane : this.lanes)
        {
            result.addAll(lane.accessibleAdjacentLanes(lateralDirection, getGTUType()));
        }
        return result;
    }

    /** {@inheritDoc} */
    @Override
    public final Set<LaneBasedGTU<?>> parallel(final LateralDirectionality lateralDirection,
        final DoubleScalar.Abs<TimeUnit> when) throws RemoteException, NetworkException
    {
        Set<Lane> adjacentLanes = adjacentLanes(lateralDirection);
        /*-                       new HashSet<Lane>();
        for (Lane lane : this.lanes)
        {
            adjacentLanes.addAll(lane.accessibleAdjacentLanes(lateralDirection, getGTUType()));
        }
         */
        Set<LaneBasedGTU<?>> gtuSet = new HashSet<LaneBasedGTU<?>>();
        for (Lane adjacentLane : adjacentLanes)
        {
            gtuSet.addAll(parallel(adjacentLane, when));
        }
        return gtuSet;
    }

    /** {@inheritDoc} */
    public final DoubleScalar.Abs<TimeUnit> timeAtDistance(final DoubleScalar.Rel<LengthUnit> distance)
    {
        Double result = solveTimeForDistance(distance);
        if (null == result)
        {
            return null;
        }
        return new DoubleScalar.Abs<TimeUnit>(this.lastEvaluationTime.getSI() + result, TimeUnit.SECOND);
    }

    /** {@inheritDoc} */
    public final DoubleScalar.Rel<TimeUnit> deltaTimeForDistance(final DoubleScalar.Rel<LengthUnit> distance)
    {
        Double result = solveTimeForDistance(distance);
        if (null == result)
        {
            return null;
        }
        return new DoubleScalar.Rel<TimeUnit>(result, TimeUnit.SECOND);
    }

    /** {@inheritDoc} */
    @Override
    @SuppressWarnings("checkstyle:designforextension")
    public void destroy()
    {
        while (!this.lanes.isEmpty())
        {
            Lane lane = this.lanes.get(0);
            removeLane(lane);
            lane.removeGTU(this);
        }
    }

    /**
     * Determine show long it will take for this GTU to cover the specified distance (both time and distance since the last
     * evaluation time).
     * @param distance double; the distance
     * @return Double; the relative time, or null when this GTU stops before covering the specified distance
     */
    private Double solveTimeForDistance(final DoubleScalar.Rel<LengthUnit> distance)
    {
        /*
         * Currently (!) a (Lane based) GTU commits to a constant acceleration until the next evaluation time. When/If that is
         * changed, this method will have to be re-written.
         */
        double c = -distance.getSI();
        double a = this.acceleration.getSI() / 2;
        double b = this.speed.getSI();
        if (0 == a)
        {
            if (b > 0)
            {
                return -c / b;
            }
            return null;
        }
        // Solve a * t^2 + b * t + c = 0
        double discriminant = b * b - 4 * a * c;
        if (discriminant < 0)
        {
            return null;
        }
        // The solutions are (-b +/- sqrt(discriminant)) / 2 / a
        double solution1 = (-b - Math.sqrt(discriminant)) / 2 / a;
        double solution2 = (-b + Math.sqrt(discriminant)) / 2 / a;
        if (solution1 < 0 && solution2 < 0)
        {
            return null;
        }
        if (solution1 < 0)
        {
            return solution2;
        }
        if (solution2 < 0)
        {
            return solution1;
        }
        // Both are >= 0; return the smallest one
        if (solution1 < solution2)
        {
            return solution1;
        }
        return solution2;
    }

    /**
     * Retrieve the GTUFollowingModel of this GTU.
     * @return GTUFollowingModel
     */
    public final GTUFollowingModel getGTUFollowingModel()
    {
        return this.gtuFollowingModel;
    }

    /** {@inheritDoc} */
    @Override
    public final DirectedPoint getLocation() throws RemoteException
    {
        synchronized (this.lanes)
        {
            try
            {
                if (this.lanes.size() == 0)
                {
                    if (getSimulator().isRunning())
                    {
                        // getSimulator().stop();
                        System.out.println("GTU " + this.getId() + " is not on any lane");
                    }
                    // This happens temporarily when a GTU is moved to another Lane
                    return new DirectedPoint(Double.MAX_VALUE, Double.MAX_VALUE, 0);
                }
                Lane lane = this.lanes.get(0);
                // TODO solve problem when point is still on previous lane.
                DoubleScalar.Rel<LengthUnit> longitudinalPos = position(lane, getReference());
                double fraction = (longitudinalPos.getSI() + getLength().getSI() / 2.0) / lane.getLength().getSI();
                LineString line = lane.getCenterLine();
                LengthIndexedLine lil = new LengthIndexedLine(line);
                // if (fraction > 1)
                // {
                // System.out.println("fraction is " + fraction);
                // }
                double useFraction = fraction;
                if (fraction < 0)
                {
                    useFraction = 0;
                }
                if (fraction > 0.99)
                {
                    useFraction = 0.99;
                }
                // DO NOT MODIFY THE RESULT OF extractPoint (it may be one of the coordinates in line).
                Coordinate c = new Coordinate(lil.extractPoint(useFraction * line.getLength()));
                c.z = 0d;
                Coordinate cb = lil.extractPoint((useFraction + 0.01) * line.getLength());
                double angle = Math.atan2(cb.y - c.y, cb.x - c.x);
                // FindBugs does not like this comparison - looking for a way to suppress that warning
                if (fraction != useFraction)
                {
                    c =
                        new Coordinate(c.x + (fraction - useFraction) * 100 * (cb.x - c.x), c.y + (fraction - useFraction)
                            * 100 * (cb.y - c.y), c.z);
                }
                if (Double.isNaN(c.x))
                {
                    System.out.println("Bad");
                }
                return new DirectedPoint(c.x, c.y, c.z + 0.01 /* raise it slightly above the lane surface */, 0.0, 0.0,
                    angle);
            }
            catch (Exception ne)
            {
                System.err.println(this.getId());
                ne.printStackTrace();
                return new DirectedPoint(0, 0, 0);
            }
        }
    }

    /** {@inheritDoc} */
    @Override
    public final Bounds getBounds() throws RemoteException
    {
        double dx = 0.5 * getLength().doubleValue();
        double dy = 0.5 * getWidth().doubleValue();
        return new BoundingBox(new Point3d(-dx, -dy, 0.0), new Point3d(dx, dy, 0.0));
    }

    /**
     * Description of Car at specified time.
     * @param lane the position on this lane will be returned.
     * @param when DoubleScalarAbs&lt;TimeUnit&gt;; the time
     * @return String; description of this Car at the specified time
     */
    public final String toString(final Lane lane, final DoubleScalar.Abs<TimeUnit> when)
    {
        double pos = Double.NaN;
        try
        {
            pos = this.position(lane, getFront(), when).getSI();
        }
        catch (NetworkException exception)
        {
            // exception.printStackTrace();
        }
        // A space in the format after the % becomes a space for positive numbers or a minus for negative numbers
        return String.format("Car %5d lastEval %6.1fs, nextEval %6.1fs, % 9.3fm, v % 6.3fm/s, a % 6.3fm/s^2", getId(),
            this.lastEvaluationTime.getSI(), getNextEvaluationTime().getSI(), pos, this.getLongitudinalVelocity(when)
                .getSI(), this.getAcceleration(when).getSI());
    }

}