RollingLaneStructureRecord.java

package org.opentrafficsim.road.gtu.lane.perception;

import java.io.Serializable;
import java.util.ArrayList;
import java.util.HashSet;
import java.util.LinkedHashSet;
import java.util.List;
import java.util.Set;

import org.djunits.value.vdouble.scalar.Length;
import org.opentrafficsim.core.gtu.GTUDirectionality;
import org.opentrafficsim.core.gtu.GTUException;
import org.opentrafficsim.core.gtu.GTUType;
import org.opentrafficsim.core.gtu.NestedCache;
import org.opentrafficsim.core.gtu.Try;
import org.opentrafficsim.core.network.LateralDirectionality;
import org.opentrafficsim.core.network.Link;
import org.opentrafficsim.core.network.NetworkException;
import org.opentrafficsim.core.network.Node;
import org.opentrafficsim.core.network.route.Route;
import org.opentrafficsim.road.network.lane.Lane;

import nl.tudelft.simulation.language.Throw;

/**
 * A LaneStructureRecord contains information about the lanes that can be accessed from this lane by a GTUType. It tells whether
 * there is a left and/or right lane by pointing to other LaneStructureRecords, and which successor LaneStructureRecord(s) there
 * are at the end of the lane of this LaneStructureRecord. All information (left, right, next) is calculated relative to the
 * driving direction of the GTU that owns this structure.
 * <p>
 * Copyright (c) 2013-2018 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
 * BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
 * </p>
 * $LastChangedDate: 2015-07-24 02:58:59 +0200 (Fri, 24 Jul 2015) $, @version $Revision: 1147 $, by $Author: averbraeck $,
 * initial version Feb 21, 2016 <br>
 * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
 * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
 */
public class RollingLaneStructureRecord implements LaneStructureRecord, Serializable
{
    /** */
    private static final long serialVersionUID = 20160400L;

    /** Cache of allows route information. */
    // TODO clear on network change, with an event listener?
    private static NestedCache<Boolean> allowsRouteCache =
            new NestedCache<>(Lane.class, Route.class, GTUType.class, Boolean.class);

    /** The lane of the LSR. */
    private final Lane lane;

    /** The direction in which we process this lane. */
    private final GTUDirectionality gtuDirectionality;

    /** The left LSR or null if not available. Left and right are relative to the <b>driving</b> direction. */
    private RollingLaneStructureRecord left;

    /** Legal left lane change possibility. */
    private boolean mayChangeLeft;

    /** The right LSR or null if not available. Left and right are relative to the <b>driving</b> direction. */
    private RollingLaneStructureRecord right;

    /** Legal right lane change possibility. */
    private boolean mayChangeRight;

    /** Where this lane was cut-off resulting in no next lanes, if so. */
    private Length cutOffEnd = null;

    /** Where this lane was cut-off resulting in no prev lanes, if so. */
    private Length cutOffStart = null;

    /** Distance to start of the record, negative for backwards. */
    private Length startDistance;

    /**
     * The next LSRs. The list is empty if no LSRs are available. Next is relative to the driving direction, not to the design
     * line direction.
     */
    private List<RollingLaneStructureRecord> nextList = new ArrayList<>();

    /**
     * The previous LSRs. The list is empty if no LSRs are available. Previous is relative to the driving direction, not to the
     * design line direction.
     */
    private List<RollingLaneStructureRecord> prevList = new ArrayList<>();

    /** Record who's start start distance is used to calculate the start distance of this record. */
    private RollingLaneStructureRecord source;

    /** Start distance link between records. */
    private RecordLink sourceLink;

    /** Set of records who's starting position depends on this record. */
    private final Set<RollingLaneStructureRecord> dependentRecords = new LinkedHashSet<>();

