[wpilibj] Allow passing DS Instance to Robot and OpModes (#8626)

Some discussion with the tech team showed that there were some real
advantages to being able to pass a 2nd type. It allows separating the DS
and Robot. Additionally, we can make the DriverStationBase class
actually usable instead of the existing DriverStation class which is
impossible to handle in intellisense because it has too much.

This won't fully be doable in C++, but we will need to implement
something similar in python.

wpilibj/src/main/java/org/wpilib/driverstation/DefaultUserControls.java

@@ -0,0 +1,31 @@
+// 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 org.wpilib.driverstation;
+
+/**
+ * A default implementation of UserControls that provides Gamepad instances for each of the 6
+ * joystick ports provided by the DS.
+ */
+public class DefaultUserControls implements UserControls {
+  private final Gamepad[] m_gamepads;
+
+  /** Constructs a DefaultUserControls instance with Gamepads for each port. */
+  public DefaultUserControls() {
+    m_gamepads = new Gamepad[DriverStation.kJoystickPorts];
+    for (int i = 0; i < m_gamepads.length; i++) {
+      m_gamepads[i] = new Gamepad(i);
+    }
+  }
+
+  /**
+   * Returns the Gamepad instance for the specified port.
+   *
+   * @param port The joystick port number.
+   * @return The Gamepad instance for the given port.
+   */
+  public Gamepad getGamepad(int port) {
+    return m_gamepads[port];
+  }
+}

wpilibj/src/main/java/org/wpilib/driverstation/UserControls.java

@@ -0,0 +1,15 @@
+// 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 org.wpilib.driverstation;
+
+/**
+ * An interface representing user controls such as gamepads or joysticks. If your main robot class
+ * has a UserControlsInstance attribute with a class implementing this interface, the constructor is
+ * able to receive an instance of that class. Additionally, any OpModes can also receive that same
+ * instance.
+ *
+ * <p>The implementation of this class must have a default constructor
+ */
+public interface UserControls {}

wpilibj/src/main/java/org/wpilib/driverstation/UserControlsInstance.java

@@ -0,0 +1,26 @@
+// 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 org.wpilib.driverstation;
+
+import java.lang.annotation.ElementType;
+import java.lang.annotation.Retention;
+import java.lang.annotation.RetentionPolicy;
+import java.lang.annotation.Target;
+
+/**
+ * An annotation to specify the UserControls implementation class to be used for a robot. Apply this
+ * annotation to your main robot class, providing a class that implements the UserControls
+ * interface.
+ */
+@Target(ElementType.TYPE)
+@Retention(RetentionPolicy.RUNTIME)
+public @interface UserControlsInstance {
+  /**
+   * The UserControls implementation class to be used.
+   *
+   * @return The class that implements UserControls.
+   */
+  Class<? extends UserControls> value();
+}

wpilibj/src/main/java/org/wpilib/framework/OpModeRobot.java

@@ -6,17 +6,19 @@ package org.wpilib.framework;
 
 import java.io.File;
 import java.io.IOException;
-import java.lang.reflect.Constructor;
 import java.lang.reflect.Modifier;
 import java.net.URL;
 import java.util.Enumeration;
 import java.util.HashMap;
 import java.util.Map;
+import java.util.Optional;
 import java.util.concurrent.atomic.AtomicReference;
 import java.util.function.Supplier;
 import java.util.jar.JarEntry;
 import java.util.jar.JarFile;
 import org.wpilib.driverstation.DriverStation;
+import org.wpilib.driverstation.UserControls;
+import org.wpilib.driverstation.UserControlsInstance;
 import org.wpilib.hardware.hal.ControlWord;
 import org.wpilib.hardware.hal.DriverStationJNI;
 import org.wpilib.hardware.hal.HAL;
@@ -27,6 +29,7 @@ import org.wpilib.opmode.OpMode;
 import org.wpilib.opmode.Teleop;
 import org.wpilib.opmode.TestOpMode;
 import org.wpilib.util.Color;
+import org.wpilib.util.ConstructorMatch;
 import org.wpilib.util.WPIUtilJNI;
 
 /**
@@ -56,51 +59,68 @@ public abstract class OpModeRobot extends RobotBase {
     DriverStation.reportError("Error adding OpMode " + cls.getSimpleName() + ": " + message, false);
   }
 
-  /**
-   * Find a public constructor to instantiate the opmode. Prefer a single-arg public constructor
-   * whose parameter type is assignable from this.getClass() (if multiple, pick the most specific
-   * parameter type). Otherwise return the public no-arg constructor. Return null if neither exists.
-   */
-  private Constructor<?> findOpModeConstructor(Class<?> cls) {
-    Constructor<?> bestCtor = null;
-    Class<?> bestParam = null;
-    for (Constructor<?> ctor : cls.getConstructors()) {
-      Class<?>[] params = ctor.getParameterTypes();
-      if (params.length != 1) {
-        continue;
-      }
-      Class<?> param = params[0];
-      if (!param.isAssignableFrom(getClass())) {
-        continue;
-      }
-      if (bestCtor == null || bestParam.isAssignableFrom(param)) {
-        bestCtor = ctor;
-        bestParam = param;
-      }
+  private final Optional<Class<? extends UserControls>> m_userControlsBaseClass;
+  private UserControls m_userControlsInstance;
+
+  void setUserControlsInstance(UserControls userControlsInstance) {
+    if (m_userControlsBaseClass.isEmpty()) {
+      throw new IllegalStateException("No UserControls class specified");
     }
-    if (bestCtor != null) {
-      return bestCtor;
-    }
-    try {
-      return cls.getConstructor();
-    } catch (NoSuchMethodException e) {
-      return null;
+
+    if (!m_userControlsBaseClass.get().isAssignableFrom(userControlsInstance.getClass())) {
+      throw new IllegalArgumentException(
+          userControlsInstance.getClass().getSimpleName()
+              + " is not assignable to "
+              + m_userControlsBaseClass.get().getSimpleName());
     }
+    m_userControlsInstance = userControlsInstance;
   }
 
-  private OpMode constructOpModeClass(Class<?> cls) {
-    Constructor<?> constructor = findOpModeConstructor(cls);
-    if (constructor == null) {
+  /**
+   * Find a public constructor to instantiate the opmode. This constructor can have up to 2
+   * parameters. The first parameter (if present) must be assignable from this.getClass(). The
+   * second parameter (if present) must be assignable from DriverStationBase. If multiple, first
+   * sort by most parameters, then by most specific first, then by most specific second.
+   */
+  private <T> Optional<ConstructorMatch<T>> findOpModeConstructor(Class<T> cls) {
+    Optional<ConstructorMatch<T>> ctor;
+
+    // try 2-parameter constructor
+    ctor = ConstructorMatch.findBestConstructor(cls, getClass(), m_userControlsInstance.getClass());
+    if (ctor.isPresent()) {
+      return ctor;
+    }
+
+    // try 1-parameter constructor with RobotBase parameter
+    ctor = ConstructorMatch.findBestConstructor(cls, getClass());
+    if (ctor.isPresent()) {
+      return ctor;
+    }
+
+    // try 1-parameter constructor with UserControls parameter
+    ctor = ConstructorMatch.findBestConstructor(cls, m_userControlsInstance.getClass());
+    if (ctor.isPresent()) {
+      return ctor;
+    }
+
+    // try no-parameter constructor
+    ctor = ConstructorMatch.findBestConstructor(cls);
+    if (ctor.isPresent()) {
+      return ctor;
+    }
+
+    return Optional.empty();
+  }
+
+  private <T extends OpMode> T constructOpModeClass(Class<T> cls) {
+    Optional<ConstructorMatch<T>> constructor = findOpModeConstructor(cls);
+    if (constructor.isEmpty()) {
       DriverStation.reportError(
           "No suitable constructor to instantiate OpMode " + cls.getSimpleName(), true);
       return null;
     }
     try {
-      if (constructor.getParameterCount() == 1) {
-        return (OpMode) constructor.newInstance(this);
-      } else {
-        return (OpMode) constructor.newInstance();
-      }
+      return constructor.get().newInstance(this, m_userControlsInstance);
     } catch (ReflectiveOperationException e) {
       DriverStation.reportError(
           "Could not instantiate OpMode " + cls.getSimpleName(), e.getStackTrace());
@@ -128,7 +148,7 @@ public abstract class OpModeRobot extends RobotBase {
     }
     // it must have a public no-arg constructor or a public constructor that accepts this class
     // (or a superclass/interface) as an argument
-    if (findOpModeConstructor(cls) == null) {
+    if (findOpModeConstructor(cls).isEmpty()) {
       throw new IllegalArgumentException(
           "missing public no-arg constructor or constructor accepting "
               + getClass().getSimpleName());
@@ -288,7 +308,7 @@ public abstract class OpModeRobot extends RobotBase {
   }
 
   private void addOpModeClassImpl(
-      Class<?> cls,
+      Class<? extends OpMode> cls,
       RobotMode mode,
       String name,
       String group,
@@ -305,7 +325,7 @@ public abstract class OpModeRobot extends RobotBase {
   }
 
   private void addAnnotatedOpModeImpl(
-      Class<?> cls, Autonomous auto, Teleop teleop, TestOpMode test) {
+      Class<? extends OpMode> cls, Autonomous auto, Teleop teleop, TestOpMode test) {
     checkOpModeClass(cls);
 
     // add an opmode for each annotation
@@ -364,10 +384,10 @@ public abstract class OpModeRobot extends RobotBase {
   private void addAnnotatedOpModeClass(String name) {
     // trim ".class" from end
     String className = name.replace('/', '.').substring(0, name.length() - 6);
-    Class<?> cls;
+    Class<? extends OpMode> cls;
     try {
-      cls = Class.forName(className);
-    } catch (ClassNotFoundException e) {
+      cls = Class.forName(className).asSubclass(OpMode.class);
+    } catch (ClassNotFoundException | ClassCastException e) {
       return;
     }
     Autonomous auto = cls.getAnnotation(Autonomous.class);
@@ -468,6 +488,14 @@ public abstract class OpModeRobot extends RobotBase {
   /** Constructor. */
   @SuppressWarnings("this-escape")
   public OpModeRobot() {
+    // Check to see if we have a DS annotation
+    UserControlsInstance userControlsAnnotation =
+        getClass().getAnnotation(UserControlsInstance.class);
+    if (userControlsAnnotation != null) {
+      m_userControlsBaseClass = Optional.of(userControlsAnnotation.value());
+    } else {
+      m_userControlsBaseClass = Optional.empty();
+    }
     // Scan for annotated opmode classes within the derived class's package and subpackages
     addAnnotatedOpModeClasses(getClass().getPackage());
     DriverStation.publishOpModes();

wpilibj/src/main/java/org/wpilib/framework/RobotBase.java

@@ -4,9 +4,11 @@
 
 package org.wpilib.framework;
 
-import java.lang.reflect.Constructor;
+import java.util.Optional;
 import java.util.concurrent.locks.ReentrantLock;
 import org.wpilib.driverstation.DriverStation;
+import org.wpilib.driverstation.UserControls;
+import org.wpilib.driverstation.UserControlsInstance;
 import org.wpilib.hardware.hal.HAL;
 import org.wpilib.hardware.hal.HALUtil;
 import org.wpilib.math.util.MathShared;
@@ -18,6 +20,7 @@ import org.wpilib.system.Notifier;
 import org.wpilib.system.RuntimeType;
 import org.wpilib.system.Timer;
 import org.wpilib.system.WPILibVersion;
+import org.wpilib.util.ConstructorMatch;
 import org.wpilib.util.WPIUtilJNI;
 import org.wpilib.vision.stream.CameraServerShared;
 import org.wpilib.vision.stream.CameraServerSharedStore;
@@ -286,28 +289,29 @@ public abstract class RobotBase implements AutoCloseable {
   private static boolean m_suppressExitWarning;
 
   private static <T extends RobotBase> T constructRobot(Class<T> robotClass) throws Throwable {
-    Constructor<?>[] constructors = robotClass.getConstructors();
-    Constructor<?> defaultConstructor = null;
-    for (Constructor<?> constructor : constructors) {
-      Class<?>[] paramTypes = constructor.getParameterTypes();
-      if (paramTypes.length == 0) {
-        if (defaultConstructor != null) {
-          throw new IllegalArgumentException(
-              "Multiple default constructors found in robot class " + robotClass.getName());
-        }
-        defaultConstructor = constructor;
-      }
+    UserControlsInstance userControlsAttribute =
+        robotClass.getDeclaredAnnotation(UserControlsInstance.class);
+    UserControls userControlsInstance = null;
+    Optional<ConstructorMatch<T>> constructorMatch;
+    if (userControlsAttribute != null) {
+      var userControlsClass = userControlsAttribute.value();
+      userControlsInstance = userControlsClass.getDeclaredConstructor().newInstance();
+      constructorMatch = ConstructorMatch.findBestConstructor(robotClass, userControlsClass);
+    } else {
+      constructorMatch = ConstructorMatch.findBestConstructor(robotClass);
     }
 
-    T robot;
-
-    if (defaultConstructor != null) {
-      robot = robotClass.cast(defaultConstructor.newInstance());
-    } else {
+    if (constructorMatch.isEmpty()) {
       throw new IllegalArgumentException(
           "No valid constructor found in robot class " + robotClass.getName());
     }
 
+    T robot = constructorMatch.get().newInstance(userControlsInstance);
+
+    if (robot instanceof OpModeRobot opModeRobot) {
+      // Insert the UserControls instance into the opModeRobot for use when constructing opmodes
+      opModeRobot.setUserControlsInstance(userControlsInstance);
+    }
     return robot;
   }
 
@@ -392,9 +396,10 @@ public abstract class RobotBase implements AutoCloseable {
   /**
    * Starting point for the applications.
    *
+   * @param <T> Robot subclass.
    * @param robotClass Robot subclass type.
    */
-  public static void startRobot(Class<? extends RobotBase> robotClass) {
+  public static <T extends RobotBase> void startRobot(Class<T> robotClass) {
     // Check that the MSVC runtime is valid.
     WPIUtilJNI.checkMsvcRuntime();
 

wpiutil/src/main/java/org/wpilib/util/ConstructorMatch.java

@@ -0,0 +1,176 @@
+// 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 org.wpilib.util;
+
+import java.lang.reflect.Constructor;
+import java.util.List;
+import java.util.Optional;
+
+/**
+ * Utility class to find the best matching constructor for a given set of parameter types. The
+ * constructor must have parameter types that are assignable from the given parameter types, and the
+ * parameter types must not be assignable to each other. If multiple constructors match, the one
+ * with the most specific parameter types is chosen. If there is still a tie, the one with the most
+ * specific first parameter type is chosen, then the second parameter type, and so on.
+ *
+ * @param <T> the type of the class to find the constructor for
+ */
+public class ConstructorMatch<T> {
+  private final Constructor<T> m_constructor;
+  private final List<Class<?>> m_parameterTypes;
+
+  /**
+   * Constructs a ConstructorMatch with the given constructor and parameter types. The parameter
+   * types must not be assignable to each other.
+   *
+   * @param constructor the constructor to match
+   * @param parameterTypes the parameter types for the constructor
+   */
+  public ConstructorMatch(Constructor<T> constructor, Class<?>... parameterTypes) {
+    m_constructor = constructor;
+    m_parameterTypes = List.of(parameterTypes);
+    if (!isValidParameterPack(parameterTypes)) {
+      throw new IllegalArgumentException("Parameter types must not be assignable to each other");
+    }
+  }
+
+  private static boolean isValidParameterPack(Class<?>... types) {
+    // Verify that all of the parameter types are not assignable to each other
+    for (int i = 0; i < types.length; i++) {
+      // Don't allow object parameters, as they would match any parameter type
+      // and prevent more specific matches from being found
+      if (types[i].equals(Object.class)) {
+        return false;
+      }
+
+      for (int j = i + 1; j < types.length; j++) {
+        if (types[i].isAssignableFrom(types[j]) || types[j].isAssignableFrom(types[i])) {
+          return false;
+        }
+      }
+    }
+    return true;
+  }
+
+  /**
+   * Creates a new instance of the constructor's class using the given arguments. The arguments must
+   * match the parameter types of the constructor, and must not be assignable to each other. The
+   * order of the arguments does not matter, as they will be matched to the parameter types.
+   * Duplicate arguments are ignored, as the first match will match.
+   *
+   * @param args the arguments to pass to the constructor
+   * @return a new instance of the constructor's class
+   * @throws ReflectiveOperationException if the constructor cannot be invoked
+   */
+  public T newInstance(Object... args) throws ReflectiveOperationException {
+    Object[] parameterArgs = new Object[m_parameterTypes.size()];
+    // Find the incoming argument that matches each parameter type
+    for (int i = 0; i < m_parameterTypes.size(); i++) {
+      boolean found = false;
+      for (Object arg : args) {
+        if (m_parameterTypes.get(i).isAssignableFrom(arg.getClass())) {
+          parameterArgs[i] = arg;
+          found = true;
+          break;
+        }
+      }
+      if (!found) {
+        throw new IllegalArgumentException(
+            "No argument found for parameter type " + m_parameterTypes.get(i));
+      }
+    }
+    return m_constructor.newInstance(parameterArgs);
+  }
+
+  /**
+   * Finds the best matching constructor for the given class and parameter types. The constructor
+   * must have parameter types that are assignable from the given parameter types, and the parameter
+   * types must not be assignable to each other. If multiple constructors match, the one with the
+   * most specific parameter types is chosen. If there is still a tie, the one with the most
+   * specific first parameter type is chosen, then the second parameter type, and so on. The order
+   * of the parameter types does not matter, as they will be matched to the constructor's parameter
+   * types. Duplicate parameter types are ignored, as the first match will match.
+   *
+   * @param <T> the type of the class to find the constructor for
+   * @param clazz the class to find the constructor for
+   * @param parameterTypes the parameter types to match
+   * @return an Optional containing the best matching ConstructorMatch, or empty if no match is
+   *     found
+   */
+  public static <T> Optional<ConstructorMatch<T>> findBestConstructor(
+      Class<T> clazz, Class<?>... parameterTypes) {
+    if (!isValidParameterPack(parameterTypes)) {
+      return Optional.empty();
+    }
+    Constructor<T> bestCtor = null;
+    Class<?>[] bestParameterTypes = new Class<?>[parameterTypes.length];
+    @SuppressWarnings("unchecked")
+    Constructor<T>[] constructors = (Constructor<T>[]) clazz.getConstructors();
+    for (Constructor<T> constructor : constructors) {
+      Class<?>[] ctorParameterTypes = constructor.getParameterTypes();
+      if (ctorParameterTypes.length != parameterTypes.length) {
+        continue;
+      }
+      boolean matches = true;
+      for (int i = 0; i < parameterTypes.length; i++) {
+        // Don't allow object parameters, as they would match any parameter type and
+        // prevent more specific matches from being found
+        if (ctorParameterTypes[i].equals(Object.class)) {
+          matches = false;
+          break;
+        }
+        if (!ctorParameterTypes[i].isAssignableFrom(parameterTypes[i])) {
+          matches = false;
+          break;
+        }
+      }
+      if (!matches) {
+        continue;
+      }
+      boolean better = false;
+      if (bestCtor == null) {
+        better = true;
+      } else {
+        // Check if this constructor is more specific than the best one found so far
+        // Order by parameter order so that if one constructor has a more specific
+        // parameter type for the first parameter, it is preferred over a constructor
+        // that has a more specific parameter type for the second parameter
+        for (int i = 0; i < parameterTypes.length; i++) {
+          if (ctorParameterTypes[i] != bestParameterTypes[i]) {
+            if (bestParameterTypes[i].isAssignableFrom(ctorParameterTypes[i])) {
+              better = true;
+            }
+            break;
+          }
+        }
+      }
+      if (better) {
+        bestCtor = constructor;
+        System.arraycopy(ctorParameterTypes, 0, bestParameterTypes, 0, parameterTypes.length);
+      }
+    }
+    return bestCtor == null
+        ? Optional.empty()
+        : Optional.of(new ConstructorMatch<>(bestCtor, bestParameterTypes));
+  }
+
+  /**
+   * Returns the constructor that was matched.
+   *
+   * @return the constructor that was matched
+   */
+  public Constructor<T> getConstructor() {
+    return m_constructor;
+  }
+
+  /**
+   * Returns the parameter types for the constructor.
+   *
+   * @return the parameter types for the constructor
+   */
+  public List<Class<?>> getParameterTypes() {
+    return m_parameterTypes;
+  }
+}

wpiutil/src/test/java/org/wpilib/util/ConstructorMatchTest.java

@@ -0,0 +1,278 @@
+// 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 org.wpilib.util;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import java.util.Optional;
+import org.junit.jupiter.api.Test;
+
+@SuppressWarnings({
+  "PMD.TestClassWithoutTestCases",
+  "PMD.UnusedFormalParameter",
+  "RedundantModifier"
+})
+class ConstructorMatchTest {
+  public static class TestClass {
+    public TestClass() {}
+
+    public TestClass(String s) {}
+
+    public TestClass(Optional<?> o) {}
+
+    public TestClass(String s, Optional<?> o) {}
+  }
+
+  public static class TestInvalidParameterClass {
+    public TestInvalidParameterClass(String s, Object o) {}
+  }
+
+  @Test
+  void testTooManyParameters() {
+    var ctor = ConstructorMatch.findBestConstructor(TestClass.class, String.class, Object.class);
+    assertTrue(ctor.isEmpty());
+  }
+
+  @Test
+  void testUnassignableParameters() {
+    var ctor =
+        ConstructorMatch.findBestConstructor(
+            TestInvalidParameterClass.class, String.class, Object.class);
+    assertTrue(ctor.isEmpty());
+  }
+
+  @Test
+  void testValidConstructorNoArgs() throws ReflectiveOperationException {
+    var ctor = ConstructorMatch.findBestConstructor(TestClass.class);
+    assertTrue(ctor.isPresent());
+    ctor.get().newInstance();
+    ctor.get().newInstance("test", Optional.empty());
+    ctor.get().newInstance(Optional.empty(), "test");
+    ctor.get().newInstance("test");
+    ctor.get().newInstance(Optional.empty());
+  }
+
+  @Test
+  void testValidConstructorString() throws ReflectiveOperationException {
+    var ctor = ConstructorMatch.findBestConstructor(TestClass.class, String.class);
+    assertTrue(ctor.isPresent());
+    ctor.get().newInstance("test", Optional.empty());
+    ctor.get().newInstance(Optional.empty(), "test");
+    ctor.get().newInstance("test");
+  }
+
+  @Test
+  void testInvalidConstructorString() throws ReflectiveOperationException {
+    var ctor = ConstructorMatch.findBestConstructor(TestClass.class, String.class);
+    assertTrue(ctor.isPresent());
+    assertThrows(IllegalArgumentException.class, () -> ctor.get().newInstance());
+    assertThrows(IllegalArgumentException.class, () -> ctor.get().newInstance(Optional.empty()));
+  }
+
+  @Test
+  void testValidConstructorOptional() throws ReflectiveOperationException {
+    var ctor = ConstructorMatch.findBestConstructor(TestClass.class, Optional.class);
+    assertTrue(ctor.isPresent());
+    ctor.get().newInstance("test", Optional.empty());
+    ctor.get().newInstance(Optional.empty(), "test");
+    ctor.get().newInstance(Optional.empty());
+  }
+
+  @Test
+  void testInvalidConstructorOptional() throws ReflectiveOperationException {
+    var ctor = ConstructorMatch.findBestConstructor(TestClass.class, Optional.class);
+    assertTrue(ctor.isPresent());
+    assertThrows(IllegalArgumentException.class, () -> ctor.get().newInstance());
+    assertThrows(IllegalArgumentException.class, () -> ctor.get().newInstance("test"));
+  }
+
+  @Test
+  void testValidConstructorStringOptional() throws ReflectiveOperationException {
+    var ctor = ConstructorMatch.findBestConstructor(TestClass.class, String.class, Optional.class);
+    assertTrue(ctor.isPresent());
+    ctor.get().newInstance("test", Optional.empty());
+    ctor.get().newInstance(Optional.empty(), "test");
+  }
+
+  @Test
+  void testInvalidConstructorStringOptional() throws ReflectiveOperationException {
+    var ctor = ConstructorMatch.findBestConstructor(TestClass.class, String.class, Optional.class);
+    assertTrue(ctor.isPresent());
+    assertThrows(IllegalArgumentException.class, () -> ctor.get().newInstance());
+    assertThrows(IllegalArgumentException.class, () -> ctor.get().newInstance("test"));
+    assertThrows(IllegalArgumentException.class, () -> ctor.get().newInstance(Optional.empty()));
+  }
+
+  // Since this is built for opmodes, we're going to write tests that assume that
+  // scenario.
+
+  public interface UserControls {}
+
+  public static class DefaultUserControls implements UserControls {}
+
+  public static class CustomUserControls implements UserControls {}
+
+  public static class RobotBase {}
+
+  public static class RobotWithNoUserControls extends RobotBase {}
+
+  public static class RobotWithDefaultUserControls extends RobotBase {
+    public RobotWithDefaultUserControls(DefaultUserControls controls) {}
+  }
+
+  @Test
+  void testRobotWithDefaultUserControls() throws ReflectiveOperationException {
+    var ctor =
+        ConstructorMatch.findBestConstructor(
+            RobotWithDefaultUserControls.class, DefaultUserControls.class);
+    assertTrue(ctor.isPresent());
+    ctor.get().newInstance(new DefaultUserControls());
+    assertThrows(
+        IllegalArgumentException.class, () -> ctor.get().newInstance(new CustomUserControls()));
+  }
+
+  public static class RobotWithCustomUserControls extends RobotBase {
+    public RobotWithCustomUserControls(CustomUserControls controls) {}
+  }
+
+  @Test
+  void testRobotWithCustomUserControls() throws ReflectiveOperationException {
+    var ctor =
+        ConstructorMatch.findBestConstructor(
+            RobotWithCustomUserControls.class, CustomUserControls.class);
+    assertTrue(ctor.isPresent());
+    ctor.get().newInstance(new CustomUserControls());
+    assertThrows(
+        IllegalArgumentException.class, () -> ctor.get().newInstance(new DefaultUserControls()));
+  }
+
+  public static class RobotWithUserControls extends RobotBase {
+    public RobotWithUserControls(UserControls controls) {}
+  }
+
+  @Test
+  void testRobotWithUserControls() throws ReflectiveOperationException {
+    var ctor =
+        ConstructorMatch.findBestConstructor(RobotWithUserControls.class, UserControls.class);
+    assertTrue(ctor.isPresent());
+    ctor.get().newInstance(new DefaultUserControls());
+    ctor.get().newInstance(new CustomUserControls());
+  }
+
+  public static class OpModeWithRobotBase {
+    public OpModeWithRobotBase(RobotBase robot) {}
+  }
+
+  @Test
+  void testOpModeWithRobotBase() throws ReflectiveOperationException {
+    var ctor = ConstructorMatch.findBestConstructor(OpModeWithRobotBase.class, RobotBase.class);
+    assertTrue(ctor.isPresent());
+    var defaultUserControls = new DefaultUserControls();
+    var customUserControls = new CustomUserControls();
+    ctor.get().newInstance(new RobotWithNoUserControls(), defaultUserControls);
+    ctor.get()
+        .newInstance(new RobotWithDefaultUserControls(defaultUserControls), defaultUserControls);
+    ctor.get().newInstance(new RobotWithCustomUserControls(customUserControls), customUserControls);
+    ctor.get().newInstance(new RobotWithUserControls(defaultUserControls), defaultUserControls);
+  }
+
+  public static class OpModeWithRobotWithNoUserControls {
+    public OpModeWithRobotWithNoUserControls(RobotWithNoUserControls robot) {}
+  }
+
+  @Test
+  void testOpModeWithRobotWithNoUserControls() throws ReflectiveOperationException {
+    var ctor =
+        ConstructorMatch.findBestConstructor(
+            OpModeWithRobotWithNoUserControls.class, RobotWithNoUserControls.class);
+    assertTrue(ctor.isPresent());
+    var defaultUserControls = new DefaultUserControls();
+    ctor.get().newInstance(new RobotWithNoUserControls(), defaultUserControls);
+  }
+
+  public static class OpModeWithRobotWithDefaultUserControls {
+    public OpModeWithRobotWithDefaultUserControls(RobotWithDefaultUserControls robot) {}
+
+    public OpModeWithRobotWithDefaultUserControls(
+        RobotWithDefaultUserControls robot, DefaultUserControls controls) {}
+
+    public OpModeWithRobotWithDefaultUserControls(DefaultUserControls controls) {}
+  }
+
+  @Test
+  void testOpModeWithRobotWithDefaultUserControlsRobotArg() throws ReflectiveOperationException {
+    var ctor =
+        ConstructorMatch.findBestConstructor(
+            OpModeWithRobotWithDefaultUserControls.class, RobotWithDefaultUserControls.class);
+    assertTrue(ctor.isPresent());
+    var defaultUserControls = new DefaultUserControls();
+    ctor.get()
+        .newInstance(new RobotWithDefaultUserControls(defaultUserControls), defaultUserControls);
+  }
+
+  @Test
+  void testOpModeWithRobotWithDefaultUserControlsControlsArg() throws ReflectiveOperationException {
+    var ctor =
+        ConstructorMatch.findBestConstructor(
+            OpModeWithRobotWithDefaultUserControls.class, DefaultUserControls.class);
+    assertTrue(ctor.isPresent());
+    var defaultUserControls = new DefaultUserControls();
+    ctor.get()
+        .newInstance(new RobotWithDefaultUserControls(defaultUserControls), defaultUserControls);
+  }
+
+  @Test
+  void testOpModeWithRobotWithDefaultUserControlsNoArgs() throws ReflectiveOperationException {
+    var ctor =
+        ConstructorMatch.findBestConstructor(
+            OpModeWithRobotWithDefaultUserControls.class,
+            RobotWithDefaultUserControls.class,
+            DefaultUserControls.class);
+    assertTrue(ctor.isPresent());
+    var defaultUserControls = new DefaultUserControls();
+    ctor.get()
+        .newInstance(new RobotWithDefaultUserControls(defaultUserControls), defaultUserControls);
+  }
+
+  public static class MostSpecificFirstArg {
+    public MostSpecificFirstArg(RobotBase robot, DefaultUserControls controls) {}
+
+    public MostSpecificFirstArg(RobotWithDefaultUserControls robot, UserControls controls) {}
+  }
+
+  @Test
+  void testMostSpecificFirstArg() throws ReflectiveOperationException {
+    var ctor =
+        ConstructorMatch.findBestConstructor(
+            MostSpecificFirstArg.class,
+            RobotWithDefaultUserControls.class,
+            DefaultUserControls.class);
+    assertTrue(ctor.isPresent());
+    var parameterTypes = ctor.get().getParameterTypes();
+    assertEquals(RobotWithDefaultUserControls.class, parameterTypes.get(0));
+    assertEquals(UserControls.class, parameterTypes.get(1));
+  }
+
+  public static class MostSpecificSecondArg {
+    public MostSpecificSecondArg(RobotBase robot, DefaultUserControls controls) {}
+
+    public MostSpecificSecondArg(RobotBase robot, UserControls controls) {}
+  }
+
+  @Test
+  void testMostSpecificSecondArg() throws ReflectiveOperationException {
+    var ctor =
+        ConstructorMatch.findBestConstructor(
+            MostSpecificSecondArg.class,
+            RobotWithDefaultUserControls.class,
+            DefaultUserControls.class);
+    assertTrue(ctor.isPresent());
+    var parameterTypes = ctor.get().getParameterTypes();
+    assertEquals(RobotBase.class, parameterTypes.get(0));
+    assertEquals(DefaultUserControls.class, parameterTypes.get(1));
+  }
+}