From 5cdffeaba1cc64e880969a39feac8d80ed3c5c09 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Thu, 22 Oct 2020 20:55:12 -0700
Subject: [PATCH] [sim] StepTiming(): incrementally step Notifiers in sequence
 (#2794)

Currently, StepTiming() advances the time by the given delta, then runs
any Notifiers that expired within that timeframe until their expiration
times are in the future. This doesn't reflect how the Notifiers would
actually run on a real robot. For example, if a Notifier measures the
time between calls for state-space model advancement, it would measure
a large jump in time once, then zero for subsequent runs until the
Notifier was caught up to the current time.

With this change, the time is incremented by the full delta or until the
soonest Notifier, whichever has the smaller delta, then Notifiers set to
expire at that time are run. This is repeated until the time has been
advanced by the full delta. For the state-space model Notifier situation
mentioned before, it would measure multiple small time jumps instead of
one big one.
---
 hal/src/main/native/sim/MockHooks.cpp | 16 ++++++++++++++--
 1 file changed, 14 insertions(+), 2 deletions(-)

diff --git a/hal/src/main/native/sim/MockHooks.cpp b/hal/src/main/native/sim/MockHooks.cpp
index fe29fe6540..8fe387f4de 100644
--- a/hal/src/main/native/sim/MockHooks.cpp
+++ b/hal/src/main/native/sim/MockHooks.cpp
@@ -5,6 +5,7 @@
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
+#include <algorithm>
 #include <atomic>
 #include <chrono>
 #include <cstdio>
@@ -14,6 +15,7 @@
 
 #include "MockHooksInternal.h"
 #include "NotifierInternal.h"
+#include "hal/simulation/NotifierData.h"
 
 static std::atomic<bool> programStarted{false};
 
@@ -93,8 +95,18 @@ HAL_Bool HALSIM_IsTimingPaused(void) { return IsTimingPaused(); }
 
 void HALSIM_StepTiming(uint64_t delta) {
   WaitNotifiers();
-  StepTiming(delta);
-  WakeupWaitNotifiers();
+
+  while (delta > 0) {
+    int32_t status = 0;
+    uint64_t curTime = HAL_GetFPGATime(&status);
+    uint64_t nextTimeout = HALSIM_GetNextNotifierTimeout();
+    uint64_t step = std::min(delta, nextTimeout - curTime);
+
+    StepTiming(step);
+    delta -= step;
+
+    WakeupWaitNotifiers();
+  }
 }
 
 void HALSIM_StepTimingAsync(uint64_t delta) {