    /**
     * Constructor.
     * @param lane Lane; lane
     * @param direction GTUDirectionality; direction of travel for the GTU
     * @param startDistanceSource LaneStructureRecord; record on which the start distance is based
     * @param recordLink RecordLink; link type to source
     */
    public RollingLaneStructureRecord(final Lane lane, final GTUDirectionality direction,
            final RollingLaneStructureRecord startDistanceSource, final RecordLink recordLink)
    {
        this.lane = lane;
        this.gtuDirectionality = direction;
        this.source = startDistanceSource;
        this.sourceLink = recordLink;
        if (startDistanceSource != null)
        {
            startDistanceSource.dependentRecords.add(this);
        }
    }

    /**
     * @param lane the lane of the LSR
     * @param direction the direction on which we process this lane
     * @param startDistance distance to start of the record, negative for backwards
     */
    public RollingLaneStructureRecord(final Lane lane, final GTUDirectionality direction, final Length startDistance)
    {
        this.lane = lane;
        this.gtuDirectionality = direction;
        this.startDistance = startDistance;

        this.source = null;
        this.sourceLink = null;
    }

    /** {@inheritDoc} */
    @Override
    public Length getLength()
    {
        return getLane().getLength();
    }

    /**
     * Change the source of the distance.
     * @param startDistanceSource LaneStructureRecord; record on which the start distance is based
     * @param recordLink RecordLink; link type to source
     */
    final void changeStartDistanceSource(final RollingLaneStructureRecord startDistanceSource, final RecordLink recordLink)
    {
        // clear link
        if (this.source != null)
        {
            this.source.dependentRecords.remove(this);
        }
        // set new link
        this.source = startDistanceSource;
        this.sourceLink = recordLink;
        if (this.source != null)
        {
            this.source.dependentRecords.add(this);
        }
    }

    /**
     * Updates the start distance, including all records who's start distance depends on this value. Advised is to only initiate
     * this at the root record. Note that before this is invoked, all record-links should be updated.
     * @param fractionalPosition double; fractional position at the current cross-section
     * @param laneStructure LaneStructure; parent lane structure
     */
    final void updateStartDistance(final double fractionalPosition, final RollingLaneStructure laneStructure)
    {
        this.startDistance = this.sourceLink.calculateStartDistance(this.source, this, fractionalPosition);
        for (RollingLaneStructureRecord record : this.dependentRecords)
        {
            record.updateStartDistance(fractionalPosition, laneStructure);
        }
    }

    /**
     * Returns the source of the start distance.
     * @return LaneStructureRecord; source of the start distance
     */
    final RollingLaneStructureRecord getStartDistanceSource()
    {
        return this.source;
    }

    /** {@inheritDoc} */
    @Override
    public final Node getFromNode()
    {
        return this.gtuDirectionality.isPlus() ? this.lane.getParentLink().getStartNode()
                : this.lane.getParentLink().getEndNode();
    }

    /** {@inheritDoc} */
    @Override
    public final Node getToNode()
    {
        return this.gtuDirectionality.isPlus() ? this.lane.getParentLink().getEndNode()
                : this.lane.getParentLink().getStartNode();
    }

    /**
     * @return whether the link to which this lane belongs splits, i.e. some of the parallel, connected lanes lead to a
     *         different destination than others
     */
    @Deprecated
    public final boolean isLinkSplit()
    {
        if (isCutOffEnd())
        {
            // if the end is a split, it's out of range
            return false;
        }
        Set<Node> toNodes = new HashSet<>();
        LaneStructureRecord lsr = this;
        while (lsr != null)
        {
            for (LaneStructureRecord next : lsr.getNext())
            {
                toNodes.add(next.getToNode());
            }
            lsr = lsr.getLeft();
        }
        lsr = this.getRight();
        while (lsr != null)
        {
            for (LaneStructureRecord next : lsr.getNext())
            {
                toNodes.add(next.getToNode());
            }
            lsr = lsr.getRight();
        }
        return toNodes.size() > 1;
    }

