package org.opentrafficsim.road.gtu.lane.plan.operational;
   
   import java.util.ArrayList;
   import java.util.Arrays;
   import java.util.Iterator;
   import java.util.List;
   import java.util.Map;
   
   import org.djunits.unit.LengthUnit;
  import org.djunits.value.vdouble.scalar.Acceleration;
  import org.djunits.value.vdouble.scalar.Duration;
  import org.djunits.value.vdouble.scalar.Length;
  import org.djunits.value.vdouble.scalar.Speed;
  import org.djunits.value.vdouble.scalar.Time;
  import org.djutils.draw.point.OrientedPoint2d;
  import org.djutils.draw.point.Point2d;
  import org.djutils.exceptions.Throw;
  import org.djutils.logger.CategoryLogger;
  import org.opentrafficsim.base.parameters.ParameterException;
  import org.opentrafficsim.core.geometry.OtsGeometryException;
  import org.opentrafficsim.core.geometry.OtsLine2d;
  import org.opentrafficsim.core.gtu.GtuException;
  import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan;
  import org.opentrafficsim.core.gtu.plan.operational.OperationalPlanException;
  import org.opentrafficsim.core.gtu.plan.operational.Segment;
  import org.opentrafficsim.core.gtu.plan.operational.Segments;
  import org.opentrafficsim.core.network.LateralDirectionality;
  import org.opentrafficsim.core.network.NetworkException;
  import org.opentrafficsim.road.gtu.lane.LaneBasedGtu;
  import org.opentrafficsim.road.network.lane.Lane;
  import org.opentrafficsim.road.network.lane.LanePosition;
  import org.opentrafficsim.road.network.lane.object.detector.LaneDetector;
  import org.opentrafficsim.road.network.lane.object.detector.SinkDetector;
  
  import nl.tudelft.simulation.dsol.SimRuntimeException;
  import nl.tudelft.simulation.dsol.formalisms.eventscheduling.SimEventInterface;
  
  /**
   * Builder for several often used operational plans. E.g., decelerate to come to a full stop at the end of a shape; accelerate
   * to reach a certain speed at the end of a curve; drive constant on a curve; decelerate or accelerate to reach a given end
   * speed at the end of a curve, etc.<br>
   * TODO driving with negative speeds (backward driving) is not yet supported.
   * <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://tudelft.nl/staff/p.knoppers-1">Peter Knoppers</a>
   */
  public final class LaneOperationalPlanBuilder
  {
  
      /**
       * Minimum distance of an operational plan path; anything shorter will be truncated to 0. <br>
       * If objects related to e.g. molecular movements are simulated using this code, a setter for this parameter will be needed.
       */
      private static final Length MINIMUM_CREDIBLE_PATH_LENGTH = new Length(0.001, LengthUnit.METER);
  
      /** Constructor. */
      LaneOperationalPlanBuilder()
      {
          // class should not be instantiated
      }
  
      /**
       * Build a plan with a path and a given start speed to try to reach a provided end speed. Acceleration or deceleration is as
       * provided, until the end speed is reached. After this, constant end speed is used to reach the end point of the path.
       * There is no guarantee that the end speed is actually reached by this plan. If the end speed is zero, and it is reached
       * before completing the path, a truncated path that ends where the GTU stops is used instead.
       * @param gtu LaneBasedGtu; the GTU for debugging purposes
       * @param startTime Time; the current time or a time in the future when the plan should start
       * @param startSpeed Speed; the speed at the start of the path
       * @param acceleration Acceleration; the acceleration to use
       * @param timeStep Duration; time step for the plan
       * @param deviative boolean; whether the plan is deviative
       * @return the operational plan to accomplish the given end speed
       * @throws OperationalPlanException when the construction of the operational path fails
       * @throws OtsGeometryException in case the lanes are not connected or firstLanePositiion is larger than the length of the
       *             first lane
       */
      public static LaneBasedOperationalPlan buildAccelerationPlan(final LaneBasedGtu gtu, final Time startTime,
              final Speed startSpeed, final Acceleration acceleration, final Duration timeStep, final boolean deviative)
              throws OperationalPlanException, OtsGeometryException
      {
          Segments segments = Segments.off(startSpeed, timeStep, acceleration);
          Length distance = Length.ZERO;
          for (Segment segment : segments.getSegments())
          {
              distance = distance.plus(segment.totalDistance());
          }
  
          if (startSpeed.si <= OperationalPlan.DRIFTING_SPEED_SI && acceleration.le(Acceleration.ZERO)
                  || distance.le(MINIMUM_CREDIBLE_PATH_LENGTH))
          {
              OrientedPoint2d point = gtu.getLocation();
              Point2d p2 = new Point2d(point.x + Math.cos(point.getDirZ()), point.y + Math.sin(point.getDirZ()));
              OtsLine2d path = new OtsLine2d(point, p2);
              return new LaneBasedOperationalPlan(gtu, path, startTime, Segments.standStill(timeStep), deviative);
          }
 
         OtsLine2d path = createPathAlongCenterLine(gtu, distance);
         return new LaneBasedOperationalPlan(gtu, path, startTime, segments, deviative);
     }
 
     /**
      * Creates a path along lane center lines.
      * @param gtu LaneBasedGtu; gtu
      * @param distance Length; minimum distance
      * @return OtsLine2d; path along lane center lines
      * @throws OtsGeometryException when any of the OtsLine2d operations fails
      */
     public static OtsLine2d createPathAlongCenterLine(final LaneBasedGtu gtu, final Length distance) throws OtsGeometryException
     {
         OtsLine2d path = null;
         try
         {
             LanePosition ref = gtu.getReferencePosition();
             double f = ref.lane().fraction(ref.position());
             if (f < 1.0)
             {
                 if (f >= 0.0)
                 {
                     path = ref.lane().getCenterLine().extractFractional(f, 1.0);
                 }
                 else
                 {
                     path = ref.lane().getCenterLine().extractFractional(0.0, 1.0);
                 }
             }
             Lane prevFrom = null;
             Lane from = ref.lane();
             Length prevPos = null;
             Length pos = ref.position();
             int n = 1;
             while (path == null || path.getLength().si < distance.si + n * Lane.MARGIN.si)
             {
                 n++;
                 prevFrom = from;
                 if (null == from)
                 {
                     CategoryLogger.always().warn("About to die: GTU {} has null from value", gtu.getId());
                 }
                 from = gtu.getNextLaneForRoute(from);
                 // if (from != null && from.getType().equals(Lane.SHOULDER))
                 // {
                 // CategoryLogger.always().warn("GTU {} on link {} will move on to shoulder.", gtu.getId(),
                 // ref.getLane().getLink().getId());
                 // }
                 prevPos = pos;
                 pos = Length.ZERO;
                 if (from == null)
                 {
                     // check sink detector
                     for (LaneDetector detector : prevFrom.getDetectors(prevPos, prevFrom.getLength(), gtu.getType()))
                     {
                         if (detector instanceof SinkDetector && ((SinkDetector) detector).willDestroy(gtu))
                         {
                             // just add some length so the GTU is happy to go to the sink
                             OrientedPoint2d end = path.getLocationExtendedSI(distance.si + n * Lane.MARGIN.si);
                             List<Point2d> points = new ArrayList<>(Arrays.asList(path.getPoints()));
                             points.add(end);
                             return new OtsLine2d(points);
                         }
                     }
                     // force lane change, and create path from there
                     for (LateralDirectionality lat : new LateralDirectionality[] {LateralDirectionality.LEFT,
                             LateralDirectionality.RIGHT})
                     {
                         Lane latLane = prevFrom.getAdjacentLane(lat, gtu.getType());
                         if (latLane != null && gtu.getNextLaneForRoute(latLane) != null)
                         {
                             gtu.changeLaneInstantaneously(lat);
                             CategoryLogger.always().warn("GTU {} on link {} is forced to change lane towards {}", gtu.getId(),
                                     ref.lane().getLink().getId(), lat);
                             return createPathAlongCenterLine(gtu, distance);
                         }
                     }
                     CategoryLogger.always().error("GTU {} on link {} has nowhere to go and no sink detector either",
                             gtu.getId(), ref.lane().getLink().getId());
                     gtu.destroy();
                     return path;
                 }
                 if (path == null)
                 {
                     path = from.getCenterLine();
                 }
                 else
                 {
                     path = OtsLine2d.concatenate(Lane.MARGIN.si, path, from.getCenterLine());
                 }
             }
         }
         catch (GtuException exception)
         {
             throw new RuntimeException("Error during creation of path.", exception);
         }
         return path;
     }
 
     /**
      * Build a plan with a path and a given start speed to try to reach a provided end speed. Acceleration or deceleration is as
      * provided, until the end speed is reached. After this, constant end speed is used to reach the end point of the path.
      * There is no guarantee that the end speed is actually reached by this plan. If the end speed is zero, and it is reached
      * before completing the path, a truncated path that ends where the GTU stops is used instead.
      * @param gtu LaneBasedGtu; the GTU for debugging purposes
      * @param laneChangeDirectionality LateralDirectionality; direction of lane change (on initiation only, after that not
      *            important)
      * @param startPosition OrientedPoint2d; current position
      * @param startTime Time; the current time or a time in the future when the plan should start
      * @param startSpeed Speed; the speed at the start of the path
      * @param acceleration Acceleration; the acceleration to use
      * @param timeStep Duration; time step for the plan
      * @param laneChange LaneChange; lane change status
      * @return the operational plan to accomplish the given end speed
      * @throws OperationalPlanException when the construction of the operational path fails
      * @throws OtsGeometryException in case the lanes are not connected or firstLanePositiion is larger than the length of the
      *             first lane
      */
     @SuppressWarnings("checkstyle:parameternumber")
     public static LaneBasedOperationalPlan buildAccelerationLaneChangePlan(final LaneBasedGtu gtu,
             final LateralDirectionality laneChangeDirectionality, final OrientedPoint2d startPosition, final Time startTime,
             final Speed startSpeed, final Acceleration acceleration, final Duration timeStep, final LaneChange laneChange)
             throws OperationalPlanException, OtsGeometryException
     {
 
         // on first call during lane change, use laneChangeDirectionality as laneChange.getDirection() is NONE
         // on successive calls, use laneChange.getDirection() as laneChangeDirectionality is NONE (i.e. no LC initiated)
         LateralDirectionality direction = laneChange.isChangingLane() ? laneChange.getDirection() : laneChangeDirectionality;
 
         Segments segments = Segments.off(startSpeed, timeStep, acceleration);
         Length distance = Length.ZERO;
         for (Segment segment : segments.getSegments())
         {
             distance = distance.plus(segment.totalDistance());
         }
 
         try
         {
             // get position on from lane
             Map<Lane, Length> positions = gtu.positions(gtu.getReference());
             LanePosition ref = gtu.getReferencePosition();
             Iterator<Lane> iterator = ref.lane().accessibleAdjacentLanesPhysical(direction, gtu.getType()).iterator();
             Lane adjLane = iterator.hasNext() ? iterator.next() : null;
             LanePosition from = null;
             if (laneChange.getDirection() == null || (adjLane != null && positions.containsKey(adjLane)))
             {
                 // reference lane is from lane, this is ok
                 from = ref;
             }
             else
             {
                 // reference lane is to lane, this should be accounted for
                 for (Lane lane : positions.keySet())
                 {
                     if (lane.accessibleAdjacentLanesPhysical(direction, gtu.getType()).contains(ref.lane()))
                     {
                         from = new LanePosition(lane, positions.get(lane));
                         break;
                     }
                 }
             }
             Throw.when(from == null, RuntimeException.class, "From lane could not be determined during lane change.");
 
             // get path and make plan
             OtsLine2d path = laneChange.getPath(timeStep, gtu, from, startPosition, distance, direction);
             LaneBasedOperationalPlan plan = new LaneBasedOperationalPlan(gtu, path, startTime, segments, true);
             return plan;
         }
         catch (GtuException exception)
         {
             throw new RuntimeException("Error during creation of lane change plan.", exception);
         }
     }
 
     /**
      * Build an operational plan based on a simple operational plan and status info.
      * @param gtu LaneBasedGtu; gtu
      * @param startTime Time; start time for plan
      * @param simplePlan SimpleOperationalPlan; simple operational plan
      * @param laneChange LaneChange; lane change status
      * @return operational plan
      * @throws ParameterException if parameter is not defined
      * @throws GtuException gtu exception
      * @throws NetworkException network exception
      * @throws OperationalPlanException operational plan exeption
      */
     public static LaneBasedOperationalPlan buildPlanFromSimplePlan(final LaneBasedGtu gtu, final Time startTime,
             final SimpleOperationalPlan simplePlan, final LaneChange laneChange)
             throws ParameterException, GtuException, NetworkException, OperationalPlanException
     {
         Acceleration acc = gtu.getVehicleModel().boundAcceleration(simplePlan.getAcceleration(), gtu);
 
         if (gtu.isInstantaneousLaneChange())
         {
             if (simplePlan.isLaneChange())
             {
                 gtu.changeLaneInstantaneously(simplePlan.getLaneChangeDirection());
             }
             try
             {
                 return LaneOperationalPlanBuilder.buildAccelerationPlan(gtu, startTime, gtu.getSpeed(), acc,
                         simplePlan.getDuration(), false);
             }
             catch (OtsGeometryException exception)
             {
                 throw new OperationalPlanException(exception);
             }
         }
 
         // gradual lane change
         try
         {
             if (!simplePlan.isLaneChange() && !laneChange.isChangingLane())
             {
                 return LaneOperationalPlanBuilder.buildAccelerationPlan(gtu, startTime, gtu.getSpeed(), acc,
                         simplePlan.getDuration(), true);
             }
             if (gtu.getSpeed().si == 0.0 && acc.si <= 0.0)
             {
                 return LaneOperationalPlanBuilder.buildAccelerationPlan(gtu, startTime, gtu.getSpeed(), acc,
                         simplePlan.getDuration(), false);
             }
             return LaneOperationalPlanBuilder.buildAccelerationLaneChangePlan(gtu, simplePlan.getLaneChangeDirection(),
                     gtu.getLocation(), startTime, gtu.getSpeed(), acc, simplePlan.getDuration(), laneChange);
         }
         catch (OtsGeometryException exception)
         {
             throw new OperationalPlanException(exception);
         }
     }
 
     /**
      * Schedules a lane change finalization after the given distance is covered. This distance is known as the plan is created,
      * but at that point no time can be derived as the plan is required for that. Hence, this method can be scheduled at the
      * same time (sequentially after creation of the plan) to then schedule the actual finalization by deriving time from
      * distance with the plan.
      * @param gtu LaneBasedGtu; gtu
      * @param distance Length; distance
      * @param laneChangeDirection LateralDirectionality; lane change direction
      * @throws SimRuntimeException on bad time
      */
     public static void scheduleLaneChangeFinalization(final LaneBasedGtu gtu, final Length distance,
             final LateralDirectionality laneChangeDirection) throws SimRuntimeException
     {
         Time time = gtu.getOperationalPlan().timeAtDistance(distance);
         if (Double.isNaN(time.si))
         {
             // rounding...
             time = gtu.getOperationalPlan().getEndTime();
         }
         SimEventInterface<Duration> event = gtu.getSimulator().scheduleEventAbsTime(time, (short) 6, gtu, "finalizeLaneChange",
                 new Object[] {laneChangeDirection});
         gtu.setFinalizeLaneChangeEvent(event);
     }
 
 }