001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.math.interpolation;
006
007import edu.wpi.first.math.MathUtil;
008import java.util.NavigableMap;
009import java.util.TreeMap;
010
011/**
012 * The TimeInterpolatableBuffer provides an easy way to estimate past measurements. One application
013 * might be in conjunction with the DifferentialDrivePoseEstimator, where knowledge of the robot
014 * pose at the time when vision or other global measurement were recorded is necessary, or for
015 * recording the past angles of mechanisms as measured by encoders.
016 *
017 * @param <T> The type stored in this buffer.
018 */
019public class TimeInterpolatableBuffer<T> {
020  private final double m_historySize;
021  private final InterpolateFunction<T> m_interpolatingFunc;
022  private final NavigableMap<Double, T> m_buffer = new TreeMap<>();
023
024  private TimeInterpolatableBuffer(
025      InterpolateFunction<T> interpolateFunction, double historySizeSeconds) {
026    this.m_historySize = historySizeSeconds;
027    this.m_interpolatingFunc = interpolateFunction;
028  }
029
030  /**
031   * Create a new TimeInterpolatableBuffer.
032   *
033   * @param interpolateFunction The function used to interpolate between values.
034   * @param historySizeSeconds The history size of the buffer.
035   * @param <T> The type of data to store in the buffer.
036   * @return The new TimeInterpolatableBuffer.
037   */
038  public static <T> TimeInterpolatableBuffer<T> createBuffer(
039      InterpolateFunction<T> interpolateFunction, double historySizeSeconds) {
040    return new TimeInterpolatableBuffer<>(interpolateFunction, historySizeSeconds);
041  }
042
043  /**
044   * Create a new TimeInterpolatableBuffer that stores a given subclass of {@link Interpolatable}.
045   *
046   * @param historySizeSeconds The history size of the buffer.
047   * @param <T> The type of {@link Interpolatable} to store in the buffer.
048   * @return The new TimeInterpolatableBuffer.
049   */
050  public static <T extends Interpolatable<T>> TimeInterpolatableBuffer<T> createBuffer(
051      double historySizeSeconds) {
052    return new TimeInterpolatableBuffer<>(Interpolatable::interpolate, historySizeSeconds);
053  }
054
055  /**
056   * Create a new TimeInterpolatableBuffer to store Double values.
057   *
058   * @param historySizeSeconds The history size of the buffer.
059   * @return The new TimeInterpolatableBuffer.
060   */
061  public static TimeInterpolatableBuffer<Double> createDoubleBuffer(double historySizeSeconds) {
062    return new TimeInterpolatableBuffer<>(MathUtil::interpolate, historySizeSeconds);
063  }
064
065  /**
066   * Add a sample to the buffer.
067   *
068   * @param timeSeconds The timestamp of the sample.
069   * @param sample The sample object.
070   */
071  public void addSample(double timeSeconds, T sample) {
072    cleanUp(timeSeconds);
073    m_buffer.put(timeSeconds, sample);
074  }
075
076  /**
077   * Removes samples older than our current history size.
078   *
079   * @param time The current timestamp.
080   */
081  private void cleanUp(double time) {
082    while (!m_buffer.isEmpty()) {
083      var entry = m_buffer.firstEntry();
084      if (time - entry.getKey() >= m_historySize) {
085        m_buffer.remove(entry.getKey());
086      } else {
087        return;
088      }
089    }
090  }
091
092  /** Clear all old samples. */
093  public void clear() {
094    m_buffer.clear();
095  }
096
097  /**
098   * Sample the buffer at the given time. If the buffer is empty, this will return null.
099   *
100   * @param timeSeconds The time at which to sample.
101   * @return The interpolated value at that timestamp. Might be null.
102   */
103  @SuppressWarnings("UnnecessaryParentheses")
104  public T getSample(double timeSeconds) {
105    if (m_buffer.isEmpty()) {
106      return null;
107    }
108
109    // Special case for when the requested time is the same as a sample
110    var nowEntry = m_buffer.get(timeSeconds);
111    if (nowEntry != null) {
112      return nowEntry;
113    }
114
115    var topBound = m_buffer.ceilingEntry(timeSeconds);
116    var bottomBound = m_buffer.floorEntry(timeSeconds);
117
118    // Return null if neither sample exists, and the opposite bound if the other is null
119    if (topBound == null && bottomBound == null) {
120      return null;
121    } else if (topBound == null) {
122      return bottomBound.getValue();
123    } else if (bottomBound == null) {
124      return topBound.getValue();
125    } else {
126      // Otherwise, interpolate. Because T is between [0, 1], we want the ratio of (the difference
127      // between the current time and bottom bound) and (the difference between top and bottom
128      // bounds).
129      return m_interpolatingFunc.interpolate(
130          bottomBound.getValue(),
131          topBound.getValue(),
132          ((timeSeconds - bottomBound.getKey()) / (topBound.getKey() - bottomBound.getKey())));
133    }
134  }
135
136  public interface InterpolateFunction<T> {
137    /**
138     * Return the interpolated value. This object is assumed to be the starting position, or lower
139     * bound.
140     *
141     * @param start The lower bound, or start.
142     * @param end The upper bound, or end.
143     * @param t How far between the lower and upper bound we are. This should be bounded in [0, 1].
144     * @return The interpolated value.
145     */
146    @SuppressWarnings("ParameterName")
147    T interpolate(T start, T end, double t);
148  }
149}