[hal, wpilib] Add OpMode support (#7744)

User code:
- OpModeRobot used as the robot base class
- LinearOpMode and PeriodicOpMode are provided opmode base classes
- In Java, annotations can be used to automatically register opmode classes

Additional user code functionality:
- OpMode (string) is available in addition to the overall
auto/teleop/test robot mode
- OpMode does not indicate enable (enable/disable is still separate)
- The HAL API uses integer UIDs; these are exposed at the user API level
as well for faster checks
- User code creates opmodes on startup (these have name, category,
description, etc).

DS:
- DS will present opmode selection lists for auto and teleop for
match/practice. During a match, the DS will automatically activate the
selected opmode in the corresponding match period.
- For testing, an overall mode is selected (e.g. teleop/auto/test) and a
single opmode is selected

Future work:
- Command framework support/integration
- Python annotation support
- Unit tests (needs race-free DS sim updates)
- Porting of examples

Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com>

hal/src/main/native/cpp/jni/simulation/SimulatorJNI.cpp

@@ -7,6 +7,7 @@
 #include "BufferCallbackStore.h"
 #include "CallbackStore.h"
 #include "ConstBufferCallbackStore.h"
+#include "OpModeOptionsCallbackStore.h"
 #include "SimDeviceDataJNI.h"
 #include "org_wpilib_hardware_hal_simulation_SimulatorJNI.h"
 #include "wpi/hal/HAL.h"
@@ -20,9 +21,35 @@ static JavaVM* jvm = nullptr;
 static JClass notifyCallbackCls;
 static JClass bufferCallbackCls;
 static JClass constBufferCallbackCls;
+static JClass biConsumerCls;
 static jmethodID notifyCallbackCallback;
 static jmethodID bufferCallbackCallback;
 static jmethodID constBufferCallbackCallback;
+static jmethodID biConsumerCallback;
+
+static const JClassInit classes[] = {
+    {"org/wpilib/hardware/hal/simulation/NotifyCallback", &notifyCallbackCls},
+    {"org/wpilib/hardware/hal/simulation/BufferCallback", &bufferCallbackCls},
+    {"org/wpilib/hardware/hal/simulation/ConstBufferCallback",
+     &constBufferCallbackCls},
+    {"java/util/function/BiConsumer", &biConsumerCls},
+};
+
+static const struct JMethodInit {
+  JClass* cls;
+  const char* name;
+  const char* sig;
+  jmethodID* method;
+} methods[] = {
+    {&notifyCallbackCls, "callbackNative", "(Ljava/lang/String;IJD)V",
+     &notifyCallbackCallback},
+    {&bufferCallbackCls, "callback", "(Ljava/lang/String;[BI)V",
+     &bufferCallbackCallback},
+    {&constBufferCallbackCls, "callback", "(Ljava/lang/String;[BI)V",
+     &constBufferCallbackCallback},
+    {&biConsumerCls, "accept", "(Ljava/lang/Object;Ljava/lang/Object;)V",
+     &biConsumerCallback},
+};
 
 namespace wpi::hal::sim {
 jint SimOnLoad(JavaVM* vm, void* reserved) {
@@ -33,45 +60,24 @@ jint SimOnLoad(JavaVM* vm, void* reserved) {
     return JNI_ERR;
   }
 
-  notifyCallbackCls =
-      JClass(env, "org/wpilib/hardware/hal/simulation/NotifyCallback");
-  if (!notifyCallbackCls) {
-    return JNI_ERR;
+  for (auto& c : classes) {
+    *c.cls = JClass(env, c.name);
+    if (!*c.cls) {
+      return JNI_ERR;
+    }
   }
 
-  notifyCallbackCallback = env->GetMethodID(notifyCallbackCls, "callbackNative",
-                                            "(Ljava/lang/String;IJD)V");
-  if (!notifyCallbackCallback) {
-    return JNI_ERR;
-  }
-
-  bufferCallbackCls =
-      JClass(env, "org/wpilib/hardware/hal/simulation/BufferCallback");
-  if (!bufferCallbackCls) {
-    return JNI_ERR;
-  }
-
-  bufferCallbackCallback = env->GetMethodID(bufferCallbackCls, "callback",
-                                            "(Ljava/lang/String;[BI)V");
-  if (!bufferCallbackCallback) {
-    return JNI_ERR;
-  }
-
-  constBufferCallbackCls =
-      JClass(env, "org/wpilib/hardware/hal/simulation/ConstBufferCallback");
-  if (!constBufferCallbackCls) {
-    return JNI_ERR;
-  }
-
-  constBufferCallbackCallback = env->GetMethodID(
-      constBufferCallbackCls, "callback", "(Ljava/lang/String;[BI)V");
-  if (!constBufferCallbackCallback) {
-    return JNI_ERR;
+  for (auto& m : methods) {
+    *m.method = env->GetMethodID(*m.cls, m.name, m.sig);
+    if (!*m.method) {
+      return JNI_ERR;
+    }
   }
 
   InitializeStore();
   InitializeBufferStore();
   InitializeConstBufferStore();
+  InitializeOpModeOptionsStore();
   if (!InitializeSimDeviceDataJNI(env)) {
     return JNI_ERR;
   }
@@ -88,6 +94,7 @@ void SimOnUnload(JavaVM* vm, void* reserved) {
   notifyCallbackCls.free(env);
   bufferCallbackCls.free(env);
   constBufferCallbackCls.free(env);
+  biConsumerCls.free(env);
   FreeSimDeviceDataJNI(env);
   jvm = nullptr;
 }
@@ -108,6 +115,9 @@ jmethodID GetConstBufferCallback() {
   return constBufferCallbackCallback;
 }
 
+jmethodID GetBiConsumerCallback() {
+  return biConsumerCallback;
+}
 }  // namespace wpi::hal::sim
 
 extern "C" {
@@ -159,6 +169,32 @@ Java_org_wpilib_hardware_hal_simulation_SimulatorJNI_getProgramStarted
   return HALSIM_GetProgramStarted();
 }
 
+/*
+ * Class:     org_wpilib_hardware_hal_simulation_SimulatorJNI
+ * Method:    setProgramState
+ * Signature: (J)V
+ */
+JNIEXPORT void JNICALL
+Java_org_wpilib_hardware_hal_simulation_SimulatorJNI_setProgramState
+  (JNIEnv*, jclass, jlong word)
+{
+  HALSIM_SetProgramState({word});
+}
+
+/*
+ * Class:     org_wpilib_hardware_hal_simulation_SimulatorJNI
+ * Method:    nativeGetProgramState
+ * Signature: ()J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_wpilib_hardware_hal_simulation_SimulatorJNI_nativeGetProgramState
+  (JNIEnv*, jclass)
+{
+  HAL_ControlWord word;
+  HALSIM_GetProgramState(&word);
+  return word.value;
+}
+
 /*
  * Class:     org_wpilib_hardware_hal_simulation_SimulatorJNI
  * Method:    restartTiming