package org.opentrafficsim.road.gtu.lane;

import java.util.ArrayList;
import java.util.Collections;
import java.util.Comparator;
import java.util.Iterator;
import java.util.LinkedHashMap;
import java.util.LinkedHashSet;
import java.util.List;
import java.util.Map;
import java.util.Map.Entry;
import java.util.Set;
import java.util.SortedMap;

import org.djunits.unit.DirectionUnit;
import org.djunits.unit.DurationUnit;
import org.djunits.unit.LengthUnit;
import org.djunits.unit.PositionUnit;
import org.djunits.value.vdouble.scalar.Acceleration;
import org.djunits.value.vdouble.scalar.Direction;
import org.djunits.value.vdouble.scalar.Duration;
import org.djunits.value.vdouble.scalar.Length;
import org.djunits.value.vdouble.scalar.Speed;
import org.djunits.value.vdouble.scalar.Time;
import org.djutils.draw.Transform2d;
import org.djutils.draw.bounds.Bounds3d;
import org.djutils.draw.line.LineSegment2d;
import org.djutils.draw.line.PolyLine3d;
import org.djutils.draw.line.Ray3d;
import org.djutils.draw.point.OrientedPoint3d;
import org.djutils.draw.point.Point2d;
import org.djutils.draw.point.Point3d;
import org.djutils.exceptions.Throw;
import org.djutils.exceptions.Try;
import org.djutils.immutablecollections.ImmutableMap;
import org.djutils.logger.CategoryLogger;
import org.djutils.multikeymap.MultiKeyMap;
import org.opentrafficsim.base.parameters.ParameterException;
import org.opentrafficsim.core.dsol.OTSSimulatorInterface;
import org.opentrafficsim.core.geometry.OTSGeometryUtil;
import org.opentrafficsim.core.gtu.AbstractGTU;
import org.opentrafficsim.core.gtu.GTU;
import org.opentrafficsim.core.gtu.GTUDirectionality;
import org.opentrafficsim.core.gtu.GTUException;
import org.opentrafficsim.core.gtu.GTUType;
import org.opentrafficsim.core.gtu.RelativePosition;
import org.opentrafficsim.core.gtu.TurnIndicatorStatus;
import org.opentrafficsim.core.gtu.perception.EgoPerception;
import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan;
import org.opentrafficsim.core.gtu.plan.operational.OperationalPlanBuilder;
import org.opentrafficsim.core.gtu.plan.operational.OperationalPlanException;
import org.opentrafficsim.core.network.LateralDirectionality;
import org.opentrafficsim.core.network.NetworkException;
import org.opentrafficsim.core.perception.Historical;
import org.opentrafficsim.core.perception.HistoricalValue;
import org.opentrafficsim.core.perception.HistoryManager;
import org.opentrafficsim.core.perception.collections.HistoricalArrayList;
import org.opentrafficsim.core.perception.collections.HistoricalList;
import org.opentrafficsim.road.gtu.lane.perception.LanePerception;
import org.opentrafficsim.road.gtu.lane.perception.PerceptionCollectable;
import org.opentrafficsim.road.gtu.lane.perception.RelativeLane;
import org.opentrafficsim.road.gtu.lane.perception.categories.InfrastructurePerception;
import org.opentrafficsim.road.gtu.lane.perception.categories.neighbors.NeighborsPerception;
import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayGTU;
import org.opentrafficsim.road.gtu.lane.plan.operational.LaneBasedOperationalPlan;
import org.opentrafficsim.road.gtu.strategical.LaneBasedStrategicalPlanner;
import org.opentrafficsim.road.network.OTSRoadNetwork;
import org.opentrafficsim.road.network.RoadNetwork;
import org.opentrafficsim.road.network.lane.CrossSectionLink;
import org.opentrafficsim.road.network.lane.DirectedLanePosition;
import org.opentrafficsim.road.network.lane.Lane;
import org.opentrafficsim.road.network.lane.LaneDirection;
import org.opentrafficsim.road.network.lane.object.sensor.SingleSensor;
import org.opentrafficsim.road.network.speed.SpeedLimitInfo;
import org.opentrafficsim.road.network.speed.SpeedLimitTypes;

import nl.tudelft.simulation.dsol.SimRuntimeException;
import nl.tudelft.simulation.dsol.formalisms.eventscheduling.SimEventInterface;
import nl.tudelft.simulation.dsol.simtime.SimTimeDoubleUnit;

/**
 * This class contains most of the code that is needed to run a lane based GTU. <br>
 * The starting point of a LaneBasedTU is that it can be in <b>multiple lanes</b> at the same time. This can be due to a lane
 * change (lateral), or due to crossing a link (front of the GTU is on another Lane than rear of the GTU). If a Lane is shorter
 * than the length of the GTU (e.g. when we do node expansion on a crossing, this is very well possible), a GTU could occupy
 * dozens of Lanes at the same time.
 * <p>
 * When calculating a headway, the GTU has to look in successive lanes. 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.
 * <p>
 * To decide its movement, an AbstractLaneBasedGTU applies its car following algorithm and lane change algorithm to set the
 * acceleration and any lane change operation to perform. It then schedules the triggers that will add it to subsequent lanes
 * and remove it from current lanes as needed during the time step that is has committed to. Finally, it re-schedules its next
 * movement evaluation with the simulator.
 * <p>
 * Copyright (c) 2013-2020 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
 * BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
 * </p>
 * @version $Revision: 1408 $, $LastChangedDate: 2015-09-24 15:17:25 +0200 (Thu, 24 Sep 2015) $, by $Author: pknoppers $,
 *          initial version Oct 22, 2014 <br>
 * @author <a href="https://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
 * @author <a href="https://www.tudelft.nl/pknoppers">Peter Knoppers</a>
 */
public abstract class AbstractLaneBasedGTU2 extends AbstractGTU implements LaneBasedGTU
{
    /** */
    private static final long serialVersionUID = 20140822L;

    /** Lanes. */
    private final HistoricalList<CrossSection> crossSections;

    /** Reference lane index (0 = left or only lane, 1 = right lane). */
    private int referenceLaneIndex = 0;

    /** Time of reference position cache. */
    private double referencePositionTime = Double.NaN;

    /** Cached reference position. */
    private DirectedLanePosition cachedReferencePosition = null;

    /** Pending leave triggers for each lane. */
    private SimEventInterface<SimTimeDoubleUnit> pendingLeaveTrigger;

    /** Pending enter triggers for each lane. */
    private SimEventInterface<SimTimeDoubleUnit> pendingEnterTrigger;

    /** Event to finalize lane change. */
    private SimEventInterface<SimTimeDoubleUnit> finalizeLaneChangeEvent;

    /** Sensor events. */
    private Set<SimEventInterface<SimTimeDoubleUnit>> sensorEvents = new LinkedHashSet<>();

    /** Cached desired speed. */
    private Speed cachedDesiredSpeed;

    /** Time desired speed was cached. */
    private Time desiredSpeedTime;

    /** Cached car-following acceleration. */
    private Acceleration cachedCarFollowingAcceleration;

    /** Time car-following acceleration was cached. */
    private Time carFollowingAccelerationTime;

    /** The object to lock to make the GTU thread safe. */
    private Object lock = new Object();

    /** The threshold distance for differences between initial locations of the GTU on different lanes. */
    @SuppressWarnings("checkstyle:visibilitymodifier")
    public static Length initialLocationThresholdDifference = new Length(1.0, LengthUnit.MILLIMETER);

    /** Margin to add to plan to check of the path will enter the next section. */
    public static Length eventMargin = Length.instantiateSI(50.0);

