package org.opentrafficsim.road.gtu.lane.perception.categories.neighbors;
   
   import java.util.HashMap;
   import java.util.HashSet;
   import java.util.Map;
   import java.util.Set;
   import java.util.SortedSet;
   
   import org.djunits.value.vdouble.scalar.Duration;
  import org.djunits.value.vdouble.scalar.Length;
  import org.djunits.value.vdouble.scalar.Time;
  import org.opentrafficsim.base.TimeStampedObject;
  import org.opentrafficsim.base.parameters.ParameterException;
  import org.opentrafficsim.base.parameters.ParameterTypeDuration;
  import org.opentrafficsim.base.parameters.ParameterTypes;
  import org.opentrafficsim.base.parameters.Parameters;
  import org.opentrafficsim.core.gtu.GTUException;
  import org.opentrafficsim.core.gtu.perception.PerceptionException;
  import org.opentrafficsim.core.network.LateralDirectionality;
  import org.opentrafficsim.core.network.NetworkException;
  import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
  import org.opentrafficsim.road.gtu.lane.perception.LanePerception;
  import org.opentrafficsim.road.gtu.lane.perception.PerceptionCollectable;
  import org.opentrafficsim.road.gtu.lane.perception.RelativeLane;
  import org.opentrafficsim.road.gtu.lane.perception.categories.AbstractDelayedPerceptionCategory;
  import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayGTU;
  
  import nl.tudelft.simulation.dsol.SimRuntimeException;
  
  /**
   * <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/node/13">OpenTrafficSim License</a>.
   * <p>
   * @version $Revision$, $LastChangedDate$, by $Author$, initial version 17 feb. 2017 <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 abstract class AbstractDelayedNeighborsPerception extends AbstractDelayedPerceptionCategory
          implements NeighborsPerception
  {
  
      /** */
      private static final long serialVersionUID = 20170217L;
  
      /** Reaction time parameter type. */
      protected static final ParameterTypeDuration TR = ParameterTypes.TR;
  
      /** Time step parameter type. */
      protected static final ParameterTypeDuration DT = ParameterTypes.DT;
  
      /** First update time. */
      private Time initialTime = null;
  
      /** Wrapped direct perception. */
      private final DirectNeighborsPerception direct;
  
      /** Reaction time. */
      private Duration reactionTime = null;
  
      /** Time step of planner. */
      private Duration plannerTimeStep = null;
  
      /** Remainder between reaction time and planner time step. */
      private Duration remainder = null;
  
      /** Info type id base for first leaders. */
      public static final String FIRSTLEADERS = "firstLeaders";
  
      /** Info type id base for first followers. */
      public static final String FIRSTFOLLOWERS = "firstFollower";
  
      /** Info type id base for gtu alongside. */
      public static final String GTUALONGSIDE = "gtuAlongside";
  
      /** Info type id base for leaders. */
      public static final String LEADERS = "leaders";
  
      /** Info type id base for followers. */
      public static final String FOLLOWERS = "followers";
  
      /** Info type id for cross-section. */
      public static final NeighborsInfoType<SortedSet<RelativeLane>> CROSSSECTION = new NeighborsInfoType<>("cross-section");
  
      /** Id for odometer info type. */
      public static final NeighborsInfoType<Length> ODOMETER = new NeighborsInfoType<>("odometer");
  
      /** Override for left lane change. */
      private boolean gtuAlongsideLeftOverride = false;
  
      /** Override for right lane change. */
      private boolean gtuAlongsideRightOverride = false;
  
      /**
       * Constructor.
       * @param perception LanePerception; perception
       */
      public AbstractDelayedNeighborsPerception(final LanePerception perception)
     {
         super(perception);
         this.direct = new DirectNeighborsPerception(perception, HeadwayGtuType.COPY);
     }
 
     /** {@inheritDoc} */
     @Override
     public final void updateAll() throws GTUException, NetworkException, ParameterException
     {
 
         if (this.remainder == null)
         {
             try
             {
                 // TODO The reaction time may differ between observed objects and vary over time
                 Parameters params = getPerception().getGtu().getParameters();
                 this.reactionTime = params.getParameter(TR);
                 this.plannerTimeStep = params.getParameter(DT);
                 double rem;
                 if (this.reactionTime.eq0())
                 {
                     rem = 0;
                 }
                 else if (this.reactionTime.gt(this.plannerTimeStep))
                 {
                     rem = this.reactionTime.si % this.plannerTimeStep.si;
                 }
                 else
                 {
                     rem = this.plannerTimeStep.si - this.reactionTime.si;
                 }
                 this.remainder = Duration.createSI(rem);
             }
             catch (ParameterException | GTUException exception)
             {
                 throw new RuntimeException("Exception while setting up delayed neighors perception.", exception);
             }
         }
 
         // direct perception in first few time steps; build up history
         Time now = getPerception().getGtu().getSimulator().getSimulatorTime();
         if (this.initialTime == null)
         {
             this.initialTime = now;
         }
         if (now.minus(this.initialTime).le(this.reactionTime))
         {
             updateAllDelayed();
             return;
         }
 
         if (this.remainder.eq0())
         {
             // reaction time is multiple of time step, just do it now
             updateAllDelayed();
         }
         else
         {
             // schedule actual update slightly in the future: this will be the snapshot for a future time step
             Time scheduledTime = now.plus(this.remainder);
             try
             {
                 getPerception().getGtu().getSimulator().scheduleEventAbs(scheduledTime, this, this, "updateAllDelayed", null);
             }
             catch (SimRuntimeException exception)
             {
                 throw new RuntimeException("Scheduling perception update in the past.", exception);
             }
         }
 
         /*
          * During the reaction time, an instantaneous lane change by a neighbor may be performed, which may cause an
          * unreasonable lane change of the subject vehicle if it is not considered. Therefore, the 'gtuAlongSide' information is
          * amended with a current snapshot of the surroundings. If the current first leaders/followers contains a GTU that the
          * delayed leaders/followers do not contain, and that is within 50m, 'gtuAlongSide' is overruled with 'true', preventing
          * a lane change.
          */
         if (getPerception().getLaneStructure().getExtendedCrossSection().contains(RelativeLane.LEFT))
         {
             this.direct.updateFirstFollowers(LateralDirectionality.LEFT);
             this.direct.updateFirstLeaders(LateralDirectionality.LEFT);
             this.direct.updateGtuAlongside(LateralDirectionality.LEFT);
             this.gtuAlongsideLeftOverride = newFirstLeaderOrFollower(getFollowers(RelativeLane.LEFT),
                     this.direct.getFirstFollowers(LateralDirectionality.LEFT))
                     || newFirstLeaderOrFollower(getLeaders(RelativeLane.LEFT),
                             this.direct.getFirstLeaders(LateralDirectionality.LEFT))
                     || this.direct.isGtuAlongside(LateralDirectionality.LEFT);
         }
         if (getPerception().getLaneStructure().getExtendedCrossSection().contains(RelativeLane.RIGHT))
         {
             this.direct.updateFirstFollowers(LateralDirectionality.RIGHT);
             this.direct.updateFirstLeaders(LateralDirectionality.RIGHT);
             this.direct.updateGtuAlongside(LateralDirectionality.RIGHT);
             this.gtuAlongsideRightOverride = newFirstLeaderOrFollower(getFollowers(RelativeLane.RIGHT),
                     this.direct.getFirstFollowers(LateralDirectionality.RIGHT))
                     || newFirstLeaderOrFollower(getLeaders(RelativeLane.RIGHT),
                             this.direct.getFirstLeaders(LateralDirectionality.RIGHT))
                     || this.direct.isGtuAlongside(LateralDirectionality.RIGHT);
         }
 
     }
 
     /**
      * Returns whether there is a gtu in the current set that is not present in the delayed set.
      * @param delayedSet Iterable&lt;? extends HeadwayGTU&gt;; delayed set
      * @param currentSet Set&lt;? extends HeadwayGTU&gt;; current set
      * @return whether there is a gtu in the current set that is not present in the delayed set
      */
     private boolean newFirstLeaderOrFollower(final Iterable<? extends HeadwayGTU> delayedSet,
             final Set<? extends HeadwayGTU> currentSet)
     {
         Set<String> set = new HashSet<>();
         for (HeadwayGTU gtu : delayedSet)
         {
             set.add(gtu.getId());
         }
         for (HeadwayGTU gtu : currentSet)
         {
             if (!set.contains(gtu.getId()) && gtu.getDistance().si < 50)
             {
                 return true;
             }
         }
         return false;
     }
 
     /**
      * Returns whether to override the gtu alongside boolean as true.
      * @param lat LateralDirectionality; lateral direction
      * @return whether to override the gtu alongside boolean as true
      */
     public final boolean isGtuAlongsideOverride(final LateralDirectionality lat)
     {
         return lat.isLeft() ? this.gtuAlongsideLeftOverride : this.gtuAlongsideRightOverride;
     }
 
     /**
      * Performs actual update.
      * @throws ParameterException if parameter is not present or is given a wrong value
      * @throws NetworkException on error in the network
      * @throws GTUException if not initialized
      */
     // TODO private when DSOL allows
     protected void updateAllDelayed() throws GTUException, NetworkException, ParameterException
     {
 
         try
         {
             getGtu().getReferencePosition();
         }
         catch (GTUException exception)
         {
             // GTU was destroyed
             return;
         }
 
         this.direct.updateAll();
         // below code is a copy of the updateAll() method in the direct perception TODO structure better
         if (getPerception().getLaneStructure().getExtendedCrossSection().contains(RelativeLane.LEFT))
         {
             updateFirstLeaders(LateralDirectionality.LEFT);
             updateFirstFollowers(LateralDirectionality.LEFT);
             updateGtuAlongside(LateralDirectionality.LEFT);
         }
         if (getPerception().getLaneStructure().getExtendedCrossSection().contains(RelativeLane.RIGHT))
         {
             updateFirstLeaders(LateralDirectionality.RIGHT);
             updateFirstFollowers(LateralDirectionality.RIGHT);
             updateGtuAlongside(LateralDirectionality.RIGHT);
         }
         for (RelativeLane lane : getPerception().getLaneStructure().getExtendedCrossSection())
         {
             updateLeaders(lane);
             updateFollowers(lane);
         }
         setInfo(CROSSSECTION,
                 new TimeStampedObject<>(getPerception().getLaneStructure().getExtendedCrossSection(), getTimestamp()));
         setInfo(ODOMETER, new TimeStampedObject<>(getGtu().getOdometer(), getTimestamp()));
     }
 
     /** {@inheritDoc} */
     @Override
     public final void updateFirstLeaders(final LateralDirectionality lat)
             throws ParameterException, GTUException, NetworkException
     {
         setInfo(NeighborsInfoType.getSortedSetType(FIRSTLEADERS), new RelativeLane(lat, 1),
                 this.direct.getTimeStampedFirstLeaders(lat));
     }
 
     /** {@inheritDoc} */
     @Override
     public final void updateFirstFollowers(final LateralDirectionality lat)
             throws GTUException, ParameterException, NetworkException
     {
         setInfo(NeighborsInfoType.getSortedSetType(FIRSTFOLLOWERS), new RelativeLane(lat, 1),
                 this.direct.getTimeStampedFirstFollowers(lat));
     }
 
     /** {@inheritDoc} */
     @Override
     public final void updateGtuAlongside(final LateralDirectionality lat) throws GTUException, ParameterException
     {
         setInfo(NeighborsInfoType.getBooleanType(GTUALONGSIDE), new RelativeLane(lat, 1),
                 this.direct.isGtuAlongsideTimeStamped(lat));
     }
 
     /** {@inheritDoc} */
     @Override
     public final void updateLeaders(final RelativeLane lane) throws ParameterException, GTUException, NetworkException
     {
         setInfo(NeighborsInfoType.getIterableType(LEADERS), lane, this.direct.getTimeStampedLeaders(lane));
     }
 
     /** {@inheritDoc} */
     @Override
     public final void updateFollowers(final RelativeLane lane) throws GTUException, NetworkException, ParameterException
     {
         setInfo(NeighborsInfoType.getIterableType(FOLLOWERS), lane, this.direct.getTimeStampedFollowers(lane));
     }
 
     /**
      * Returns the cross-section on which the most recent observed neighbors were determined.
      * @return cross-section on which the most recent observed neighbors were determined
      */
     public final SortedSet<RelativeLane> getDelayedCrossSection()
     {
         try
         {
             return getInfo(CROSSSECTION).getObject();
         }
         catch (PerceptionException exception)
         {
             throw new RuntimeException("Crosssection was not perceived.", exception);
         }
     }
 
     /**
      * Delayed information about the type of the neighbors. <br>
      * @param <T> data type of info
      */
     public static final class NeighborsInfoType<T> extends DelayedInfoType<T>
     {
 
         /** Map of id's and lane info types. */
         private static final Map<String, NeighborsInfoType<?>> LANEINFOTYPES = new HashMap<>();
 
         /**
          * Construct new info.
          * @param id String; id
          */
         public NeighborsInfoType(final String id)
         {
             super(id, TR);
         }
 
         /**
          * Returns a (cached) info type for a sorted set of GTU's.
          * @param id String; id
          * @return info type
          */
         @SuppressWarnings("unchecked")
         public static NeighborsInfoType<SortedSet<HeadwayGTU>> getSortedSetType(final String id)
         {
             if (!LANEINFOTYPES.containsKey(id))
             {
                 LANEINFOTYPES.put(id, new NeighborsInfoType<SortedSet<HeadwayGTU>>(id));
             }
             return (NeighborsInfoType<SortedSet<HeadwayGTU>>) LANEINFOTYPES.get(id);
         }
 
         /**
          * Returns a (cached) info type for a sorted set of GTU's.
          * @param id String; id
          * @return info type
          */
         @SuppressWarnings("unchecked")
         public static NeighborsInfoType<PerceptionCollectable<HeadwayGTU, LaneBasedGTU>> getIterableType(final String id)
         {
             if (!LANEINFOTYPES.containsKey(id))
             {
                 LANEINFOTYPES.put(id, new NeighborsInfoType<SortedSet<HeadwayGTU>>(id));
             }
             return (NeighborsInfoType<PerceptionCollectable<HeadwayGTU, LaneBasedGTU>>) LANEINFOTYPES.get(id);
         }
 
         /**
          * Returns a (cached) info type for a sorted set of GTU's.
          * @param id String; id
          * @return info type
          */
         @SuppressWarnings("unchecked")
         public static NeighborsInfoType<Boolean> getBooleanType(final String id)
         {
             if (!LANEINFOTYPES.containsKey(id))
             {
                 LANEINFOTYPES.put(id, new NeighborsInfoType<SortedSet<HeadwayGTU>>(id));
             }
             return (NeighborsInfoType<Boolean>) LANEINFOTYPES.get(id);
         }
 
         /** {@inheritDoc} */
         @Override
         public String toString()
         {
             return "NeighborsInfoType []";
         }
 
     }
 
 }