    /**
     * @return whether the link to which this lane belongs merges, i.e. some of the parallel, connected lanes follow from a
     *         different origin than others
     */
    public final boolean isLinkMerge()
    {
        if (isCutOffStart())
        {
            // if the start is a merge, it's out of range
            return false;
        }
        Set<Node> fromNodes = new HashSet<>();
        LaneStructureRecord lsr = this;
        while (lsr != null)
        {
            for (LaneStructureRecord prev : lsr.getPrev())
            {
                fromNodes.add(prev.getFromNode());
            }
            lsr = lsr.getLeft();
        }
        lsr = this.getRight();
        while (lsr != null)
        {
            for (LaneStructureRecord prev : lsr.getPrev())
            {
                fromNodes.add(prev.getFromNode());
            }
            lsr = lsr.getRight();
        }
        return fromNodes.size() > 1;
    }

    /** {@inheritDoc} */
    @Override
    public final boolean allowsRoute(final Route route, final GTUType gtuType) throws NetworkException
    {
        return allowsRoute(route, gtuType, false);
    }

    /** {@inheritDoc} */
    @Override
    public final boolean allowsRouteAtEnd(final Route route, final GTUType gtuType) throws NetworkException
    {
        return allowsRoute(route, gtuType, true);
    }

    /**
     * Returns whether (the end of) this lane allows the route to be followed, using caching.
     * @param route Route; the route to follow
     * @param gtuType GTUType; gtu type
     * @param end boolean; whether to consider the end (or otherwise the lane itself, i.e. allow lane change from this lane)
     * @return whether the end of this lane allows the route to be followed
     * @throws NetworkException if no destination node
     */
    private boolean allowsRoute(final Route route, final GTUType gtuType, final boolean end) throws NetworkException
    {
        return allowsRouteCache.getValue(() -> Try.assign(() -> allowsRoute0(route, gtuType, end), "no destination"), this.lane,
                route, gtuType, end);
    }

    /**
     * Returns whether (the end of) this lane allows the route to be followed.
     * @param route Route; the route to follow
     * @param gtuType GTUType; gtu type
     * @param end boolean; whether to consider the end (or otherwise the lane itself, i.e. allow lane change from this lane)
     * @return whether the end of this lane allows the route to be followed
     * @throws NetworkException if no destination node
     */
    private boolean allowsRoute0(final Route route, final GTUType gtuType, final boolean end) throws NetworkException
    {

        // driving without route
        if (route == null)
        {
            return true;
        }

        // start with simple check
        int from = route.indexOf(getFromNode());
        int to = route.indexOf(getToNode());
        if (from == -1 || to == -1 || from != to - 1)
        {
            return leadsToRoute(route, gtuType, null);
        }

        // link is on the route, but lane markings may still prevent the route from being followed
        Set<LaneStructureRecord> currentSet = new LinkedHashSet<>();
        Set<LaneStructureRecord> nextSet = new LinkedHashSet<>();
        currentSet.add(this);

        boolean firstLoop = true;
        while (!currentSet.isEmpty())
        {

            if (!firstLoop || end)
            {
                // move longitudinal
                for (LaneStructureRecord laneRecord : currentSet)
                {
                    to = route.indexOf(laneRecord.getToNode());
                    if (to == route.getNodes().size() - 2)
                    {
                        // check connector
                        for (Link link : laneRecord.getToNode().nextLinks(gtuType, laneRecord.getLane().getParentLink()))
                        {
                            if (link.getLinkType().isConnector())
                            {
                                if ((link.getStartNode().equals(laneRecord.getToNode())
                                        && link.getEndNode().equals(route.destinationNode()))
                                        || (link.getEndNode().equals(laneRecord.getToNode())
                                                && link.getStartNode().equals(route.destinationNode())))
                                {
                                    return true;
                                }
                            }
                        }
                    }
                    for (LaneStructureRecord next : laneRecord.getNext())
                    {
                        if (next.getToNode().equals(route.destinationNode()))
                        {
                            // reached destination, by definition ok
                            return true;
                        }
                        if (route.indexOf(next.getToNode()) == to + 1)
                        {
                            nextSet.add(next);
                        }
                    }
                }
                currentSet = nextSet;
                nextSet = new LinkedHashSet<>();
            }
            firstLoop = false;

            // move lateral
            nextSet.addAll(currentSet);
            for (LaneStructureRecord laneRecord : currentSet)
            {
                while (laneRecord.legalLeft() && !nextSet.contains(laneRecord.getLeft()))
                {
                    nextSet.add(laneRecord.getLeft());
                    laneRecord = laneRecord.getLeft();
                }
            }
            for (LaneStructureRecord laneRecord : currentSet)
            {
                while (laneRecord.legalRight() && !nextSet.contains(laneRecord.getRight()))
                {
                    nextSet.add(laneRecord.getRight());
                    laneRecord = laneRecord.getRight();
                }
            }

            // none of the next lanes was on the route
            if (nextSet.isEmpty())
            {
                return false;
            }

            // reached a link on the route where all lanes can be reached?
            int nLanesOnNextLink = 0;
            LaneStructureRecord nextRecord = nextSet.iterator().next();
            for (Lane l : nextRecord.getLane().getParentLink().getLanes())
            {
                if (l.getLaneType().getDirectionality(gtuType).getDirectionalities().contains(nextRecord.getDirection()))
                {
                    nLanesOnNextLink++;
                }
            }
            if (nextSet.size() == nLanesOnNextLink)
            {
                // in this case we don't need to look further, anything is possible again
                return true;
            }

            currentSet = nextSet;
            nextSet = new LinkedHashSet<>();

        }

        // never reached our destination or a link with all lanes accessible
        return false;
    }

