diff --git a/src/lib/hsim/sim.ml b/src/lib/hsim/sim.ml index 5ab2dc9..9251cd7 100644 --- a/src/lib/hsim/sim.ml +++ b/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 -> ()); - let ms = match zin with Some z -> zset ms z | None -> ms 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; + (* 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 = - let ms, ss, last = get_mstate s, get_sstate s, get_last s in - (match last with None -> () | Some { h; u; _ } -> ignore (u h)); + (** Continuous step. *) + let step_continuous + (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 = min h (min stop (hor ms)) in + let (h, f, z), ss = step ss stop 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 - | Continuous -> step_continuous st + | Discrete -> dstep 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 state = get_init m.body.state s.state, al in + let al = List.map (fun a -> run_assert a get_s) assertions in + let state = get_init m.state s.state, al in - let step_discrete ?(reinit=false) (st, al) = - let m=m.body in + let dstep ?(reinit=false) (st, al) = 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 -> - let mode, now, stop = Discrete, 0.0, i.h in - step_discrete (set_running ~mode ~input:i ~now ~stop st, al) + | Some input -> + let mode, now, stop = Discrete, 0.0, input.h in + 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) - | Continuous -> step_continuous (st, al) + | Discrete -> dstep (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 }