SpeedLimitProspect.java

package org.opentrafficsim.road.network.speed;

import java.io.Serializable;
import java.util.ArrayList;
import java.util.LinkedHashMap;
import java.util.List;
import java.util.Map;
import java.util.SortedSet;
import java.util.TreeSet;

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.djutils.exceptions.Throw;

/**
 * Prospect of speed limits ahead, both legal and otherwise (e.g. curve, speed bump).
 * <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/wjschakel">Wouter Schakel</a>
 */
public class SpeedLimitProspect implements Serializable
{

    /** */
    private static final long serialVersionUID = 20160501L;

    /** Spatial prospect of speed info. */
    private final SortedSet<SpeedLimitEntry<?>> prospect = new TreeSet<>();

    /** Source objects for the speed info additions. */
    private final Map<Object, SpeedLimitEntry<?>> addSources = new LinkedHashMap<>();

    /** Source objects for the speed info removals. */
    private final Map<Object, SpeedLimitEntry<?>> removeSources = new LinkedHashMap<>();

    /** Last odometer value. */
    private Length odometer;

    /**
     * Constructor.
     * @param odometer odometer value
     */
    public SpeedLimitProspect(final Length odometer)
    {
        this.odometer = odometer;
    }

    /**
     * Updates the distance values.
     * @param newOdometer odometer value
     */
    public void update(final Length newOdometer)
    {
        Length dx = newOdometer.minus(this.odometer);
        for (SpeedLimitEntry<?> entry : this.prospect)
        {
            entry.move(dx);
        }
    }

    /**
     * Returns whether the given source is already added in the prospect.
     * @param source source
     * @return whether the given source is already added in the prospect
     */
    public final boolean containsAddSource(final Object source)
    {
        return this.addSources.containsKey(source);
    }

    /**
     * Returns whether the given source is already removed in the prospect.
     * @param source source
     * @return whether the given source is already removed in the prospect
     */
    public final boolean containsRemoveSource(final Object source)
    {
        return this.removeSources.containsKey(source);
    }

    /**
     * Returns the odometer value at which the last update was performed.
     * @return odometer value at which the last update was performed
     */
    public final Length getOdometer()
    {
        return this.odometer;
    }

    /**
     * Sets the speed info of a speed limit type.
     * @param distance location to set info for a speed limit type
     * @param speedLimitType speed limit type to set the info for
     * @param speedInfo speed info to set
     * @param source source object
     * @param <T> class of speed info
     * @throws IllegalStateException if speed info for a specific speed limit type is set or removed twice at the same distance
     * @throws IllegalStateException if speed info for a specific speed limit type is set twice with negative distance
     * @throws NullPointerException if any input is null
     */
    public final <T> void addSpeedInfo(final Length distance, final SpeedLimitType<T> speedLimitType, final T speedInfo,
            final Object source)
    {
        Throw.whenNull(distance, "Distance may not be null.");
        Throw.whenNull(speedLimitType, "Speed limit type may not be null.");
        Throw.whenNull(speedInfo, "Speed info may not be null.");
        checkAndAdd(new SpeedLimitEntry<>(distance, speedLimitType, speedInfo), source, false);
    }

    /**
     * Removes the speed info of a speed limit type.
     * @param distance distance to remove speed info of a speed limit type
     * @param speedLimitType speed limit type to remove speed info of
     * @param source source object
     * @throws IllegalStateException if speed info for a specific speed limit type is set or removed twice at the same distance
     * @throws IllegalArgumentException if the speed limit type is {@code MAX_VEHICLE_SPEED}
     * @throws IllegalArgumentException if the distance is negative
     * @throws NullPointerException if any input is null
     */
    @SuppressWarnings({"unchecked", "rawtypes"})
    public final void removeSpeedInfo(final Length distance, final SpeedLimitType<?> speedLimitType, final Object source)
    {
        Throw.whenNull(distance, "Distance may not be null.");
        Throw.when(distance.si < 0, IllegalArgumentException.class,
                "Removing speed info in the past is not allowed. " + "Only add still active speed info.");
        Throw.whenNull(speedLimitType, "Speed limit type may not be null.");
        Throw.when(speedLimitType.equals(SpeedLimitTypes.MAX_VEHICLE_SPEED), IllegalArgumentException.class,
                "May not remove the maximum vehicle speed.");
        // null value does not comply to being a T for SpeedLimitType<T> but is separately treated
        checkAndAdd(new SpeedLimitEntry(distance, speedLimitType, null), source, true);
    }

