001/**
002 * Copyright 2024 The Space Cookies : Girl Scout Troop #62868 and FRC Team #1868
003 * Open Source Software; you may modify and/or share it under the terms of
004 * the 3-Clause BSD License found in the root directory of this project.
005 */
006
007package tagalong.math;
008
009import java.util.Arrays;
010import java.util.Comparator;
011
012/**
013 * Linearized Lookup Table
014 */
015public class LinearizedLookupTable {
016  private double[] _ids;
017  private double[] _values;
018  private double max, min;
019
020  /**
021   * Constructs a lookup table containing IDs and their corresponding values
022   *
023   * @param id array of identifiers
024   * @param values array of values corresponding to the IDs
025   */
026  public LinearizedLookupTable(double[] id, double[] values) {
027    if (id.length != values.length)
028      throw new IllegalArgumentException();
029    int length = id.length;
030
031    // 1. Fill third array with indices
032    Integer[] c = new Integer[length];
033    for (int i = 0; i < length; ++i) c[i] = i;
034
035    // 2. Sort third array, with the comparator peaking into original arrays
036    // TODO: put your own comparator function here
037    Arrays.sort(c, Comparator.comparing(i -> { return id[i]; }));
038
039    // 3. Populate new arrays using the indices
040    _ids = new double[length];
041    _values = new double[length];
042    for (int i = 0; i < length; i++) {
043      int newI = c[i];
044      _ids[i] = id[newI];
045      _values[i] = values[newI];
046    }
047
048    max = _ids[_ids.length - 1];
049    min = _ids[0];
050  }
051
052  /**
053   * Returns the value that the ID maps to on a continuous, linearized function
054   *
055   * @param id ID contained in the lookup table
056   * @return value using linearized lookup
057   */
058  public double lookup(double id) {
059    double newId = Math.max(Math.min(id, max), min);
060
061    int index = Arrays.binarySearch(_ids, newId);
062    if (index < 0) {
063      int lIndex = -index - 1;
064      int hIndex = -index;
065      if (hIndex >= _ids.length) {
066        hIndex = _ids.length - 1;
067        lIndex = _ids.length - 2;
068      }
069      double slope = (_values[hIndex] - _values[lIndex]) / (_ids[hIndex] - _ids[lIndex]);
070      return _values[lIndex] + slope * (newId - _ids[lIndex]);
071    } else {
072      return _values[index];
073    }
074  }
075
076  /**
077   * Gets length of ID array
078   *
079   * @return length of ID array
080   */
081  public int size() {
082    return _ids.length;
083  }
084}