DriverStationBackend.java

// TODO - RobotCore - This file has been modified only to change the number of joystick ports.
 
// 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.internal;
 
import java.util.HashMap;
import java.util.Map;
import java.util.Optional;
import java.util.OptionalDouble;
import java.util.OptionalInt;
import java.util.concurrent.locks.ReentrantLock;
import java.util.stream.Collectors;
import org.wpilib.datalog.BooleanArrayLogEntry;
import org.wpilib.datalog.DataLog;
import org.wpilib.datalog.FloatArrayLogEntry;
import org.wpilib.datalog.IntegerArrayLogEntry;
import org.wpilib.datalog.StringLogEntry;
import org.wpilib.datalog.StructLogEntry;
import org.wpilib.driverstation.Alliance;
import org.wpilib.driverstation.MatchType;
import org.wpilib.driverstation.POVDirection;
import org.wpilib.driverstation.TouchpadFinger;
import org.wpilib.framework.OpModeRobot;
import org.wpilib.framework.RobotBase;
import org.wpilib.framework.TimedRobot;
import org.wpilib.hardware.hal.AllianceStationID;
import org.wpilib.hardware.hal.ControlWord;
import org.wpilib.hardware.hal.DriverStationJNI;
import org.wpilib.hardware.hal.HAL;
import org.wpilib.hardware.hal.MatchInfoData;
import org.wpilib.hardware.hal.OpModeOption;
import org.wpilib.hardware.hal.RobotMode;
import org.wpilib.networktables.BooleanPublisher;
import org.wpilib.networktables.IntegerPublisher;
import org.wpilib.networktables.NetworkTableInstance;
import org.wpilib.networktables.StringPublisher;
import org.wpilib.networktables.StringTopic;
import org.wpilib.networktables.StructPublisher;
import org.wpilib.util.Alert;
import org.wpilib.util.AlertException;
import org.wpilib.util.Color;
import org.wpilib.util.UsageReporting;
import org.wpilib.util.WPIUtilJNI;
import org.wpilib.util.concurrent.EventVector;

public final class DriverStationBackend {
  public static final int JOYSTICK_PORTS = 1;
 
  private static final long[] m_metadataCache = new long[7];
  private static final float[] m_touchpadFingersCache = new float[8];
 
  private static int availableToCount(long available) {
    // Top bit has to be set
    if (available < 0) {
      return 64;
    }
 
    int count = 0;
 
    // Top bit not set, we will eventually get a 0 bit
    while ((available & 0x1) != 0) {
      count++;
      available >>= 1;
    }
    return count;
  }
 
  private static final class HALJoystickTouchpadFinger {
    float m_x;
    float m_y;
    boolean m_down;
  }
 
  private static class HALJoystickTouchpad {
    final HALJoystickTouchpadFinger[] m_fingers =
        new HALJoystickTouchpadFinger[DriverStationJNI.MAX_JOYSTICK_TOUCHPAD_FINGERS];
    int m_count;
 
    HALJoystickTouchpad() {
      for (int i = 0; i < m_fingers.length; i++) {
        m_fingers[i] = new HALJoystickTouchpadFinger();
      }
    }
  }
 
  private static class HALJoystickTouchpads {
    final HALJoystickTouchpad[] m_touchpads =
        new HALJoystickTouchpad[DriverStationJNI.MAX_JOYSTICK_TOUCHPADS];
    int m_count;
 
    HALJoystickTouchpads() {
      for (int i = 0; i < m_touchpads.length; i++) {
        m_touchpads[i] = new HALJoystickTouchpad();
      }
    }
  }
 
  private static final class HALJoystickButtons {
    long m_buttons;
    long m_available;
  }
 
  private static class HALJoystickAxes {
    final float[] m_axes;
    int m_available;
 
    HALJoystickAxes(int count) {
      m_axes = new float[count];
    }
  }
 
  private static class HALJoystickAxesRaw {
    final short[] m_axes;
 
    @SuppressWarnings("unused")
    int m_available;
 
    HALJoystickAxesRaw(int count) {
      m_axes = new short[count];
    }
  }
 
  private static class HALJoystickPOVs {
    final byte[] m_povs;
    int m_available;
 
    HALJoystickPOVs(int count) {
      m_povs = new byte[count];
      for (int i = 0; i < count; i++) {
        m_povs[i] = 0;
      }
    }
  }
 
  private static String opModeToString(long id) {
    if (id == 0) {
      return "";
    }
    m_opModesMutex.lock();
    try {
      OpModeOption option = m_opModes.get(id);
      if (option != null) {
        return option.name;
      }
    } finally {
      m_opModesMutex.unlock();
    }
    return "<" + id + ">";
  }
 
  private static class MatchDataSender {
    private static final String SMART_DASHBOARD_TYPE = "DriverStation";
 
    final StringPublisher gameData;
    final StringPublisher eventName;
    final IntegerPublisher matchNumber;
    final IntegerPublisher replayNumber;
    final IntegerPublisher matchType;
    final BooleanPublisher alliance;
    final IntegerPublisher station;
    final IntegerPublisher allianceStation;
    final StructPublisher<ControlWord> controlWord;
    final StringPublisher opMode;
    String oldEventName = "";
    String oldGameData = "";
    int oldMatchNumber;
    int oldReplayNumber;
    int oldMatchType;
    boolean oldIsRedAlliance = true;
    int oldStationNumber = 1;
    int oldAllianceStation = 0;
    final ControlWord oldControlWord = new ControlWord();
    final ControlWord currentControlWord = new ControlWord();
 
    MatchDataSender() {
      var table = NetworkTableInstance.getDefault().getTable("DriverStation");
      table
          .getStringTopic(".type")
          .publishEx(
              StringTopic.TYPE_STRING, "{\"SmartDashboard\":\"" + SMART_DASHBOARD_TYPE + "\"}")
          .set(SMART_DASHBOARD_TYPE);
      gameData = table.getStringTopic("GameData").publish();
      gameData.set("");
      eventName = table.getStringTopic("EventName").publish();
      eventName.set("");
      matchNumber = table.getIntegerTopic("MatchNumber").publish();
      matchNumber.set(0);
      replayNumber = table.getIntegerTopic("ReplayNumber").publish();
      replayNumber.set(0);
      matchType = table.getIntegerTopic("MatchType").publish();
      matchType.set(0);
      alliance = table.getBooleanTopic("IsRedAlliance").publish();
      alliance.set(true);
      station = table.getIntegerTopic("StationNumber").publish();
      station.set(1);
      allianceStation = table.getIntegerTopic("AllianceStationID").publish();
      allianceStation.set(0);
      controlWord = table.getStructTopic("ControlWord", ControlWord.struct).publish();
      controlWord.set(oldControlWord);
      opMode = table.getStringTopic("OpMode").publish();
      opMode.set("");
    }
 