    /** Turn indicator status. */
    private final Historical<TurnIndicatorStatus> turnIndicatorStatus;

    /** Caching on or off. */
    // TODO: should be indicated with a Parameter
    public static boolean CACHING = true;

    /** cached position count. */
    // TODO: can be removed after testing period
    public static int CACHED_POSITION = 0;

    /** cached position count. */
    // TODO: can be removed after testing period
    public static int NON_CACHED_POSITION = 0;

    /** Vehicle model. */
    private VehicleModel vehicleModel = VehicleModel.MINMAX;

    /** Whether the GTU perform lane changes instantaneously or not. */
    private boolean instantaneousLaneChange = false;

    /**
     * Construct a Lane Based GTU.
     * @param id String; the id of the GTU
     * @param gtuType GTUType; the type of GTU, e.g. TruckType, CarType, BusType
     * @param network OTSRoadNetwork; the network that the GTU is initially registered in
     * @throws GTUException when initial values are not correct
     */
    public AbstractLaneBasedGTU2(final String id, final GTUType gtuType, final OTSRoadNetwork network) throws GTUException
    {
        super(id, gtuType, network.getSimulator(), network);
        OTSSimulatorInterface simulator = network.getSimulator();
        HistoryManager historyManager = simulator.getReplication().getHistoryManager(simulator);
        this.crossSections = new HistoricalArrayList<>(historyManager);
        this.turnIndicatorStatus = new HistoricalValue<>(historyManager, TurnIndicatorStatus.NOTPRESENT);
    }

    /**
     * @param strategicalPlanner LaneBasedStrategicalPlanner; the strategical planner (e.g., route determination) to use
     * @param initialLongitudinalPositions Set&lt;DirectedLanePosition&gt;; the initial positions of the car on one or more
     *            lanes with their directions
     * @param initialSpeed Speed; the initial speed of the car on the lane
     * @throws NetworkException when the GTU cannot be placed on the given lane
     * @throws SimRuntimeException when the move method cannot be scheduled
     * @throws GTUException when initial values are not correct
     */
    public void init(final LaneBasedStrategicalPlanner strategicalPlanner,
            final Set<DirectedLanePosition> initialLongitudinalPositions, final Speed initialSpeed)
            throws NetworkException, SimRuntimeException, GTUException
    {
        Throw.when(null == initialLongitudinalPositions, GTUException.class, "InitialLongitudinalPositions is null");
        Throw.when(0 == initialLongitudinalPositions.size(), GTUException.class, "InitialLongitudinalPositions is empty set");

        OrientedPoint3d lastPoint = null;
        for (DirectedLanePosition pos : initialLongitudinalPositions)
        {
            // Throw.when(lastPoint != null && pos.getLocation().distance(lastPoint) > initialLocationThresholdDifference.si,
            // GTUException.class, "initial locations for GTU have distance > " + initialLocationThresholdDifference);
            lastPoint = pos.getLocation();
        }
        OrientedPoint3d initialLocation = lastPoint;

        // Give the GTU a 1 micrometer long operational plan, or a stand-still plan, so the first move and events will work
        Time now = getSimulator().getSimulatorTime();
        try
        {
            if (initialSpeed.si < OperationalPlan.DRIFTING_SPEED_SI)
            {
                this.operationalPlan
                        .set(new OperationalPlan(this, initialLocation, now, new Duration(1E-6, DurationUnit.SECOND)));
            }
            else
            {
                Point3d p2 = new Point3d(initialLocation.x + 1E-6 * Math.cos(initialLocation.getDirZ()),
                        initialLocation.y + 1E-6 * Math.sin(initialLocation.getDirZ()), initialLocation.z);
                PolyLine3d path = new PolyLine3d(initialLocation, p2);
                this.operationalPlan.set(OperationalPlanBuilder.buildConstantSpeedPlan(this, path, now, initialSpeed));
            }
        }
        catch (OperationalPlanException e)
        {
            throw new RuntimeException("Initial operational plan could not be created.", e);
        }

        // register the GTU on the lanes
        List<DirectedLanePosition> inits = new ArrayList<>(); // need to sort them
        inits.addAll(initialLongitudinalPositions);
        Collections.sort(inits, new Comparator<DirectedLanePosition>()
        {
            @Override
            public int compare(final DirectedLanePosition o1, final DirectedLanePosition o2)
            {
                Length length1 =
                        o1.getGtuDirection().isPlus() ? o1.getPosition() : o1.getLane().getLength().minus(o1.getPosition());
                Length length2 =
                        o2.getGtuDirection().isPlus() ? o2.getPosition() : o2.getLane().getLength().minus(o2.getPosition());
                return length1.compareTo(length2);
            }
        });
        for (DirectedLanePosition directedLanePosition : inits)
        {
            List<Lane> lanes = new ArrayList<>();
            lanes.add(directedLanePosition.getLane());
            this.crossSections.add(new CrossSection(lanes, directedLanePosition.getGtuDirection())); // enter lane part 1
        }

        // init event
        DirectedLanePosition referencePosition = getReferencePosition();
        fireTimedEvent(LaneBasedGTU.LANEBASED_INIT_EVENT,
                new Object[] {getId(), OTSGeometryUtil.doubleVector(initialLocation, PositionUnit.METER),
                        Direction.instantiateSI(initialLocation.getDirZ()), getLength(), getWidth(),
                        referencePosition.getLane().getParentLink().getId(), referencePosition.getLane().getId(),
                        referencePosition.getPosition(), referencePosition.getGtuDirection().name(), getGTUType().getId()},
                getSimulator().getSimulatorTime());

        // register the GTU on the lanes
        for (DirectedLanePosition directedLanePosition : initialLongitudinalPositions)
        {
            Lane lane = directedLanePosition.getLane();
            lane.addGTU(this, directedLanePosition.getPosition()); // enter lane part 2
        }

        // initiate the actual move
        super.init(strategicalPlanner, initialLocation, initialSpeed);

        this.referencePositionTime = Double.NaN; // remove cache, it may be invalid as the above init results in a lane change

    }

    /**
     * {@inheritDoc} All lanes the GTU is on will be left.
     */
    @Override
    public synchronized void setParent(final GTU gtu) throws GTUException
    {
        leaveAllLanes();
        super.setParent(gtu);
    }

    /**
     * Removes the registration between this GTU and all the lanes.
     */
    private void leaveAllLanes()
    {
        for (CrossSection crossSection : this.crossSections)
        {
            boolean removeFromParentLink = true;
            for (Lane lane : crossSection.getLanes())
            {
                // GTU should be on this lane as we loop the registration
                Length pos = Try.assign(() -> position(lane, getReference()), "Unexpected exception.");
                lane.removeGTU(this, removeFromParentLink, pos);
                removeFromParentLink = false;
            }
        }
        this.crossSections.clear();
    }

    /**
     * Reinitializes the GTU on the network using the existing strategical planner and zero speed.
     * @param initialLongitudinalPositions Set&lt;DirectedLanePosition&gt;; initial position
     * @throws NetworkException when the GTU cannot be placed on the given lane
     * @throws SimRuntimeException when the move method cannot be scheduled
     * @throws GTUException when initial values are not correct
     */
    public void reinit(final Set<DirectedLanePosition> initialLongitudinalPositions)
            throws NetworkException, SimRuntimeException, GTUException
    {
        init(getStrategicalPlanner(), initialLongitudinalPositions, Speed.ZERO);
    }

