From 3eecf0f71e220b50c57ea3f13e26dd24bf9a04e0 Mon Sep 17 00:00:00 2001
From: Joseph Eng <s-engjo@bsd405.org>
Date: Wed, 19 Jul 2023 20:14:15 -0700
Subject: [PATCH] Java optimizations (with a corresponding change to C++) Cache
 Optional.empty() Use separate lists for toCancel queue

---
 .../wpilibj2/command/CommandScheduler.java    | 29 ++++++++-----------
 .../cpp/frc2/command/CommandScheduler.cpp     | 13 +++++----
 2 files changed, 20 insertions(+), 22 deletions(-)

diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java
index e2593da8080..4e995f70d32 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java
@@ -44,16 +44,6 @@
  * <p>This class is provided by the NewCommands VendorDep
  */
 public final class CommandScheduler implements Sendable, AutoCloseable {
-  private static class CancelData {
-    public final Command m_command;
-    public final Optional<Command> m_interruptor;
-
-    CancelData(Command command, Optional<Command> interruptor) {
-      m_command = command;
-      m_interruptor = interruptor;
-    }
-  }
-
   /** The Singleton Instance. */
   private static CommandScheduler instance;
 
@@ -69,6 +59,8 @@ public static synchronized CommandScheduler getInstance() {
     return instance;
   }
 
+  private static final Optional<Command> kNoInterruptor = Optional.empty();
+
   private final Set<Command> m_composedCommands = Collections.newSetFromMap(new WeakHashMap<>());
 
   // A set of the currently-running commands.
@@ -98,7 +90,8 @@ public static synchronized CommandScheduler getInstance() {
   // scheduled/canceled during run
   private boolean m_inRunLoop;
   private final Set<Command> m_toSchedule = new LinkedHashSet<>();
-  private final List<CancelData> m_toCancel = new ArrayList<>();
+  private final List<Command> m_toCancelCommands = new ArrayList<>();
+  private final List<Optional<Command>> m_toCancelInterruptors = new ArrayList<>();
 
   private final Watchdog m_watchdog = new Watchdog(TimedRobot.kDefaultPeriod, () -> {});
 
@@ -285,7 +278,7 @@ public void run() {
       if (!command.runsWhenDisabled() && RobotState.isDisabled()) {
         command.end(true);
         for (BiConsumer<Command, Optional<Command>> action : m_interruptActions) {
-          action.accept(command, Optional.empty());
+          action.accept(command, kNoInterruptor);
         }
         m_requirements.keySet().removeAll(command.getRequirements());
         iterator.remove();
@@ -316,12 +309,13 @@ public void run() {
       schedule(command);
     }
 
-    for (CancelData cancelData : m_toCancel) {
-      cancel(cancelData.m_command, cancelData.m_interruptor);
+    for (int i = 0; i < m_toCancelCommands.size(); i++) {
+      cancel(m_toCancelCommands.get(i), m_toCancelInterruptors.get(i));
     }
 
     m_toSchedule.clear();
-    m_toCancel.clear();
+    m_toCancelCommands.clear();
+    m_toCancelInterruptors.clear();
 
     // Add default commands for un-required registered subsystems.
     for (Map.Entry<Subsystem, Command> subsystemCommand : m_subsystems.entrySet()) {
@@ -453,7 +447,7 @@ public Command getDefaultCommand(Subsystem subsystem) {
    */
   public void cancel(Command... commands) {
     for (Command command : commands) {
-      cancel(command, Optional.empty());
+      cancel(command, kNoInterruptor);
     }
   }
 
@@ -473,7 +467,8 @@ private void cancel(Command command, Optional<Command> interruptor) {
       return;
     }
     if (m_inRunLoop) {
-      m_toCancel.add(new CancelData(command, interruptor));
+      m_toCancelCommands.add(command);
+      m_toCancelInterruptors.add(interruptor);
       return;
     }
     if (!isScheduled(command)) {
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp
index 4365f0528fa..fdf9bfcf2a3 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp
@@ -56,7 +56,8 @@ class CommandScheduler::Impl {
 
   bool inRunLoop = false;
   wpi::SmallVector<Command*, 4> toSchedule;
-  wpi::SmallVector<std::pair<Command*, std::optional<Command*>>, 4> toCancel;
+  wpi::SmallVector<Command*, 4> toCancelCommands;
+  wpi::SmallVector<std::optional<Command*>, 4> toCancelInterruptors;
 };
 
 template <typename TMap, typename TKey>
@@ -226,12 +227,13 @@ void CommandScheduler::Run() {
     Schedule(command);
   }
 
-  for (auto&& cancelData : m_impl->toCancel) {
-    Cancel(cancelData.first, cancelData.second);
+  for (size_t i = 0; i < m_impl->toCancelCommands.size(); i++) {
+    Cancel(m_impl->toCancelCommands[i], m_impl->toCancelInterruptors[i]);
   }
 
   m_impl->toSchedule.clear();
-  m_impl->toCancel.clear();
+  m_impl->toCancelCommands.clear();
+  m_impl->toCancelInterruptors.clear();
 
   // Add default commands for un-required registered subsystems.
   for (auto&& subsystem : m_impl->subsystems) {
@@ -326,7 +328,8 @@ void CommandScheduler::Cancel(Command* command,
   }
 
   if (m_impl->inRunLoop) {
-    m_impl->toCancel.emplace_back(command, interruptor);
+    m_impl->toCancelCommands.emplace_back(command);
+    m_impl->toCancelInterruptors.emplace_back(interruptor);
     return;
   }