    @SuppressWarnings("VariableDeclarationUsageDistance")
    private void sendMatchData() {
      AllianceStationID allianceID = DriverStationJNI.getAllianceStation();
      final int stationNumber =
          switch (allianceID) {
            case BLUE_1, RED_1 -> 1;
            case BLUE_2, RED_2 -> 2;
            case BLUE_3, RED_3 -> 3;
            case UNKNOWN -> 0;
          };
      final boolean isRedAlliance =
          switch (allianceID) {
            case BLUE_1, BLUE_2, BLUE_3 -> false;
            case RED_1, RED_2, RED_3, UNKNOWN -> true;
          };
 
      String currentEventName;
      String currentGameData;
      int currentMatchNumber;
      int currentReplayNumber;
      int currentMatchType;
      m_cacheDataMutex.lock();
      try {
        currentEventName = DriverStationBackend.m_matchInfo.eventName;
        currentGameData = DriverStationBackend.m_gameData;
        currentMatchNumber = DriverStationBackend.m_matchInfo.matchNumber;
        currentReplayNumber = DriverStationBackend.m_matchInfo.replayNumber;
        currentMatchType = DriverStationBackend.m_matchInfo.matchType;
      } finally {
        m_cacheDataMutex.unlock();
      }
      DriverStationJNI.getControlWord(currentControlWord);
 
      if (oldIsRedAlliance != isRedAlliance) {
        alliance.set(isRedAlliance);
        oldIsRedAlliance = isRedAlliance;
      }
      if (oldStationNumber != stationNumber) {
        station.set(stationNumber);
        oldStationNumber = stationNumber;
      }
      if (oldAllianceStation != allianceID.ordinal()) {
        allianceStation.set(allianceID.ordinal());
        oldAllianceStation = allianceID.ordinal();
      }
      if (!oldEventName.equals(currentEventName)) {
        eventName.set(currentEventName);
        oldEventName = currentEventName;
      }
      if (!oldGameData.equals(currentGameData)) {
        gameData.set(currentGameData);
        oldGameData = currentGameData;
      }
      if (currentMatchNumber != oldMatchNumber) {
        matchNumber.set(currentMatchNumber);
        oldMatchNumber = currentMatchNumber;
      }
      if (currentReplayNumber != oldReplayNumber) {
        replayNumber.set(currentReplayNumber);
        oldReplayNumber = currentReplayNumber;
      }
      if (currentMatchType != oldMatchType) {
        matchType.set(currentMatchType);
        oldMatchType = currentMatchType;
      }
      if (!currentControlWord.equals(oldControlWord)) {
        long currentOpModeId = currentControlWord.getOpModeId();
        if (currentOpModeId != oldControlWord.getOpModeId()) {
          opMode.set(opModeToString(currentOpModeId));
        }
        controlWord.set(currentControlWord);
        oldControlWord.update(currentControlWord);
      }
    }
  }
 
  private static class JoystickLogSender {
    JoystickLogSender(DataLog log, int stick, long timestamp) {
      m_stick = stick;
 
      m_logButtons = new BooleanArrayLogEntry(log, "DS:joystick" + stick + "/buttons", timestamp);
      m_logAxes = new FloatArrayLogEntry(log, "DS:joystick" + stick + "/axes", timestamp);
      m_logPOVs = new IntegerArrayLogEntry(log, "DS:joystick" + stick + "/povs", timestamp);
 
      appendButtons(m_joystickButtons[m_stick], timestamp);
      appendAxes(m_joystickAxes[m_stick], timestamp);
      appendPOVs(m_joystickPOVs[m_stick], timestamp);
    }
 
    public void send(long timestamp) {
      HALJoystickButtons buttons = m_joystickButtons[m_stick];
      if (buttons.m_available != m_prevButtons.m_available
          || buttons.m_buttons != m_prevButtons.m_buttons) {
        appendButtons(buttons, timestamp);
      }
 
      HALJoystickAxes axes = m_joystickAxes[m_stick];
      int available = axes.m_available;
      boolean needToLog = false;
      if (available != m_prevAxes.m_available) {
        needToLog = true;
      } else {
        for (int i = 0; i < m_prevAxes.m_axes.length; i++) {
          if (axes.m_axes[i] != m_prevAxes.m_axes[i]) {
            needToLog = true;
            break;
          }
        }
      }
      if (needToLog) {
        appendAxes(axes, timestamp);
      }
 
      HALJoystickPOVs povs = m_joystickPOVs[m_stick];
      available = m_joystickPOVs[m_stick].m_available;
      needToLog = false;
      if (available != m_prevPOVs.m_available) {
        needToLog = true;
      } else {
        for (int i = 0; i < m_prevPOVs.m_povs.length; i++) {
          if (povs.m_povs[i] != m_prevPOVs.m_povs[i]) {
            needToLog = true;
            break;
          }
        }
      }
      if (needToLog) {
        appendPOVs(povs, timestamp);
      }
    }
 
    void appendButtons(HALJoystickButtons buttons, long timestamp) {
      int count = availableToCount(buttons.m_available);
      if (m_sizedButtons == null || m_sizedButtons.length != count) {
        m_sizedButtons = new boolean[count];
      }
      long buttonsValue = buttons.m_buttons;
      for (int i = 0; i < count; i++) {
        m_sizedButtons[i] = (buttonsValue & (1L << i)) != 0;
      }
      m_logButtons.append(m_sizedButtons, timestamp);
      m_prevButtons.m_available = buttons.m_available;
      m_prevButtons.m_buttons = buttons.m_buttons;
    }
 
    void appendAxes(HALJoystickAxes axes, long timestamp) {
      int count = availableToCount(axes.m_available);
      if (m_sizedAxes == null || m_sizedAxes.length != count) {
        m_sizedAxes = new float[count];
      }
      System.arraycopy(axes.m_axes, 0, m_sizedAxes, 0, count);
      m_logAxes.append(m_sizedAxes, timestamp);
      m_prevAxes.m_available = axes.m_available;
      System.arraycopy(axes.m_axes, 0, m_prevAxes.m_axes, 0, count);
    }
 
    void appendPOVs(HALJoystickPOVs povs, long timestamp) {
      int count = availableToCount(povs.m_available);
      if (m_sizedPOVs == null || m_sizedPOVs.length != count) {
        m_sizedPOVs = new long[count];
      }
      for (int i = 0; i < count; i++) {
        m_sizedPOVs[i] = povs.m_povs[i];
      }
      m_logPOVs.append(m_sizedPOVs, timestamp);
      m_prevPOVs.m_available = povs.m_available;
      System.arraycopy(povs.m_povs, 0, m_prevPOVs.m_povs, 0, count);
    }
 
    final int m_stick;
    boolean[] m_sizedButtons;
    float[] m_sizedAxes;
    long[] m_sizedPOVs;
    final HALJoystickButtons m_prevButtons = new HALJoystickButtons();
    final HALJoystickAxes m_prevAxes = new HALJoystickAxes(DriverStationJNI.MAX_JOYSTICK_AXES);
    final HALJoystickPOVs m_prevPOVs = new HALJoystickPOVs(DriverStationJNI.MAX_JOYSTICK_POVS);
    final BooleanArrayLogEntry m_logButtons;
    final FloatArrayLogEntry m_logAxes;
    final IntegerArrayLogEntry m_logPOVs;
  }
 
  private static class DataLogSender {
    DataLogSender(DataLog log, boolean logJoysticks, long timestamp) {
      m_logControlWord =
          StructLogEntry.create(log, "DS:controlWord", ControlWord.struct, timestamp);
 
      // append initial control word value
      m_logControlWord.append(m_controlWordCache, timestamp);
      m_oldControlWord.update(m_controlWordCache);
 
      // append initial opmode value
      m_logOpMode = new StringLogEntry(log, "DS:opMode", timestamp);
      m_logOpMode.append(m_opModeCache, timestamp);
 
      if (logJoysticks) {
        m_joysticks = new JoystickLogSender[JOYSTICK_PORTS];
        for (int i = 0; i < JOYSTICK_PORTS; i++) {
          m_joysticks[i] = new JoystickLogSender(log, i, timestamp);
        }
      } else {
        m_joysticks = new JoystickLogSender[0];
      }
    }
 
