From 3960045663dc647d8fb70fab3844e74ce54b51a6 Mon Sep 17 00:00:00 2001
From: me-it-is <raffertymclain@gmail.com>
Date: Fri, 25 Apr 2025 21:58:05 -0700
Subject: [PATCH] [wpimath] TrapezoidProfile: Fix null pointer when calling
 timeLeftUntil (#7894)

---
 .../first/math/trajectory/TrapezoidProfile.java    |  7 ++++---
 .../include/frc/trajectory/TrapezoidProfile.h      | 14 ++++++++------
 .../math/trajectory/TrapezoidProfileTest.java      |  7 +++++++
 .../native/cpp/trajectory/TrapezoidProfileTest.cpp |  7 +++++++
 4 files changed, 26 insertions(+), 9 deletions(-)

diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java
index 448c8384cf..7e7bbdcff8 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java
@@ -43,7 +43,7 @@ public class TrapezoidProfile {
   private int m_direction;
 
   private final Constraints m_constraints;
-  private State m_current;
+  private State m_current = new State();
 
   private double m_endAccel;
   private double m_endFullSpeed;
@@ -187,7 +187,8 @@ public class TrapezoidProfile {
    * Returns the time left until a target distance in the profile is reached.
    *
    * @param target The target distance.
-   * @return The time left until a target distance in the profile is reached.
+   * @return The time left until a target distance in the profile is reached, or zero if no goal was
+   *     set.
    */
   public double timeLeftUntil(double target) {
     double position = m_current.position * m_direction;
@@ -253,7 +254,7 @@ public class TrapezoidProfile {
   /**
    * Returns the total time the profile takes to reach the goal.
    *
-   * @return The total time the profile takes to reach the goal.
+   * @return The total time the profile takes to reach the goal, or zero if no goal was set.
    */
   public double totalTime() {
     return m_endDecel;
diff --git a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h
index 9dd82f4eee..72224fda83 100644
--- a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h
+++ b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h
@@ -200,7 +200,8 @@ class TrapezoidProfile {
    * Returns the time left until a target distance in the profile is reached.
    *
    * @param target The target distance.
-   * @return The time left until a target distance in the profile is reached.
+   * @return The time left until a target distance in the profile is reached, or
+   * zero if no goal was set.
    */
   constexpr units::second_t TimeLeftUntil(Distance_t target) const {
     Distance_t position = m_current.position * m_direction;
@@ -271,7 +272,8 @@ class TrapezoidProfile {
   /**
    * Returns the total time the profile takes to reach the goal.
    *
-   * @return The total time the profile takes to reach the goal.
+   * @return The total time the profile takes to reach the goal, or zero if no
+   * goal was set.
    */
   constexpr units::second_t TotalTime() const { return m_endDecel; }
 
@@ -311,14 +313,14 @@ class TrapezoidProfile {
   }
 
   // The direction of the profile, either 1 for forwards or -1 for inverted
-  int m_direction;
+  int m_direction = 1;
 
   Constraints m_constraints;
   State m_current;
 
-  units::second_t m_endAccel;
-  units::second_t m_endFullSpeed;
-  units::second_t m_endDecel;
+  units::second_t m_endAccel = 0_s;
+  units::second_t m_endFullSpeed = 0_s;
+  units::second_t m_endDecel = 0_s;
 };
 
 }  // namespace frc
diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java
index 64dfa4784d..72b730420f 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java
@@ -246,4 +246,11 @@ class TrapezoidProfileTest {
       }
     }
   }
+
+  @Test
+  void initalizationOfCurrentState() {
+    var profile = new TrapezoidProfile(new TrapezoidProfile.Constraints(1, 1));
+    assertNear(profile.timeLeftUntil(0), 0, 1e-10);
+    assertNear(profile.totalTime(), 0, 1e-10);
+  }
 }
diff --git a/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
index 39cbc3b0ea..e83da634b6 100644
--- a/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
@@ -234,3 +234,10 @@ TEST(TrapezoidProfileTest, TimingBeforeNegativeGoal) {
     }
   }
 }
+
+TEST(TrapezoidProfileTest, InitalizationOfCurrentState) {
+  frc::TrapezoidProfile<units::meter>::Constraints constraints{1_mps, 1_mps_sq};
+  frc::TrapezoidProfile<units::meter> profile{constraints};
+  EXPECT_NEAR_UNITS(profile.TimeLeftUntil(0_m), 0_s, 1e-10_s);
+  EXPECT_NEAR_UNITS(profile.TotalTime(), 0_s, 1e-10_s);
+}