package org.opentrafficsim.road.network.factory;
   
   import java.util.ArrayList;
   import java.util.Arrays;
   import java.util.LinkedHashMap;
   import java.util.List;
   import java.util.Map;
   
   import javax.naming.NamingException;
  
  import org.djunits.unit.LengthUnit;
  import org.djunits.value.vdouble.scalar.Length;
  import org.djunits.value.vdouble.scalar.Speed;
  import org.djutils.exceptions.Throw;
  import org.djutils.exceptions.Try;
  import org.opentrafficsim.core.dsol.OTSSimulatorInterface;
  import org.opentrafficsim.core.geometry.Bezier;
  import org.opentrafficsim.core.geometry.OTSGeometryException;
  import org.opentrafficsim.core.geometry.OTSLine3D;
  import org.opentrafficsim.core.geometry.OTSPoint3D;
  import org.opentrafficsim.core.gtu.GTUType;
  import org.opentrafficsim.core.network.LateralDirectionality;
  import org.opentrafficsim.core.network.LinkType;
  import org.opentrafficsim.core.network.NetworkException;
  import org.opentrafficsim.core.network.OTSNode;
  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.Lane;
  import org.opentrafficsim.road.network.lane.LaneType;
  import org.opentrafficsim.road.network.lane.OTSRoadNode;
  import org.opentrafficsim.road.network.lane.Shoulder;
  import org.opentrafficsim.road.network.lane.Stripe;
  import org.opentrafficsim.road.network.lane.Stripe.Permeable;
  import org.opentrafficsim.road.network.lane.changing.LaneKeepingPolicy;
  
  import nl.tudelft.simulation.dsol.simulators.DEVSSimulatorInterface;
  import nl.tudelft.simulation.language.d3.DirectedPoint;
  
  /**
   * <p>
   * Copyright (c) 2013-2019 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-09-16 19:20:07 +0200 (Wed, 16 Sep 2015) $, @version $Revision: 1405 $, by $Author: averbraeck $,
   * initial version 30 okt. 2014 <br>
   * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
   */
  public final class LaneFactory
  {
  
      /** Stripe width. */
      private static final Length STRIPE_WIDTH = Length.instantiateSI(0.2);
  
      /** Angle above which a Bezier curve is used over a straight line. */
      private static final double BEZIER_MARGIN = Math.toRadians(0.5);
  
      /** Link. */
      private final CrossSectionLink link;
  
      /** Offset for next cross section elements. Left side of lane when building left to right, and vice versa. */
      private Length offset;
  
      /** Lane width to use (negative when building left to right). */
      private Length laneWidth0;
  
      /** Start offset. */
      private Length offsetStart = Length.ZERO;
  
      /** End offset. */
      private Length offsetEnd = Length.ZERO;
  
      /** Lane type to use. */
      private LaneType laneType0;
  
      /** Speed limit to use. */
      private Speed speedLimit0;
  
      /** created lanes. */
      private final List<Lane> lanes = new ArrayList<>();
  
      /**
       * @param network OTSRoadNetwork; network
       * @param from OTSRoadNode; from node
       * @param to OTSRoadNode; to node
       * @param type LinkType; link type
       * @param simulator OTSSimulatorInterface; simulator
       * @param policy LaneKeepingPolicy; lane keeping policy
       * @throws OTSGeometryException if no valid line can be created
       * @throws NetworkException if the link exists, or a node does not exist, in the network
       */
      public LaneFactory(final OTSRoadNetwork network, final OTSRoadNoderk/lane/OTSRoadNode.html#OTSRoadNode">OTSRoadNode from, final OTSRoadNode to, final LinkType type,
              final OTSSimulatorInterface simulator, final LaneKeepingPolicy policy) throws OTSGeometryException, NetworkException
      {
          this(network, from, to, type, simulator, policy, makeLine(from, to));
      }
  
      /**
       * @param network OTSRoadNetwork; network
      * @param from OTSRoadNode; from node
      * @param to OTSRoadNode; to node
      * @param type LinkType; link type
      * @param simulator OTSSimulatorInterface; simulator
      * @param policy LaneKeepingPolicy; lane keeping policy
      * @param line OTSLine3D; line
      * @throws NetworkException if the link exists, or a node does not exist, in the network
      */
     public LaneFactory(final OTSRoadNetwork network, final OTSRoadNoderk/lane/OTSRoadNode.html#OTSRoadNode">OTSRoadNode from, final OTSRoadNode to, final LinkType type,
             final OTSSimulatorInterface simulator, final LaneKeepingPolicy policy, final OTSLine3D line) throws NetworkException
     {
         this.link = new CrossSectionLink(network, from.getId() + to.getId(), from, to, type, line, simulator, policy);
     }
 
     /**
      * Creates a line between two nodes. If the nodes and their directions are on a straight line, a straight line is created.
      * Otherwise a default Bezier curve is created.
      * @param from OTSNode; from node
      * @param to OTSNode; to node
      * @return OTSLine3D; line
      * @throws OTSGeometryException if no valid line can be created
      */
     private static OTSLine3D makeLine(final OTSNode from, final OTSNode to) throws OTSGeometryException
     {
         // Straight or bezier?
         double rotCrow = Math.atan2(to.getLocation().y - from.getLocation().y, to.getLocation().x - from.getLocation().x);
         double dRot = from.getLocation().getRotZ() - rotCrow;
         while (dRot < -Math.PI)
         {
             dRot += 2.0 * Math.PI;
         }
         while (dRot > Math.PI)
         {
             dRot -= 2.0 * Math.PI;
         }
         OTSLine3D line;
         if (from.getLocation().getRotZ() != to.getLocation().getRotZ() || Math.abs(dRot) > BEZIER_MARGIN)
         {
             line = Bezier.cubic(from.getLocation(), to.getLocation());
         }
         else
         {
             line = new OTSLine3D(from.getPoint(), to.getPoint());
         }
         return line;
     }
 
     /**
      * Prepare the factory to add lanes from left to right.
      * @param leftLanes double; number of lanes left from the link design line
      * @param laneWidth Length; lane width
      * @param laneType LaneType; lane type
      * @param speedLimit Speed; speed limit
      * @return LaneFactory this lane factory for method chaining
      */
     public LaneFactory leftToRight(final double leftLanes, final Length laneWidth, final LaneType laneType,
             final Speed speedLimit)
     {
         this.offset = laneWidth.times(leftLanes);
         this.laneWidth0 = laneWidth.neg();
         this.laneType0 = laneType;
         this.speedLimit0 = speedLimit;
         Try.execute(
                 () -> new Stripe(this.link, this.offset.plus(this.offsetStart), this.offset.plus(this.offsetEnd), STRIPE_WIDTH),
                 "Unexpected exception while building link.");
         return this;
     }
 
     /**
      * Prepare the factory to add lanes from right to left.
      * @param rightLanes double; number of lanes right from the link design line
      * @param laneWidth Length; lane width
      * @param laneType LaneType; lane type
      * @param speedLimit Speed; speed limit
      * @return LaneFactory this lane factory for method chaining
      */
     public LaneFactory rightToLeft(final double rightLanes, final Length laneWidth, final LaneType laneType,
             final Speed speedLimit)
     {
         this.offset = laneWidth.times(-rightLanes);
         this.laneWidth0 = laneWidth;
         this.laneType0 = laneType;
         this.speedLimit0 = speedLimit;
         Try.execute(() -> new Stripe(this.link, this.offset, this.offset, STRIPE_WIDTH),
                 "Unexpected exception while building link.");
         return this;
     }
 
     /**
      * Set start offset.
      * @param startOffset Length; offset
      * @return LaneFactory this lane factory for method chaining
      */
     public LaneFactory setOffsetStart(final Length startOffset)
     {
         this.offsetStart = startOffset;
         return this;
     }
 
     /**
      * Set end offset.
      * @param endOffset Length; offset
      * @return LaneFactory this lane factory for method chaining
      */
     public LaneFactory setOffsetEnd(final Length endOffset)
     {
         this.offsetEnd = endOffset;
         return this;
     }
 
     /**
      * Adds a lane pair for each permeable, where the permeable determines the right-hand side line when building from left to
      * right and vice versa. The left-most line is created in {@code leftToRight()}, meaning that each permeable describes
      * permeablility between a lane and it's right-hand neighbor, when building left to right (and vice versa). For no allowed
      * lane changes use {@code null}. This method internally adds {@code null} to create the final continuous stripe.
      * @param permeable Permeable...; permeable per lane pair, for N lanes N-1 should be provided
      * @return this LaneFactory this lane factory for method chaining
      */
     public LaneFactory addLanes(final Permeable... permeable)
     {
         List<Permeable> list = new ArrayList<>(Arrays.asList(permeable));
         list.add(null);
         for (Permeable perm : list)
         {
             Length startOffset = this.offset.plus(this.laneWidth0.times(0.5)).plus(this.offsetStart);
             Length endOffset = this.offset.plus(this.laneWidth0.times(0.5)).plus(this.offsetEnd);
             this.lanes.add(Try.assign(
                     () -> new Lane(this.link, "Lane " + (this.lanes.size() + 1), startOffset, endOffset, this.laneWidth0.abs(),
                             this.laneWidth0.abs(), this.laneType0, this.speedLimit0),
                     "Unexpected exception while building link."));
             this.offset = this.offset.plus(this.laneWidth0);
             Stripetml#Stripe">Stripe stripe = Try.assign(() -> new Stripe(this.link, this.offset.plus(this.offsetStart),
                     this.offset.plus(this.offsetEnd), STRIPE_WIDTH), "Unexpected exception while building link.");
             if (perm != null)
             {
                 stripe.addPermeability(this.link.getNetwork().getGtuType(GTUType.DEFAULTS.VEHICLE), perm);
             }
         }
         return this;
     }
 
     /**
      * Adds 1 or 2 shoulders to the current set of lanes.
      * @param width Length; width of the shoulder
      * @param lat LateralDirectionality; side of shoulder, use {@code null} or {@code NONE} for both
      * @return LaneFactory this lane factory for method chaining
      * @throws IllegalStateException if no lanes are defined
      */
     public LaneFactory addShoulder(final Length width, final LateralDirectionality lat)
     {
         Throw.when(this.lanes.isEmpty(), IllegalStateException.class, "Lanes should be defined before adding shoulder(s).");
         if (lat == null || lat.isNone() || lat.isLeft())
         {
             Length startOffset = null;
             Length endOffset = null;
             for (Lane lane : this.lanes)
             {
                 if (startOffset == null
                         || lane.getDesignLineOffsetAtBegin().plus(lane.getBeginWidth().times(0.5)).gt(startOffset))
                 {
                     startOffset = lane.getDesignLineOffsetAtBegin().plus(lane.getBeginWidth().times(0.5));
                 }
                 if (endOffset == null || lane.getDesignLineOffsetAtEnd().plus(lane.getEndWidth().times(0.5)).gt(endOffset))
                 {
                     endOffset = lane.getDesignLineOffsetAtEnd().plus(lane.getEndWidth().times(0.5));
                 }
             }
             Length start = startOffset.plus(width.times(0.5));
             Length end = endOffset.plus(width.times(0.5));
             Try.assign(() -> new Shoulder(this.link, "Left shoulder", start, end, width, width),
                     "Unexpected exception while building link.");
         }
         if (lat == null || lat.isNone() || lat.isRight())
         {
             Length startOffset = null;
             Length endOffset = null;
             for (Lane lane : this.lanes)
             {
                 if (startOffset == null
                         || lane.getDesignLineOffsetAtBegin().minus(lane.getBeginWidth().times(0.5)).lt(startOffset))
                 {
                     startOffset = lane.getDesignLineOffsetAtBegin().minus(lane.getBeginWidth().times(0.5));
                 }
                 if (endOffset == null
                         || lane.getDesignLineOffsetAtEnd().minus(lane.getEndWidth().times(0.5)).lt(endOffset))
                 {
                     endOffset = lane.getDesignLineOffsetAtEnd().minus(lane.getEndWidth().times(0.5));
                 }
             }
             Length start = startOffset.minus(width.times(0.5));
             Length end = endOffset.minus(width.times(0.5));
             Try.assign(() -> new Shoulder(this.link, "Right shoulder", start, end, width, width),
                     "Unexpected exception while building link.");
         }
         return this;
     }
 
     /**
      * Returns the created lanes in build order.
      * @return List&lt;Lane&gt; created lanes in build order
      */
     public List<Lane> getLanes()
     {
         return this.lanes;
     }
 
     /**
      * Create a Link along intermediate coordinates from one Node to another.
      * @param network RoadNetwork; the network
      * @param name String; name of the new Link
      * @param from OTSRoadNode; start Node of the new Link
      * @param to OTSRoadNode; end Node of the new Link
      * @param intermediatePoints OTSPoint3D[]; array of intermediate coordinates (may be null); the intermediate points may
      *            contain the coordinates of the from node and to node
      * @param simulator OTSSimulatorInterface; the simulator for this network
      * @return Link; the newly constructed Link
      * @throws OTSGeometryException when the design line is degenerate (only one point or duplicate point)
      * @throws NetworkException if link already exists in the network, if name of the link is not unique, or if the start node
      *             or the end node of the link are not registered in the network.
      */
     public static CrossSectionLink makeLink(final RoadNetwork network, final String name, final OTSRoadNode from, 
             final OTSRoadNode to, final OTSPoint3D[] intermediatePoints, final OTSSimulatorInterface simulator)
             throws OTSGeometryException, NetworkException
     {
         List<OTSPoint3D> pointList =
                 intermediatePoints == null ? new ArrayList<>() : new ArrayList<>(Arrays.asList(intermediatePoints));
         if (pointList.size() == 0 || !from.getPoint().equals(pointList.get(0)))
         {
             pointList.add(0, from.getPoint());
         }
         if (pointList.size() == 0 || !to.getPoint().equals(pointList.get(pointList.size() - 1)))
         {
             pointList.add(to.getPoint());
         }
 
         /*-
         // see if an intermediate point needs to be created to the start of the link in the right direction
         OTSPoint3D s1 = pointList.get(0);
         OTSPoint3D s2 = pointList.get(1);
         double dy = s2.y - s1.y;
         double dx = s2.x - s1.x;
         double a = from.getLocation().getRotZ();
         if (Math.abs(a - Math.atan2(dy, dx)) > 1E-6)
         {
             double r = Math.min(1.0, Math.sqrt(dy * dy + dx * dx) / 4.0); 
             OTSPoint3D extra = new OTSPoint3D(s1.x + r * Math.cos(a), s1.y + r * Math.sin(a), s1.z);
             pointList.add(1, extra);
         }
         
         // see if an intermediate point needs to be created to the end of the link in the right direction
         s1 = pointList.get(pointList.size() - 2);
         s2 = pointList.get(pointList.size() - 1);
         dy = s2.y - s1.y;
         dx = s2.x - s1.x;
         a = to.getLocation().getRotZ() - Math.PI;
         if (Math.abs(a - Math.atan2(dy, dx)) > 1E-6)
         {
             double r = Math.min(1.0, Math.sqrt(dy * dy + dx * dx) / 4.0); 
             OTSPoint3D extra = new OTSPoint3D(s2.x + r * Math.cos(a), s2.y + r * Math.sin(a), s2.z);
             pointList.add(pointList.size() - 2, extra);
         }
          */
 
         OTSLine3D designLine = new OTSLine3D(pointList);
         CrossSectionLinkane/CrossSectionLink.html#CrossSectionLink">CrossSectionLink link = new CrossSectionLink(network, name, from, to, network.getLinkType(LinkType.DEFAULTS.ROAD),
                 designLine, simulator, LaneKeepingPolicy.KEEPRIGHT);
         return link;
     }
 
     /**
      * Create one Lane.
      * @param link CrossSectionLink; the link that owns the new Lane
      * @param id String; the id of this lane, should be unique within the link
      * @param laneType LaneType; the type of the new Lane
      * @param latPosAtStart Length; the lateral position of the new Lane with respect to the design line of the link at the
      *            start of the link
      * @param latPosAtEnd Length; the lateral position of the new Lane with respect to the design line of the link at the end of
      *            the link
      * @param width Length; the width of the new Lane
      * @param speedLimit Speed; the speed limit on the new Lane
      * @param simulator DEVSSimulatorInterface.TimeDoubleUnit; the simulator
      * @return Lane
      * @throws NetworkException on network inconsistency
      * @throws OTSGeometryException when creation of center line or contour fails
      */
     @SuppressWarnings("checkstyle:parameternumber")
     private static Lane makeLane(final CrossSectionLink link, final String id, final LaneType laneType,
             final Length latPosAtStart, final Length latPosAtEnd, final Length width, final Speed speedLimit,
             final DEVSSimulatorInterface.TimeDoubleUnit simulator) throws NetworkException, OTSGeometryException
     {
         Map<GTUType, Speed> speedMap = new LinkedHashMap<>();
         speedMap.put(link.getNetwork().getGtuType(GTUType.DEFAULTS.VEHICLE), speedLimit);
         Lane/network/lane/Lane.html#Lane">Lane result = new Lane(link, id, latPosAtStart, latPosAtEnd, width, width, laneType, speedMap);
         return result;
     }
 
     /**
      * Create a simple Lane.
      * @param network RoadNetwork; the network
      * @param name String; name of the Lane (and also of the Link that owns it)
      * @param from OTSRoadNode; starting node of the new Lane
      * @param to OTSRoadNode; ending node of the new Lane
      * @param intermediatePoints OTSPoint3D[]; intermediate coordinates or null to create a straight road; the intermediate
      *            points may contain the coordinates of the from node and to node
      * @param laneType LaneType; type of the new Lane
      * @param speedLimit Speed; the speed limit on the new Lane
      * @param simulator OTSSimulatorInterface; the simulator
      * @return Lane; the new Lane
      * @throws NetworkException on network inconsistency
      * @throws OTSGeometryException when creation of center line or contour fails
      */
     public static Lane makeLane(final RoadNetwork network, final String name, final OTSRoadNoderk/lane/OTSRoadNode.html#OTSRoadNode">OTSRoadNode from, final OTSRoadNode to,
             final OTSPoint3D[] intermediatePoints, final LaneType laneType, final Speed speedLimit,
             final OTSSimulatorInterface simulator) throws NetworkException, OTSGeometryException
     {
         Length width = new Length(4.0, LengthUnit.METER);
         final CrossSectionLink link = makeLink(network, name, from, to, intermediatePoints, simulator);
         Length latPos = new Length(0.0, LengthUnit.METER);
         return makeLane(link, "lane", laneType, latPos, latPos, width, speedLimit, simulator);
     }
 
     /**
      * Create a simple road with the specified number of Lanes.<br>
      * This method returns an array of Lane. These lanes are embedded in a Link that can be accessed through the getParentLink
      * method of the Lane.
      * @param network RoadNetwork; the network
      * @param name String; name of the Link
      * @param from OTSNode; starting node of the new Lane
      * @param to OTSNode; ending node of the new Lane
      * @param intermediatePoints OTSPoint3D[]; intermediate coordinates or null to create a straight road; the intermediate
      *            points may contain the coordinates of the from node and to node
      * @param laneCount int; number of lanes in the road
      * @param laneOffsetAtStart int; extra offset from design line in lane widths at start of link
      * @param laneOffsetAtEnd int; extra offset from design line in lane widths at end of link
      * @param laneType LaneType; type of the new Lanes
      * @param speedLimit Speed; the speed limit on all lanes
      * @param simulator OTSSimulatorInterface; the simulator
      * @return Lane&lt;String, String&gt;[]; array containing the new Lanes
      * @throws NetworkException on topological problems
      * @throws OTSGeometryException when creation of center line or contour fails
      */
     @SuppressWarnings("checkstyle:parameternumber")
     public static Lane[] makeMultiLane(final RoadNetwork network, final String name, final OTSRoadNode from, 
             final OTSRoadNode to, final OTSPoint3D[] intermediatePoints, final int laneCount, final int laneOffsetAtStart, 
             final int laneOffsetAtEnd, final LaneType laneType, final Speed speedLimit, final OTSSimulatorInterface simulator)
             throws NetworkException, OTSGeometryException
     {
         final CrossSectionLink link = makeLink(network, name, from, to, intermediatePoints, simulator);
         Laneetwork/lane/Lane.html#Lane">Lane[] result = new Lane[laneCount];
         Length width = new Length(4.0, LengthUnit.METER);
         for (int laneIndex = 0; laneIndex < laneCount; laneIndex++)
         {
             // Be ware! LEFT is lateral positive, RIGHT is lateral negative.
             Length latPosAtStart = new Length((-0.5 - laneIndex - laneOffsetAtStart) * width.getSI(), LengthUnit.SI);
             Length latPosAtEnd = new Length((-0.5 - laneIndex - laneOffsetAtEnd) * width.getSI(), LengthUnit.SI);
             result[laneIndex] =
                     makeLane(link, "lane." + laneIndex, laneType, latPosAtStart, latPosAtEnd, width, speedLimit, simulator);
         }
         return result;
     }
 
     /**
      * Create a simple road with the specified number of Lanes.<br>
      * This method returns an array of Lane. These lanes are embedded in a Link that can be accessed through the getParentLink
      * method of the Lane.
      * @param network RoadNetwork; the network
      * @param name String; name of the Link
      * @param from OTSRoadNode; starting node of the new Lane
      * @param to OTSRoadNode; ending node of the new Lane
      * @param intermediatePoints OTSPoint3D[]; intermediate coordinates or null to create a straight road; the intermediate
      *            points may contain the coordinates of the from node and to node
      * @param laneCount int; number of lanes in the road
      * @param laneType LaneType; type of the new Lanes
      * @param speedLimit Speed; Speed the speed limit (applies to all generated lanes)
      * @param simulator OTSSimulatorInterface; the simulator
      * @return Lane&lt;String, String&gt;[]; array containing the new Lanes
      * @throws NamingException when names cannot be registered for animation
      * @throws NetworkException on topological problems
      * @throws OTSGeometryException when creation of center line or contour fails
      */
     @SuppressWarnings("checkstyle:parameternumber")
     public static Lane[] makeMultiLane(final RoadNetwork network, final String name, final OTSRoadNoderk/lane/OTSRoadNode.html#OTSRoadNode">OTSRoadNode from, final OTSRoadNode to,
             final OTSPoint3D[] intermediatePoints, final int laneCount, final LaneType laneType, final Speed speedLimit,
             final OTSSimulatorInterface simulator) throws NamingException, NetworkException, OTSGeometryException
     {
         return makeMultiLane(network, name, from, to, intermediatePoints, laneCount, 0, 0, laneType, speedLimit, simulator);
     }
 
     /**
      * Create a simple road with the specified number of Lanes, based on a Bezier curve.<br>
      * This method returns an array of Lane. These lanes are embedded in a Link that can be accessed through the getParentLink
      * method of the Lane.
      * @param network RoadNetwork; the network
      * @param name String; name of the Link
      * @param n1 OTSRoadNode; control node for the start direction
      * @param n2 OTSRoadNode; starting node of the new Lane
      * @param n3 OTSRoadNode; ending node of the new Lane
      * @param n4 OTSRoadNode; control node for the end direction
      * @param laneCount int; number of lanes in the road
      * @param laneOffsetAtStart int; extra offset from design line in lane widths at start of link
      * @param laneOffsetAtEnd int; extra offset from design line in lane widths at end of link
      * @param laneType LaneType; type of the new Lanes
      * @param speedLimit Speed; the speed limit on all lanes
      * @param simulator OTSSimulatorInterface; the simulator
      * @return Lane&lt;String, String&gt;[]; array containing the new Lanes
      * @throws NamingException when names cannot be registered for animation
      * @throws NetworkException on topological problems
      * @throws OTSGeometryException when creation of center line or contour fails
      */
     @SuppressWarnings("checkstyle:parameternumber")
     public static Lane[] makeMultiLaneBezier(final RoadNetwork network, final String name, final OTSRoadNode n1, 
             final OTSRoadNodework/lane/OTSRoadNode.html#OTSRoadNode">OTSRoadNodework/lane/OTSRoadNode.html#OTSRoadNode">OTSRoadNode n2, final OTSRoadNodework/lane/OTSRoadNode.html#OTSRoadNode">OTSRoadNode n3, final OTSRoadNode n4, final int laneCount, final int laneOffsetAtStart, 
             final int laneOffsetAtEnd, final LaneType laneType, final Speed speedLimit, final OTSSimulatorInterface simulator)
             throws NamingException, NetworkException, OTSGeometryException
     {
         OTSLine3D bezier = makeBezier(n1, n2, n3, n4);
         final CrossSectionLink link = makeLink(network, name, n2, n3, bezier.getPoints(), simulator);
         Laneetwork/lane/Lane.html#Lane">Lane[] result = new Lane[laneCount];
         Length width = new Length(4.0, LengthUnit.METER);
         for (int laneIndex = 0; laneIndex < laneCount; laneIndex++)
         {
             // Be ware! LEFT is lateral positive, RIGHT is lateral negative.
             Length latPosAtStart = new Length((-0.5 - laneIndex - laneOffsetAtStart) * width.getSI(), LengthUnit.SI);
             Length latPosAtEnd = new Length((-0.5 - laneIndex - laneOffsetAtEnd) * width.getSI(), LengthUnit.SI);
             result[laneIndex] =
                     makeLane(link, "lane." + laneIndex, laneType, latPosAtStart, latPosAtEnd, width, speedLimit, simulator);
         }
         return result;
     }
 
     /**
      * @param n1 OTSNode; node 1
      * @param n2 OTSNode; node 2
      * @param n3 OTSNode; node 3
      * @param n4 OTSNode; node 4
      * @return line between n2 and n3 with start-direction n1--&gt;n2 and end-direction n3--&gt;n4
      * @throws OTSGeometryException on failure of Bezier curve creation
      */
     public static OTSLine3D makeBezier(final OTSNode n1, final OTSNode n2, final OTSNode n3, final OTSNode n4)
             throws OTSGeometryException
     {
         OTSPoint3D p1 = n1.getPoint();
         OTSPoint3D p2 = n2.getPoint();
         OTSPoint3D p3 = n3.getPoint();
         OTSPoint3D p4 = n4.getPoint();
         DirectedPoint dp1 = new DirectedPoint(p2.x, p2.y, p2.z, 0.0, 0.0, Math.atan2(p2.y - p1.y, p2.x - p1.x));
         DirectedPoint dp2 = new DirectedPoint(p3.x, p3.y, p3.z, 0.0, 0.0, Math.atan2(p4.y - p3.y, p4.x - p3.x));
         return Bezier.cubic(dp1, dp2);
     }
 }