package org.opentrafficsim.road.network;
   
   import java.util.Iterator;
   import java.util.LinkedHashMap;
   import java.util.List;
   import java.util.Map;
   import java.util.Set;
   import java.util.SortedSet;
   import java.util.TreeSet;
  
  import org.djunits.value.vdouble.scalar.Length;
  import org.djutils.base.Identifiable;
  import org.djutils.exceptions.Throw;
  import org.djutils.immutablecollections.ImmutableSortedSet;
  import org.djutils.immutablecollections.ImmutableTreeSet;
  import org.djutils.multikeymap.MultiKeyMap;
  import org.jgrapht.GraphPath;
  import org.jgrapht.alg.shortestpath.DijkstraShortestPath;
  import org.jgrapht.graph.SimpleDirectedWeightedGraph;
  import org.opentrafficsim.core.dsol.OtsSimulatorInterface;
  import org.opentrafficsim.core.gtu.GtuType;
  import org.opentrafficsim.core.network.LateralDirectionality;
  import org.opentrafficsim.core.network.Link;
  import org.opentrafficsim.core.network.Network;
  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.CrossSectionLink;
  import org.opentrafficsim.road.network.lane.Lane;
  
  /**
   * RoadNetwork adds the ability to retrieve lane change information.
   * <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" target="_blank">Alexander Verbraeck</a>
   * @author <a href="https://github.com/wjschakel">Wouter Schakel</a>
   */
  public class RoadNetwork extends Network
  {
      /** */
      private static final long serialVersionUID = 1L;
  
      /** Cached lane graph for legal connections, per GTU type. */
      private Map<GtuType, RouteWeightedGraph> legalLaneGraph = new LinkedHashMap<>();
  
      /** Cached lane graph for physical connections. */
      private RouteWeightedGraph physicalLaneGraph = null;
  
      /** Cached legal lane change info, over complete length of route. */
      private MultiKeyMap<SortedSet<LaneChangeInfo>> legalLaneChangeInfoCache =
              new MultiKeyMap<>(GtuType.class, Route.class, Lane.class);
  
      /** Cached physical lane change info, over complete length of route. */
      private MultiKeyMap<SortedSet<LaneChangeInfo>> physicalLaneChangeInfoCache = new MultiKeyMap<>(Route.class, Lane.class);
  
      /**
       * Construction of an empty network.
       * @param id the network id.
       * @param simulator the DSOL simulator engine
       */
      public RoadNetwork(final String id, final OtsSimulatorInterface simulator)
      {
          super(id, simulator);
      }
  
      /**
       * Returns lane change info from the given lane. Distances are given from the start of the lane and will never exceed the
       * given range. This method returns {@code null} if no valid path exists. If there are no reasons to change lane within
       * range, an empty set is returned.
       * @param lane from lane.
       * @param route route.
       * @param gtuType GTU Type.
       * @param range maximum range of info to consider, from the start of the given lane.
       * @param laneAccessLaw lane access law.
       * @return lane change info from the given lane, or {@code null} if no path exists.
       */
      public ImmutableSortedSet<LaneChangeInfo> getLaneChangeInfo(final Lane lane, final Route route, final GtuType gtuType,
              final Length range, final LaneAccessLaw laneAccessLaw)
      {
          Throw.whenNull(lane, "Lane may not be null.");
          Throw.whenNull(route, "Route may not be null.");
          Throw.whenNull(gtuType, "GTU type may not be null.");
          Throw.whenNull(range, "Range may not be null.");
          Throw.whenNull(laneAccessLaw, "Lane access law may not be null.");
          Throw.when(range.le0(), IllegalArgumentException.class, "Range should be a positive value.");
  
          // get the complete info
          SortedSet<LaneChangeInfo> info = getCompleteLaneChangeInfo(lane, route, gtuType, laneAccessLaw);
          if (info == null)
          {
              return null;
          }
  
          // find first LaneChangeInfo beyond range, if any
          LaneChangeInfo lcInfoBeyondHorizon = null;
          Iterator<LaneChangeInfo> iterator = info.iterator();
          while (lcInfoBeyondHorizon == null && iterator.hasNext())
         {
             LaneChangeInfo lcInfo = iterator.next();
             if (lcInfo.remainingDistance().gt(range))
             {
                 lcInfoBeyondHorizon = lcInfo;
             }
         }
 
         // return subset in range
         if (lcInfoBeyondHorizon != null)
         {
             return new ImmutableTreeSet<>(info.headSet(lcInfoBeyondHorizon));
         }
         return new ImmutableTreeSet<>(info); // empty, or all in range
     }
 
     /**
      * Returns the complete (i.e. without range) lane change info from the given lane. It is either taken from cache, or
      * created.
      * @param lane from lane.
      * @param route route.
      * @param gtuType GTU Type.
      * @param laneAccessLaw lane access law.
      * @return complete (i.e. without range) lane change info from the given lane, or {@code null} if no path exists.
      */
     private SortedSet<LaneChangeInfo> getCompleteLaneChangeInfo(final Lane lane, final Route route, final GtuType gtuType,
             final LaneAccessLaw laneAccessLaw)
     {
         // try to get info from the right cache
         SortedSet<LaneChangeInfo> outputLaneChangeInfo;
         if (laneAccessLaw.equals(LaneAccessLaw.LEGAL))
         {
             outputLaneChangeInfo = this.legalLaneChangeInfoCache.get(gtuType, route, lane);
             // build info if required
             if (outputLaneChangeInfo == null)
             {
                 // get the right lane graph for the GTU type, or build it
                 RouteWeightedGraph graph = this.legalLaneGraph.get(gtuType);
                 if (graph == null)
                 {
                     graph = new RouteWeightedGraph();
                     this.legalLaneGraph.put(gtuType, graph);
                     buildGraph(graph, gtuType, laneAccessLaw);
                 }
                 List<LaneChangeInfoEdge> path = findPath(lane, graph, gtuType, route);
 
                 if (path != null)
                 {
                     // derive lane change info from every lane along the path and cache it
                     boolean originalPath = true;
                     while (!path.isEmpty())
                     {
                         SortedSet<LaneChangeInfo> laneChangeInfo = extractLaneChangeInfo(path);
                         if (originalPath)
                         {
                             outputLaneChangeInfo = laneChangeInfo;
                             originalPath = false;
                         }
                         this.legalLaneChangeInfoCache.put(laneChangeInfo, gtuType, route, path.get(0).fromLane());
                         path.remove(0); // next lane
                     }
                 }
             }
         }
         else if (laneAccessLaw.equals(LaneAccessLaw.PHYSICAL))
         {
             outputLaneChangeInfo = this.physicalLaneChangeInfoCache.get(route, lane);
             // build info if required
             if (outputLaneChangeInfo == null)
             {
                 // build the lane graph if required
                 if (this.physicalLaneGraph == null)
                 {
                     this.physicalLaneGraph = new RouteWeightedGraph();
                     // TODO: Is the GTU type actually relevant for physical? It is used still to find adjacent lanes.
                     buildGraph(this.physicalLaneGraph, gtuType, laneAccessLaw);
                 }
                 List<LaneChangeInfoEdge> path = findPath(lane, this.physicalLaneGraph, gtuType, route);
 
                 if (path != null)
                 {
                     // derive lane change info from every lane along the path and cache it
                     boolean originalPath = true;
                     while (!path.isEmpty())
                     {
                         SortedSet<LaneChangeInfo> laneChangeInfo = extractLaneChangeInfo(path);
                         if (originalPath)
                         {
                             outputLaneChangeInfo = laneChangeInfo;
                             originalPath = false;
                         }
                         this.physicalLaneChangeInfoCache.put(laneChangeInfo, route, path.get(0).fromLane());
                         path.remove(0); // next lane
                     }
                 }
             }
         }
         else
         {
             // in case it is inadvertently extended in the future
             throw new RuntimeException(String.format("Unknown LaneChangeLaw %s", laneAccessLaw));
         }
         return outputLaneChangeInfo;
     }
 
     /**
      * Builds the graph.
      * @param graph empty graph to build.
      * @param gtuType GTU type.
      * @param laneChangeLaw lane change law, legal or physical.
      */
     private void buildGraph(final RouteWeightedGraph graph, final GtuType gtuType, final LaneAccessLaw laneChangeLaw)
     {
         // add vertices
         boolean legal = laneChangeLaw.equals(LaneAccessLaw.LEGAL);
         for (Link link : this.getLinkMap().values())
         {
             for (Lane lane : legal ? ((CrossSectionLink) link).getLanes() : ((CrossSectionLink) link).getLanesAndShoulders())
             {
                 graph.addVertex(lane);
             }
             // each end node may be a destination for the shortest path search
             graph.addVertex(link.getEndNode());
         }
 
         // add edges
         for (Link link : this.getLinkMap().values())
         {
             if (link instanceof CrossSectionLink cLink)
             {
                 for (Lane lane : legal ? cLink.getLanes() : cLink.getLanesAndShoulders())
                 {
                     // adjacent lanes
                     for (LateralDirectionality lat : List.of(LateralDirectionality.LEFT, LateralDirectionality.RIGHT))
                     {
                         Set<Lane> adjacentLanes;
                         if (legal)
                         {
                             adjacentLanes = lane.accessibleAdjacentLanesLegal(lat, gtuType);
                         }
                         else
                         {
                             adjacentLanes = lane.accessibleAdjacentLanesPhysical(lat, gtuType);
                         }
                         for (Lane adjacentLane : adjacentLanes)
                         {
                             LaneChangeInfoEdgeType type = lat.equals(LateralDirectionality.LEFT) ? LaneChangeInfoEdgeType.LEFT
                                     : LaneChangeInfoEdgeType.RIGHT;
                             // downstream link may be null for lateral edges
                             LaneChangeInfoEdge edge = new LaneChangeInfoEdge(lane, type, null);
                             graph.addEdge(lane, adjacentLane, edge);
                         }
                     }
                     // next lanes
                     Set<Lane> nextLanes = lane.nextLanes(legal ? gtuType : null);
                     for (Lane nextLane : nextLanes)
                     {
                         LaneChangeInfoEdge edge =
                                 new LaneChangeInfoEdge(lane, LaneChangeInfoEdgeType.DOWNSTREAM, nextLane.getLink());
                         graph.addEdge(lane, nextLane, edge);
                     }
                     // add edge towards end node so that it can be used as a destination in the shortest path search
                     LaneChangeInfoEdge edge = new LaneChangeInfoEdge(lane, LaneChangeInfoEdgeType.DOWNSTREAM, null);
                     graph.addEdge(lane, lane.getLink().getEndNode(), edge);
                 }
             }
         }
     }
 
     /**
      * Returns a set of lane change info, extracted from the graph.
      * @param lane from lane.
      * @param graph graph.
      * @param gtuType GTU Type.
      * @param route route.
      * @return path derived from the graph, or {@code null} if there is no path.
      */
     private List<LaneChangeInfoEdge> findPath(final Lane lane, final RouteWeightedGraph graph, final GtuType gtuType,
             final Route route)
     {
         // if there is no route, find the destination node by moving down the links (no splits allowed)
         Node destination = null;
         Route routeForWeights = route;
         if (route == null)
         {
             destination = graph.getNoRouteDestinationNode(gtuType);
             try
             {
                 routeForWeights = getShortestRouteBetween(gtuType, lane.getLink().getStartNode(), destination);
             }
             catch (NetworkException exception)
             {
                 // this should not happen, as we obtained the destination by moving downstream towards the end of the network
                 throw new RuntimeException("Could not find route to destination.", exception);
             }
         }
         else
         {
             // otherwise, get destination node from route, which is the last node on a link with lanes (i.e. no connector)
             List<Node> nodes = route.getNodes();
             for (int i = nodes.size() - 1; i > 0; i--)
             {
                 Link link = getLink(nodes.get(i - 1), nodes.get(i));
                 if (link instanceof CrossSectionLink && !((CrossSectionLink) link).getLanes().isEmpty())
                 {
                     destination = nodes.get(i);
                     break; // found most downstream link with lanes, who's end node is the destination for lane changes
                 }
             }
             Throw.whenNull(destination, "Route has no links with lanes, "
                     + "unable to find a suitable destination node regarding lane change information.");
         }
 
         // set the route on the path for route-dependent edge weights
         graph.setRoute(routeForWeights);
 
         // find the shortest path
         GraphPath<Identifiable, LaneChangeInfoEdge> path = DijkstraShortestPath.findPathBetween(graph, lane, destination);
         return path == null ? null : path.getEdgeList();
     }
 
     /**
      * Extracts lane change info from a path.
      * @param path path.
      * @return lane change info.
      */
     private SortedSet<LaneChangeInfo> extractLaneChangeInfo(final List<LaneChangeInfoEdge> path)
     {
         SortedSet<LaneChangeInfo> info = new TreeSet<>();
         Length x = Length.ZERO; // cumulative longitudinal distance
         int n = 0; // number of applied lane changes
         boolean inLateralState = false; // consecutive lateral moves in the path create 1 LaneChangeInfo
         for (LaneChangeInfoEdge edge : path)
         {
             LaneChangeInfoEdgeType lcType = edge.laneChangeInfoEdgeType();
             int lat = lcType.equals(LaneChangeInfoEdgeType.LEFT) ? -1 : (lcType.equals(LaneChangeInfoEdgeType.RIGHT) ? 1 : 0);
 
             // check opposite lateral direction
             if (n * lat < 0)
             {
                 /*
                  * The required direction is opposite a former required direction, in which case all further lane change
                  * information is not yet of concern. For example, we first need to make 1 right lane change for a lane drop,
                  * and then later 2 lane changes to the left for a split. The latter information is pointless before the lane
                  * drop; we are not going to stay on the lane longer as it won't affect the ease of the left lane changes later.
                  */
                 break;
             }
 
             // increase n, x, and trigger (consecutive) lateral move start or stop
             if (lat == 0)
             {
                 // lateral move stop
                 if (inLateralState)
                 {
                     // TODO: isDeadEnd should be removed from LaneChangeInfo, behavior should consider legal vs. physical
                     boolean isDeadEnd = false;
                     info.add(new LaneChangeInfo(Math.abs(n), x, isDeadEnd,
                             n < 0 ? LateralDirectionality.LEFT : LateralDirectionality.RIGHT));
                     inLateralState = false;
                     // don't add the length of the previous lane, that was already done for the first lane of all lateral moves
                 }
                 else
                 {
                     // longitudinal move, we need to add distance to x
                     x = x.plus(edge.fromLane().getLength());
                 }
             }
             else
             {
                 // lateral move start
                 if (!inLateralState)
                 {
                     x = x.plus(edge.fromLane().getLength()); // need to add length of first lane of all lateral moves
                     inLateralState = true;
                 }
                 // increase lane change count (negative for left)
                 n += lat;
             }
         }
         return info;
     }
 
     /**
      * Clears all lane change info graphs and cached sets. This method should be invoked on every network change that affects
      * lane changes and the distances within which they need to be performed.
      */
     public void clearLaneChangeInfoCache()
     {
         this.legalLaneGraph.clear();
         this.physicalLaneGraph = null;
         this.legalLaneChangeInfoCache = new MultiKeyMap<>(GtuType.class, Route.class, Lane.class);
         this.physicalLaneChangeInfoCache = new MultiKeyMap<>(Route.class, Lane.class);
     }
 
     /**
      * A {@code SimpleDirectedWeightedGraph} to search over the lanes, where the weight of an edge (movement between lanes) is
      * tailored to providing lane change information. The vertex type is {@code Identifiable} such that both {@code Lane}'s and
      * {@code Node}'s can be used. The latter is required to find paths towards a destination node.
      * <p>
      * Copyright (c) 2022-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/wjschakel">Wouter Schakel</a>
      */
     private class RouteWeightedGraph extends SimpleDirectedWeightedGraph<Identifiable, LaneChangeInfoEdge>
     {
 
         /** */
         private static final long serialVersionUID = 20220923L;
 
         /** Route. */
         private Route route;
 
         /** Node in the network that is the destination if no route is used. */
         private Node noRouteDestination = null;
 
         /**
          * Constructor.
          */
         RouteWeightedGraph()
         {
             super(LaneChangeInfoEdge.class);
         }
 
         /**
          * Set the route.
          * @param route route.
          */
         public void setRoute(final Route route)
         {
             Throw.whenNull(route, "Route may not be null for lane change information.");
             this.route = route;
         }
 
         /**
          * Returns the weight of moving from one lane to the next. In order to find the latest possible location at which lane
          * changes may still be performed, the longitudinal weights are 1.0 while the lateral weights are 1.0 + 1/X, where X is
          * the number (index) of the link within the route. This favors later lane changes for the shortest path algorithm, as
          * we are interested in the distances within which the lane change have to be performed. In the case an edge is towards
          * a link that is not in a given route, a positive infinite weight is returned. Finally, when the edge is towards a
          * node, which may be the destination in a route, 0.0 is returned.
          */
         @Override
         public double getEdgeWeight(final LaneChangeInfoEdge e)
         {
             if (e.laneChangeInfoEdgeType().equals(LaneChangeInfoEdgeType.LEFT)
                     || e.laneChangeInfoEdgeType().equals(LaneChangeInfoEdgeType.RIGHT))
             {
                 int indexEndNode = this.route.indexOf(e.fromLane().getLink().getEndNode());
                 return 1.0 + 1.0 / indexEndNode; // lateral, reduce weight for further lane changes
             }
             Link toLink = e.toLink();
             if (toLink == null)
             {
                 return 0.0; // edge towards Node, which may be the destination in a Route
             }
             if (this.route.contains(toLink.getEndNode())
                     && this.route.indexOf(toLink.getEndNode()) == this.route.indexOf(toLink.getStartNode()) + 1)
             {
                 return 1.0; // downstream, always 1.0 if the next lane is on the route
             }
             return Double.POSITIVE_INFINITY; // next lane not on the route, this is a dead-end branch for the route
         }
 
         /**
          * Returns the destination node to use when no route is available. This will be the last node found moving downstream.
          * @param gtuType GTU type.
          * @return destination node to use when no route is available.
          */
         public Node getNoRouteDestinationNode(final GtuType gtuType)
         {
             if (this.noRouteDestination == null)
             {
                 // get any lane from the network
                 Lane lane = null;
                 Iterator<Identifiable> iterator = this.vertexSet().iterator();
                 while (lane == null && iterator.hasNext())
                 {
                     Identifiable next = iterator.next();
                     if (next instanceof Lane)
                     {
                         lane = (Lane) next;
                     }
                 }
                 Throw.when(lane == null, RuntimeException.class, "Requesting destination node on network without lanes.");
                 // move to downstream link for as long as there is 1 downstream link
                 try
                 {
                     Link link = lane.getLink();
                     Set<Link> downstreamLinks = link.getEndNode().nextLinks(gtuType, link);
                     while (downstreamLinks.size() == 1)
                     {
                         link = downstreamLinks.iterator().next();
                         downstreamLinks = link.getEndNode().nextLinks(gtuType, link);
                     }
                     Throw.when(downstreamLinks.size() > 1, RuntimeException.class, "Using null route on network with split. "
                             + "Unable to find a destination to find lane change info towards.");
                     this.noRouteDestination = link.getEndNode();
                 }
                 catch (NetworkException ne)
                 {
                     throw new RuntimeException("Requesting lane change info from link that does not allow the GTU type.", ne);
                 }
             }
             return this.noRouteDestination;
         }
     }
 
     /**
      * Edge between two lanes, or between a lane and a node (to provide the shortest path algorithm with a suitable
      * destination). From a list of these from a path, the lane change information along the path (distances and number of lane
      * changes) can be derived.
      * <p>
      * Copyright (c) 2022-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/wjschakel">Wouter Schakel</a>
      * @param fromLane from lane, to allow construction of distances from a path.
      * @param laneChangeInfoEdgeType the type of lane to lane movement performed along this edge.
      * @param toLink to link (of the lane this edge moves to).
      */
     private static record LaneChangeInfoEdge(Lane fromLane, LaneChangeInfoEdgeType laneChangeInfoEdgeType, Link toLink)
     {
     }
 
     /**
      * Enum to provide information on the lane to lane movement in a path.
      * <p>
      * Copyright (c) 2022-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/wjschakel">Wouter Schakel</a>
      */
     private enum LaneChangeInfoEdgeType
     {
         /** Left lane change. */
         LEFT,
 
         /** Right lane change. */
         RIGHT,
 
         /** Downstream movement, either towards a lane, or towards a node (which may be the destination in a route). */
         DOWNSTREAM;
     }
 
 }