    /** {@inheritDoc} */
    @Override
    public synchronized void changeLaneInstantaneously(final LateralDirectionality laneChangeDirection) throws GTUException
    {

        // from info
        DirectedLanePosition from = getReferencePosition();

        // obtain position on lane adjacent to reference lane and enter lanes upstream/downstream from there
        GTUDirectionality direction = getDirection(from.getLane());
        Set<Lane> adjLanes = from.getLane().accessibleAdjacentLanesPhysical(laneChangeDirection, getGTUType(), direction);
        Lane adjLane = adjLanes.iterator().next();
        Length position = adjLane.position(from.getLane().fraction(from.getPosition()));
        leaveAllLanes();
        enterLaneRecursive(new LaneDirection(adjLane, direction), position, 0);

        // stored positions no longer valid
        this.referencePositionTime = Double.NaN;
        this.cachedPositions.clear();

        // fire event
        this.fireTimedEvent(
                LaneBasedGTU.LANE_CHANGE_EVENT, new Object[] {getId(), laneChangeDirection.name(),
                        from.getLane().getParentLink().getId(), from.getLane().getId(), from.getPosition()},
                getSimulator().getSimulatorTime());

    }

    /**
     * Enters lanes upstream and downstream of the new location after an instantaneous lane change.
     * @param lane LaneDirection; considered lane
     * @param position Length; position to add GTU at
     * @param dir int; below 0 for upstream, above 0 for downstream, 0 for both
     * @throws GTUException on exception
     */
    private void enterLaneRecursive(final LaneDirection lane, final Length position, final int dir) throws GTUException
    {
        List<Lane> lanes = new ArrayList<>();
        lanes.add(lane.getLane());
        int index = dir > 0 ? this.crossSections.size() : 0;
        this.crossSections.add(index, new CrossSection(lanes, lane.getDirection()));
        lane.getLane().addGTU(this, position);

        // upstream
        if (dir < 1)
        {
            Length rear = lane.getDirection().isPlus() ? position.plus(getRear().getDx()) : position.minus(getRear().getDx());
            Length before = null;
            if (lane.getDirection().isPlus() && rear.si < 0.0)
            {
                before = rear.neg();
            }
            else if (lane.getDirection().isMinus() && rear.si > lane.getLength().si)
            {
                before = rear.minus(lane.getLength());
            }
            if (before != null)
            {
                GTUDirectionality upDir = lane.getDirection();
                ImmutableMap<Lane, GTUDirectionality> upstream = lane.getLane().upstreamLanes(upDir, getGTUType());
                if (!upstream.isEmpty())
                {
                    Lane upLane = null;
                    for (Lane nextUp : upstream.keySet())
                    {
                        for (CrossSection crossSection : this.crossSections)
                        {
                            if (crossSection.getLanes().contains(nextUp))
                            {
                                // multiple upstream lanes could belong to the same link, we pick an arbitrary lane
                                // (a conflict should solve this)
                                upLane = nextUp;
                                break;
                            }
                        }
                    }
                    if (upLane == null)
                    {
                        // the rear is on an upstream section we weren't before the lane change, due to curvature, we pick an
                        // arbitrary lane (a conflict should solve this)
                        upLane = upstream.keySet().iterator().next();
                    }
                    upDir = upstream.get(upLane);
                    LaneDirection next = new LaneDirection(upLane, upDir);
                    Length nextPos = upDir.isPlus() ? next.getLength().minus(before).minus(getRear().getDx())
                            : before.plus(getRear().getDx());
                    enterLaneRecursive(next, nextPos, -1);
                }
            }
        }

        // downstream
        if (dir > -1)
        {
            Length front =
                    lane.getDirection().isPlus() ? position.plus(getFront().getDx()) : position.minus(getFront().getDx());
            Length passed = null;
            if (lane.getDirection().isPlus() && front.si > lane.getLength().si)
            {
                passed = front.minus(lane.getLength());
            }
            else if (lane.getDirection().isMinus() && front.si < 0.0)
            {
                passed = front.neg();
            }
            if (passed != null)
            {
                LaneDirection next = lane.getNextLaneDirection(this);
                Length nextPos = next.getDirection().isPlus() ? passed.minus(getFront().getDx())
                        : next.getLength().minus(passed).plus(getFront().getDx());
                enterLaneRecursive(next, nextPos, 1);
            }
        }
    }

    /**
     * Register on lanes in target lane.
     * @param laneChangeDirection LateralDirectionality; direction of lane change
     * @throws GTUException exception
     */
    @Override
    public synchronized void initLaneChange(final LateralDirectionality laneChangeDirection) throws GTUException
    {
        List<CrossSection> newLanes = new ArrayList<>();
        int index = laneChangeDirection.isLeft() ? 0 : 1;
        int numRegistered = 0;
        OrientedPoint3d point = getLocation();
        Map<Lane, Double> addToLanes = new LinkedHashMap<>();
        for (CrossSection crossSection : this.crossSections)
        {
            List<Lane> resultingLanes = new ArrayList<>();
            Lane lane = crossSection.getLanes().get(0);
            resultingLanes.add(lane);
            Set<Lane> laneSet = lane.accessibleAdjacentLanesLegal(laneChangeDirection, getGTUType(), getDirection(lane));
            if (laneSet.size() > 0)
            {
                numRegistered++;
                Lane adjacentLane = laneSet.iterator().next();
                double f = OTSGeometryUtil.projectFractional(adjacentLane.getCenterLine(), null, null, point.x, point.y);
                if (f < 0.0 || f > 1.0)
                {
                    // the GTU is upstream or downstream of the lane, or on the edge and we have rounding problems
                    // in either case we add the GTU at an extreme
                    // (this is only for ordering on the lane, the position is not used otherwise)
                    Length pos = position(lane, getReference());
                    addToLanes.put(adjacentLane, pos.si < lane.getLength().si / 2 ? 0.0 : 1.0);
                }
                else
                {
                    f = crossSection.getDirection().isPlus() ? f : 1.0 - f;
                    addToLanes.put(adjacentLane, adjacentLane.getLength().times(f).si / adjacentLane.getLength().si);
                }
                resultingLanes.add(index, adjacentLane);
            }
            newLanes.add(new CrossSection(resultingLanes, crossSection.getDirection()));
        }
        Throw.when(numRegistered == 0, GTUException.class, "Gtu %s starting %s lane change, but no adjacent lane found.",
                getId(), laneChangeDirection);
        this.crossSections.clear();
        this.crossSections.addAll(newLanes);
        for (Entry<Lane, Double> entry : addToLanes.entrySet())
        {
            entry.getKey().addGTU(this, entry.getValue());
        }
        this.referenceLaneIndex = 1 - index;
    }

    /**
     * Performs the finalization of a lane change by leaving the from lanes.
     * @param laneChangeDirection LateralDirectionality; direction of lane change
     * @throws GTUException if position or direction could not be obtained
     */
    protected synchronized void finalizeLaneChange(final LateralDirectionality laneChangeDirection) throws GTUException
    {
        List<CrossSection> newLanes = new ArrayList<>();
        Lane fromLane = null;
        Length fromPosition = null;
        GTUDirectionality fromDirection = null;
        for (CrossSection crossSection : this.crossSections)
        {
            Lane lane = crossSection.getLanes().get(this.referenceLaneIndex);
            if (lane != null)
            {
                Length pos = position(lane, RelativePosition.REFERENCE_POSITION);
                if (0.0 <= pos.si && pos.si <= lane.getLength().si)
                {
                    fromLane = lane;
                    fromPosition = pos;
                    fromDirection = getDirection(lane);
                }
                lane.removeGTU(this, false, pos);
            }
            List<Lane> remainingLane = new ArrayList<>();
            remainingLane.add(crossSection.getLanes().get(1 - this.referenceLaneIndex));
            newLanes.add(new CrossSection(remainingLane, crossSection.getDirection()));
        }
        this.crossSections.clear();
        this.crossSections.addAll(newLanes);
        this.referenceLaneIndex = 0;

        Throw.when(fromLane == null, RuntimeException.class, "No from lane for lane change event.");
        DirectedLanePosition from;
        try
        {
            from = new DirectedLanePosition(fromLane, fromPosition, fromDirection);
        }
        catch (GTUException exception)
        {
            throw new RuntimeException(exception);
        }
        this.fireTimedEvent(
                LaneBasedGTU.LANE_CHANGE_EVENT, new Object[] {getId(), laneChangeDirection.name(),
                        from.getLane().getParentLink().getId(), from.getLane().getId(), from.getPosition()},
                getSimulator().getSimulatorTime());

        this.finalizeLaneChangeEvent = null;
    }