    public void send(long timestamp) {
      // append control word value changes
      if (!m_controlWordCache.equals(m_oldControlWord)) {
        // append opmode value changes
        long opModeId = m_controlWordCache.getOpModeId();
        if (opModeId != m_oldControlWord.getOpModeId()) {
          m_logOpMode.append(m_opModeCache, timestamp);
        }
 
        m_logControlWord.append(m_controlWordCache, timestamp);
        m_oldControlWord.update(m_controlWordCache);
      }
 
      // append joystick value changes
      for (JoystickLogSender joystick : m_joysticks) {
        joystick.send(timestamp);
      }
    }
 
    final ControlWord m_oldControlWord = new ControlWord();
    final StructLogEntry<ControlWord> m_logControlWord;
    final StringLogEntry m_logOpMode;
 
    final JoystickLogSender[] m_joysticks;
  }
 
  // Joystick User Data
  private static HALJoystickAxes[] m_joystickAxes = new HALJoystickAxes[JOYSTICK_PORTS];
  private static HALJoystickAxesRaw[] m_joystickAxesRaw = new HALJoystickAxesRaw[JOYSTICK_PORTS];
  private static HALJoystickPOVs[] m_joystickPOVs = new HALJoystickPOVs[JOYSTICK_PORTS];
  private static HALJoystickButtons[] m_joystickButtons = new HALJoystickButtons[JOYSTICK_PORTS];
  private static HALJoystickTouchpads[] m_joystickTouchpads =
      new HALJoystickTouchpads[JOYSTICK_PORTS];
  private static MatchInfoData m_matchInfo = new MatchInfoData();
  private static String m_gameData = "";
  private static ControlWord m_controlWord = new ControlWord();
  private static String m_opMode = "";
  private static EventVector m_refreshEvents = new EventVector();
 
  // Joystick Cached Data
  private static HALJoystickAxes[] m_joystickAxesCache = new HALJoystickAxes[JOYSTICK_PORTS];
  private static HALJoystickAxesRaw[] m_joystickAxesRawCache =
      new HALJoystickAxesRaw[JOYSTICK_PORTS];
  private static HALJoystickPOVs[] m_joystickPOVsCache = new HALJoystickPOVs[JOYSTICK_PORTS];
  private static HALJoystickButtons[] m_joystickButtonsCache =
      new HALJoystickButtons[JOYSTICK_PORTS];
  private static HALJoystickTouchpads[] m_joystickTouchpadsCache =
      new HALJoystickTouchpads[JOYSTICK_PORTS];
  private static MatchInfoData m_matchInfoCache = new MatchInfoData();
  private static String m_gameDataCache = "";
  private static ControlWord m_controlWordCache = new ControlWord();
 
  private static String m_opModeCache = "";
 
  // Joystick button rising/falling edge flags
  private static long[] m_joystickButtonsPressed = new long[JOYSTICK_PORTS];
  private static long[] m_joystickButtonsReleased = new long[JOYSTICK_PORTS];
 
  private static final MatchDataSender m_matchDataSender;
  private static DataLogSender m_dataLogSender;
 
  private static final ReentrantLock m_cacheDataMutex = new ReentrantLock();
 
  private enum JoystickResourceAlert {
    BUTTON("ButtonUnavailable", "Button"),
    AXIS("AxisUnavailable", "axis"),
    POV("POVUnavailable", "POV");
 
    final String idSuffix;
    final String label;
 
    JoystickResourceAlert(String idSuffix, String label) {
      this.idSuffix = idSuffix;
      this.label = label;
    }
 
    boolean isAvailable(int stick, int resource) {
      return switch (this) {
        case BUTTON -> (getStickButtonsAvailable(stick) & (1L << resource)) != 0;
        case AXIS -> (getStickAxesAvailable(stick) & (1 << resource)) != 0;
        case POV -> (getStickPOVsAvailable(stick) & (1 << resource)) != 0;
      };
    }
 
    String getText(int stick, int resource) {
      return "Joystick " + label + " " + resource + " on port " + stick + " not available";
    }
  }
 
  private static final JoystickResourceAlert[] JOYSTICK_RESOURCE_ALERTS =
      JoystickResourceAlert.values();
 
  private static final class JoystickAlertState {
    Alert alert;
    boolean active;
  }
 
  private static final class JoystickAlerts {
    final int stick;
    final String connectionText;
    final JoystickAlertState connectionAlert = new JoystickAlertState();
    final JoystickAlertState[] resourceAlerts =
        new JoystickAlertState[JOYSTICK_RESOURCE_ALERTS.length];
    final JoystickAlertState touchpadFingerAlert = new JoystickAlertState();
    final int[] resourceAlertValues = new int[JOYSTICK_RESOURCE_ALERTS.length];
    int touchpadFingerAlertTouchpad;
    int touchpadFingerAlertFinger;
    boolean initialized;
 
    JoystickAlerts(int stick) {
      this.stick = stick;
      connectionText =
          "Joystick on port " + stick + " not available, check if controller is plugged in";
      for (int i = 0; i < resourceAlerts.length; i++) {
        resourceAlerts[i] = new JoystickAlertState();
      }
      initializeAlerts();
    }
 
    boolean initializeAlerts() {
      closeAlerts();
 
      Alert newConnectionAlert = null;
      Alert[] newResourceAlerts = new Alert[JOYSTICK_RESOURCE_ALERTS.length];
      Alert newTouchpadFingerAlert = null;
 
      try {
        newConnectionAlert =
            new Alert(
                "DriverStation",
                "joystick" + stick + "Disconnected",
                connectionText,
                Alert.Level.HIGH);
        for (JoystickResourceAlert resourceAlert : JOYSTICK_RESOURCE_ALERTS) {
          newResourceAlerts[resourceAlert.ordinal()] =
              new Alert(
                  "DriverStation",
                  "joystick" + stick + resourceAlert.idSuffix,
                  "",
                  Alert.Level.MEDIUM);
        }
        newTouchpadFingerAlert =
            new Alert(
                "DriverStation",
                "joystick" + stick + "TouchpadFingerUnavailable",
                "",
                Alert.Level.MEDIUM);
      } catch (AlertException ex) {
        closeAlert(newConnectionAlert);
        for (Alert alert : newResourceAlerts) {
          closeAlert(alert);
        }
        closeAlert(newTouchpadFingerAlert);
        initialized = false;
        return false;
      }
 
      connectionAlert.alert = newConnectionAlert;
      for (JoystickResourceAlert resourceAlert : JOYSTICK_RESOURCE_ALERTS) {
        resourceAlerts[resourceAlert.ordinal()].alert = newResourceAlerts[resourceAlert.ordinal()];
      }
      touchpadFingerAlert.alert = newTouchpadFingerAlert;
      initialized = true;
      return true;
    }
 
    void closeAlerts() {
      closeAlert(connectionAlert);
      for (JoystickAlertState alertState : resourceAlerts) {
        closeAlert(alertState);
      }
      closeAlert(touchpadFingerAlert);
      initialized = false;
    }
 
    static void closeAlert(JoystickAlertState alertState) {
      closeAlert(alertState.alert);
      alertState.alert = null;
      alertState.active = false;
    }
 
    static void closeAlert(Alert alert) {
      if (alert == null) {
        return;
      }
 
      try {
        alert.close();
      } catch (AlertException ex) {
        // Ignore stale handles while cleaning up a failed initialization attempt.
      }
    }
 
