From 7a1424486a5955876f88c29b3ac6fb71ce3d9fdb Mon Sep 17 00:00:00 2001
From: Tomasz Bialek <tbialek@ihmc.org>
Date: Thu, 31 Oct 2024 17:30:43 -0500
Subject: [PATCH] Moved Throttler to ihmc-commons

---
 .../us/ihmc/commons/thread/Throttler.java     | 170 ++++++++++++++++++
 1 file changed, 170 insertions(+)
 create mode 100644 src/main/java/us/ihmc/commons/thread/Throttler.java

diff --git a/src/main/java/us/ihmc/commons/thread/Throttler.java b/src/main/java/us/ihmc/commons/thread/Throttler.java
new file mode 100644
index 0000000..2f4bc1c
--- /dev/null
+++ b/src/main/java/us/ihmc/commons/thread/Throttler.java
@@ -0,0 +1,170 @@
+package us.ihmc.commons.thread;
+
+import us.ihmc.commons.Conversions;
+
+/**
+ * Throttler is used to allow things to happen at a slower rate
+ * than the parent update thread. It features a run method,
+ * which returns whether or not enough time has passed to run something.
+ * Alternatively, it features a waitAndRun method, which allows the
+ * user to sleep until it is ready for the next thing.
+ * <p>
+ * This class is useful as an alternative to creating additional
+ * threads when appropriate. It does not create any threads so
+ * there are no concurrency issues.
+ * <p>
+ * Throttler's {@link #run} methods work best when polled at much higher frequencies
+ * than the requested throttled frequency. We have put a mechanism in place
+ * to handle poll frequencies near or lower than requested, but the result
+ * will be jittery and inaccurate. In those cases, it is best to spin
+ * up a separate thread. See us.ihmc.tools.thread.RestartableThrottledThread.
+ * <p>
+ * Throttler's {@link #waitAndRun} methods have a different nature. They
+ * obviously cannot be called faster than the requested frequency. They
+ * are designed to handle waiting the extra time after variable amounts
+ * of computation in order to run that computation at a steady rate.
+ * <p>
+ * Example:
+ *
+ * <pre>
+ *    Throttler throttler = new Throttler().setFrequency(5.0);
+ *
+ *    while (true)
+ *    {
+ *       if (throttler.run())
+ *       {
+ *          // do stuff
+ *       }
+ *    }
+ * </pre>
+ */
+public class Throttler
+{
+   private double resetTime = Double.NaN;
+   private double optionallySetPeriod = Double.NaN;
+   private transient double currentTime;
+   private transient double overtime;
+
+   /**
+    * Set the period.
+    * <p>
+    * Syntactic sugar to be clear about what a constant passed in would be.
+    * For example, as a field:
+    *
+    * <pre>
+    *    private final Throttler throttler = new Throttler().setPeriod(5.0);
+    * </pre>
+    */
+   public Throttler setPeriod(double period)
+   {
+      optionallySetPeriod = period;
+      return this;
+   }
+
+   /**
+    * Set the frequency.
+    * <p>
+    * Syntactic sugar to be clear about what a constant passed in would be.
+    * For example, as a field:
+    *
+    * <pre>
+    *    private final Throttler throttler = new Throttler().setFrequency(5.0);
+    * </pre>
+    */
+   public Throttler setFrequency(double frequency)
+   {
+      optionallySetPeriod = Conversions.hertzToSeconds(frequency);
+      return this;
+   }
+
+   /**
+    * For use if the user set the period with the {@link #setPeriod} method.
+    * @return Whether or not enough time has passed to run your thing again.
+    *         It is recommended to call this at several times the desired throttled rate.
+    *         Calling this more often is directly proportional to the resulting accuracy.
+    */
+   public boolean run()
+   {
+      return run(optionallySetPeriod);
+   }
+
+   /**
+    * @param period for passing in dynamically calculated periods
+    * @return Whether or not enough time has passed to run your thing again.
+    *         It is recommended to call this at several times the desired throttled rate.
+    *         Calling this more often is directly proportional to the resulting accuracy.
+    */
+   public boolean run(double period)
+   {
+      currentTime = Conversions.nanosecondsToSeconds(System.nanoTime());
+
+      if (Double.isNaN(resetTime)) // First run
+      {
+         resetTime = currentTime;
+         return true;
+      }
+      else
+      {
+         calculateOvertime(period);
+
+         boolean periodHasElapsed = overtime >= 0.0;
+
+         if (periodHasElapsed)
+         {
+            // We subtract the overtime to achieve a more accurate rate
+            resetTime = currentTime - overtime;
+         }
+
+         return periodHasElapsed;
+      }
+   }
+
+   /**
+    * Sleeps until enough time has passed to run your thing again.
+    * <p>
+    * For use if the user set the period with the {@link #setPeriod} method.
+    */
+   public void waitAndRun()
+   {
+      waitAndRun(optionallySetPeriod);
+   }
+
+   /**
+    * Sleeps until enough time has passed to run your thing again.
+    *
+    * @param period for passing in dynamically calculated periods
+    */
+   public void waitAndRun(double period)
+   {
+      currentTime = Conversions.nanosecondsToSeconds(System.nanoTime());
+
+      if (Double.isNaN(resetTime)) // First run
+      {
+         resetTime = currentTime;
+      }
+      else
+      {
+         calculateOvertime(period);
+
+         if (overtime < 0.0)
+         {
+            // Guarantees to sleep at least this amount (i.e. will sleep too long)
+            currentTime += ThreadTools.parkAtLeast(-overtime);
+
+            calculateOvertime(period);
+         }
+
+         // We subtract the overtime to achieve a more accurate rate
+         resetTime = currentTime - overtime;
+      }
+   }
+
+   private void calculateOvertime(double period)
+   {
+      double elapsedTime = currentTime - resetTime;
+
+      // Limit the overtime to half the period, to prevent building up
+      // when the run methods are called too infrequently.
+      overtime = Math.min(elapsedTime - period, 0.5 * period);
+   }
+}