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.Optional;
010import java.util.TreeMap;
011
012/**
013 * The TimeInterpolatableBuffer provides an easy way to estimate past measurements. One application
014 * might be in conjunction with the DifferentialDrivePoseEstimator, where knowledge of the robot
015 * pose at the time when vision or other global measurement were recorded is necessary, or for
016 * recording the past angles of mechanisms as measured by encoders.
017 *
018 * @param <T> The type stored in this buffer.
019 */
020public final class TimeInterpolatableBuffer<T> {
021  private final double m_historySize;
022  private final Interpolator<T> m_interpolatingFunc;
023  private final NavigableMap<Double, T> m_pastSnapshots = new TreeMap<>();
024
025  private TimeInterpolatableBuffer(Interpolator<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      Interpolator<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_pastSnapshots.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_pastSnapshots.isEmpty()) {
083      var entry = m_pastSnapshots.firstEntry();
084      if (time - entry.getKey() >= m_historySize) {
085        m_pastSnapshots.remove(entry.getKey());
086      } else {
087        return;
088      }
089    }
090  }
091
092  /** Clear all old samples. */
093  public void clear() {
094    m_pastSnapshots.clear();
095  }
096
097  /**
098   * Sample the buffer at the given time. If the buffer is empty, an empty Optional is returned.
099   *
100   * @param timeSeconds The time at which to sample.
101   * @return The interpolated value at that timestamp or an empty Optional.
102   */
103  public Optional<T> getSample(double timeSeconds) {
104    if (m_pastSnapshots.isEmpty()) {
105      return Optional.empty();
106    }
107
108    // Special case for when the requested time is the same as a sample
109    var nowEntry = m_pastSnapshots.get(timeSeconds);
110    if (nowEntry != null) {
111      return Optional.of(nowEntry);
112    }
113
114    var topBound = m_pastSnapshots.ceilingEntry(timeSeconds);
115    var bottomBound = m_pastSnapshots.floorEntry(timeSeconds);
116
117    // Return null if neither sample exists, and the opposite bound if the other is null
118    if (topBound == null && bottomBound == null) {
119      return Optional.empty();
120    } else if (topBound == null) {
121      return Optional.of(bottomBound.getValue());
122    } else if (bottomBound == null) {
123      return Optional.of(topBound.getValue());
124    } else {
125      // Otherwise, interpolate. Because T is between [0, 1], we want the ratio of (the difference
126      // between the current time and bottom bound) and (the difference between top and bottom
127      // bounds).
128      return Optional.of(
129          m_interpolatingFunc.interpolate(
130              bottomBound.getValue(),
131              topBound.getValue(),
132              (timeSeconds - bottomBound.getKey()) / (topBound.getKey() - bottomBound.getKey())));
133    }
134  }
135
136  /**
137   * Grant access to the internal sample buffer. Used in Pose Estimation to replay odometry inputs
138   * stored within this buffer.
139   *
140   * @return The internal sample buffer.
141   */
142  public NavigableMap<Double, T> getInternalBuffer() {
143    return m_pastSnapshots;
144  }
145}