Improve ability to reschedule phased loops in AOS

This makes it so that users can reliably reschedule phased loops in a
well-defined way, for situations, e.g., where there may be a need to
update the offset of the loop dynamically.

This slightly changes the default semantics around dynamically changing
the interval/offset of a phased loop, but I don't actually see anyone
doing that currently, and there were some poorly-defined corner cases
anyways.

References: PRO-18058
Change-Id: I679c905da1582d250deb7f9607c8517ed34e5a25
Signed-off-by: James Kuszmaul <james.kuszmaul@bluerivertech.com>
diff --git a/aos/util/phased_loop.cc b/aos/util/phased_loop.cc
index 53f6f4d..1f61775 100644
--- a/aos/util/phased_loop.cc
+++ b/aos/util/phased_loop.cc
@@ -17,15 +17,26 @@
 
 void PhasedLoop::set_interval_and_offset(
     const monotonic_clock::duration interval,
-    const monotonic_clock::duration offset) {
+    const monotonic_clock::duration offset,
+    std::optional<monotonic_clock::time_point> monotonic_now) {
   // Update last_time_ to the new offset so that we have an even interval
-  last_time_ += offset - offset_;
+  // In doing so, set things so that last_time_ will only ever decrease on calls
+  // to set_interval_and_offset.
+  last_time_ += offset - offset_ -
+                (offset > offset_ ? interval : monotonic_clock::duration(0));
 
   interval_ = interval;
   offset_ = offset;
   CHECK(offset_ >= monotonic_clock::duration(0));
   CHECK(interval_ > monotonic_clock::duration(0));
   CHECK(offset_ < interval_);
+  // Reset effectively clears the skipped iteration count and ensures that the
+  // last time is in the interval (monotonic_now - interval, monotonic_now],
+  // which means that a call to Iterate(monotonic_now) will return 1 and set a
+  // wakeup time after monotonic_now.
+  if (monotonic_now.has_value()) {
+    Iterate(monotonic_now.value());
+  }
 }
 
 monotonic_clock::duration PhasedLoop::OffsetFromIntervalAndTime(
diff --git a/aos/util/phased_loop.h b/aos/util/phased_loop.h
index 307b400..64a3302 100644
--- a/aos/util/phased_loop.h
+++ b/aos/util/phased_loop.h
@@ -1,6 +1,8 @@
 #ifndef AOS_UTIL_PHASED_LOOP_H_
 #define AOS_UTIL_PHASED_LOOP_H_
 
+#include <optional>
+
 #include "aos/time/time.h"
 
 namespace aos {
@@ -21,8 +23,30 @@
       const monotonic_clock::duration offset = monotonic_clock::duration(0));
 
   // Updates the offset and interval.
-  void set_interval_and_offset(const monotonic_clock::duration interval,
-                               const monotonic_clock::duration offset);
+  //
+  // After a call to set_interval_and_offset with monotonic_now = nullopt, the
+  // following will hold, for any allowed values of interval and offset:
+  // auto original_time = loop.sleep_time();
+  // loop.set_interval_and_offset(interval, offset);
+  // CHECK_LE(loop.sleep_time(), original_time);
+  // CHECK_EQ(0, loop.Iterate(original_time));
+  //
+  // Note that this will not be the behavior that all (or even necessarily most)
+  // users want, since it doesn't necessarily preserve a "keep the iteration
+  // time as consistent as possible" concept. However, it *is* better defined
+  // than the alternative, where if you decrease the offset by, e.g., 1ms on a
+  // 100ms interval, then the behavior will vary depending on whather you are
+  // going from 0ms->999ms offset or from 1ms->0ms offset.
+  //
+  // If monotonic_now is set, then the following will hold:
+  // auto original_time = loop.sleep_time();
+  // loop.set_interval_and_offset(interval, offset, monotonic_now);
+  // CHECK_LE(loop.sleep_time(), monotonic_now);
+  // CHECK_EQ(0, loop.Iterate(monotonic_now));
+  void set_interval_and_offset(
+      const monotonic_clock::duration interval,
+      const monotonic_clock::duration offset,
+      std::optional<monotonic_clock::time_point> monotonic_now = std::nullopt);
 
   // Computes the offset given an interval and a time that we should trigger.
   static monotonic_clock::duration OffsetFromIntervalAndTime(
diff --git a/aos/util/phased_loop_test.cc b/aos/util/phased_loop_test.cc
index 5e85ea0..de4483c 100644
--- a/aos/util/phased_loop_test.cc
+++ b/aos/util/phased_loop_test.cc
@@ -1,6 +1,7 @@
 #include "aos/util/phased_loop.h"
 
 #include "aos/time/time.h"
+#include "glog/logging.h"
 #include "gtest/gtest.h"
 
 namespace aos {
@@ -230,7 +231,23 @@
   ASSERT_EQ(5, loop.Iterate(last_time));
   for (int i = 1; i < kCount; i++) {
     const auto offset = kOffset - milliseconds(i);
-    loop.set_interval_and_offset(kInterval, offset);
+    // First, set the interval/offset without specifying a "now". If we then
+    // attempt to Iterate() to the same time as the last iteration, this should
+    // always result in zero cycles elapsed.
+    {
+      const monotonic_clock::time_point original_time = loop.sleep_time();
+      loop.set_interval_and_offset(kInterval, offset);
+      EXPECT_EQ(original_time - milliseconds(1), loop.sleep_time());
+      EXPECT_EQ(0, loop.Iterate(last_time));
+    }
+
+    // Now, explicitly update/clear things to last_time. This should have the
+    // same behavior as not specifying a monotonic_now.
+    {
+      loop.set_interval_and_offset(kInterval, offset, last_time);
+      EXPECT_EQ(0, loop.Iterate(last_time));
+    }
+
     const auto next_time = last_time - milliseconds(1) + kAllIterationsInterval;
     EXPECT_EQ(kIterations, loop.Iterate(next_time));
     last_time = next_time;
@@ -238,6 +255,43 @@
 }
 
 // Tests that the phased loop is correctly adjusting when the offset is
+// incremented multiple times.
+TEST_F(PhasedLoopTest, IncrementingOffset) {
+  constexpr int kCount = 5;
+  constexpr int kIterations = 10;
+  const auto kOffset = milliseconds(0);
+  const auto kInterval = milliseconds(1000);
+  const auto kAllIterationsInterval = kInterval * kIterations;
+
+  PhasedLoop loop(kInterval, monotonic_clock::epoch(), kOffset);
+  auto last_time = monotonic_clock::epoch() + kOffset + (kInterval * 3);
+  ASSERT_EQ(4, loop.Iterate(last_time));
+  for (int i = 1; i < kCount; i++) {
+    const auto offset = kOffset + milliseconds(i);
+    {
+      const monotonic_clock::time_point original_time = loop.sleep_time();
+      loop.set_interval_and_offset(kInterval, offset);
+      EXPECT_EQ(original_time - kInterval + milliseconds(1), loop.sleep_time());
+      EXPECT_EQ(0, loop.Iterate(last_time));
+    }
+    // Now, explicitly update/clear things to a set time. We add a milliseconds
+    // so that when we call Iterate() next we actually get the expected number
+    // of iterations (otherwise, there is an iteration that would happen at
+    // last_time + 1 that gets counted, which is correct behavior, and so just
+    // needs to be accounted for somehow).
+    {
+      loop.set_interval_and_offset(kInterval, offset,
+                                   last_time + milliseconds(1));
+      EXPECT_EQ(0, loop.Iterate(last_time + milliseconds(1)));
+    }
+
+    const auto next_time = last_time + milliseconds(1) + kAllIterationsInterval;
+    EXPECT_EQ(kIterations, loop.Iterate(next_time));
+    last_time = next_time;
+  }
+}
+
+// Tests that the phased loop is correctly adjusting when the offset is
 // changed to 0.
 TEST_F(PhasedLoopTest, ChangingOffset) {
   const auto kOffset = milliseconds(900);