fix: update stop time correctly

src/lib/hsim/sim.ml

@@ -7,12 +7,31 @@ module Sim (S : SimState) =
   struct
     include S
 
+    (** Discrete step. *)
     let step_discrete
-      s step hor fder fzer cget zset csize zsize jump reset reinit
+      (s : ('a, 'b, 'ms, 'ss, 'zin) state)
+      (step : 'ms -> time -> 'a -> 'b * 'ms)
+      (hor : 'ms -> time)
+      (fder : 'ms -> time -> 'a -> 'y -> 'yder)
+      (fzer : 'ms -> time -> 'a -> 'y -> 'zout)
+      (cget : 'ms -> 'y)
+      (zset : 'ms -> 'zin -> 'ms)
+      (csize : int)
+      (zsize : int)
+      (jump : 'ms -> bool)
+      (reset : ('y, 'yder) ivp * ('y, 'zout) zc -> 'ss -> 'ss)
+      (reinit : bool)
+    : 'b value * ('a, 'b, 'ms, 'ss, 'zin) state
     = let ms, ss = get_mstate s, get_sstate s in
       let zin, last = get_zin s, get_last s in
-      (match last with Some { h; u; _ } -> ignore (u h) | None -> ());
+      (* Since the last output value might have been used before this call,
-      let ms = match zin with Some z -> zset ms z | None -> ms in
+         which may have changed the solver state, we call it at its horizon to
+         make sure the solver state is correct. *)
+      Option.iter (fun { h; u; _ } -> ignore (u h)) last;
+      (* Similarly, we update the zero-crossing information from the last step
+         now, so that it is used at the right time (and not when using the last
+         output value). *)
+      let ms = Option.fold ~none:ms ~some:(zset ms) zin in
       let i, now, stop = get_input s, get_now s, get_stop s in
       let o, ms = step ms now (i.u now) in
       let s =
@@ -27,19 +46,29 @@ module Sim (S : SimState) =
           let zc = { init; fzer; size=zsize } in
           let ss = reset (ivp, zc) ss in
           let input = { i with h=i.h -. now; u=Utils.offset i.u now } in
-          let mode, stop, now = Continuous, i.h, 0.0 in
+          let mode, stop, now = Continuous, input.h, 0.0 in
           update ms ss (set_running ~mode ~input ~stop ~now s)
         end else set_running ~mode:Continuous s in
       let o = Utils.dot o in
       o, (set_last (Some o) (set_zin None s))
 
-    let step_continuous s step cset fout hor =
+    (** Continuous step. *)
-      let ms, ss, last = get_mstate s, get_sstate s, get_last s in
+    let step_continuous
-      (match last with None -> () | Some { h; u; _ } -> ignore (u h));
+      (s : ('a, 'b, 'ms, 'ss, 'zin) state)
+      (step : 'ss -> time -> (time * (time -> 'y) * 'zin option) * 'ss)
+      (cset : 'ms -> 'y -> 'ms)
+      (fout : 'ms -> time -> 'a -> 'y -> 'b)
+      (hor : 'ms -> time)
+    : 'b value * ('a, 'b, 'ms, 'ss, 'zin) state * 'ms value
+    = let ms, ss, last = get_mstate s, get_sstate s, get_last s in
+      (* Since the last output value might have been used before this call,
+         which may have changed the solver state, we call it at its horizon to
+         make sure the solver state is correct. *)
+      Option.iter (fun { h; u; _ } -> ignore (u h)) last;
       let i, now, stop = get_input s, get_now s, get_stop s in
       let stop = min stop (hor ms) in
-      let (h, f, z), ss = step ss (min stop (hor ms)) in
+      let (h, f, z), ss = step ss stop in
-      let h = min h (min stop (hor ms)) in
+      let h = min h stop in
       let ms = cset ms (f h) in
       let fy t = f (now +. t) in
       let fms t = cset ms (fy t) in
@@ -60,11 +89,11 @@ module Sim (S : SimState) =
       (DNode s : ('y, 'yder, 'zin, 'zout) solver)
     : ('p * (('y, 'yder) ivp * ('y, 'zout) zc), 'a, 'b) sim
     = let state = get_init m.state s.state in
-      let step_discrete ?(reinit=false) st =
+      let dstep ?(reinit=false) st =
         let o, s = step_discrete st m.step m.horizon m.fder m.fzer m.cget m.zset
           m.csize m.zsize m.jump s.reset reinit in
         Some o, s in
-      let step_continuous st =
+      let cstep st =
         let o, s, _ = step_continuous st s.step m.cset m.fout m.horizon in
         Some o, s in
 
@@ -72,11 +101,11 @@ module Sim (S : SimState) =
         | Some i ->
             let mode, now, stop = Discrete, 0.0, i.h in
             let reinit = i.c = Discontinuous in
-            step_discrete ~reinit (set_running ~mode ~input:i ~now ~stop st)
+            dstep ~reinit (set_running ~mode ~input:i ~now ~stop st)
         | None ->
             if is_running st then match get_mode st with
-            | Discrete -> step_discrete st
+            | Discrete   -> dstep st
-            | Continuous -> step_continuous st
+            | Continuous -> cstep st
             else None, st in
 
       let reset (pm, ps) st =
@@ -90,13 +119,12 @@ module Sim (S : SimState) =
       'a 'b. ('p, 'a, 'b, 'y, 'yder, 'zin, 'zout) hnode_a ->
         (unit -> ('y, 'yder, 'zin, 'zout) solver) ->
         ('p * (('y, 'yder) ivp * ('y, 'zout) zc), 'a, 'b) sim =
-    fun (HNodeA m) get_s ->
+    fun (HNodeA { body=m; assertions }) get_s ->
       let DNode s = get_s () in
-      let al = List.map (fun a -> run_assert a get_s) m.assertions in
+      let al = List.map (fun a -> run_assert a get_s) assertions in
-      let state = get_init m.body.state s.state, al in
+      let state = get_init m.state s.state, al in
 
-      let step_discrete ?(reinit=false) (st, al) =
+      let dstep ?(reinit=false) (st, al) =
-        let m=m.body in
         let o, st =
           step_discrete st m.step m.horizon m.fder m.fzer m.cget m.zset m.csize
           m.zsize m.jump s.reset reinit in
@@ -105,9 +133,9 @@ module Sim (S : SimState) =
           DNode { a with state }) al in
         Some o, (st, al) in
 
-      let step_continuous (st, al) =
+      let cstep (st, al) =
         let ({ h; _ } as o), st, u =
-          step_continuous st s.step m.body.cset m.body.fout m.body.horizon in
+          step_continuous st s.step m.cset m.fout m.horizon in
         let al = List.map (fun (DNode a) ->
           (* Step assertions repeatedly until they reach the horizon. *)
           let rec step s =
@@ -120,18 +148,20 @@ module Sim (S : SimState) =
         Some o, (st, al) in
 
       let step (st, al) = function
-        | Some i ->
+        | Some input ->
-            let mode, now, stop = Discrete, 0.0, i.h in
+            let mode, now, stop = Discrete, 0.0, input.h in
-            step_discrete (set_running ~mode ~input:i ~now ~stop st, al)
+            dstep (set_running ~mode ~input ~now ~stop st, al)
         | None ->
             if is_running st then match get_mode st with
-            | Discrete   -> step_discrete (st, al)
+            | Discrete   -> dstep (st, al)
-            | Continuous -> step_continuous (st, al)
+            | Continuous -> cstep (st, al)
             else None, (st, al) in
 
       let reset (pm, ps) (st, al) =
-        let ms = m.body.reset pm (get_mstate st) in
+        let ms = m.reset pm (get_mstate st) in
         let ss = s.reset ps (get_sstate st) in
+        let al = List.map (fun (DNode a) ->
+          DNode { a with state = a.reset (pm, ps) a.state }) al in
         update ms ss (set_idle st), al in
 
       DNode { state; step; reset }