    /** {@inheritDoc} */
    @Override
    public void setFinalizeLaneChangeEvent(final SimEventInterface<SimTimeDoubleUnit> event)
    {
        this.finalizeLaneChangeEvent = event;
    }

    /** {@inheritDoc} */
    @Override
    public final synchronized GTUDirectionality getDirection(final Lane lane) throws GTUException
    {
        for (CrossSection crossSection : this.crossSections)
        {
            if (crossSection.getLanes().contains(lane))
            {
                return crossSection.getDirection();
            }
        }
        throw new GTUException("getDirection: GTU does not contain " + lane);
    }

    /** {@inheritDoc} */
    @Override
    protected synchronized boolean move(final OrientedPoint3d fromLocation)
            throws SimRuntimeException, GTUException, OperationalPlanException, NetworkException, ParameterException
    {
        if (this.isDestroyed())
        {
            return false;
        }
        try
        {
            if (this.crossSections.isEmpty())
            {
                destroy();
                return false; // Done; do not re-schedule execution of this move method.
            }

            // cancel events, if any
            cancelAllEvents();

            // generate the next operational plan and carry it out
            // in case of an instantaneous lane change, fractionalLinkPositions will be accordingly adjusted to the new lane
            boolean error = super.move(fromLocation);
            if (error)
            {
                return error;
            }

            DirectedLanePosition dlp = getReferencePosition();

            scheduleEnterEvent();
            scheduleLeaveEvent();

            // sensors
            for (CrossSection crossSection : this.crossSections)
            {
                for (Lane lane : crossSection.getLanes())
                {
                    scheduleTriggers(lane, crossSection.getDirection());
                }
            }

            fireTimedEvent(LaneBasedGTU.LANEBASED_MOVE_EVENT,
                    new Object[] {getId(), OTSGeometryUtil.doubleVector(fromLocation, PositionUnit.METER),
                            new Direction(fromLocation.getDirZ(), DirectionUnit.EAST_RADIAN), getSpeed(), getAcceleration(),
                            getTurnIndicatorStatus().name(), getOdometer(), dlp.getLane().getParentLink().getId(),
                            dlp.getLane().getId(), dlp.getPosition(), dlp.getGtuDirection().name()},
                    getSimulator().getSimulatorTime());

            return false;

        }
        catch (Exception ex)
        {
            try
            {
                getErrorHandler().handle(this, ex);
            }
            catch (Exception exception)
            {
                throw new GTUException(exception);
            }
            return true;
        }

    }

    /**
     * Cancels all future events.
     */
    private void cancelAllEvents()
    {
        if (this.pendingEnterTrigger != null)
        {
            getSimulator().cancelEvent(this.pendingEnterTrigger);
        }
        if (this.pendingLeaveTrigger != null)
        {
            getSimulator().cancelEvent(this.pendingLeaveTrigger);
        }
        if (this.finalizeLaneChangeEvent != null)
        {
            getSimulator().cancelEvent(this.finalizeLaneChangeEvent);
        }
        for (SimEventInterface<SimTimeDoubleUnit> event : this.sensorEvents)
        {
            if (event.getAbsoluteExecutionTime().gt(getSimulator().getSimTime()))
            {
                getSimulator().cancelEvent(event);
            }
        }
        this.sensorEvents.clear();
    }

    /**
     * Checks whether the GTU will enter a next cross-section during the (remainder of) the tactical plan. Only one event will
     * be scheduled. Possible additional events are scheduled upon entering the cross-section.
     * @throws GTUException exception
     * @throws OperationalPlanException exception
     * @throws SimRuntimeException exception
     */
    protected void scheduleEnterEvent() throws GTUException, OperationalPlanException, SimRuntimeException
    {
        CrossSection lastCrossSection = this.crossSections.get(this.crossSections.size() - 1);
        // heuristic to prevent geometric calculation if the next section is quite far away anyway
        Length remain = remainingEventDistance();
        Lane lane = lastCrossSection.getLanes().get(this.referenceLaneIndex);
        Length position = position(lane, getFront());
        boolean possiblyNearNextSection =
                lastCrossSection.getDirection().isPlus() ? lane.getLength().minus(position).lt(remain) : position.lt(remain);
        if (possiblyNearNextSection)
        {
            CrossSectionLink link = lastCrossSection.getLanes().get(0).getParentLink();
            LineSegment2d enterLine =
                    lastCrossSection.getDirection().isPlus() ? link.getEndLine().project() : link.getStartLine().project();
            Time enterTime = timeAtLine(enterLine, getFront());
            if (enterTime != null)
            {
                if (enterTime.lt(getSimulator().getSimulatorTime()))
                {
                    System.err.println(
                            "Time travel? enterTime=" + enterTime + "; simulator time=" + getSimulator().getSimulatorTime());
                    enterTime = getSimulator().getSimulatorTime();
                }
                this.pendingEnterTrigger = getSimulator().scheduleEventAbs(enterTime, this, this, "enterCrossSection", null);
            }
        }
    }

    /**
     * Appends a new cross-section at the downstream end. Possibly schedules a next enter event.
     * @throws GTUException exception
     * @throws OperationalPlanException exception
     * @throws SimRuntimeException exception
     */
    protected synchronized void enterCrossSection() throws GTUException, OperationalPlanException, SimRuntimeException
    {
        CrossSection lastCrossSection = this.crossSections.get(this.crossSections.size() - 1);
        LaneDirection laneDirection =
                new LaneDirection(lastCrossSection.getLanes().get(this.referenceLaneIndex), lastCrossSection.getDirection());
        LaneDirection nextLaneDirection = laneDirection.getNextLaneDirection(this);
        if (nextLaneDirection == null)
        {
            forceLaneChangeFinalization();
            return;
        }
        double insertFraction = nextLaneDirection.getDirection().isPlus() ? 0.0 : 1.0;
        List<Lane> nextLanes = new ArrayList<>();
        for (int i = 0; i < lastCrossSection.getLanes().size(); i++)
        {
            if (i == this.referenceLaneIndex)
            {
                nextLanes.add(nextLaneDirection.getLane());
            }
            else
            {
                Lane lane = lastCrossSection.getLanes().get(i);
                ImmutableMap<Lane, GTUDirectionality> lanes = lane.downstreamLanes(laneDirection.getDirection(), getGTUType());
                if (lanes.size() == 1)
                {
                    Lane nextLane = lanes.keySet().iterator().next();
                    nextLanes.add(nextLane);
                }
                else
                {
                    boolean added = false;
                    for (Lane nextLane : lanes.keySet())
                    {
                        if (nextLane.getParentLink().equals(nextLaneDirection.getLane().getParentLink())
                                && nextLane.accessibleAdjacentLanesPhysical(
                                        this.referenceLaneIndex == 0 ? LateralDirectionality.LEFT : LateralDirectionality.RIGHT,
                                        getGTUType(), nextLaneDirection.getDirection()).contains(nextLaneDirection.getLane()))
                        {
                            nextLanes.add(nextLane);
                            added = true;
                            break;
                        }
                    }
                    if (!added)
                    {
                        forceLaneChangeFinalization();
                        return;
                    }
                }
            }
        }
        this.crossSections.add(new CrossSection(nextLanes, nextLaneDirection.getDirection()));
        for (Lane lane : nextLanes)
        {
            lane.addGTU(this, insertFraction);
        }
        this.pendingEnterTrigger = null;
        scheduleEnterEvent();
        for (Lane lane : nextLanes)
        {
            scheduleTriggers(lane, nextLaneDirection.getDirection());
        }
    }

