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.util;
006
007import java.util.TreeMap;
008
009/**
010 * Interpolating Tree Maps are used to get values at points that are not defined by making a guess
011 * from points that are defined. This uses linear interpolation.
012 *
013 * @deprecated Use {@link edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap} instead
014 */
015@Deprecated(forRemoval = true, since = "2024")
016public class InterpolatingTreeMap<K extends Number, V extends Number> {
017  private final TreeMap<K, V> m_map = new TreeMap<>();
018
019  /**
020   * Inserts a key-value pair.
021   *
022   * @param key The key.
023   * @param value The value.
024   */
025  public void put(K key, V value) {
026    m_map.put(key, value);
027  }
028
029  /**
030   * Returns the value associated with a given key.
031   *
032   * <p>If there's no matching key, the value returned will be a linear interpolation between the
033   * keys before and after the provided one.
034   *
035   * @param key The key.
036   * @return The value associated with the given key.
037   */
038  public Double get(K key) {
039    V val = m_map.get(key);
040    if (val == null) {
041      K ceilingKey = m_map.ceilingKey(key);
042      K floorKey = m_map.floorKey(key);
043
044      if (ceilingKey == null && floorKey == null) {
045        return null;
046      }
047      if (ceilingKey == null) {
048        return m_map.get(floorKey).doubleValue();
049      }
050      if (floorKey == null) {
051        return m_map.get(ceilingKey).doubleValue();
052      }
053      V floor = m_map.get(floorKey);
054      V ceiling = m_map.get(ceilingKey);
055
056      return interpolate(floor, ceiling, inverseInterpolate(ceilingKey, key, floorKey));
057    } else {
058      return val.doubleValue();
059    }
060  }
061
062  /** Clears the contents. */
063  public void clear() {
064    m_map.clear();
065  }
066
067  /**
068   * Return the value interpolated between val1 and val2 by the interpolant d.
069   *
070   * @param val1 The lower part of the interpolation range.
071   * @param val2 The upper part of the interpolation range.
072   * @param d The interpolant in the range [0, 1].
073   * @return The interpolated value.
074   */
075  private double interpolate(V val1, V val2, double d) {
076    double dydx = val2.doubleValue() - val1.doubleValue();
077    return dydx * d + val1.doubleValue();
078  }
079
080  /**
081   * Return where within interpolation range [0, 1] q is between down and up.
082   *
083   * @param up Upper part of interpolation range.
084   * @param q Query.
085   * @param down Lower part of interpolation range.
086   * @return Interpolant in range [0, 1].
087   */
088  private double inverseInterpolate(K up, K q, K down) {
089    double upperToLower = up.doubleValue() - down.doubleValue();
090    if (upperToLower <= 0) {
091      return 0.0;
092    }
093    double queryToLower = q.doubleValue() - down.doubleValue();
094    if (queryToLower <= 0) {
095      return 0.0;
096    }
097    return queryToLower / upperToLower;
098  }
099}