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  /**
026   * Constructs a TimeInterpolatableBuffer.
027   *
028   * @param interpolateFunction Interpolation function.
029   * @param historySize The history size of the buffer in seconds.
030   */
031  private TimeInterpolatableBuffer(Interpolator<T> interpolateFunction, double historySize) {
032    this.m_historySize = historySize;
033    this.m_interpolatingFunc = interpolateFunction;
034  }
035
036  /**
037   * Create a new TimeInterpolatableBuffer.
038   *
039   * @param interpolateFunction The function used to interpolate between values.
040   * @param historySize The history size of the buffer in seconds.
041   * @param <T> The type of data to store in the buffer.
042   * @return The new TimeInterpolatableBuffer.
043   */
044  public static <T> TimeInterpolatableBuffer<T> createBuffer(
045      Interpolator<T> interpolateFunction, double historySize) {
046    return new TimeInterpolatableBuffer<>(interpolateFunction, historySize);
047  }
048
049  /**
050   * Create a new TimeInterpolatableBuffer that stores a given subclass of {@link Interpolatable}.
051   *
052   * @param historySize The history size of the buffer in seconds.
053   * @param <T> The type of {@link Interpolatable} to store in the buffer.
054   * @return The new TimeInterpolatableBuffer.
055   */
056  public static <T extends Interpolatable<T>> TimeInterpolatableBuffer<T> createBuffer(
057      double historySize) {
058    return new TimeInterpolatableBuffer<>(Interpolatable::interpolate, historySize);
059  }
060
061  /**
062   * Create a new TimeInterpolatableBuffer to store Double values.
063   *
064   * @param historySize The history size of the buffer in seconds.
065   * @return The new TimeInterpolatableBuffer.
066   */
067  public static TimeInterpolatableBuffer<Double> createDoubleBuffer(double historySize) {
068    return new TimeInterpolatableBuffer<>(MathUtil::interpolate, historySize);
069  }
070
071  /**
072   * Add a sample to the buffer.
073   *
074   * @param time The timestamp of the sample in seconds.
075   * @param sample The sample object.
076   */
077  public void addSample(double time, T sample) {
078    cleanUp(time);
079    m_pastSnapshots.put(time, sample);
080  }
081
082  /**
083   * Removes samples older than our current history size.
084   *
085   * @param time The current timestamp in seconds.
086   */
087  private void cleanUp(double time) {
088    while (!m_pastSnapshots.isEmpty()) {
089      var entry = m_pastSnapshots.firstEntry();
090      if (time - entry.getKey() >= m_historySize) {
091        m_pastSnapshots.remove(entry.getKey());
092      } else {
093        return;
094      }
095    }
096  }
097
098  /** Clear all old samples. */
099  public void clear() {
100    m_pastSnapshots.clear();
101  }
102
103  /**
104   * Sample the buffer at the given time. If the buffer is empty, an empty Optional is returned.
105   *
106   * @param time The time at which to sample in seconds.
107   * @return The interpolated value at that timestamp or an empty Optional.
108   */
109  public Optional<T> getSample(double time) {
110    if (m_pastSnapshots.isEmpty()) {
111      return Optional.empty();
112    }
113
114    // Special case for when the requested time is the same as a sample
115    var nowEntry = m_pastSnapshots.get(time);
116    if (nowEntry != null) {
117      return Optional.of(nowEntry);
118    }
119
120    var topBound = m_pastSnapshots.ceilingEntry(time);
121    var bottomBound = m_pastSnapshots.floorEntry(time);
122
123    // Return null if neither sample exists, and the opposite bound if the other is null
124    if (topBound == null && bottomBound == null) {
125      return Optional.empty();
126    } else if (topBound == null) {
127      return Optional.of(bottomBound.getValue());
128    } else if (bottomBound == null) {
129      return Optional.of(topBound.getValue());
130    } else {
131      // Otherwise, interpolate. Because T is between [0, 1], we want the ratio of (the difference
132      // between the current time and bottom bound) and (the difference between top and bottom
133      // bounds).
134      return Optional.of(
135          m_interpolatingFunc.interpolate(
136              bottomBound.getValue(),
137              topBound.getValue(),
138              (time - bottomBound.getKey()) / (topBound.getKey() - bottomBound.getKey())));
139    }
140  }
141
142  /**
143   * Grant access to the internal sample buffer. Used in Pose Estimation to replay odometry inputs
144   * stored within this buffer.
145   *
146   * @return The internal sample buffer.
147   */
148  public NavigableMap<Double, T> getInternalBuffer() {
149    return m_pastSnapshots;
150  }
151}