    /**
     * Helper method for {@code enterCrossSection}. In some cases the GTU should first finalize the lane change. This method
     * checks whether such an event is scheduled, and performs it. This method then re-attempts to enter the cross-section. So
     * the calling method should return after calling this.
     * @throws GTUException exception
     * @throws OperationalPlanException exception
     * @throws SimRuntimeException exception
     */
    private void forceLaneChangeFinalization() throws GTUException, OperationalPlanException, SimRuntimeException
    {
        if (this.finalizeLaneChangeEvent != null)
        {
            // a lane change should be finalized at this time, but the event is later in the queue, force it now
            SimEventInterface<SimTimeDoubleUnit> tmp = this.finalizeLaneChangeEvent;
            finalizeLaneChange(this.referenceLaneIndex == 0 ? LateralDirectionality.RIGHT : LateralDirectionality.LEFT);
            getSimulator().cancelEvent(tmp);
            enterCrossSection();
        }
        // or a sink sensor should delete us
    }

    /**
     * Checks whether the GTU will leave a cross-section during the (remainder of) the tactical plan. Only one event will be
     * scheduled. Possible additional events are scheduled upon leaving the cross-section.
     * @throws GTUException exception
     * @throws OperationalPlanException exception
     * @throws SimRuntimeException exception
     */
    protected void scheduleLeaveEvent() throws GTUException, OperationalPlanException, SimRuntimeException
    {
        if (this.crossSections.isEmpty())
        {
            CategoryLogger.always().error("GTU {} has empty crossSections", this);
            return;
        }
        CrossSection firstCrossSection = this.crossSections.get(0);
        // check, if reference lane is not in first cross section
        boolean possiblyNearNextSection =
                !getReferencePosition().getLane().equals(firstCrossSection.getLanes().get(this.referenceLaneIndex));
        if (!possiblyNearNextSection)
        {
            Length remain = remainingEventDistance();
            Lane lane = firstCrossSection.getLanes().get(this.referenceLaneIndex);
            Length position = position(lane, getRear());
            possiblyNearNextSection = firstCrossSection.getDirection().isPlus() ? lane.getLength().minus(position).lt(remain)
                    : position.lt(remain);
        }
        if (possiblyNearNextSection)
        {
            CrossSectionLink link = firstCrossSection.getLanes().get(0).getParentLink();
            LineSegment2d leaveLine =
                    firstCrossSection.getDirection().isPlus() ? link.getEndLine().project() : link.getStartLine().project();
            Time leaveTime = timeAtLine(leaveLine, getRear());
            if (leaveTime == null)
            {
                // no intersect, let's do a check on the rear
                Lane lane = this.crossSections.get(0).getLanes().get(this.referenceLaneIndex);
                Length pos = position(lane, getRear());
                if (pos.gt(lane.getLength()))
                {
                    pos = position(lane, getRear());
                    this.pendingLeaveTrigger = getSimulator().scheduleEventNow(this, this, "leaveCrossSection", null);
                    getSimulator().getLogger().always().info("Forcing leave for GTU {} on lane {}", getId(), lane.getFullId());
                }
            }
            if (leaveTime != null)
            {
                if (leaveTime.lt(getSimulator().getSimulatorTime()))
                {
                    System.err.println(
                            "Time travel? leaveTime=" + leaveTime + "; simulator time=" + getSimulator().getSimulatorTime());
                    leaveTime = getSimulator().getSimulatorTime();
                }
                this.pendingLeaveTrigger = getSimulator().scheduleEventAbs(leaveTime, this, this, "leaveCrossSection", null);
            }
        }
    }

    /**
     * Removes registration between the GTU and the lanes in the most upstream cross-section. Possibly schedules a next leave
     * event.
     * @throws GTUException exception
     * @throws OperationalPlanException exception
     * @throws SimRuntimeException exception
     */
    protected synchronized void leaveCrossSection() throws GTUException, OperationalPlanException, SimRuntimeException
    {
        List<Lane> lanes = this.crossSections.get(0).getLanes();
        for (int i = 0; i < lanes.size(); i++)
        {
            Lane lane = lanes.get(i);
            if (lane != null)
            {
                lane.removeGTU(this, i == lanes.size() - 1, position(lane, getReference()));
            }
        }
        this.crossSections.remove(0);
        this.pendingLeaveTrigger = null;
        scheduleLeaveEvent();
    }

    /**
     * Schedules all trigger events during the current operational plan on the lane.
     * @param lane Lane; lane
     * @param direction GTUDirectionality; direction
     * @throws GTUException exception
     * @throws OperationalPlanException exception
     * @throws SimRuntimeException exception
     */
    protected void scheduleTriggers(final Lane lane, final GTUDirectionality direction)
            throws GTUException, OperationalPlanException, SimRuntimeException
    {
        double min;
        double max;
        Length remain = remainingEventDistance();
        if (direction.isPlus())
        {
            min = position(lane, getRear()).si;
            max = min + remain.si + getLength().si;
        }
        else
        {
            max = position(lane, getRear()).si;
            min = max - remain.si - getLength().si;
        }
        SortedMap<Double, List<SingleSensor>> sensors = lane.getSensorMap(getGTUType(), direction).subMap(min, max);
        for (List<SingleSensor> list : sensors.values())
        {
            for (SingleSensor sensor : list)
            {
                RelativePosition pos = this.getRelativePositions().get(sensor.getPositionType());
                PolyLine3d sensorGeometry = sensor.getGeometry();
                OrientedPoint3d sensorLocation = sensor.getLocation();

                Transform2d transform =
                        new Transform2d().translate(sensorLocation.x, sensorLocation.y).rotation(sensorLocation.dirZ);
                // System.out.println(
                // "sensorLocation: " + sensorLocation + ", transform(1,0): " + transform.transform(new Point2d(1, 0)));
                LineSegment2d segment = new LineSegment2d(transform.transform(sensorGeometry.getFirst().project()),
                        transform.transform(sensorGeometry.getLast().project()));
                // System.out.println("calling timeAtLine for " + this + ", sensor " + sensor.getId() + ", " + segment);
                Time time = timeAtLine(segment, pos);
                // System.out.println("\t result is " + time);
                if (time != null)
                {
                    this.sensorEvents.add(getSimulator().scheduleEventAbs(time, this, sensor, "trigger", new Object[] {this}));
                }
            }
        }
    }

    /**
     * Returns a safe distance beyond which a line will definitely not be crossed during the current operational plan.
     * @return Length; safe distance beyond which a line will definitely not be crossed during the current operational plan
     * @throws OperationalPlanException exception
     */
    private Length remainingEventDistance() throws OperationalPlanException
    {
        if (getOperationalPlan() instanceof LaneBasedOperationalPlan)
        {
            LaneBasedOperationalPlan plan = (LaneBasedOperationalPlan) getOperationalPlan();
            return plan.getTotalLength().minus(plan.getTraveledDistance(getSimulator().getSimulatorTime())).plus(eventMargin);
        }
        return getOperationalPlan().getTotalLength().plus(eventMargin);
    }