    boolean prepareForReport() {
      if (isJoystickConnected(stick)) {
        setAlert(connectionAlert, false, connectionText, true);
        return true;
      }
 
      clearResourceAlerts();
      setAlert(connectionAlert, true, connectionText, true);
      return false;
    }
 
    void refresh() {
      if (!isJoystickConnected(stick)) {
        clearResourceAlerts();
        return;
      }
 
      if (connectionAlert.active) {
        setAlert(connectionAlert, false, connectionText, true);
      }
 
      for (JoystickResourceAlert resourceAlert : JOYSTICK_RESOURCE_ALERTS) {
        refreshResourceAlert(resourceAlert);
      }
 
      if (touchpadFingerAlert.active
          && getStickTouchpadFingerAvailable(
              stick, touchpadFingerAlertTouchpad, touchpadFingerAlertFinger)) {
        setAlert(touchpadFingerAlert, false);
      }
    }
 
    void clearAllAlerts() {
      setAlert(connectionAlert, false, connectionText, true);
      clearResourceAlerts();
    }
 
    void clearResourceAlerts() {
      for (JoystickAlertState alertState : resourceAlerts) {
        setAlert(alertState, false);
      }
      setAlert(touchpadFingerAlert, false);
    }
 
    void refreshResourceAlert(JoystickResourceAlert resourceAlert) {
      int alertIndex = resourceAlert.ordinal();
      JoystickAlertState alertState = resourceAlerts[alertIndex];
      if (alertState.active && resourceAlert.isAvailable(stick, resourceAlertValues[alertIndex])) {
        setAlert(alertState, false);
      }
    }
 
    void reportResourceWarning(JoystickResourceAlert resourceAlert, int resource) {
      int alertIndex = resourceAlert.ordinal();
      resourceAlertValues[alertIndex] = resource;
      setAlert(resourceAlerts[alertIndex], true, resourceAlert.getText(stick, resource), false);
    }
 
    void reportTouchpadFingerWarning(int touchpad, int finger) {
      String text =
          "Joystick touchpad finger "
              + finger
              + " on touchpad "
              + touchpad
              + " on port "
              + stick
              + " not available";
      touchpadFingerAlertTouchpad = touchpad;
      touchpadFingerAlertFinger = finger;
      setAlert(touchpadFingerAlert, true, text, false);
    }
 
    boolean applyAlert(
        JoystickAlertState alertState, boolean active, String text, boolean verifyActive) {
      if (alertState.alert == null) {
        return false;
      }
 
      try {
        if (active && !text.isEmpty()) {
          alertState.alert.setText(text);
        }
        alertState.alert.set(active);
        return !active || !verifyActive || alertState.alert.get();
      } catch (AlertException ex) {
        return false;
      }
    }
 
    boolean setAlert(JoystickAlertState alertState, boolean active) {
      return setAlert(alertState, active, "", false);
    }
 
    boolean setAlert(
        JoystickAlertState alertState, boolean active, String text, boolean verifyActive) {
      if (!active && !alertState.active) {
        return true;
      }
 
      if (active && !initialized && !initializeAlerts()) {
        return false;
      }
 
      if (!applyAlert(alertState, active, text, verifyActive)) {
        if (!active) {
          alertState.active = false;
          return true;
        }
        if (!initializeAlerts()) {
          return false;
        }
        if (!applyAlert(alertState, active, text, verifyActive)) {
          return false;
        }
      }
      alertState.active = active;
      return true;
    }
  }
 
  private static final ReentrantLock m_joystickAlertMutex = new ReentrantLock();
  private static boolean m_silenceJoystickAlerts;
  private static final JoystickAlerts[] m_joystickAlerts = new JoystickAlerts[JOYSTICK_PORTS];
 
  private static volatile boolean m_userProgramStarted = false;
  private static final Map<Long, OpModeOption> m_opModes = new HashMap<>();
  private static final ReentrantLock m_opModesMutex = new ReentrantLock();

  private DriverStationBackend() {}
 
  static {
    HAL.initialize();
 
    for (int i = 0; i < JOYSTICK_PORTS; i++) {
      m_joystickButtons[i] = new HALJoystickButtons();
      m_joystickAxes[i] = new HALJoystickAxes(DriverStationJNI.MAX_JOYSTICK_AXES);
      m_joystickAxesRaw[i] = new HALJoystickAxesRaw(DriverStationJNI.MAX_JOYSTICK_AXES);
      m_joystickPOVs[i] = new HALJoystickPOVs(DriverStationJNI.MAX_JOYSTICK_POVS);
      m_joystickTouchpads[i] = new HALJoystickTouchpads();
 
      m_joystickButtonsCache[i] = new HALJoystickButtons();
      m_joystickAxesCache[i] = new HALJoystickAxes(DriverStationJNI.MAX_JOYSTICK_AXES);
      m_joystickAxesRawCache[i] = new HALJoystickAxesRaw(DriverStationJNI.MAX_JOYSTICK_AXES);
      m_joystickPOVsCache[i] = new HALJoystickPOVs(DriverStationJNI.MAX_JOYSTICK_POVS);
      m_joystickTouchpadsCache[i] = new HALJoystickTouchpads();
    }
 
    m_matchDataSender = new MatchDataSender();
  }

  public static void reportError(String error, boolean printTrace) {
    reportErrorImpl(true, 1, error, printTrace);
  }

  public static void reportError(String error, StackTraceElement[] stackTrace) {
    reportErrorImpl(true, 1, error, stackTrace);
  }

  public static void reportWarning(String warning, boolean printTrace) {
    reportErrorImpl(false, 1, warning, printTrace);
  }

  public static void reportWarning(String warning, StackTraceElement[] stackTrace) {
    reportErrorImpl(false, 1, warning, stackTrace);
  }
 
  private static void reportErrorImpl(boolean isError, int code, String error, boolean printTrace) {
    reportErrorImpl(isError, code, error, printTrace, Thread.currentThread().getStackTrace(), 3);
  }
 
  private static void reportErrorImpl(
      boolean isError, int code, String error, StackTraceElement[] stackTrace) {
    reportErrorImpl(isError, code, error, true, stackTrace, 0);
  }
 
  private static void reportErrorImpl(
      boolean isError,
      int code,
      String error,
      boolean printTrace,
      StackTraceElement[] stackTrace,
      int stackTraceFirst) {
    String locString;
    if (stackTrace.length >= stackTraceFirst + 1) {
      locString = stackTrace[stackTraceFirst].toString();
    } else {
      locString = "";
    }
    StringBuilder traceString = new StringBuilder();
    if (printTrace) {
      boolean haveLoc = false;
      for (int i = stackTraceFirst; i < stackTrace.length; i++) {
        String loc = stackTrace[i].toString();
        traceString.append("\tat ").append(loc).append('\n');
        // get first user function
        if (!haveLoc && !loc.startsWith("org.wpilib")) {
          locString = loc;
          haveLoc = true;
        }
      }
    }
    DriverStationJNI.sendError(isError, code, error, locString, traceString.toString(), true);
  }

  public static void reportCrash(String error, StackTraceElement[] stackTrace) {
    reportCrashImpl(error, stackTrace, 0);
  }
 
