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