From 0606da64c9890e4d2f81ab17828b02bba527e75a Mon Sep 17 00:00:00 2001
From: Brendan Raykoff <99614905+Braykoff@users.noreply.github.com>
Date: Fri, 7 Jun 2024 20:24:34 -0400
Subject: [PATCH] [wpimath] LinearFilter lastValue(): Fix runtime exception
 (#6713)

---
 .../src/main/java/edu/wpi/first/math/filter/LinearFilter.java | 4 +++-
 wpimath/src/main/native/include/frc/filter/LinearFilter.h     | 4 +++-
 2 files changed, 6 insertions(+), 2 deletions(-)

diff --git a/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java b/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java
index 850b7f2f75..4af3da002c 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java
@@ -56,6 +56,7 @@ public class LinearFilter {
   private final DoubleCircularBuffer m_outputs;
   private final double[] m_inputGains;
   private final double[] m_outputGains;
+  private double m_lastOutput;
 
   private static int instances;
 
@@ -310,6 +311,7 @@ public class LinearFilter {
       m_outputs.addFirst(retVal);
     }
 
+    m_lastOutput = retVal;
     return retVal;
   }
 
@@ -319,7 +321,7 @@ public class LinearFilter {
    * @return The last value.
    */
   public double lastValue() {
-    return m_outputs.getFirst();
+    return m_lastOutput;
   }
 
   /**
diff --git a/wpimath/src/main/native/include/frc/filter/LinearFilter.h b/wpimath/src/main/native/include/frc/filter/LinearFilter.h
index 3f5462d594..52eec9de2b 100644
--- a/wpimath/src/main/native/include/frc/filter/LinearFilter.h
+++ b/wpimath/src/main/native/include/frc/filter/LinearFilter.h
@@ -367,6 +367,7 @@ class LinearFilter {
       m_outputs.push_front(retVal);
     }
 
+    m_lastOutput = retVal;
     return retVal;
   }
 
@@ -375,13 +376,14 @@ class LinearFilter {
    *
    * @return The last value.
    */
-  T LastValue() const { return m_outputs.front(); }
+  T LastValue() const { return m_lastOutput; }
 
  private:
   wpi::circular_buffer<T> m_inputs;
   wpi::circular_buffer<T> m_outputs;
   std::vector<double> m_inputGains;
   std::vector<double> m_outputGains;
+  T m_lastOutput{0.0};
 
   /**
    * Factorial of n.