    /**
     * Checks the speed limit entry before adding to the prospect.
     * @param speedLimitEntry speed limit entry to add
     * @param source source object
     * @param remove whether the source causes a removal of info
     * @throws IllegalStateException if the speed entry forms an undefined set with any existing entry
     */
    private void checkAndAdd(final SpeedLimitEntry<?> speedLimitEntry, final Object source, final boolean remove)
    {
        for (SpeedLimitEntry<?> s : this.prospect)
        {
            if (s.getSpeedLimitType().equals(speedLimitEntry.getSpeedLimitType()))
            {
                /*
                 * For entries at the same distance, the speed limit type may not be the same, this leaves us with an undefined
                 * state as it cannot be derived which remains valid further on.
                 */
                Throw.when(s.getDistance().equals(speedLimitEntry.getDistance()), IllegalStateException.class,
                        "Info " + "of speed limit type '%s' is set twice at the same location (%s). This is undefined. "
                                + "Either remove speed info, or overwrite with new speed info.",
                        s.getSpeedLimitType(), s.getDistance());
            }
        }
        if (remove)
        {
            SpeedLimitEntry<?> prev = this.removeSources.get(source);
            if (prev != null)
            {
                this.prospect.remove(prev);
            }
            this.removeSources.put(source, speedLimitEntry);
        }
        else
        {
            SpeedLimitEntry<?> prev = this.addSources.get(source);
            if (prev != null)
            {
                this.prospect.remove(prev);
            }
            this.addSources.put(source, speedLimitEntry);
        }
        this.prospect.add(speedLimitEntry);
    }

    /**
     * Returns the distances at which a change in the prospect is present in order (upstream first). If multiple changes are
     * present at the same distance, only one distance is returned in the list.
     * @return distances at which a change in the prospect is present in order (upstream first)
     */
    public final List<Length> getDistances()
    {
        List<Length> list = new ArrayList<>();
        for (SpeedLimitEntry<?> speedLimitEntry : this.prospect)
        {
            list.add(speedLimitEntry.getDistance());
        }
        return list;
    }

    /**
     * Returns the distances at which a change of the given speed limit type in the prospect is present in order (most upstream
     * first). If multiple changes are present at the same distance, only one distance is returned in the list.
     * @param speedLimitType speed limit type to get the distances of
     * @return distances at which a change of the given speed limit type in the prospect is present in order
     */
    public final List<Length> getDistances(final SpeedLimitType<?> speedLimitType)
    {
        return getDistancesInRange(speedLimitType, null, null);
    }

    /**
     * Returns the upstream distances at which a change of the given speed limit type in the prospect is present in order (most
     * upstream first). If multiple changes are present at the same distance, only one distance is returned in the list.
     * @param speedLimitType speed limit type to get the distances of
     * @return distances at which a change of the given speed limit type in the prospect is present in order
     */
    public final List<Length> getUpstreamDistances(final SpeedLimitType<?> speedLimitType)
    {
        return getDistancesInRange(speedLimitType, null, Length.ZERO);
    }

    /**
     * Returns the downstream distances at which a change of the given speed limit type in the prospect is present in order
     * (most upstream first). If multiple changes are present at the same distance, only one distance is returned in the list.
     * @param speedLimitType speed limit type to get the distances of
     * @return distances at which a change of the given speed limit type in the prospect is present in order
     */
    public final List<Length> getDownstreamDistances(final SpeedLimitType<?> speedLimitType)
    {
        return getDistancesInRange(speedLimitType, Length.ZERO, null);
    }

