From 3f10a165c8b1c7c923960498c8a761397e29dfcb Mon Sep 17 00:00:00 2001
From: Abhi Sachi <abhi.p.sachi@gmail.com>
Date: Mon, 6 Feb 2023 18:01:31 -0700
Subject: [PATCH] Button box and complex command chooser

---
 .../robot/commands/ComplexCommandChooser.java | 77 +++++++++++++++++++
 .../frc4388/utility/controller/ButtonBox.java | 24 ++++++
 2 files changed, 101 insertions(+)
 create mode 100644 src/main/java/frc4388/robot/commands/ComplexCommandChooser.java
 create mode 100644 src/main/java/frc4388/utility/controller/ButtonBox.java

diff --git a/src/main/java/frc4388/robot/commands/ComplexCommandChooser.java b/src/main/java/frc4388/robot/commands/ComplexCommandChooser.java
new file mode 100644
index 0000000..0ed5218
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/ComplexCommandChooser.java
@@ -0,0 +1,77 @@
+// 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.
+
+package frc4388.robot.commands;
+
+import java.util.Collections;
+import java.util.HashMap;
+import java.util.Set;
+import java.util.function.BooleanSupplier;
+import java.util.function.Consumer;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.Subsystem;
+
+public class ComplexCommandChooser extends CommandBase {
+
+  private HashMap<Command, BooleanSupplier> commandMap;
+
+  /** Creates a new CommandChooser.
+   * @author Aarav Shah
+   * @author Daniel Thomas McGrath
+  */
+  public ComplexCommandChooser(HashMap<Command, BooleanSupplier> commandMap) {
+    this.commandMap = commandMap;
+
+    Set<Subsystem> allReqs = Collections.emptySet();
+
+    for(Command command : commandMap.keySet())
+      allReqs.addAll(command.getRequirements());
+
+    addRequirements(allReqs.toArray(Subsystem[]::new));
+  }
+
+  /**
+   * Runs an operation on every command in the group
+   * 
+   * @param consumer operation to run
+   */
+  public void runCommands(Consumer<Command> consumer) {
+    Set<Subsystem> reqCheck = Collections.emptySet();
+    
+    for(Command command : commandMap.keySet()) {
+      boolean reqFree = true;
+      for(Subsystem req : (Subsystem[]) command.getRequirements().toArray())
+        reqFree &= !reqCheck.contains(req);
+
+      if(commandMap.get(command).getAsBoolean() && reqFree) {
+        consumer.accept(command);
+        reqCheck.addAll(command.getRequirements());
+      }
+    }
+  }
+
+  @Override
+  public void initialize() { runCommands(c -> c.initialize()); }
+
+  @Override
+  public void execute() { runCommands(c -> c.execute()); }
+
+  @Override
+  public void end(boolean interrupted) { runCommands(c -> c.end(interrupted)); }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    boolean finished = true;
+
+    // Checks that everything is finished
+    //! command will not finish if there is an unfinished command
+    for(Command command : commandMap.keySet())
+      if(commandMap.get(command).getAsBoolean()) finished &= command.isFinished();
+    
+    return finished;
+  }
+}
\ No newline at end of file
diff --git a/src/main/java/frc4388/utility/controller/ButtonBox.java b/src/main/java/frc4388/utility/controller/ButtonBox.java
new file mode 100644
index 0000000..46366d0
--- /dev/null
+++ b/src/main/java/frc4388/utility/controller/ButtonBox.java
@@ -0,0 +1,24 @@
+package frc4388.utility.controller;
+
+import edu.wpi.first.wpilibj.GenericHID;
+
+public class ButtonBox extends GenericHID {
+    public ButtonBox(int port) {
+        super(port);
+    }
+
+    public enum Button {
+        kRightSwitch(1),
+        kMiddleSwitch(2),
+        kLeftSwitch(3),
+        kRightButton(4),
+        kLeftButton(5);
+    
+        @SuppressWarnings("MemberName")
+        public final int value;
+    
+        Button(int value) {
+            this.value = value;
+        }
+    }
+}
\ No newline at end of file