  private static void reportCrashImpl(
      String error, StackTraceElement[] stackTrace, int stackTraceFirst) {
    String locString;
    if (stackTrace.length >= stackTraceFirst + 1) {
      locString = stackTrace[stackTraceFirst].toString();
    } else {
      locString = "";
    }
    StringBuilder traceString = new StringBuilder();
    boolean haveLoc = false;
    for (int i = stackTraceFirst; i < stackTrace.length; i++) {
      String loc = stackTrace[i].toString();
      traceString.append("\tat ").append(loc).append('\n');
      // get first user function
      if (!haveLoc && !loc.startsWith("org.wpilib")) {
        locString = loc;
        haveLoc = true;
      }
    }
    DriverStationJNI.sendProgramCrash(error, locString, traceString.toString());
    // Sleep to ensure message is sent before crash
    try {
      Thread.sleep(100);
    } catch (InterruptedException e) {
      Thread.currentThread().interrupt();
    }
  }

  public static boolean getStickButton(final int stick, final int button) {
    if (stick < 0 || stick >= JOYSTICK_PORTS) {
      throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
    }
    if (button < 0 || button >= DriverStationJNI.MAX_JOYSTICK_BUTTONS) {
      throw new IllegalArgumentException("Joystick Button is out of range");
    }
 
    long mask = 1L << button;
 
    m_cacheDataMutex.lock();
    try {
      if ((m_joystickButtons[stick].m_available & mask) != 0) {
        return (m_joystickButtons[stick].m_buttons & mask) != 0;
      }
    } finally {
      m_cacheDataMutex.unlock();
    }
 
    reportJoystickResourceWarning(stick, JoystickResourceAlert.BUTTON, button);
    return false;
  }

  public static Optional<Boolean> getStickButtonIfAvailable(final int stick, final int button) {
    if (stick < 0 || stick >= JOYSTICK_PORTS) {
      throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
    }
    if (button < 0 || button >= DriverStationJNI.MAX_JOYSTICK_BUTTONS) {
      throw new IllegalArgumentException("Joystick Button is out of range");
    }
 
    long mask = 1L << button;
 
    m_cacheDataMutex.lock();
    try {
      if ((m_joystickButtons[stick].m_available & mask) != 0) {
        return Optional.of((m_joystickButtons[stick].m_buttons & mask) != 0);
      }
    } finally {
      m_cacheDataMutex.unlock();
    }
    return Optional.empty();
  }

  public static boolean getStickButtonPressed(final int stick, final int button) {
    if (stick < 0 || stick >= JOYSTICK_PORTS) {
      throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
    }
    if (button < 0 || button >= DriverStationJNI.MAX_JOYSTICK_BUTTONS) {
      throw new IllegalArgumentException("Joystick Button is out of range");
    }
 
    long mask = 1L << button;
 
    m_cacheDataMutex.lock();
    try {
      if ((m_joystickButtons[stick].m_available & mask) != 0) {
        // If button was pressed, clear flag and return true
        if ((m_joystickButtonsPressed[stick] & mask) != 0) {
          m_joystickButtonsPressed[stick] &= ~mask;
          return true;
        } else {
          return false;
        }
      }
    } finally {
      m_cacheDataMutex.unlock();
    }
 
    reportJoystickResourceWarning(stick, JoystickResourceAlert.BUTTON, button);
    return false;
  }

  public static boolean getStickButtonReleased(final int stick, final int button) {
    if (stick < 0 || stick >= JOYSTICK_PORTS) {
      throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
    }
    if (button < 0 || button >= DriverStationJNI.MAX_JOYSTICK_BUTTONS) {
      throw new IllegalArgumentException("Joystick Button is out of range");
    }
 
    long mask = 1L << button;
 
    m_cacheDataMutex.lock();
    try {
      if ((m_joystickButtons[stick].m_available & mask) != 0) {
        // If button was released, clear flag and return true
        if ((m_joystickButtonsReleased[stick] & mask) != 0) {
          m_joystickButtonsReleased[stick] &= ~mask;
          return true;
        } else {
          return false;
        }
      }
    } finally {
      m_cacheDataMutex.unlock();
    }
 
    reportJoystickResourceWarning(stick, JoystickResourceAlert.BUTTON, button);
    return false;
  }

  public static double getStickAxis(int stick, int axis) {
    if (stick < 0 || stick >= JOYSTICK_PORTS) {
      throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
    }
    if (axis < 0 || axis >= DriverStationJNI.MAX_JOYSTICK_AXES) {
      throw new IllegalArgumentException("Joystick axis is out of range");
    }
 
    int mask = 1 << axis;
 
    m_cacheDataMutex.lock();
    try {
      if ((m_joystickAxes[stick].m_available & mask) != 0) {
        return m_joystickAxes[stick].m_axes[axis];
      }
    } finally {
      m_cacheDataMutex.unlock();
    }
 
    reportJoystickResourceWarning(stick, JoystickResourceAlert.AXIS, axis);
    return 0.0;
  }

  public static TouchpadFinger getStickTouchpadFinger(int stick, int touchpad, int finger) {
    if (stick < 0 || stick >= JOYSTICK_PORTS) {
      throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
    }
    if (touchpad < 0 || touchpad >= DriverStationJNI.MAX_JOYSTICK_TOUCHPADS) {
      throw new IllegalArgumentException("Joystick touchpad is out of range");
    }
    if (finger < 0 || finger >= DriverStationJNI.MAX_JOYSTICK_TOUCHPAD_FINGERS) {
      throw new IllegalArgumentException("Joystick touchpad finger is out of range");
    }
 
    int touchpadCount;
    m_cacheDataMutex.lock();
    try {
      touchpadCount = m_joystickTouchpads[stick].m_count;
      if (touchpad < touchpadCount) {
        HALJoystickTouchpad tp = m_joystickTouchpads[stick].m_touchpads[touchpad];
        if (finger < tp.m_count) {
          return new TouchpadFinger(
              tp.m_fingers[finger].m_down, tp.m_fingers[finger].m_x, tp.m_fingers[finger].m_y);
        }
      }
    } finally {
      m_cacheDataMutex.unlock();
    }
 
    reportJoystickTouchpadFingerWarning(stick, touchpad, finger);
    return new TouchpadFinger(false, 0.0f, 0.0f);
  }

  public static boolean getStickTouchpadFingerAvailable(int stick, int touchpad, int finger) {
    if (stick < 0 || stick >= JOYSTICK_PORTS) {
      throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
    }
    if (touchpad < 0 || touchpad >= DriverStationJNI.MAX_JOYSTICK_TOUCHPADS) {
      throw new IllegalArgumentException("Joystick touchpad is out of range");
    }
    if (finger < 0 || finger >= DriverStationJNI.MAX_JOYSTICK_TOUCHPAD_FINGERS) {
      throw new IllegalArgumentException("Joystick touchpad finger is out of range");
    }
 
    int touchpadCount;
    m_cacheDataMutex.lock();
    try {
      touchpadCount = m_joystickTouchpads[stick].m_count;
      if (touchpad < touchpadCount) {
        HALJoystickTouchpad tp = m_joystickTouchpads[stick].m_touchpads[touchpad];
        if (finger < tp.m_count) {
          return true;
        }
      }
    } finally {
      m_cacheDataMutex.unlock();
    }
 
    return false;
  }