    /**
     * Returns whether continuing on this lane will allow the route to be followed, while the lane itself is not on the route.
     * @param route Route; the route to follow
     * @param gtuType GTUType; gtu type
     * @param original LaneStructureRecord; source record, should be {@code null} to prevent loop recognition on first iteration
     * @return whether continuing on this lane will allow the route to be followed
     * @throws NetworkException if no destination node
     */
    private boolean leadsToRoute(final Route route, final GTUType gtuType, final LaneStructureRecord original)
            throws NetworkException
    {
        if (original == this)
        {
            return false; // stop loop
        }
        if (original != null && allowsRoute(route, gtuType))
        {
            return true;
        }
        // move downstream until we are at the route
        for (LaneStructureRecord record : getNext())
        {
            boolean leadsTo =
                    ((RollingLaneStructureRecord) record).leadsToRoute(route, gtuType, original == null ? this : original);
            if (leadsTo)
            {
                return true;
            }
        }
        return false;
    }

    /** {@inheritDoc} */
    @Override
    public final RollingLaneStructureRecord getLeft()
    {
        return this.left;
    }

    /**
     * @param leftRecord set the left LSR or null if not available. Left and right are relative to the <b>driving</b> direction.
     * @param gtuType GTU type
     */
    public final void setLeft(final RollingLaneStructureRecord leftRecord, final GTUType gtuType)
    {
        this.left = leftRecord;
        this.mayChangeLeft = getLane().accessibleAdjacentLanesLegal(LateralDirectionality.LEFT, gtuType, this.gtuDirectionality)
                .contains(leftRecord.getLane());
        if (getLane().getFullId().equals("1023.FORWARD3") && !this.mayChangeLeft)
        {
            System.out.println("Lane 1023.FORWARD3 allows left:" + this.mayChangeLeft);
        }
    }

    /** {@inheritDoc} */
    @Override
    public final boolean legalLeft()
    {
        return this.mayChangeLeft;
    }

    /** {@inheritDoc} */
    @Override
    public final boolean physicalLeft()
    {
        return this.left != null;
    }

    /** {@inheritDoc} */
    @Override
    public final RollingLaneStructureRecord getRight()
    {
        return this.right;
    }

    /**
     * @param rightRecord set the right LSR or null if not available. Left and right are relative to the <b>driving</b>
     *            direction
     * @param gtuType GTU type
     */
    public final void setRight(final RollingLaneStructureRecord rightRecord, final GTUType gtuType)
    {
        this.right = rightRecord;
        this.mayChangeRight =
                getLane().accessibleAdjacentLanesLegal(LateralDirectionality.RIGHT, gtuType, this.gtuDirectionality)
                        .contains(rightRecord.getLane());
    }

