Fix/iterative init sleep

SimLoop/README.md

@@ -376,6 +376,8 @@ RLOG has one frame per tick and the usual completeness checks.
   the first `loop()` on the first ACTIVE tick, `loop()` per tick, `stop()` - with `telemetry.update()`
   after each, as the SDK does. Each call runs on the OpMode's own thread while the sim waits (never both
   at once); breakpoints still work, and a debugger that suspends all threads pauses the watchdog too.
+  Under `forTeamClass` a wait in `init()` holds it for ticks of sim time, as the SDK lets `init()` block;
+  waits in the other calls are refused.
 - **`LinearOpMode`**: `runOpMode()` runs on its own thread in strict lockstep with the sim (never both at
   once). A tick ends when the OpMode repeats a lifecycle check (`opModeIsActive()`, `isStopRequested()`,
   `opModeInInit()`, `isStarted()`) it already made this tick at the same place, so one pass of

SimLoop/docs/packages/opmode.md

@@ -56,7 +56,10 @@ One INIT tick, `initLoopTicks` INIT_LOOP ticks, `activeTicks` ACTIVE ticks, then
 4. every fake device updates, then the `eachTick` step runs.
 
 An iterative `OpMode` gets the SDK's own call order: `init()`, `init_loop()` each INIT_LOOP tick, `start()`
-and the first `loop()` on the first ACTIVE tick, `loop()` each tick after, `stop()` on STOP.
+and the first `loop()` on the first ACTIVE tick, `loop()` each tick after, `stop()` on STOP. Under
+`forTeamClass`, `init()` may wait, as the SDK lets it: a `Thread.sleep` there holds it for `ceil(ms / tick)`
+ticks, and the OpMode's next call comes on the tick after it returns. See
+[Running your timer code unchanged](../unchanged-team-code.md).
 
 A `LinearOpMode`'s `runOpMode()` runs on its own thread in lockstep with the sim. One pass of
 `while (opModeIsActive()) { ... }` is one tick. `waitForStart()` holds until PLAY. `sleep(ms)` holds for

SimLoop/docs/packages/simclock.md

@@ -72,7 +72,8 @@ Your classes are loaded a second time for each run. So:
   can see are listed in `result.warnings()`.
 - **It will not move `Object.wait`, `Thread.join`, latches, futures or executors.**
 - **It refuses some things outright**, and fails the run even if your code catches the refusal: a
-  `Thread.sleep` on any thread but a `LinearOpMode`'s own, and a method reference `this::sleep` (write
+  `Thread.sleep` on any thread but a `LinearOpMode`'s own or an iterative OpMode's own during `init()`,
+  and a method reference `this::sleep` (write
   `ms -> sleep(ms)` instead).
 - **It will not rewrite an OpMode instance you built yourself.** Only `forTeamClass` loads classes through
   it.

SimLoop/docs/unchanged-team-code.md