  public static OptionalDouble getStickAxisIfAvailable(int stick, int axis) {
    if (stick < 0 || stick >= JOYSTICK_PORTS) {
      throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
    }
    if (axis < 0 || axis >= DriverStationJNI.MAX_JOYSTICK_AXES) {
      throw new IllegalArgumentException("Joystick axis is out of range");
    }
 
    int mask = 1 << axis;
 
    m_cacheDataMutex.lock();
    try {
      if ((m_joystickAxes[stick].m_available & mask) != 0) {
        return OptionalDouble.of(m_joystickAxes[stick].m_axes[axis]);
      }
    } finally {
      m_cacheDataMutex.unlock();
    }
 
    return OptionalDouble.empty();
  }

  public static POVDirection getStickPOV(int stick, int pov) {
    if (stick < 0 || stick >= JOYSTICK_PORTS) {
      throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
    }
    if (pov < 0 || pov >= DriverStationJNI.MAX_JOYSTICK_POVS) {
      throw new IllegalArgumentException("Joystick POV is out of range");
    }
 
    int mask = 1 << pov;
 
    m_cacheDataMutex.lock();
    try {
      if ((m_joystickPOVs[stick].m_available & mask) != 0) {
        return POVDirection.of(m_joystickPOVs[stick].m_povs[pov]);
      }
    } finally {
      m_cacheDataMutex.unlock();
    }
 
    reportJoystickResourceWarning(stick, JoystickResourceAlert.POV, pov);
    return POVDirection.CENTER;
  }

  public static long getStickButtons(final int stick) {
    if (stick < 0 || stick >= JOYSTICK_PORTS) {
      throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
    }
 
    m_cacheDataMutex.lock();
    try {
      return m_joystickButtons[stick].m_buttons;
    } finally {
      m_cacheDataMutex.unlock();
    }
  }

  public static int getStickAxesMaximumIndex(int stick) {
    return availableToCount(getStickAxesAvailable(stick));
  }

  public static int getStickAxesAvailable(int stick) {
    if (stick < 0 || stick >= JOYSTICK_PORTS) {
      throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
    }
 
    m_cacheDataMutex.lock();
    try {
      return m_joystickAxes[stick].m_available;
    } finally {
      m_cacheDataMutex.unlock();
    }
  }

  public static int getStickPOVsMaximumIndex(int stick) {
    return availableToCount(getStickPOVsAvailable(stick));
  }

  public static int getStickPOVsAvailable(int stick) {
    if (stick < 0 || stick >= JOYSTICK_PORTS) {
      throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
    }
 
    m_cacheDataMutex.lock();
    try {
      return m_joystickPOVs[stick].m_available;
    } finally {
      m_cacheDataMutex.unlock();
    }
  }

  public static int getStickButtonsMaximumIndex(int stick) {
    return availableToCount(getStickButtonsAvailable(stick));
  }

  public static long getStickButtonsAvailable(int stick) {
    if (stick < 0 || stick >= JOYSTICK_PORTS) {
      throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
    }
 
    m_cacheDataMutex.lock();
    try {
      return m_joystickButtons[stick].m_available;
    } finally {
      m_cacheDataMutex.unlock();
    }
  }

  public static boolean getJoystickIsGamepad(int stick) {
    if (stick < 0 || stick >= JOYSTICK_PORTS) {
      throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
    }
 
    return DriverStationJNI.getJoystickIsGamepad((byte) stick) == 1;
  }

  public static int getJoystickGamepadType(int stick) {
    if (stick < 0 || stick >= JOYSTICK_PORTS) {
      throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
    }
 
    return DriverStationJNI.getJoystickGamepadType((byte) stick);
  }

  public static int getJoystickSupportedOutputs(int stick) {
    if (stick < 0 || stick >= JOYSTICK_PORTS) {
      throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
    }
 
    return DriverStationJNI.getJoystickSupportedOutputs((byte) stick);
  }

  public static String getJoystickName(int stick) {
    if (stick < 0 || stick >= JOYSTICK_PORTS) {
      throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
    }
 
    return DriverStationJNI.getJoystickName((byte) stick);
  }

  public static boolean isJoystickConnected(int stick) {
    return getStickAxesAvailable(stick) != 0
        || getStickButtonsAvailable(stick) != 0
        || getStickPOVsAvailable(stick) != 0;
  }

  public static boolean isEnabled() {
    m_cacheDataMutex.lock();
    try {
      return m_controlWord.isEnabled() && m_controlWord.isDSAttached();
    } finally {
      m_cacheDataMutex.unlock();
    }
  }

  public static boolean isDisabled() {
    return !isEnabled();
  }

  public static boolean isEStopped() {
    m_cacheDataMutex.lock();
    try {
      return m_controlWord.isEStopped();
    } finally {
      m_cacheDataMutex.unlock();
    }
  }

  public static RobotMode getRobotMode() {
    if (!m_userProgramStarted) {
      return RobotMode.UNKNOWN;
    }
 
    m_cacheDataMutex.lock();
    try {
      return m_controlWord.getRobotMode();
    } finally {
      m_cacheDataMutex.unlock();
    }
  }

  public static boolean isAutonomous() {
    return getRobotMode() == RobotMode.AUTONOMOUS;
  }

  public static boolean isAutonomousEnabled() {
    m_cacheDataMutex.lock();
    try {
      return isAutonomous() && isEnabled();
    } finally {
      m_cacheDataMutex.unlock();
    }
  }

  public static boolean isTeleop() {
    return getRobotMode() == RobotMode.TELEOPERATED;
  }

  public static boolean isTeleopEnabled() {
    m_cacheDataMutex.lock();
    try {
      return isTeleop() && isEnabled();
    } finally {
      m_cacheDataMutex.unlock();
    }
  }

  public static boolean isUtility() {
    return getRobotMode() == RobotMode.UTILITY;
  }

  public static boolean isUtilityEnabled() {
    m_cacheDataMutex.lock();
    try {
      return isUtility() && isEnabled();
    } finally {
      m_cacheDataMutex.unlock();
    }
  }
 
  private static int convertColorToInt(Color color) {
    if (color == null) {
      return -1;
    }
    return (((int) (color.red * 255) & 0xff) << 16)
        | (((int) (color.green * 255) & 0xff) << 8)
        | ((int) (color.blue * 255) & 0xff);
  }

  @SuppressWarnings("PMD.UseStringBufferForStringAppends")
  public static long addOpMode(
      RobotMode mode,
      String name,
      String group,
      String description,
      Color textColor,
      Color backgroundColor) {
    if (name.isBlank()) {
      throw new IllegalArgumentException("OpMode name must be non-blank");
    }
    // find unique ID
    m_opModesMutex.lock();
    try {
      String nameCopy = name;
      for (; ; ) {
        long id = OpModeOption.makeId(mode, nameCopy.hashCode());
        OpModeOption existing = m_opModes.get(id);
        if (existing == null) {
          m_opModes.put(
              id,
              new OpModeOption(
                  id,
                  name,
                  group,
                  description,
                  convertColorToInt(textColor),
                  convertColorToInt(backgroundColor)));
          return id;
        }
        if (existing.getMode() == mode && existing.name.equals(name)) {
          // already exists
          throw new IllegalArgumentException("OpMode " + name + " already exists for mode " + mode);
        }
        // collision, try again with space appended
        nameCopy += ' ';
      }
    } finally {
      m_opModesMutex.unlock();
    }
  }

  public static long addOpMode(RobotMode mode, String name, String group, String description) {
    return addOpMode(mode, name, group, description, null, null);
  }

  public static long addOpMode(RobotMode mode, String name, String group) {
    return addOpMode(mode, name, group, "");
  }

  public static long addOpMode(RobotMode mode, String name) {
    return addOpMode(mode, name, "");
  }

