Optimize (#32)

* Don't use streams when you don't have to, and remove old bug check

* Removed more period `stream`s, and cleaned code

Author: levyishai

src/main/java/org/trigon/hardware/BaseInputs.java renamed to src/main/java/org/trigon/hardware/InputsBase.java

@@ -6,7 +6,7 @@
 
 import java.util.NoSuchElementException;
 
-public abstract class BaseInputs implements LoggableInputs {
+public abstract class InputsBase implements LoggableInputs {
     protected LogTable latestTable = null;
     private final String name;
     private double lastErrorTimestamp = 0;
@@ -16,7 +16,7 @@ public abstract class BaseInputs implements LoggableInputs {
      *
      * @param name the name of the instance. Used for error messages
      */
-    public BaseInputs(String name) {
+    public InputsBase(String name) {
         this.name = name;
     }
 

src/main/java/org/trigon/hardware/SignalThreadBase.java

@@ -18,6 +18,15 @@ public class SignalThreadBase extends Thread {
     private final String name;
     protected double threadFrequencyHertz = 50;
 
+    @SuppressWarnings("ConstantConditions")
+    public static double[] queueToDoubleArray(Queue<Double> queue) {
+        final double[] array = new double[queue.size()];
+        for (int i = 0; i < array.length; i++)
+            array[i] = queue.poll();
+
+        return array;
+    }
+
     /**
      * Creates a new SignalThreadBase.
      *
@@ -44,7 +53,7 @@ public void setThreadFrequencyHertz(double threadFrequencyHertz) {
      */
     public void updateLatestTimestamps() {
         if (!RobotHardwareStats.isReplay()) {
-            threadInputs.timestamps = timestamps.stream().mapToDouble(Double::doubleValue).toArray();
+            threadInputs.timestamps = queueToDoubleArray(timestamps);
             timestamps.clear();
         }
         Logger.processInputs(name, threadInputs);

src/main/java/org/trigon/hardware/misc/KeyboardController.java

@@ -2,7 +2,6 @@
 
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import org.littletonrobotics.junction.networktables.LoggedNetworkBoolean;
-import org.littletonrobotics.junction.networktables.LoggedNetworkBoolean;
 
 /**
  * A class that represents a keyboard controller. Used to get input from a keyboard.

src/main/java/org/trigon/hardware/phoenix6/Phoenix6Inputs.java

@@ -1,16 +1,16 @@
 package org.trigon.hardware.phoenix6;
 
 import com.ctre.phoenix6.BaseStatusSignal;
-import com.ctre.phoenix6.StatusSignal;
 import org.littletonrobotics.junction.LogTable;
-import org.trigon.hardware.BaseInputs;
+import org.trigon.hardware.InputsBase;
 import org.trigon.hardware.RobotHardwareStats;
+import org.trigon.hardware.SignalThreadBase;
 
 import java.util.HashMap;
 import java.util.Map;
 import java.util.Queue;
 
-public class Phoenix6Inputs extends BaseInputs {
+public class Phoenix6Inputs extends InputsBase {
     private final HashMap<String, Queue<Double>> signalToThreadedQueue = new HashMap<>();
     private final Phoenix6SignalThread signalThread = Phoenix6SignalThread.getInstance();
     private BaseStatusSignal[] signals = new BaseStatusSignal[0];
@@ -70,19 +70,16 @@ public void registerSignal(BaseStatusSignal statusSignal, double updateFrequency
 
     private void updateThreadedSignalsToTable(LogTable table) {
         for (Map.Entry<String, Queue<Double>> entry : signalToThreadedQueue.entrySet()) {
-            table.put(entry.getKey(), entry.getValue().stream().mapToDouble(Double::doubleValue).toArray());
+            table.put(entry.getKey(), SignalThreadBase.queueToDoubleArray(entry.getValue()));
             entry.getValue().clear();
         }
     }
 
     private void updateSignalsToTable(LogTable table) {
         BaseStatusSignal.refreshAll(signals);
 
-        for (BaseStatusSignal signal : signals) {
-            if (signal.getName().equals("ClosedLoopReference")) // This signal isn't updated correctly by `BaseStatusSignal.updateAll` for some reason.
-                ((StatusSignal<Double>) signal).refresh();
+        for (BaseStatusSignal signal : signals)
             table.put(signal.getName(), signal.getValueAsDouble());
-        }
     }
 
     private void addSignalToSignalsArray(BaseStatusSignal statusSignal) {

src/main/java/org/trigon/hardware/rev/spark/SparkInputs.java

@@ -1,14 +1,15 @@
 package org.trigon.hardware.rev.spark;
 
 import org.littletonrobotics.junction.LogTable;
-import org.trigon.hardware.BaseInputs;
+import org.trigon.hardware.InputsBase;
 import org.trigon.hardware.RobotHardwareStats;
+import org.trigon.hardware.SignalThreadBase;
 
 import java.util.HashMap;
 import java.util.Map;
 import java.util.Queue;
 
-public class SparkInputs extends BaseInputs {
+public class SparkInputs extends InputsBase {
     private final HashMap<String, Queue<Double>> signalToThreadedQueue = new HashMap<>();
     private final SparkSignalThread signalThread = SparkSignalThread.getInstance();
     private SparkStatusSignal[] signals = new SparkStatusSignal[0];
@@ -61,7 +62,7 @@ public void registerThreadedSignal(SparkStatusSignal statusSignal) {
 
     private void updateThreadedSignalsToTable(LogTable table) {
         for (Map.Entry<String, Queue<Double>> entry : signalToThreadedQueue.entrySet()) {
-            table.put(entry.getKey(), entry.getValue().stream().mapToDouble(Double::doubleValue).toArray());
+            table.put(entry.getKey(), SignalThreadBase.queueToDoubleArray(entry.getValue()));
             entry.getValue().clear();
         }
     }

src/main/java/org/trigon/utilities/QuickSortHandler.java

@@ -1,6 +1,5 @@
 package org.trigon.utilities;
 
-import java.util.Arrays;
 import java.util.function.ToDoubleFunction;
 
 /**
@@ -51,7 +50,7 @@ private static <T> void quickSortByDoubleValue(T[] array, ToDoubleFunction<T> to
      * @return the index of the new pivot in the whole array
      */
     private static <T> int partitionArrayAndGetNewPivot(T[] array, ToDoubleFunction<T> toDoubleFunction, int startIndex, int endIndex) {
-        final double[] doubleArray = Arrays.stream(array).mapToDouble(toDoubleFunction).toArray();
+        final double[] doubleArray = convertArrayToDoubleArray(array, toDoubleFunction);
         final double pivot = doubleArray[endIndex];
 
         int i = startIndex;
@@ -65,6 +64,13 @@ private static <T> int partitionArrayAndGetNewPivot(T[] array, ToDoubleFunction<
         return i;
     }
 
+    private static <T> double[] convertArrayToDoubleArray(T[] array, ToDoubleFunction<T> toDoubleFunction) {
+        final double[] doubleArray = new double[array.length];
+        for (int i = 0; i < array.length; i++)
+            doubleArray[i] = toDoubleFunction.applyAsDouble(array[i]);
+        return doubleArray;
+    }
+
     /**
      * Swaps the values held by two indices in an array.
      *