    /**
     * Returns the distances between limits at which a change of the given speed limit type in the prospect is present in order
     * (most upstream first). If multiple changes are present at the same distance, only one distance is returned in the list.
     * @param speedLimitType speed limit type to get the distances of
     * @param min minimum distance, may be {@code null} for no minimum limit
     * @param max maximum distance, may be {@code null} for no maximum limit
     * @return distances at which a change of the given speed limit type in the prospect is present in order
     */
    private List<Length> getDistancesInRange(final SpeedLimitType<?> speedLimitType, final Length min, final Length max)
    {
        List<Length> list = new ArrayList<>();
        for (SpeedLimitEntry<?> speedLimitEntry : this.prospect)
        {
            if (speedLimitEntry.getSpeedLimitType().equals(speedLimitType)
                    && (min == null || speedLimitEntry.getDistance().gt(min))
                    && (max == null || speedLimitEntry.getDistance().le(max)))
            {
                list.add(speedLimitEntry.getDistance());
            }
        }
        return list;
    }

    /**
     * Returns whether the given speed limit type is changed at the given distance.
     * @param distance distance to check
     * @param speedLimitType speed limit type to check
     * @return whether the given speed limit type is changed at the given distance
     * @throws NullPointerException if distance is null
     */
    public final boolean speedInfoChanged(final Length distance, final SpeedLimitType<?> speedLimitType)
    {
        Throw.whenNull(distance, "Distance may not be null.");
        for (SpeedLimitEntry<?> sle : this.prospect)
        {
            if (sle.getDistance().eq(distance) && sle.getSpeedLimitType().equals(speedLimitType))
            {
                return true;
            }
        }
        return false;
    }

    /**
     * Returns the speed info of given speed limit type where it changed. If the change was removing the speed limit type (e.g.
     * end of corner), then {@code null} is returned.
     * @param distance distance where the info changed
     * @param speedLimitType speed limit type
     * @return speed info of given speed limit type where it changed, {@code null} if speed limit type is removed
     * @throws IllegalArgumentException if the speed info did not change at the given distance for the speed limit type
     * @param <T> class of the speed limit type info
     */
    public final <T> T getSpeedInfoChange(final Length distance, final SpeedLimitType<T> speedLimitType)
    {
        for (SpeedLimitEntry<?> sle : this.prospect)
        {
            if (sle.getDistance().eq(distance) && sle.getSpeedLimitType().equals(speedLimitType))
            {
                @SuppressWarnings("unchecked")
                T info = (T) sle.getSpeedInfo();
                return info;
            }
        }
        throw new IllegalArgumentException("Speed info of speed limit type '" + speedLimitType.getId()
                + "' is requested at a distance '" + distance + "' where the info is not changed.");
    }

    /**
     * Returns the speed info at a given location.
     * @param distance where to get the speed info
     * @return speed info at a given distance
     * @throws NullPointerException if distance is null
     */
    public final SpeedLimitInfo getSpeedLimitInfo(final Length distance)
    {
        Throw.whenNull(distance, "Distance may not be null.");
        SpeedLimitInfo speedLimitInfo = new SpeedLimitInfo();
        for (SpeedLimitEntry<?> speedLimitEntry : this.prospect)
        {
            // use compareTo as this also determines order in this.prospect
            if (speedLimitEntry.getDistance().compareTo(distance) > 0)
            {
                // remaining entries are further ahead
                return speedLimitInfo;
            }
            // make appropriate change to speedLimitInfo
            if (speedLimitEntry.getSpeedInfo() == null)
            {
                speedLimitInfo.removeSpeedInfo(speedLimitEntry.getSpeedLimitType());
            }
            else
            {
                // method addSpeedInfo guarantees that speedInfo in speedLimitEntry is T
                // for speedLimitType in speedLimitEntry is SpeedLimitType<T>, null is checked above
                setAsType(speedLimitInfo, speedLimitEntry);
            }
        }
        return speedLimitInfo;
    }