    /**
     * Compute the intersection of a line segment and an infinitely high wall.
     * @param segmentFrom Point3d; start point of the line segment
     * @param segmentTo Point3d; end point of the line segment
     * @param wall LineSegment2d; location of the wall
     * @return Point3d; the intersection of the line segment and the wall, or null if the line segment does not intersect the
     *         wall
     */
    private Point3d interSectionOfLineSegmentAndWall(final Point3d segmentFrom, final Point3d segmentTo,
            final LineSegment2d wall)
    {
        Point2d intersection = Point2d.intersectionOfLineSegments(segmentFrom.project(), segmentTo.project(),
                wall.getStartPoint(), wall.getEndPoint());
        if (null == intersection)
        {
            return null;
        }
        // Figure out the elevation at the intersection point
        Point2d projectedFrom = segmentFrom.project();
        return segmentFrom.interpolate(segmentTo,
                projectedFrom.distance(intersection) / projectedFrom.distance(segmentTo.project()));
    }

    /**
     * Returns an estimation of when the relative position will reach the line. Returns {@code null} if this does not occur
     * during the current operational plan.
     * @param line PolyLine3d; line, i.e. lateral line at link start or lateral entrance of sensor
     * @param relativePosition RelativePosition; position to cross the line
     * @return estimation of when the relative position will reach the line, {@code null} if this does not occur during the
     *         current operational plan
     * @throws GTUException position error
     */
    private Time timeAtLine(final LineSegment2d line, final RelativePosition relativePosition) throws GTUException
    {
        PolyLine3d path = getOperationalPlan().getPath();
        Point3d[] points;
        double adjust;
        if (relativePosition.getDx().gt0())
        {
            // as the position is downstream of the reference, we need to attach some distance at the end
            points = new Point3d[path.size() + 1];
            int nextIndex = 0;
            for (Iterator<Point3d> iterator = path.getPoints(); iterator.hasNext();)
            {
                points[nextIndex++] = iterator.next();
            }
            // add a final point at the end that is DX further
            points[nextIndex] = path.getLocationExtended(path.getLength() + relativePosition.getDx().si);
            // XXX: why not simply REPLACE the last point?
            adjust = -relativePosition.getDx().si;
        }
        else if (relativePosition.getDx().lt0())
        {
            points = new Point3d[path.size() + 1];
            int nextIndex = 1;
            for (Iterator<Point3d> iterator = path.getPoints(); iterator.hasNext();)
            {
                points[nextIndex++] = iterator.next();
            }
            points[0] = path.getLocationExtended(relativePosition.getDx().si); // add first point
            // XXX: why not simply REPLACE the first point?
            adjust = 0.0;
        }
        else
        {
            points = new Point3d[path.size()];
            int nextIndex = 0;
            for (Iterator<Point3d> iterator = path.getPoints(); iterator.hasNext();)
            {
                points[nextIndex++] = iterator.next();
            }
            // System.arraycopy(path.getPoints(), 0, points, 0, path.size());
            adjust = 0.0;
        }

        // Find the (first) intersection of the path of the operational plan with the wall
        double cumulativeDistance = 0.0;
        for (int i = 0; i < points.length - 1; i++)
        {
            Point3d intersect;
            intersect = interSectionOfLineSegmentAndWall(points[i], points[i + 1], line);
            if (intersect != null)
            {
                cumulativeDistance += points[i].distance(intersect);
                cumulativeDistance += adjust; // , 0.0); // possible rear is already considered in first segment
                // return time at distance
                if (cumulativeDistance < 0.0)
                {
                    return getSimulator().getSimulatorTime();
                }
                if (cumulativeDistance <= getOperationalPlan().getTotalLength().si)
                {
                    return getOperationalPlan().timeAtDistance(Length.instantiateSI(cumulativeDistance));
                }
                // ref will cross the line, but GTU will not travel enough for rear to cross
                return null;
            }
            else if (i < points.length - 2)
            {
                cumulativeDistance += points[i].distance(points[i + 1]);
            }
        }
        // No intersection found; GTU does not cross the wall during the current operational plan
        return null;
    }

    /** {@inheritDoc} */
    @Override
    public final Map<Lane, Length> positions(final RelativePosition relativePosition) throws GTUException
    {
        return positions(relativePosition, getSimulator().getSimulatorTime());
    }

    /** {@inheritDoc} */
    @Override
    public final Map<Lane, Length> positions(final RelativePosition relativePosition, final Time when) throws GTUException
    {
        Map<Lane, Length> positions = new LinkedHashMap<>();
        for (CrossSection crossSection : this.crossSections.get(when))
        {
            for (Lane lane : crossSection.getLanes())
            {
                positions.put(lane, position(lane, relativePosition, when));
            }
        }
        return positions;
    }

    /** {@inheritDoc} */
    @Override
    public final Length position(final Lane lane, final RelativePosition relativePosition) throws GTUException
    {
        return position(lane, relativePosition, getSimulator().getSimulatorTime());
    }

    /** Caching of time field for last stored position(s). */
    private double cachePositionsTime = Double.NaN;

    /** Caching of operation plan for last stored position(s). */
    private OperationalPlan cacheOperationalPlan = null;

    /** caching of last stored position(s). */
    private MultiKeyMap<Length> cachedPositions = new MultiKeyMap<>(Lane.class, RelativePosition.class);

