From 36d514eae7fb29376642296494efaebb08d705c5 Mon Sep 17 00:00:00 2001
From: Ryan Blue <ryanzblue@gmail.com>
Date: Tue, 29 Aug 2023 16:16:15 -0400
Subject: [PATCH] [commands] Refactor C++ ScheduleCommand to use SmallSet
 (#5568)

Remove SetUtilities.h
---
 .../cpp/frc2/command/ScheduleCommand.cpp      |  6 +++--
 .../include/frc2/command/ProxyCommand.h       |  1 -
 .../include/frc2/command/ScheduleCommand.h    |  5 ++--
 .../include/frc2/command/SetUtilities.h       | 27 -------------------
 .../native/cpp/frc2/command/CommandTestBase.h |  1 -
 5 files changed, 6 insertions(+), 34 deletions(-)
 delete mode 100644 wpilibNewCommands/src/main/native/include/frc2/command/SetUtilities.h

diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/ScheduleCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/ScheduleCommand.cpp
index 54c3042ae1..c96f21835e 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/ScheduleCommand.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/ScheduleCommand.cpp
@@ -7,11 +7,13 @@
 using namespace frc2;
 
 ScheduleCommand::ScheduleCommand(std::span<Command* const> toSchedule) {
-  SetInsert(m_toSchedule, toSchedule);
+  for (auto cmd : toSchedule) {
+    m_toSchedule.insert(cmd);
+  }
 }
 
 ScheduleCommand::ScheduleCommand(Command* toSchedule) {
-  SetInsert(m_toSchedule, {&toSchedule, 1});
+  m_toSchedule.insert(toSchedule);
 }
 
 void ScheduleCommand::Initialize() {
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ProxyCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/ProxyCommand.h
index 63d37693e3..a3e0ab323e 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/ProxyCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ProxyCommand.h
@@ -11,7 +11,6 @@
 
 #include "frc2/command/Command.h"
 #include "frc2/command/CommandHelper.h"
-#include "frc2/command/SetUtilities.h"
 
 namespace frc2 {
 /**
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ScheduleCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/ScheduleCommand.h
index eedb72b111..3c960db71f 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/ScheduleCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ScheduleCommand.h
@@ -6,11 +6,10 @@
 
 #include <span>
 
-#include <wpi/SmallVector.h>
+#include <wpi/SmallSet.h>
 
 #include "frc2/command/Command.h"
 #include "frc2/command/CommandHelper.h"
-#include "frc2/command/SetUtilities.h"
 
 namespace frc2 {
 /**
@@ -44,6 +43,6 @@ class ScheduleCommand : public CommandHelper<Command, ScheduleCommand> {
   bool RunsWhenDisabled() const override;
 
  private:
-  wpi::SmallVector<Command*, 4> m_toSchedule;
+  wpi::SmallSet<Command*, 4> m_toSchedule;
 };
 }  // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/SetUtilities.h b/wpilibNewCommands/src/main/native/include/frc2/command/SetUtilities.h
deleted file mode 100644
index e70e17b9d5..0000000000
--- a/wpilibNewCommands/src/main/native/include/frc2/command/SetUtilities.h
+++ /dev/null
@@ -1,27 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-#pragma once
-
-#include <span>
-
-#include <wpi/SmallVector.h>
-
-namespace frc2 {
-template <typename T>
-void SetInsert(wpi::SmallVectorImpl<T*>& vector, std::span<T* const> toAdd) {
-  for (auto addCommand : toAdd) {
-    bool exists = false;
-    for (auto existingCommand : vector) {
-      if (addCommand == existingCommand) {
-        exists = true;
-        break;
-      }
-    }
-    if (!exists) {
-      vector.emplace_back(addCommand);
-    }
-  }
-}
-}  // namespace frc2
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.h b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.h
index 41a794ef44..f7261c66d8 100644
--- a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.h
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.h
@@ -13,7 +13,6 @@
 
 #include "frc2/command/CommandHelper.h"
 #include "frc2/command/CommandScheduler.h"
-#include "frc2/command/SetUtilities.h"
 #include "frc2/command/Subsystem.h"
 #include "gmock/gmock.h"
 #include "make_vector.h"