LaneBasedStrategicalRoutePlanner

package org.opentrafficsim.road.gtu.strategical;

import java.util.ArrayList;
import java.util.Iterator;
import java.util.List;
import java.util.Optional;
import java.util.Set;

import org.djunits.value.vdouble.scalar.Duration;
import org.djutils.exceptions.Throw;
import org.djutils.exceptions.Try;
import org.opentrafficsim.base.OtsRuntimeException;
import org.opentrafficsim.core.gtu.GtuException;
import org.opentrafficsim.core.gtu.GtuType;
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.gtu.lane.LaneBasedGtu;
import org.opentrafficsim.road.gtu.lane.tactical.LaneBasedTacticalPlanner;
import org.opentrafficsim.road.network.lane.CrossSectionLink;
import org.opentrafficsim.road.network.lane.Lane;
import org.opentrafficsim.road.network.lane.LanePosition;

/**
 * This is the standard strategical route planner with a fixed tactical planner. If no route is supplied, but there is a
 * destination, a route will be drawn from a {@code RouteGenerator}, or using length-based shortest path if that is also not
 * specified. If the route is ever {@code null}, a route will be constructed from the origin to the current link, and from the
 * current link to the destination.
 * <p>
 * Copyright (c) 2013-2024 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
 * BSD-style license. See <a href="https://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
 * </p>
 * @author <a href="https://github.com/averbraeck">Alexander Verbraeck</a>
 * @author <a href="https://github.com/peter-knoppers">Peter Knoppers</a>
 */
public class LaneBasedStrategicalRoutePlanner implements LaneBasedStrategicalPlanner
{
    /** GTU. */
    private final LaneBasedGtu gtu;

    /** The route to drive. */
    private Route route;

    /** Origin node. */
    private final Node origin;

    /** Destination node. */
    private Node destination;

    /** The fixed tactical planner to use for the GTU. */
    private final LaneBasedTacticalPlanner fixedTacticalPlanner;

    /** Route supplier. */
    private final RouteGenerator routeGenerator;

    /**
     * Constructor for a strategical planner without route. This can only be used if the network does not have splits, or split
     * fractions are used.
     * @param fixedTacticalPlanner the tactical planner to use for the GTU
     * @param gtu GTU
     * @throws GtuException if fixed tactical planner == null
     */
    public LaneBasedStrategicalRoutePlanner(final LaneBasedTacticalPlanner fixedTacticalPlanner, final LaneBasedGtu gtu)
            throws GtuException
    {
        this(fixedTacticalPlanner, null, gtu, null, null, RouteGenerator.NULL);
    }

    /**
     * Constructor for a strategical planner with route. If the route is {@code null}, a shortest path to the destination is
     * derived.
     * @param fixedTacticalPlanner the tactical planner to use for the GTU
     * @param route the route to drive
     * @param gtu GTU
     * @param origin origin node
     * @param destination destination node
     * @param routeGenerator route generator
     * @throws GtuException if fixed tactical planner == null
     */
    public LaneBasedStrategicalRoutePlanner(final LaneBasedTacticalPlanner fixedTacticalPlanner, final Route route,
            final LaneBasedGtu gtu, final Node origin, final Node destination, final RouteGenerator routeGenerator)
            throws GtuException
    {
        this.gtu = gtu;
        this.route = route;
        this.origin = origin;
        this.destination = destination;
        this.fixedTacticalPlanner = fixedTacticalPlanner;
        Throw.when(fixedTacticalPlanner == null, GtuException.class,
                "Fixed Tactical Planner for a Strategical planner is null");
        this.routeGenerator = routeGenerator;
    }

    @Override
    public final LaneBasedGtu getGtu()
    {
        return this.gtu;
    }

    @Override
    public final LaneBasedTacticalPlanner getTacticalPlanner()
    {
        return this.fixedTacticalPlanner;
    }

    @Override
    public LaneBasedTacticalPlanner getTacticalPlanner(final Duration time)
    {
        return this.fixedTacticalPlanner; // fixed anyway
    }