    /** {@inheritDoc} */
    @Override
    public final boolean legalRight()
    {
        return this.mayChangeRight;
    }

    /** {@inheritDoc} */
    @Override
    public final boolean physicalRight()
    {
        return this.right != null;
    }

    /** {@inheritDoc} */
    @Override
    public final List<RollingLaneStructureRecord> getNext()
    {
        return this.nextList;
    }

    /**
     * Clears the next list.
     */
    final void clearNextList()
    {
        this.nextList.clear();
    }

    /**
     * @param next a next LSRs to add. Next is relative to the driving direction, not to the design line direction.
     * @throws GTUException if the records is cut-off at the end
     */
    public final void addNext(final RollingLaneStructureRecord next) throws GTUException
    {
        Throw.when(this.cutOffEnd != null, GTUException.class,
                "Cannot add next records to a record that was cut-off at the end.");
        this.nextList.add(next);
    }

    /** {@inheritDoc} */
    @Override
    public final List<RollingLaneStructureRecord> getPrev()
    {
        return this.prevList;
    }

    /**
     * Clears the prev list.
     */
    final void clearPrevList()
    {
        this.prevList.clear();
    }

    /**
     * @param prev a previous LSRs to add. Previous is relative to the driving direction, not to the design line direction.
     * @throws GTUException if the records is cut-off at the start
     */
    public final void addPrev(final RollingLaneStructureRecord prev) throws GTUException
    {
        Throw.when(this.cutOffStart != null, GTUException.class,
                "Cannot add previous records to a record that was cut-off at the start.");
        this.prevList.add(prev);
    }

    /**
     * Sets this record as being cut-off, i.e. there are no next records due to cut-off.
     * @param cutOffEnd where this lane was cut-off (in the driving direction) resulting in no next lanes
     * @throws GTUException if there are next records
     */
    public final void setCutOffEnd(final Length cutOffEnd) throws GTUException
    {
        Throw.when(!this.nextList.isEmpty(), GTUException.class,
                "Setting lane record with cut-off end, but there are next records.");
        this.cutOffEnd = cutOffEnd;
    }

    /**
     * Sets this record as being cut-off, i.e. there are no previous records due to cut-off.
     * @param cutOffStart where this lane was cut-off (in the driving direction) resulting in no prev lanes
     * @throws GTUException if there are previous records
     */
    public final void setCutOffStart(final Length cutOffStart) throws GTUException
    {
        Throw.when(!this.prevList.isEmpty(), GTUException.class,
                "Setting lane record with cut-off start, but there are previous records.");
        this.cutOffStart = cutOffStart;
    }

    /** {@inheritDoc} */
    @Override
    public final boolean isCutOffEnd()
    {
        return this.cutOffEnd != null;
    }

    /** {@inheritDoc} */
    @Override
    public final boolean isCutOffStart()
    {
        return this.cutOffStart != null;
    }

    /**
     * Returns distance where the structure was cut-off.
     * @return distance where the structure was cut-off
     */
    public final Length getCutOffEnd()
    {
        return this.cutOffEnd;
    }

    /**
     * Returns distance where the structure was cut-off.
     * @return distance where the structure was cut-off
     */
    public final Length getCutOffStart()
    {
        return this.cutOffStart;
    }

    /**
     * Clears the cut-off at the end.
     */
    public final void clearCutOffEnd()
    {
        this.cutOffEnd = null;
    }

    /**
     * Clears the cut-off at the start.
     */
    public final void clearCutOffStart()
    {
        this.cutOffStart = null;
    }

    /** {@inheritDoc} */
    @Override
    public final boolean isDeadEnd()
    {
        return this.cutOffEnd == null && this.nextList.isEmpty();
    }

    /** {@inheritDoc} */
    @Override
    public final Lane getLane()
    {
        return this.lane;
    }

    /** {@inheritDoc} */
    @Override
    public final GTUDirectionality getDirection()
    {
        return this.gtuDirectionality;
    }