    /**
     * Returns the speed info at a location following an acceleration over some duration.
     * @param speed current speed
     * @param acceleration acceleration to apply
     * @param time duration of acceleration
     * @return speed info at a given distance
     * @throws NullPointerException if any input is null
     */
    public final SpeedLimitInfo getSpeedLimitInfo(final Speed speed, final Acceleration acceleration, final Duration time)
    {
        Throw.whenNull(speed, "Speed may not be null.");
        Throw.whenNull(acceleration, "Acceleration may not be null.");
        Throw.whenNull(time, "Time may not be null.");
        return getSpeedLimitInfo(new Length(speed.si * time.si + .5 * acceleration.si * time.si * time.si, LengthUnit.SI));
    }

    /**
     * Sets speed info for a speed limit type in speed limit info by explicitly casting the types. From the context it should be
     * certain that the speed info inside the speed limit entry matches the declared info type of the speed limit type inside
     * the entry, i.e. {@code speedLimitEntry.getSpeedLimitType() = SpeedLimitType<T>} and
     * {@code speedLimitEntry.getSpeedInfo() = T}.
     * @param speedLimitInfo speed limit info to put speed info in
     * @param speedLimitEntry entry with speed limit type and speed info to set
     * @param <T> underlying speed info class depending on speed limit type
     */
    @SuppressWarnings("unchecked")
    private <T> void setAsType(final SpeedLimitInfo speedLimitInfo, final SpeedLimitEntry<?> speedLimitEntry)
    {
        SpeedLimitType<T> speedLimitType = (SpeedLimitType<T>) speedLimitEntry.getSpeedLimitType();
        T speedInfoOfType = (T) speedLimitEntry.getSpeedInfo();
        speedLimitInfo.addSpeedInfo(speedLimitType, speedInfoOfType);
    }

    /**
     * Builds speed limit info with only MAX_VEHICLE_SPEED and the given speed limit type, where the speed info is obtained at
     * the given distance.
     * @param distance distance to get the speed info at
     * @param speedLimitType speed limit type of which to include the info
     * @param <T> class of speed info of given speed limit type
     * @return speed limit info with only MAX_VEHICLE_SPEED and the given speed limit type
     */
    public final <T> SpeedLimitInfo buildSpeedLimitInfo(final Length distance, final SpeedLimitType<T> speedLimitType)
    {
        SpeedLimitInfo out = new SpeedLimitInfo();
        out.addSpeedInfo(speedLimitType, getSpeedInfoChange(distance, speedLimitType));
        for (SpeedLimitEntry<?> speedLimitEntry : this.prospect)
        {
            if (speedLimitEntry.getDistance().gt(distance))
            {
                break;
            }
            if (speedLimitEntry.getSpeedLimitType().equals(SpeedLimitTypes.MAX_VEHICLE_SPEED))
            {
                out.addSpeedInfo(SpeedLimitTypes.MAX_VEHICLE_SPEED,
                        SpeedLimitTypes.MAX_VEHICLE_SPEED.getInfoClass().cast(speedLimitEntry.getSpeedInfo()));
            }
        }
        return out;
    }

    @Override
    public final String toString()
    {
        StringBuilder stringBuilder = new StringBuilder("SpeedLimitProspect [");
        String sep = "";
        for (SpeedLimitEntry<?> sle : this.prospect)
        {
            stringBuilder.append(sep).append(sle.getDistance()).append(": ");
            if (sle.getSpeedInfo() == null)
            {
                stringBuilder.append(sle.getSpeedLimitType().getId()).append("=END");
            }
            else
            {
                stringBuilder.append(sle.getSpeedLimitType().getId()).append("=");
                stringBuilder.append(sle.getSpeedInfo());
            }
            sep = ", ";
        }
        stringBuilder.append("]");
        return stringBuilder.toString();
    }

