Make Watchdog use single thread dispatch (#1347)

Notifier has one thread per instance because the callbacks must be
asynchronous. Watchdog callbacks can be synchronous, so this overhead
can be done away with via a scheduler thread akin to what the HAL
Notifier does.

wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java

@@ -7,8 +7,13 @@
 
 package edu.wpi.first.wpilibj;
 
+import java.io.Closeable;
 import java.util.HashMap;
 import java.util.Map;
+import java.util.PriorityQueue;
+import java.util.concurrent.TimeUnit;
+import java.util.concurrent.locks.Condition;
+import java.util.concurrent.locks.ReentrantLock;
 
 /**
  * A class that's a wrapper around a watchdog timer.
@@ -18,51 +23,106 @@ import java.util.Map;
  *
  * <p>The watchdog is initialized disabled, so the user needs to call enable() before use.
  */
-public class Watchdog {
-  private double m_timeout;
-  private Runnable m_callback;
-  private Notifier m_notifier;
+public class Watchdog implements Closeable, Comparable<Watchdog> {
+  private long m_startTime; // us
+  private long m_timeout; // us
+  private long m_expirationTime; // us
+  private final Runnable m_callback;
 
-  private double m_startTime;
   @SuppressWarnings("PMD.UseConcurrentHashMap")
-  private final Map<String, Double> m_epochs = new HashMap<>();
+  private final Map<String, Long> m_epochs = new HashMap<>();
   boolean m_isExpired;
 
-  /**
-   * Watchdog constructor.
-   *
-   * @param timeout The watchdog's timeout in seconds.
-   */
-  public Watchdog(double timeout) {
-    this(timeout, () -> {
-    });
+  static {
+    startDaemonThread(() -> schedulerFunc());
   }
 
+  private static final PriorityQueue<Watchdog> m_watchdogs = new PriorityQueue<>();
+  private static ReentrantLock m_queueMutex = new ReentrantLock();
+  private static Condition m_schedulerWaiter = m_queueMutex.newCondition();
+
   /**
    * Watchdog constructor.
    *
-   * @param timeout  The watchdog's timeout in seconds.
+   * @param timeout  The watchdog's timeout in seconds with microsecond resolution.
    * @param callback This function is called when the timeout expires.
    */
   public Watchdog(double timeout, Runnable callback) {
-    m_timeout = timeout;
+    m_timeout = (long) (timeout * 1.0e6);
     m_callback = callback;
-    m_notifier = new Notifier(this::timeoutFunc);
-    enable();
+  }
+
+  @Override
+  public void close() {
+    disable();
+  }
+
+  @Override
+  public int compareTo(Watchdog rhs) {
+    // Elements with sooner expiration times are sorted as lesser. The head of
+    // Java's PriorityQueue is the least element.
+    if (m_expirationTime < rhs.m_expirationTime) {
+      return -1;
+    } else if (m_expirationTime > rhs.m_expirationTime) {
+      return 1;
+    } else {
+      return 0;
+    }
   }
 
   /**
    * Get the time in seconds since the watchdog was last fed.
    */
   public double getTime() {
-    return Timer.getFPGATimestamp() - m_startTime;
+    return (RobotController.getFPGATime() - m_startTime) / 1.0e6;
+  }
+
+  /**
+   * Sets the watchdog's timeout.
+   *
+   * @param timeout The watchdog's timeout in seconds with microsecond
+   *                resolution.
+   */
+  public void setTimeout(double timeout) {
+    m_startTime = RobotController.getFPGATime();
+    m_epochs.clear();
+
+    m_queueMutex.lock();
+    try {
+      m_timeout = (long) (timeout * 1.0e6);
+      m_isExpired = false;
+
+      m_watchdogs.remove(this);
+      m_expirationTime = m_startTime + m_timeout;
+      m_watchdogs.add(this);
+      m_schedulerWaiter.signalAll();
+    } finally {
+      m_queueMutex.unlock();
+    }
+  }
+
+  /**
+   * Returns the watchdog's timeout in seconds.
+   */
+  public double getTimeout() {
+    m_queueMutex.lock();
+    try {
+      return m_timeout / 1.0e6;
+    } finally {
+      m_queueMutex.unlock();
+    }
   }
 
   /**
    * Returns true if the watchdog timer has expired.
    */
   public boolean isExpired() {
-    return m_isExpired;
+    m_queueMutex.lock();
+    try {
+      return m_isExpired;
+    } finally {
+      m_queueMutex.unlock();
+    }
   }
 
   /**
@@ -74,7 +134,7 @@ public class Watchdog {
    * @param epochName The name to associate with the epoch.
    */
   public void addEpoch(String epochName) {
-    double currentTime = Timer.getFPGATimestamp();
+    long currentTime = RobotController.getFPGATime();
     m_epochs.put(epochName, currentTime - m_startTime);
     m_startTime = currentTime;
   }
@@ -84,7 +144,7 @@ public class Watchdog {
    */
   public void printEpochs() {
     m_epochs.forEach((key, value) -> {
-      System.out.println("\t" + key + ": " + value + "s");
+      System.out.format("\t" + key + ": %.6fs\n", value / 1.0e6);
     });
   }
 
@@ -101,25 +161,100 @@ public class Watchdog {
    * Enables the watchdog timer.
    */
   public void enable() {
-    m_startTime = Timer.getFPGATimestamp();
-    m_isExpired = false;
+    m_startTime = RobotController.getFPGATime();
     m_epochs.clear();
-    m_notifier.startPeriodic(m_timeout);
+
+    m_queueMutex.lock();
+    try {
+      m_isExpired = false;
+
+      m_watchdogs.remove(this);
+      m_expirationTime = m_startTime + m_timeout;
+      m_watchdogs.add(this);
+      m_schedulerWaiter.signalAll();
+    } finally {
+      m_queueMutex.unlock();
+    }
   }
 
   /**
    * Disable the watchdog.
    */
   public void disable() {
-    m_notifier.stop();
-  }
+    m_queueMutex.lock();
+    try {
+      m_isExpired = false;
 
-  private void timeoutFunc() {
-    if (!m_isExpired) {
-      System.out.println("Watchdog not fed after " + m_timeout + "s");
-      m_callback.run();
-      m_isExpired = true;
-      disable();
+      m_watchdogs.remove(this);
+      m_schedulerWaiter.signalAll();
+    } finally {
+      m_queueMutex.unlock();
     }
   }
+
+  private static Thread startDaemonThread(Runnable target) {
+    Thread inst = new Thread(target);
+    inst.setDaemon(true);
+    inst.start();
+    return inst;
+  }
+
+
+  private static void schedulerFunc() {
+    m_queueMutex.lock();
+
+    try {
+      while (true) {
+        if (m_watchdogs.size() > 0) {
+          boolean timedOut = !awaitUntil(m_schedulerWaiter, m_watchdogs.peek().m_expirationTime);
+          if (timedOut) {
+            if (m_watchdogs.size() == 0 || m_watchdogs.peek().m_expirationTime
+                > RobotController.getFPGATime()) {
+              continue;
+            }
+
+            // If the condition variable timed out, that means a Watchdog timeout
+            // has occurred, so call its timeout function.
+            Watchdog watchdog = m_watchdogs.poll();
+
+            System.out.format("Watchdog not fed within %.6fs\n", watchdog.m_timeout / 1.0e6);
+            m_queueMutex.unlock();
+            watchdog.m_callback.run();
+            m_queueMutex.lock();
+            watchdog.m_isExpired = true;
+          }
+          // Otherwise, a Watchdog removed itself from the queue (it notifies
+          // the scheduler of this) or a spurious wakeup occurred, so just
+          // rewait with the soonest watchdog timeout.
+        } else {
+          while (m_watchdogs.size() == 0) {
+            m_schedulerWaiter.awaitUninterruptibly();
+          }
+        }
+      }
+    } finally {
+      m_queueMutex.unlock();
+    }
+  }
+
+  /**
+   * Wrapper emulating functionality of C++'s std::condition_variable::wait_until().
+   *
+   * @param cond The condition variable on which to wait.
+   * @param time The time at which to stop waiting.
+   * @return False if the deadline has elapsed upon return, else true.
+   */
+  private static boolean awaitUntil(Condition cond, long time) {
+    long delta = time - RobotController.getFPGATime();
+    try {
+      if (delta > 0) {
+        return cond.await(delta, TimeUnit.MICROSECONDS);
+      }
+    } catch (InterruptedException ex) {
+      Thread.currentThread().interrupt();
+      ex.printStackTrace();
+    }
+
+    return true;
+  }
 }