    @Override
    public final Link nextLink(final Link previousLink, final GtuType gtuType) throws NetworkException
    {
        assureRoute(gtuType);

        Node node = previousLink.getEndNode();
        // if there is no split, don't ask the route
        if (node.getLinks().size() == 1 && previousLink != null)
        {
            // end node
            return null;
        }
        if (node.getLinks().size() == 1 && previousLink == null)
        {
            // start node
            return node.getLinks().iterator().next();
        }
        if (node.getLinks().size() == 2)
        {
            for (Link link : node.getLinks())
            {
                if (!link.equals(previousLink))
                {
                    return link;
                }
            }
        }

        // if we only have one way to go, don't bother about the route yet
        Set<Link> links = node.getLinks().toSet();
        for (Iterator<Link> linkIterator = links.iterator(); linkIterator.hasNext();)
        {
            Link link = linkIterator.next();
            if (link.equals(previousLink))
            {
                // No u-turn...
                linkIterator.remove();
            }
            else
            {
                // does the directionality of the link forbid us to go in?
                if (link.getEndNode().equals(node))
                {
                    linkIterator.remove();
                }
                else
                {
                    // are there no lanes from the node into this link in the outgoing direction?
                    boolean out = false;
                    CrossSectionLink csLink = (CrossSectionLink) link;
                    for (Lane lane : csLink.getLanes())
                    {
                        if ((link.getStartNode().equals(node) && lane.getType().isCompatible(gtuType)))
                        {
                            out = true;
                            break;
                        }
                    }
                    if (!out)
                    {
                        linkIterator.remove();
                    }
                }
            }
        }

        if (links.size() == 1)
        {
            return links.iterator().next();
        }

        // more than 2 links... We have to check the route!
        if (getRoute().isEmpty())
        {
            throw new NetworkException("LaneBasedStrategicalRoutePlanner does not have a route");
        }
        int i = this.route.getNodes().indexOf(node);
        if (i == -1)
        {
            throw new NetworkException("LaneBasedStrategicalRoutePlanner is asked for a next link coming from " + previousLink
                    + ", but node " + node + " not in route " + this.route);
        }
        if (i == this.route.getNodes().size() - 1)
        {
            throw new NetworkException("LaneBasedStrategicalRoutePlanner is asked for a next link coming from " + previousLink
                    + ", but the GTU reached the last node for route " + this.route);
        }
        Node nextNode = this.route.getNode(i + 1);
        Link result = null;
        for (Link link : links)
        {
            // TODO this takes the first in the set of links that connects the correct nodes; does not handle parallel links
            // consistently
            Link l = null;
            if (link.getStartNode().equals(nextNode) && link.getEndNode().equals(node))
            {
                l = link; // FIXME: Probably this test can be removed since we only go "forward"
            }
            if (link.getEndNode().equals(nextNode) && link.getStartNode().equals(node))
            {
                l = link;
            }
            if (null != result && null != l)
            {
                throw new NetworkException("Cannot choose among multiple links from " + node + " to " + nextNode);
            }
            else if (null == result)
            {
                result = l;
            }
        }
        if (null == result)
        {
            throw new NetworkException("LaneBasedStrategicalRoutePlanner is asked for a next link coming from "
                    + previousLink.getId() + ", but no link could be found connecting node " + node + " and node " + nextNode
                    + " for route " + this.route);
        }
        return result;
    }

    @Override
    public final Optional<Route> getRoute()
    {
        assureRoute(getGtu().getType());
        // if assure route left the route null although there is a destination, we have no route generator, use shortest-path
        if (this.route == null && this.destination != null && !getGtu().isRoaming())
        {
            try
            {
                CrossSectionLink link = getGtu().getPosition().lane().getLink();
                Node from = link.getStartNode();
                this.route = link.getNetwork().getShortestRouteBetween(getGtu().getType(), from, this.destination);
            }
            catch (NetworkException exception)
            {
                throw new OtsRuntimeException("Route could not be determined.", exception);
            }
        }
        return Optional.ofNullable(this.route);
    }

    /**
     * Assures a route is available if a route is already present, or a destination and route supplier are provided.
     * @param gtuType the type of the GTU for which a route must be assured
     */
    private void assureRoute(final GtuType gtuType)
    {
        if (this.route == null && this.destination != null && !this.routeGenerator.equals(RouteGenerator.NULL)
                && !getGtu().isRoaming())
        {
            LanePosition ref = Try.assign(() -> getGtu().getPosition(), "Could not retrieve GTU reference position.");
            List<Node> nodes = new ArrayList<>();
            if (this.origin != null)
            {
                nodes.addAll(
                        this.routeGenerator.getRoute(this.origin, ref.lane().getLink().getStartNode(), gtuType).getNodes());
            }
            else
            {
                nodes.add(ref.lane().getLink().getStartNode());
            }
            Route newRoute = this.routeGenerator.getRoute(ref.lane().getLink().getEndNode(), this.destination, gtuType);
            nodes.addAll(newRoute.getNodes());
            this.route = Try.assign(() -> new Route(
                    "Route for " + gtuType + " from " + this.origin + "to " + this.destination + " via " + ref.lane().getLink(),
                    gtuType, nodes), "No route possible over nodes %s", nodes);
        }
    }

    @Override
    public final Optional<Node> getOrigin()
    {
        return Optional.ofNullable(this.origin);
    }

    @Override
    public final Optional<Node> getDestination()
    {
        return Optional.ofNullable(this.destination);
    }

    @Override
    public final String toString()
    {
        return "LaneBasedStrategicalRoutePlanner [route=" + this.route + ", fixedTacticalPlanner=" + this.fixedTacticalPlanner
                + "]";
    }

}