  public static long removeOpMode(RobotMode mode, String name) {
    if (name.isBlank()) {
      return 0;
    }
    m_opModesMutex.lock();
    try {
      // we have to loop over all entries to find the one with the correct name
      // because the of the unique ID generation scheme
      for (OpModeOption opMode : m_opModes.values()) {
        if (opMode.getMode() == mode && opMode.name.equals(name)) {
          m_opModes.remove(opMode.id);
          return opMode.id;
        }
      }
    } finally {
      m_opModesMutex.unlock();
    }
    return 0;
  }

  public static void publishOpModes() {
    m_opModesMutex.lock();
    try {
      OpModeOption[] options = new OpModeOption[m_opModes.size()];
      DriverStationJNI.setOpModeOptions(m_opModes.values().toArray(options));
    } finally {
      m_opModesMutex.unlock();
    }
 
    var modeCounts =
        m_opModes.values().stream()
            .collect(Collectors.groupingBy(OpModeOption::getMode, Collectors.counting()));
 
    for (RobotMode mode : RobotMode.values()) {
      UsageReporting.reportUsage(
          "OpMode/" + mode, String.valueOf(modeCounts.getOrDefault(mode, 0L)));
    }
  }

  public static void clearOpModes() {
    m_opModesMutex.lock();
    try {
      m_opModes.clear();
      DriverStationJNI.setOpModeOptions(new OpModeOption[0]);
    } finally {
      m_opModesMutex.unlock();
    }
  }

  public static void observeUserProgramStarting() {
    m_userProgramStarted = true;
    DriverStationJNI.observeUserProgramStarting();
  }

  public static void clearUserProgramStarted() {
    if (!RobotBase.isSimulation()) {
      // NOP unless running in sim. We don't want teams to call this; it would prevent their
      // programs from transitioning between modes and from detecting opmode selections.
      reportWarning(
          "DriverStationBackend.clearUserProgramStarted() was called! "
              + "This method exists solely for internal WPILib unit tests "
              + "and does nothing in team code.",
          true);
      return;
    }
 
    m_userProgramStarted = false;
  }

  public static long getOpModeId() {
    if (!m_userProgramStarted) {
      return 0;
    }
 
    m_cacheDataMutex.lock();
    try {
      return m_controlWord.getOpModeId();
    } finally {
      m_cacheDataMutex.unlock();
    }
  }

  public static String getOpMode() {
    if (!m_userProgramStarted) {
      return "";
    }
 
    m_cacheDataMutex.lock();
    try {
      return m_opMode;
    } finally {
      m_cacheDataMutex.unlock();
    }
  }

  public static boolean isOpMode(long id) {
    return getOpModeId() == id;
  }

  public static boolean isOpMode(String mode) {
    return getOpMode().equals(mode);
  }

  public static boolean isDSAttached() {
    m_cacheDataMutex.lock();
    try {
      return m_controlWord.isDSAttached();
    } finally {
      m_cacheDataMutex.unlock();
    }
  }

  public static boolean isFMSAttached() {
    m_cacheDataMutex.lock();
    try {
      return m_controlWord.isFMSAttached();
    } finally {
      m_cacheDataMutex.unlock();
    }
  }

  public static Optional<String> getGameData() {
    m_cacheDataMutex.lock();
    try {
      if (m_gameData == null || m_gameData.isEmpty()) {
        return Optional.empty();
      }
      return Optional.of(m_gameData);
    } finally {
      m_cacheDataMutex.unlock();
    }
  }

  public static String getEventName() {
    m_cacheDataMutex.lock();
    try {
      return m_matchInfo.eventName;
    } finally {
      m_cacheDataMutex.unlock();
    }
  }

  public static MatchType getMatchType() {
    int matchType;
    m_cacheDataMutex.lock();
    try {
      matchType = m_matchInfo.matchType;
    } finally {
      m_cacheDataMutex.unlock();
    }
    return switch (matchType) {
      case 1 -> MatchType.PRACTICE;
      case 2 -> MatchType.QUALIFICATION;
      case 3 -> MatchType.ELIMINATION;
      default -> MatchType.NONE;
    };
  }

  public static int getMatchNumber() {
    m_cacheDataMutex.lock();
    try {
      return m_matchInfo.matchNumber;
    } finally {
      m_cacheDataMutex.unlock();
    }
  }

  public static int getReplayNumber() {
    m_cacheDataMutex.lock();
    try {
      return m_matchInfo.replayNumber;
    } finally {
      m_cacheDataMutex.unlock();
    }
  }
 
  private static Map<AllianceStationID, Optional<Alliance>> m_allianceMap =
      Map.of(
          AllianceStationID.UNKNOWN, Optional.empty(),
          AllianceStationID.RED_1, Optional.of(Alliance.RED),
          AllianceStationID.RED_2, Optional.of(Alliance.RED),
          AllianceStationID.RED_3, Optional.of(Alliance.RED),
          AllianceStationID.BLUE_1, Optional.of(Alliance.BLUE),
          AllianceStationID.BLUE_2, Optional.of(Alliance.BLUE),
          AllianceStationID.BLUE_3, Optional.of(Alliance.BLUE));
 
  private static Map<AllianceStationID, OptionalInt> m_stationMap =
      Map.of(
          AllianceStationID.UNKNOWN, OptionalInt.empty(),
          AllianceStationID.RED_1, OptionalInt.of(1),
          AllianceStationID.RED_2, OptionalInt.of(2),
          AllianceStationID.RED_3, OptionalInt.of(3),
          AllianceStationID.BLUE_1, OptionalInt.of(1),
          AllianceStationID.BLUE_2, OptionalInt.of(2),
          AllianceStationID.BLUE_3, OptionalInt.of(3));

  public static Optional<Alliance> getAlliance() {
    AllianceStationID allianceStationID = DriverStationJNI.getAllianceStation();
    if (allianceStationID == null) {
      allianceStationID = AllianceStationID.UNKNOWN;
    }
 
    return m_allianceMap.get(allianceStationID);
  }

  public static OptionalInt getLocation() {
    AllianceStationID allianceStationID = DriverStationJNI.getAllianceStation();
    if (allianceStationID == null) {
      allianceStationID = AllianceStationID.UNKNOWN;
    }
 
    return m_stationMap.get(allianceStationID);
  }

  public static AllianceStationID getRawAllianceStation() {
    return DriverStationJNI.getAllianceStation();
  }

  public static double getMatchTime() {
    return DriverStationJNI.getMatchTime();
  }

  public static void silenceJoystickConnectionAlert(boolean silence) {
    m_joystickAlertMutex.lock();
    try {
      m_silenceJoystickAlerts = silence;
      if (silence && !isFMSAttached()) {
        clearJoystickAlertsLocked();
      }
    } finally {
      m_joystickAlertMutex.unlock();
    }
  }

  public static boolean isJoystickConnectionAlertSilenced() {
    m_joystickAlertMutex.lock();
    try {
      return !isFMSAttached() && m_silenceJoystickAlerts;
    } finally {
      m_joystickAlertMutex.unlock();
    }
  }

  public static void refreshControlWordFromCache(ControlWord word) {
    m_cacheDataMutex.lock();
    try {
      word.update(m_controlWord);
    } finally {
      m_cacheDataMutex.unlock();
    }
  }