    /**
     * Stores speed limit type and it's speed info with a location.
     * <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/wjschakel">Wouter Schakel</a>
     * @param <T> class of speed info
     */
    private static class SpeedLimitEntry<T> implements Comparable<SpeedLimitEntry<?>>, Serializable
    {

        /** */
        private static final long serialVersionUID = 20160501L;

        /** Location of the speed info. */
        private Length distance;

        /** Speed limit type. */
        private final SpeedLimitType<T> speedLimitType;

        /** Speed info. */
        private final T speedInfo;

        /**
         * Constructor.
         * @param distance location of the speed info
         * @param speedLimitType speed limit type
         * @param speedInfo speed info
         */
        SpeedLimitEntry(final Length distance, final SpeedLimitType<T> speedLimitType, final T speedInfo)
        {
            this.distance = distance;
            this.speedLimitType = speedLimitType;
            this.speedInfo = speedInfo;
        }

        /**
         * Returns the location of the speed info.
         * @return location of the speed info
         */
        public final Length getDistance()
        {
            return this.distance;
        }

        /**
         * Returns the speed limit type.
         * @return speed limit type
         */
        public final SpeedLimitType<T> getSpeedLimitType()
        {
            return this.speedLimitType;
        }

        /**
         * Returns the speed info.
         * @return the speed info
         */
        public final T getSpeedInfo()
        {
            return this.speedInfo;
        }

        /**
         * Move the record by a given distance.
         * @param dist distance to move
         */
        public final void move(final Length dist)
        {
            this.distance = this.distance.minus(dist);
        }

        @Override
        public final int hashCode()
        {
            final int prime = 31;
            int result = 1;
            result = prime * result + this.distance.hashCode();
            result = prime * result + this.speedInfo.hashCode();
            result = prime * result + this.speedLimitType.hashCode();
            return result;
        }

        @Override
        public final boolean equals(final Object obj)
        {
            if (this == obj)
            {
                return true;
            }
            if (obj == null)
            {
                return false;
            }
            if (getClass() != obj.getClass())
            {
                return false;
            }
            SpeedLimitEntry<?> other = (SpeedLimitEntry<?>) obj;
            if (!this.distance.equals(other.distance))
            {
                return false;
            }
            if (!this.speedLimitType.equals(other.speedLimitType))
            {
                return false;
            }
            if (this.speedInfo == null)
            {
                if (other.speedInfo != null)
                {
                    return false;
                }
            }
            else if (!this.speedInfo.equals(other.speedInfo))
            {
                return false;
            }
            return true;
        }

        @Override
        public final int compareTo(final SpeedLimitEntry<?> speedLimitEntry)
        {
            if (this.equals(speedLimitEntry))
            {
                return 0;
            }
            // order by distance
            int comp = this.distance.compareTo(speedLimitEntry.distance);
            if (comp != 0)
            {
                return comp;
            }
            // order by speed limit type
            comp = this.speedLimitType.getId().compareTo(speedLimitEntry.speedLimitType.getId());
            if (comp != 0)
            {
                return comp;
            }
            // equal distance and speed limit type is not allowed, so below code is not used
            // if this requirement changes, compareTo should still work
            if (this.speedInfo == null)
            {
                if (speedLimitEntry.speedInfo == null)
                {
                    return 0; // both null
                }
                return -1; // null under non-null
            }
            else if (speedLimitEntry.speedInfo == null)
            {
                return 1; // non-null over null
            }
            return this.speedInfo.hashCode() < speedLimitEntry.speedInfo.hashCode() ? -1 : 1;
        }

        @Override
        public final String toString()
        {
            return "SpeedLimitEntry [distance=" + this.distance + ", speedLimitType=" + this.speedLimitType + ", speedInfo="
                    + this.speedInfo + "]";
        }

    }

}