    /** {@inheritDoc} */
    @Override
    public Length position(final Lane lane, final RelativePosition relativePosition, final Time when) throws GTUException
    {
        synchronized (this)
        {
            OperationalPlan plan = getOperationalPlan(when);
            if (CACHING)
            {
                if (when.si == this.cachePositionsTime && plan == this.cacheOperationalPlan)
                {
                    Length l = this.cachedPositions.get(lane, relativePosition);
                    if (l != null && (!Double.isNaN(l.si)))
                    {
                        CACHED_POSITION++;
                        // PK verify the result; uncomment if you don't trust the cache
                        // this.cachedPositions.clear();
                        // Length difficultWay = position(lane, relativePosition, when);
                        // if (Math.abs(l.si - difficultWay.si) > 0.00001)
                        // {
                        // System.err.println("Whoops: cache returns bad value for GTU " + getId() + " cache returned " + l
                        // + ", re-computing yielded " + difficultWay);
                        // l = null; // Invalidate; to debug and try again
                        // }
                        // }
                        // if (l != null)
                        // {
                        return l;
                    }
                }
                if (when.si != this.cachePositionsTime || plan != this.cacheOperationalPlan)
                {
                    this.cachePositionsTime = Double.NaN;
                    this.cacheOperationalPlan = null;
                    this.cachedPositions.clear();
                }
            }
            NON_CACHED_POSITION++;

            synchronized (this.lock)
            {
                List<CrossSection> whenCrossSections = this.crossSections.get(when);
                double loc = Double.NaN;

                try
                {
                    int crossSectionIndex = -1;
                    int lateralIndex = -1;
                    for (int i = 0; i < whenCrossSections.size(); i++)
                    {
                        if (whenCrossSections.get(i).getLanes().contains(lane))
                        {
                            crossSectionIndex = i;
                            lateralIndex = whenCrossSections.get(i).getLanes().indexOf(lane);
                            break;
                        }
                    }
                    Throw.when(lateralIndex == -1, GTUException.class, "GTU %s is not on lane %s.", this, lane);

                    OrientedPoint3d p = plan.getLocation(when, relativePosition);
                    double f = OTSGeometryUtil.projectFractional(lane.getCenterLine(), null, null, p.x, p.y);
                    if (f >= 0.0 && f <= 1.0)
                    {
                        loc = f * lane.getLength().si;
                    }
                    else
                    {
                        // the point does not project fractionally to this lane, it has to be up- or downstream of the lane
                        // try upstream
                        double distance = 0.0;
                        for (int i = crossSectionIndex - 1; i >= 0; i--)
                        {
                            Lane tryLane = whenCrossSections.get(i).getLanes().get(lateralIndex);
                            f = OTSGeometryUtil.projectFractional(tryLane.getCenterLine(), null, null, p.x, p.y);
                            if (f >= 0.0 && f <= 1.0)
                            {
                                f = whenCrossSections.get(i).getDirection() == GTUDirectionality.DIR_PLUS ? 1 - f : f;
                                loc = distance - f * tryLane.getLength().si;
                                break;
                            }
                            distance -= tryLane.getLength().si;
                        }
                        // try downstream
                        if (Double.isNaN(loc))
                        {
                            distance = lane.getLength().si;
                            for (int i = crossSectionIndex + 1; i < whenCrossSections.size(); i++)
                            {
                                Lane tryLane = whenCrossSections.get(i).getLanes().get(lateralIndex);
                                f = OTSGeometryUtil.projectFractional(tryLane.getCenterLine(), null, null, p.x, p.y);
                                if (f >= 0.0 && f <= 1.0)
                                {
                                    f = whenCrossSections.get(i).getDirection() == GTUDirectionality.DIR_PLUS ? f : 1 - f;
                                    loc = distance + f * tryLane.getLength().si;
                                    break;
                                }
                                distance += tryLane.getLength().si;
                            }
                        }

                    }

                    if (Double.isNaN(loc))
                    {
                        // the GTU is not on the lane with the relativePosition, nor is it registered on next/previous lanes
                        // this can occur as the GTU was generated with the rear upstream of the lane, or due to rounding errors
                        // use different fraction projection fallback
                        f = Math.min(1.0, OTSGeometryUtil.projectFractional(lane.getCenterLine(), null, null, p.x, p.y));
                        if (Double.isNaN(f))
                        {
                            CategoryLogger.always().error("GTU {} at location {} cannot project itself onto {}; p is {}", this,
                                    getLocation(), lane.getCenterLine(), p);
                            plan.getLocation(when, relativePosition);
                        }
                        loc = lane.getLength().si * f;

                        // if (CACHING)
                        // {
                        // this.cachedPositions.put(cacheIndex, null);
                        // }
                        // return null;
                        // if (getOdometer().lt(getLength()))
                        // {
                        // // this occurs when the GTU is generated with the rear upstream of the lane, which we often do
                        // loc = position(lane, getFront(), when).si + relativePosition.getDx().si - getFront().getDx().si;
                        // }
                        // else
                        // {
                        // System.out.println("loc is NaN");
                        // }
                    }
                }
                catch (Exception e)
                {
                    // System.err.println(toString() + ": " + e.getMessage());
                    throw new GTUException(e);
                }

                Length length = Length.instantiateSI(loc);
                if (CACHING)
                {
                    this.cachedPositions.put(length, lane, relativePosition);
                    this.cachePositionsTime = when.si;
                    this.cacheOperationalPlan = plan;
                }
                return length;
            }
        }
    }

    /** {@inheritDoc} */
    @Override
    public DirectedLanePosition getReferencePosition() throws GTUException
    {
        synchronized (this)
        {
            if (this.referencePositionTime == getSimulator().getSimulatorTime().si)
            {
                return this.cachedReferencePosition;
            }
            Lane refLane = null;
            for (CrossSection crossSection : this.crossSections)
            {
                Lane lane = crossSection.getLanes().get(this.referenceLaneIndex);
                double fraction = fractionalPosition(lane, getReference());
                if (fraction >= 0.0 && fraction <= 1.0)
                {
                    refLane = lane;
                    break;
                }
            }
            if (refLane != null)
            {
                this.cachedReferencePosition =
                        new DirectedLanePosition(refLane, position(refLane, getReference()), this.getDirection(refLane));
                this.referencePositionTime = getSimulator().getSimulatorTime().si;
                return this.cachedReferencePosition;
            }
            CategoryLogger.always().error("The reference point of GTU {} is not on any of the lanes on which it is registered",
                    this);
            for (CrossSection crossSection : this.crossSections)
            {
                Lane lane = crossSection.getLanes().get(this.referenceLaneIndex);
                double fraction = fractionalPosition(lane, getReference());
                CategoryLogger.always().error("\tGTU is on lane \"{}\" at fraction {}", lane, fraction);
            }
            throw new GTUException(
                    "The reference point of GTU " + this + " is not on any of the lanes on which it is registered");
        }
    }

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

    /** {@inheritDoc} */
    @Override
    public final Map<Lane, Double> fractionalPositions(final RelativePosition relativePosition, final Time when)
            throws GTUException
    {
        Map<Lane, Double> positions = new LinkedHashMap<>();
        for (CrossSection crossSection : this.crossSections)
        {
            for (Lane lane : crossSection.getLanes())
            {
                positions.put(lane, fractionalPosition(lane, relativePosition, when));
            }
        }
        return positions;
    }

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

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

    /** {@inheritDoc} */
    @Override
    public final void addTrigger(final Lane lane, final SimEventInterface<SimTimeDoubleUnit> event)
    {
        throw new UnsupportedOperationException("Method addTrigger is not supported.");
    }

    /**
     * Sets a vehicle model.
     * @param vehicleModel VehicleModel; vehicle model
     */
    public void setVehicleModel(final VehicleModel vehicleModel)
    {
        this.vehicleModel = vehicleModel;
    }

    /** {@inheritDoc} */
    @Override
    public VehicleModel getVehicleModel()
    {
        return this.vehicleModel;
    }

    /** {@inheritDoc} */
    @Override
    public void destroy()
    {
        DirectedLanePosition dlp = null;
        try
        {
            dlp = getReferencePosition();
        }
        catch (GTUException e)
        {
            // ignore. not important at destroy
        }
        OrientedPoint3d location = this.getOperationalPlan() == null ? new OrientedPoint3d(0.0, 0.0, 0.0) : getLocation();
        synchronized (this.lock)
        {
            for (CrossSection crossSection : this.crossSections)
            {
                boolean removeFromParentLink = true;
                for (Lane lane : crossSection.getLanes())
                {
                    Length position;
                    try
                    {
                        position = position(lane, getReference());
                    }
                    catch (GTUException exception)
                    {
                        // TODO: hard remove over whole network
                        // TODO: logger notification
                        throw new RuntimeException(exception);
                    }
                    lane.removeGTU(this, removeFromParentLink, position);
                    removeFromParentLink = false;
                }
            }
        }
        if (dlp != null && dlp.getLane() != null)
        {
            Lane referenceLane = dlp.getLane();
            fireTimedEvent(LaneBasedGTU.LANEBASED_DESTROY_EVENT,
                    new Object[] {getId(), OTSGeometryUtil.doubleVector(location, PositionUnit.METER),
                            new Direction(location.getDirZ(), DirectionUnit.EAST_RADIAN), getOdometer(),
                            referenceLane.getParentLink().getId(), referenceLane.getId(), dlp.getPosition(),
                            dlp.getGtuDirection().name()},
                    getSimulator().getSimulatorTime());
        }
        else
        {
            fireTimedEvent(LaneBasedGTU.LANEBASED_DESTROY_EVENT,
                    new Object[] {getId(), OTSGeometryUtil.doubleVector(location, PositionUnit.METER),
                            new Direction(location.getDirZ(), DirectionUnit.EAST_RADIAN), getOdometer(), null, null,
                            Length.ZERO, null},
                    getSimulator().getSimulatorTime());
        }
        cancelAllEvents();

        super.destroy();
    }