    /** {@inheritDoc} */
    @Override
    public final Length getStartDistance()
    {
        return this.startDistance;
    }
    
    /** {@inheritDoc} */
    @Override
    public boolean isDownstreamBranch()
    {
        // DOWN, LATERAL_START and CROSS are part of the downstream branch
        return !RecordLink.UP.equals(this.sourceLink) && !RecordLink.LATERAL_END.equals(this.sourceLink);
    }

    /** {@inheritDoc} */
    @Override
    public final String toString()
    {
        // left and right may cause stack overflow
        String s;
        if (this.source == null)
        {
            s = "o";
        }
        else if (this.source == this.left)
        {
            s = "^";
        }
        else if (this.source == this.right)
        {
            s = "v";
        }
        else if (this.prevList.contains(this.source))
        {
            s = "<";
        }
        else if (this.nextList.contains(this.source))
        {
            s = ">";
        }
        else
        {
            s = "?";
        }
        return "LaneStructureRecord [lane=" + this.lane + " (" + s + "), direction=" + this.gtuDirectionality + "]";
    }

    /**
     * Link between records that defines the dependence of start position and hence how this is updated as the GTU moves.
     * <p>
     * Copyright (c) 2013-2018 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved.
     * <br>
     * BSD-style license. See <a href="http://opentrafficsim.org/node/13">OpenTrafficSim License</a>.
     * <p>
     * @version $Revision$, $LastChangedDate$, by $Author$, initial version 22 jan. 2018 <br>
     * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
     * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
     * @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
     */
    public enum RecordLink
    {

        /** This record is upstream of the start distance source. */
        UP
        {
            /** {@inheritDoc} */
            @Override
            public Length calculateStartDistance(final RollingLaneStructureRecord startDistanceSource,
                    final RollingLaneStructureRecord self, final double fractionalPosition)
            {
                return startDistanceSource.getStartDistance().minus(self.getLane().getLength());
            }
        },

        /** This record is downstream of the start distance source. */
        DOWN
        {
            /** {@inheritDoc} */
            @Override
            public Length calculateStartDistance(final RollingLaneStructureRecord startDistanceSource,
                    final RollingLaneStructureRecord self, final double fractionalPosition)
            {
                return startDistanceSource.getStartDistance().plus(startDistanceSource.getLane().getLength());
            }
        },

        /** This record is laterally adjacent to the start distance source, and found in an upstream search. */
        LATERAL_END
        {
            /** {@inheritDoc} */
            @Override
            public Length calculateStartDistance(final RollingLaneStructureRecord startDistanceSource,
                    final RollingLaneStructureRecord self, final double fractionalPosition)
            {
                return startDistanceSource.getStartDistance().plus(startDistanceSource.getLane().getLength())
                        .minus(self.getLane().getLength());
            }
        },

        /** This record is laterally adjacent to the start distance source, and found in a downstream search. */
        LATERAL_START
        {
            /** {@inheritDoc} */
            @Override
            public Length calculateStartDistance(final RollingLaneStructureRecord startDistanceSource,
                    final RollingLaneStructureRecord self, final double fractionalPosition)
            {
                return startDistanceSource.getStartDistance();
            }
        },

        /** Part of the current cross-section. */
        CROSS
        {
            /** {@inheritDoc} */
            @Override
            public Length calculateStartDistance(final RollingLaneStructureRecord startDistanceSource,
                    final RollingLaneStructureRecord self, final double fractionalPosition)
            {
                return self.getLane().getLength().multiplyBy(fractionalPosition).neg();
            }
        };

        /**
         * Calculate the start position of this record based on a neighboring source.
         * @param startDistanceSource RollingLaneStructureRecord; source record in the tree
         * @param self RollingLaneStructureRecord; own record
         * @param fractionalPosition double; fractional position on the cross-section
         * @return start position of this record based on a neighboring source
         */
        public abstract Length calculateStartDistance(RollingLaneStructureRecord startDistanceSource,
                RollingLaneStructureRecord self, double fractionalPosition);

    }

}