@@ -128,6 +128,7 @@ its top-level package (see [Pedro Pathing](#pedro-pathing-and-other-libraries)).
 | `System.currentTimeMillis()` | 1 January 2026, 00:00 UTC, plus sim time. The same date every run, so a file name made from it repeats. |
 | `sleep(ms)` in a `LinearOpMode` | Waits that many ms of **sim** time: `sleep(1000)` is exactly 50 ticks. The duration can be anything: `sleep(500)`, `sleep(ms * 2)`, `sleep(fast ? 100 : 1000)`, or `sleep(millis)` inside a helper method. |
 | `Thread.sleep(ms)`, `Thread.sleep(ms, nanos)`, `TimeUnit.MILLISECONDS.sleep(ms)` | The same as `sleep(ms)`, but like the real ones they throw `InterruptedException` if STOP is pressed while they wait. `Thread.sleep(0)` returns at once, as on the robot, even in an iterative OpMode. |
+| `Thread.sleep(ms)` and the other waits above in an iterative OpMode's `init()`, your own or a library's (Pedro Pathing's Pinpoint localizer sleeps 500 ms in its constructor) | Holds `init()` for that many ms of **sim** time, as the SDK lets `init()` block on the robot: `Thread.sleep(500)` is exactly 25 ticks. Meanwhile motors keep their power and no other OpMode method runs; `init_loop()` starts on the tick after `init()` returns, or `start()` does if PLAY came first. STOP does not interrupt the wait (the SDK does not interrupt an iterative OpMode): sim time ends there, so the rest of it takes no time, then `stop()` runs. Waits in `init_loop()`, `start()`, `loop()` and `stop()` are still refused. |
 | Android's `SystemClock.uptimeMillis()`, `elapsedRealtime()`, `elapsedRealtimeNanos()`, `SystemClock.sleep(ms)` | Sim time in ms (or ns), and a sim-time wait. (In a JVM test Android's own `SystemClock` reads 0 and waits for nothing; rewritten, it is never used.) |
 | `idle()` in a `LinearOpMode` | Like the SDK's, it takes no time by itself; the tick ends when the same `idle()` runs a second time in it. So `while (timer.seconds() < 2) { idle(); }` waits 2 s of sim time, one pass per tick after the first, and `sleep(50); idle();` takes exactly the 3 ticks of the sleep. |
 | `System::nanoTime`, `ElapsedTime::new`, `() -> System.nanoTime()` | Method references and lambdas follow along. |
@@ -192,6 +193,9 @@ OpModeRunner.forTeamClass("org.firstinspires.ftc.teamcode.MyAuto", hardwareMap)
         .run();
 ```
 
+With Pedro listed, its own waits move too: the Pinpoint localizer's 500 ms sleep while the device
+calibrates takes 25 ticks of sim time, even when your iterative OpMode builds the follower in `init()`.
+
 What that costs: Pedro's classes are then loaded afresh for each run, like yours, so a Pedro `Pose` or
 `Path` cannot be handed from the test into the run, or from one run to the next through the blackboard (see
 the trade-off below). Hand over numbers instead.
@@ -229,9 +233,10 @@ changes that let a test hand one in, which is the better fix when you can make i
 ## Error messages, and what to do
 
 **"SimLoop cannot run Thread.sleep(10) at ...loop(MyTeleOp.java:42) on sim time: MyTeleOp is an iterative
-OpMode, whose init(), init_loop(), start(), loop() and stop() must each return promptly"**
-You called `Thread.sleep` inside an iterative `OpMode`. On a robot this freezes the whole loop, gamepads
-included. Instead, start an `ElapsedTime` and check it on each `loop()` call, or make it a `LinearOpMode`.
+OpMode, whose init_loop(), start(), loop() and stop() must each return promptly"**
+You called `Thread.sleep` inside an iterative `OpMode`, outside `init()`. On a robot this freezes the whole
+loop, gamepads included. Instead, start an `ElapsedTime` and check it on each `loop()` call, or make it a
+`LinearOpMode`. A wait in `init()` is allowed, as it is on the robot.
 
 **"SimLoop refused a call in MyAuto's code, so the run fails even though that code carried on ...: SimLoop
 cannot run Thread.sleep(50) at ... on sim time: it was called on thread "arm-helper", and only MyAuto's own

SimLoop/src/main/java/org/horizon36596/simloop/opmode/IterativeOpModeStepper.java

@@ -19,14 +19,41 @@
  *
  * <p>Breakpoints in an iterative OpMode still work: a debugger stops at them on any thread, and one that
  * suspends every thread (the default in IntelliJ and Android Studio) pauses the watchdog too.
+ *
+ * <h2>A wait in {@code init()}</h2>
+ * The SDK calls {@code init()} on the OpMode's own thread and starts {@code init_loop()} only once it returns,
+ * so on a robot {@code init()} may block: a library that sleeps while a sensor calibrates (Pedro Pathing's
+ * Pinpoint localizer sleeps 500 ms in its constructor) just makes INIT take longer. Under
+ * {@link OpModeRunner#forTeamClass} such a rewritten wait holds {@code init()} for whole ticks of sim time,
+ * rounded up, through {@link #holdForSimSeconds}. While it holds:
+ * <ul>
+ *   <li>sim time moves and the devices and plants update, and no other OpMode method runs, so the tick on
+ *       which {@code init()} returns runs nothing else and {@code init_loop()} starts on the tick after;</li>
+ *   <li>gamepads and {@code time} are not updated, since the SDK updates them only before a call
+ *       ({@code OpMode.internalPreUserCode()}), but {@code getRuntime()} follows the sim clock as it follows
+ *       the real one on the robot;</li>
+ *   <li>if PLAY comes first, {@code init_loop()} never runs and {@code start()} runs, with the first
+ *       {@code loop()}, on the tick after {@code init()} returns, as {@code internalRunOpMode()} goes straight
+ *       to {@code start()} once {@code isStarted} is set;</li>
+ *   <li>if STOP comes first, the wait returns normally, because the SDK does not interrupt an iterative
+ *       OpMode at STOP ({@code OpModeInternal.internalStop()} waits for its thread to finish). Sim time ends
+ *       at STOP, so the rest of the wait passes with none; then {@code init()} finishes, {@code start()} runs
+ *       if PLAY had come, and {@code stop()} runs.</li>
+ * </ul>
+ * A wait in {@code init_loop()}, {@code start()}, {@code loop()} or {@code stop()} is still refused (see
+ * {@link SimClockBridge}).
  */
 final class IterativeOpModeStepper implements OpModeStepper {
 
     /** The SDK's own force-stop exception, thrown by {@code terminateOpModeNow()}; matched by name. */
     static final String FORCE_STOP = "com.qualcomm.robotcore.eventloop.opmode.OpModeManagerImpl$ForceStopException";
 
+    /** The name {@link #callInProgress} has while {@code init()} runs: the one call that may wait. */
+    private static final String INIT = "init()";
+
     private final OpMode opMode;
     private final OpModeContext context;
+    private final double tickSeconds;
     private final int maxWaits;
     private final CallThread thread;
 
@@ -39,6 +66,9 @@ final class IterativeOpModeStepper implements OpModeStepper {
     /** True once the call last handed over has returned or thrown. Guarded by {@link #lock}. */
     private boolean callDone;
 
+    /** True while that call is held in a wait, until the sim resumes it on the next tick. Guarded by {@link #lock}. */
+    private boolean parked;
+
     /** What that call threw, or null. Guarded by {@link #lock}. */
     private Throwable thrown;
 
@@ -48,15 +78,36 @@ final class IterativeOpModeStepper implements OpModeStepper {
     /** Set on the OpMode thread by {@code requestOpModeStop()}; read on the sim thread after the call. */
     private volatile boolean askedToStop;
 
+    /** The call running on the OpMode thread, such as {@code "init()"}, or null between calls. */
+    private volatile String callInProgress;
+
+    /** STOP has come: a wait in progress, or one started after it, takes no more ticks. */
+    private volatile boolean stopping;
+
+    /** Sim thread only: the call held in a wait across ticks, or null. */
+    private String heldCall;
+
+    /** Sim thread only: PLAY has been pressed. */
+    private boolean played;
+
+    /** Sim thread only: {@code start()} has been called. */
+    private boolean startCalled;
+
     /** Sim thread only: the OpMode has finished. */
     private boolean ended;
 
     /** Sim thread only: the OpMode thread got stuck and was left behind. */
     private boolean abandoned;
 
-    IterativeOpModeStepper(OpMode opMode, OpModeContext context, int maxWaits, ThreadGroup group) {
+    /**
+     * @param tickSeconds the length of one tick, in seconds of sim time, for rounding a wait in {@code init()}
+     * @param maxWaits    the stuck timeout, in one-millisecond waits
+     */
+    IterativeOpModeStepper(OpMode opMode, OpModeContext context, double tickSeconds, int maxWaits,
+                           ThreadGroup group) {
         this.opMode = opMode;
         this.context = context;
+        this.tickSeconds = tickSeconds;
         this.maxWaits = maxWaits;
         this.thread = new CallThread(group);
     }
@@ -74,28 +125,110 @@ public void runTick(OpModePhase phase, int tickInPhase, int tick) {
         }
         switch (phase) {
             case INIT:
-                call("init()", phase, tick, opMode::init);
+                call(INIT, phase, tick, opMode::init);
                 break;
             case INIT_LOOP:
-                call("init_loop()", phase, tick, opMode::init_loop);
+                if (!resumeHeldCall(phase, tick)) {
+                    call("init_loop()", phase, tick, opMode::init_loop);
+                }
                 break;
             case ACTIVE:
                 if (tickInPhase == 0) {
                     context.markStarted();
+                    played = true;
+                }
+                if (resumeHeldCall(phase, tick)) {
+                    break;
+                }
+                if (!startCalled) {
+                    startCalled = true;
                     call("start()", phase, tick, opMode::start);
                 }
                 if (!ended) {
                     call("loop()", phase, tick, opMode::loop);
                 }
                 break;
             case STOP:
-                stop(phase, tick);
+                // Before the held call resumes, so its wait ends now: there is no sim time after STOP.
+                stopping = true;
+                context.markStopRequested();
+                resumeHeldCall(phase, tick);
+                if (!ended && played && !startCalled) {
+                    // PLAY came while init() waited: internalRunOpMode() still calls start() before stop().
+                    startCalled = true;
+                    call("start()", phase, tick, opMode::start);
+                }
+                if (!ended) {
+                    stop(phase, tick);
+                }
                 break;
             default:
                 throw new IllegalStateException("unknown phase " + phase);
         }
     }
 
+    /** {@return true while {@code init()} is running on the OpMode thread, where a wait holds for sim time} */
+    boolean inInit() {
+        return INIT.equals(callInProgress);
+    }
+
+    /**
+     * On the OpMode's own thread, in {@code init()}: holds it for enough ticks to cover {@code seconds} of sim
+     * time, the way the sim-clock hooks ask for a rewritten {@code Thread.sleep}. Each tick the sim resumes it
+     * instead of calling another OpMode method.
+     *
+     * @param seconds how long, in seconds of sim time
+     * @return true if the wait returns normally: always, unless the run is being torn down, because the SDK
+     *         does not interrupt an iterative OpMode at STOP (after STOP the rest of the wait takes no time)
+     */
+    boolean holdForSimSeconds(double seconds) {
+        long ticks = LinearOpModeStepper.ticksFor(seconds, tickSeconds);
+        for (long t = 0; t < ticks && !stopping; t++) {
+            if (!park()) {
+                return false;
+            }
+        }
+        return true;
+    }
+
+    /**
+     * On the OpMode thread: ends this tick for the call in progress and blocks until the sim resumes it.
+     *
+     * @return false if the OpMode thread must end instead (the run is being torn down)
+     */
+    private boolean park() {
+        synchronized (lock) {
+            parked = true;
+            lock.notifyAll();
+            while (parked && !exit) {
+                try {
+                    lock.wait();
+                } catch (InterruptedException ignored) {
+                    // Only the sim resumes a held call; an interrupt is not that.
+                }
+            }
+            return !exit;
+        }
+    }
+
+    /**
+     * Resumes the call held in a wait, if there is one, for this tick.
+     *
+     * @return true if there was one: this tick belonged to it, whether or not it has now returned
+     */
+    private boolean resumeHeldCall(OpModePhase phase, int tick) {
+        if (heldCall == null) {
+            return false;
+        }
+        context.resumeUserCode();
+        synchronized (lock) {
+            parked = false;
+            lock.notifyAll();
+        }
+        finishCall(heldCall, phase, tick);
+        return true;
+    }
+
     @Override
     public boolean hasEnded() {
         return ended;
@@ -148,10 +281,31 @@ private void stop(OpModePhase phase, int tick) {
 
     private void call(String method, OpModePhase phase, int tick, Runnable userCode) {
         context.beforeUserCode();
-        Throwable failure = runOnOpModeThread(method, phase, tick, () -> {
+        callInProgress = method;
+        handOver(() -> {
             userCode.run();
             opMode.telemetry.update();
         });
+        finishCall(method, phase, tick);
+    }
+
+    /**
+     * Waits for the call on the OpMode thread to return or to be held in a wait, and once it has returned does
+     * what follows a call: surface what it threw, and run {@code stop()} if it asked to stop.
+     */
+    private void finishCall(String method, OpModePhase phase, int tick) {
+        Throwable failure;
+        synchronized (lock) {
+            awaitCallOrPark(method, phase, tick);
+            if (parked) {
+                heldCall = method;
+                context.afterUserCode(); // a resetRuntime() before the wait counts from this tick, not the next
+                return;
+            }
+            failure = thrown;
+        }
+        heldCall = null;
+        callInProgress = null;
         if (failure != null) {
             if (FORCE_STOP.equals(failure.getClass().getName())) {
                 ended = true; // terminateOpModeNow(): the SDK ends the OpMode on the spot, without stop()
@@ -168,13 +322,8 @@ private void call(String method, OpModePhase phase, int tick, Runnable userCode)
         }
     }
 
-    /**
-     * Hands {@code work} to the OpMode thread and waits, bounded, for it to finish.
-     *
-     * @return what it threw, or null
-     * @throws OpModeException if it did not finish within {@link #maxWaits} one-millisecond waits
-     */
-    private Throwable runOnOpModeThread(String method, OpModePhase phase, int tick, Runnable work) {
+    /** Hands {@code work} to the OpMode thread, starting the thread on the first call. */
+    private void handOver(Runnable work) {
         if (thread.getState() == Thread.State.NEW) {
             thread.start();
         }
@@ -183,21 +332,29 @@ private Throwable runOnOpModeThread(String method, OpModePhase phase, int tick,
             callDone = false;
             thrown = null;
             lock.notifyAll();
-            int waits = 0;
-            while (!callDone) {
-                if (waits++ >= maxWaits) {
-                    throw abandon(method, phase, tick);
-                }
-                try {
-                    lock.wait(1);
-                } catch (InterruptedException interrupted) {
-                    Thread.currentThread().interrupt();
-                    throw new OpModeException("The thread running the sim was interrupted while "
-                            + opMode.getClass().getSimpleName() + "." + method + " was running.", phase, tick,
-                            interrupted);
-                }
+        }
+    }
+
+    /**
+     * Waits, bounded, until the call on the OpMode thread has returned or is held in a wait. Caller holds the
+     * lock.
+     *
+     * @throws OpModeException if neither happened within {@link #maxWaits} one-millisecond waits
+     */
+    private void awaitCallOrPark(String method, OpModePhase phase, int tick) {
+        int waits = 0;
+        while (!callDone && !parked) {
+            if (waits++ >= maxWaits) {
+                throw abandon(method, phase, tick);
+            }
+            try {
+                lock.wait(1);
+            } catch (InterruptedException interrupted) {
+                Thread.currentThread().interrupt();
+                throw new OpModeException("The thread running the sim was interrupted while "
+                        + opMode.getClass().getSimpleName() + "." + method + " was running.", phase, tick,
+                        interrupted);
             }
-            return thrown;
         }
     }