    /** {@inheritDoc} */
    @Override
    public final Bounds3d getBounds()
    {
        return new Bounds3d(getLength().doubleValue(), getWidth().doubleValue(), 0.0);
    }

    /** {@inheritDoc} */
    @Override
    public final LaneBasedStrategicalPlanner getStrategicalPlanner()
    {
        return (LaneBasedStrategicalPlanner) super.getStrategicalPlanner();
    }

    /** {@inheritDoc} */
    @Override
    public final LaneBasedStrategicalPlanner getStrategicalPlanner(final Time time)
    {
        return (LaneBasedStrategicalPlanner) super.getStrategicalPlanner(time);
    }

    /** {@inheritDoc} */
    @Override
    public RoadNetwork getNetwork()
    {
        return (RoadNetwork) super.getPerceivableContext();
    }

    /** {@inheritDoc} */
    @Override
    public Speed getDesiredSpeed()
    {
        synchronized (this)
        {
            Time simTime = getSimulator().getSimulatorTime();
            if (this.desiredSpeedTime == null || this.desiredSpeedTime.si < simTime.si)
            {
                InfrastructurePerception infra =
                        getTacticalPlanner().getPerception().getPerceptionCategoryOrNull(InfrastructurePerception.class);
                SpeedLimitInfo speedInfo;
                if (infra == null)
                {
                    speedInfo = new SpeedLimitInfo();
                    speedInfo.addSpeedInfo(SpeedLimitTypes.MAX_VEHICLE_SPEED, getMaximumSpeed());
                }
                else
                {
                    // Throw.whenNull(infra, "InfrastructurePerception is required to determine the desired speed.");
                    speedInfo = infra.getSpeedLimitProspect(RelativeLane.CURRENT).getSpeedLimitInfo(Length.ZERO);
                }
                this.cachedDesiredSpeed =
                        Try.assign(() -> getTacticalPlanner().getCarFollowingModel().desiredSpeed(getParameters(), speedInfo),
                                "Parameter exception while obtaining the desired speed.");
                this.desiredSpeedTime = simTime;
            }
            return this.cachedDesiredSpeed;
        }
    }

    /** {@inheritDoc} */
    @Override
    public Acceleration getCarFollowingAcceleration()
    {
        synchronized (this)
        {
            Time simTime = getSimulator().getSimulatorTime();
            if (this.carFollowingAccelerationTime == null || this.carFollowingAccelerationTime.si < simTime.si)
            {
                LanePerception perception = getTacticalPlanner().getPerception();
                // speed
                EgoPerception<?, ?> ego = perception.getPerceptionCategoryOrNull(EgoPerception.class);
                Throw.whenNull(ego, "EgoPerception is required to determine the speed.");
                Speed speed = ego.getSpeed();
                // speed limit info
                InfrastructurePerception infra = perception.getPerceptionCategoryOrNull(InfrastructurePerception.class);
                Throw.whenNull(infra, "InfrastructurePerception is required to determine the desired speed.");
                SpeedLimitInfo speedInfo = infra.getSpeedLimitProspect(RelativeLane.CURRENT).getSpeedLimitInfo(Length.ZERO);
                // leaders
                NeighborsPerception neighbors = perception.getPerceptionCategoryOrNull(NeighborsPerception.class);
                Throw.whenNull(neighbors, "NeighborsPerception is required to determine the car-following acceleration.");
                PerceptionCollectable<HeadwayGTU, LaneBasedGTU> leaders = neighbors.getLeaders(RelativeLane.CURRENT);
                // obtain
                this.cachedCarFollowingAcceleration =
                        Try.assign(() -> getTacticalPlanner().getCarFollowingModel().followingAcceleration(getParameters(),
                                speed, speedInfo, leaders), "Parameter exception while obtaining the desired speed.");
                this.carFollowingAccelerationTime = simTime;
            }
            return this.cachedCarFollowingAcceleration;
        }
    }

    /** {@inheritDoc} */
    @Override
    public final TurnIndicatorStatus getTurnIndicatorStatus()
    {
        return this.turnIndicatorStatus.get();
    }

    /** {@inheritDoc} */
    @Override
    public final TurnIndicatorStatus getTurnIndicatorStatus(final Time time)
    {
        return this.turnIndicatorStatus.get(time);
    }

    /** {@inheritDoc} */
    @Override
    public final void setTurnIndicatorStatus(final TurnIndicatorStatus turnIndicatorStatus)
    {
        this.turnIndicatorStatus.set(turnIndicatorStatus);
    }

    /** {@inheritDoc} */
    @Override
    public Length getLateralPosition(final Lane lane) throws GTUException
    {
        OperationalPlan plan = getOperationalPlan();
        if (plan instanceof LaneBasedOperationalPlan && !((LaneBasedOperationalPlan) plan).isDeviative())
        {
            return Length.ZERO;
        }
        DirectedLanePosition ref = getReferencePosition();
        int latIndex = -1;
        int longIndex = -1;
        for (int i = 0; i < this.crossSections.size(); i++)
        {
            List<Lane> lanes = this.crossSections.get(i).getLanes();
            if (lanes.contains(lane))
            {
                latIndex = lanes.indexOf(lane);
            }
            if (lanes.contains(ref.getLane()))
            {
                longIndex = i;
            }
        }
        Throw.when(latIndex == -1 || longIndex == -1, GTUException.class, "GTU %s is not on %s", getId(), lane);
        Lane refCrossSectionLane = this.crossSections.get(longIndex).getLanes().get(latIndex);
        OrientedPoint3d loc = getLocation();
        double f = refCrossSectionLane.getCenterLine().projectOrthogonalFractional(new Point3d(loc.x, loc.y, 0));
        Ray3d p = Try.assign(() -> refCrossSectionLane.getCenterLine().getLocationFraction(f), GTUException.class,
                "GTU %s is not orthogonal to the reference lane.", getId());
        double d = p.distance(loc);
        d = ref.getGtuDirection().isPlus() ? d : -d;
        if (this.crossSections.get(0).getLanes().size() > 1)
        {
            return Length.instantiateSI(latIndex == 0 ? -d : d);
        }
        double x2 = p.x + Math.cos(p.phi);
        double y2 = p.y + Math.sin(p.phi);
        double det = (loc.x - p.x) * (y2 - p.y) - (loc.y - p.y) * (x2 - p.x);
        return Length.instantiateSI(det < 0.0 ? -d : d);
    }

    /** {@inheritDoc} */
    @Override
    public void setInstantaneousLaneChange(final boolean instantaneous)
    {
        this.instantaneousLaneChange = instantaneous;
    }

    /** {@inheritDoc} */
    @Override
    public boolean isInstantaneousLaneChange()
    {
        return this.instantaneousLaneChange;
    }

    /** {@inheritDoc} */
    @Override
    public String toString()
    {
        return String.format("GTU " + getId());
    }

    /** Cross section of lanes. */
    private static class CrossSection
    {

        /** Lanes. */
        private final List<Lane> lanes;

        /** GTU directionality. */
        private final GTUDirectionality direction;

        /**
         * @param lanes List&lt;Lane&gt;; lanes
         * @param direction GTUDirectionality; GTU directionality
         */
        protected CrossSection(final List<Lane> lanes, final GTUDirectionality direction)
        {
            this.lanes = lanes;
            this.direction = direction;
        }

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

        /**
         * @return direction.
         */
        protected GTUDirectionality getDirection()
        {
            return this.direction;
        }

    }

}