  public static void refreshData() {
    DriverStationJNI.refreshDSData();
 
    // Get the status of all the joysticks
    for (byte stick = 0; stick < JOYSTICK_PORTS; stick++) {
      DriverStationJNI.getAllJoystickData(
          stick,
          m_joystickAxesCache[stick].m_axes,
          m_joystickAxesRawCache[stick].m_axes,
          m_joystickPOVsCache[stick].m_povs,
          m_touchpadFingersCache,
          m_metadataCache);
      m_joystickAxesCache[stick].m_available = (int) m_metadataCache[0];
      m_joystickAxesRawCache[stick].m_available = (int) m_metadataCache[0];
      m_joystickPOVsCache[stick].m_available = (int) m_metadataCache[1];
      m_joystickButtonsCache[stick].m_available = m_metadataCache[2];
      m_joystickButtonsCache[stick].m_buttons = m_metadataCache[3];
      m_joystickTouchpadsCache[stick].m_count = (int) m_metadataCache[4];
      for (int i = 0; i < m_joystickTouchpadsCache[stick].m_count; i++) {
        long metadata = m_metadataCache[5 + i];
        m_joystickTouchpadsCache[stick].m_touchpads[i].m_fingers[0].m_down = (metadata & 0x1) != 0;
        m_joystickTouchpadsCache[stick].m_touchpads[i].m_fingers[1].m_down = (metadata & 0x2) != 0;
        m_joystickTouchpadsCache[stick].m_touchpads[i].m_count = (int) (metadata >> 2 & 0x3);
        for (int j = 0; j < m_joystickTouchpadsCache[stick].m_touchpads[i].m_count; j++) {
          m_joystickTouchpadsCache[stick].m_touchpads[i].m_fingers[j].m_x =
              m_touchpadFingersCache[i * 4 + j * 2 + 0];
          m_joystickTouchpadsCache[stick].m_touchpads[i].m_fingers[j].m_y =
              m_touchpadFingersCache[i * 4 + j * 2 + 1];
        }
      }
    }
 
    DriverStationJNI.getMatchInfo(m_matchInfoCache);
 
    // This is a pass through, so if it hasn't changed, it doesn't
    // reallocate
    m_gameDataCache = DriverStationJNI.getGameData(m_gameDataCache);
 
    DriverStationJNI.getControlWord(m_controlWordCache);
 
    m_opModeCache = opModeToString(m_controlWordCache.getOpModeId());
 
    DataLogSender dataLogSender;
    // lock joystick mutex to swap cache data
    m_cacheDataMutex.lock();
    try {
      for (int i = 0; i < JOYSTICK_PORTS; i++) {
        // If buttons weren't pressed and are now, set flags in m_buttonsPressed
        m_joystickButtonsPressed[i] |=
            ~m_joystickButtons[i].m_buttons & m_joystickButtonsCache[i].m_buttons;
 
        // If buttons were pressed and aren't now, set flags in m_buttonsReleased
        m_joystickButtonsReleased[i] |=
            m_joystickButtons[i].m_buttons & ~m_joystickButtonsCache[i].m_buttons;
      }
 
      // move cache to actual data
      HALJoystickAxes[] currentAxes = m_joystickAxes;
      m_joystickAxes = m_joystickAxesCache;
      m_joystickAxesCache = currentAxes;
 
      HALJoystickAxesRaw[] currentAxesRaw = m_joystickAxesRaw;
      m_joystickAxesRaw = m_joystickAxesRawCache;
      m_joystickAxesRawCache = currentAxesRaw;
 
      HALJoystickButtons[] currentButtons = m_joystickButtons;
      m_joystickButtons = m_joystickButtonsCache;
      m_joystickButtonsCache = currentButtons;
 
      HALJoystickPOVs[] currentPOVs = m_joystickPOVs;
      m_joystickPOVs = m_joystickPOVsCache;
      m_joystickPOVsCache = currentPOVs;
 
      HALJoystickTouchpads[] currentTouchpads = m_joystickTouchpads;
      m_joystickTouchpads = m_joystickTouchpadsCache;
      m_joystickTouchpadsCache = currentTouchpads;
 
      MatchInfoData currentInfo = m_matchInfo;
      m_matchInfo = m_matchInfoCache;
      m_matchInfoCache = currentInfo;
 
      m_gameData = m_gameDataCache;
 
      ControlWord currentWord = m_controlWord;
      m_controlWord = m_controlWordCache;
      m_controlWordCache = currentWord;
 
      String currentOpMode = m_opMode;
      m_opMode = m_opModeCache;
      m_opModeCache = currentOpMode;
 
      dataLogSender = m_dataLogSender;
    } finally {
      m_cacheDataMutex.unlock();
    }
 
    clearJoystickAlerts();
 
    m_refreshEvents.wakeup();
 
    m_matchDataSender.sendMatchData();
    if (dataLogSender != null) {
      dataLogSender.send(WPIUtilJNI.now());
    }
  }

  public static void provideRefreshedDataEventHandle(int handle) {
    m_refreshEvents.add(handle);
  }

  public static void removeRefreshedDataEventHandle(int handle) {
    m_refreshEvents.remove(handle);
  }
 
  private static JoystickAlerts getJoystickAlertsLocked(int stick) {
    JoystickAlerts alerts = m_joystickAlerts[stick];
    if (alerts == null) {
      alerts = new JoystickAlerts(stick);
      m_joystickAlerts[stick] = alerts;
    }
    return alerts;
  }
 
  private static void reportJoystickResourceWarning(
      int stick, JoystickResourceAlert resourceAlert, int resource) {
    m_joystickAlertMutex.lock();
    try {
      if (!isFMSAttached() && m_silenceJoystickAlerts) {
        return;
      }
 
      JoystickAlerts alerts = getJoystickAlertsLocked(stick);
      if (!alerts.prepareForReport()) {
        return;
      }
 
      alerts.reportResourceWarning(resourceAlert, resource);
    } finally {
      m_joystickAlertMutex.unlock();
    }
  }
 
  private static void reportJoystickTouchpadFingerWarning(int stick, int touchpad, int finger) {
    m_joystickAlertMutex.lock();
    try {
      if (!isFMSAttached() && m_silenceJoystickAlerts) {
        return;
      }
 
      JoystickAlerts alerts = getJoystickAlertsLocked(stick);
      if (!alerts.prepareForReport()) {
        return;
      }
 
      alerts.reportTouchpadFingerWarning(touchpad, finger);
    } finally {
      m_joystickAlertMutex.unlock();
    }
  }
 
  private static void clearJoystickAlerts() {
    m_joystickAlertMutex.lock();
    try {
      clearJoystickAlertsLocked();
    } finally {
      m_joystickAlertMutex.unlock();
    }
  }
 
  private static void clearJoystickAlertsLocked() {
    if (!isFMSAttached() && m_silenceJoystickAlerts) {
      for (JoystickAlerts alerts : m_joystickAlerts) {
        if (alerts != null) {
          alerts.clearAllAlerts();
        }
      }
      return;
    }
 
    for (JoystickAlerts alerts : m_joystickAlerts) {
      if (alerts != null) {
        alerts.refresh();
      }
    }
  }

  @SuppressWarnings("PMD.NonThreadSafeSingleton")
  public static void startDataLog(DataLog log, boolean logJoysticks) {
    m_cacheDataMutex.lock();
    try {
      if (m_dataLogSender == null) {
        m_dataLogSender = new DataLogSender(log, logJoysticks, WPIUtilJNI.now());
      }
    } finally {
      m_cacheDataMutex.unlock();
    }
  }

  public static void startDataLog(DataLog log) {
    startDataLog(log, true);
  }
}