(* Discontinuous blocks in discrete time *)

(* Coulomb and Viscous Friction *)
let discrete sgn(x) = if x > 0.0 then 1.0 else if x < 0.0 then -1.0 else 0.0

let discrete coulomb(gain, offset, x) = y where
  rec y = sgn(x) *. (gain *. abs_float (x) +. offset)

let discrete coulomb_scalar_input(n)(gain, offset, x) = y where
  rec
    g = gain *. abs_float (x)
  and
    forall i in 0 .. (n - 1), offseti in offset, yi out y do
      yi = sgn(x) *. (g +. offseti)
    done

let discrete coulomb_vectors(n)(gain, offset, x) = y where
  rec forall i in 0 .. (n - 1), xi in x, offseti in offset, yi out y do
        yi = sgn(xi) *. (gain *. abs_float (xi) +. offseti)
      done

(* Quantizer. Produce a staircase function *)

let discrete quantizer(q, u) = y where y = q *. floor(u /. q)

(* Saturation block. *)

let saturation(upper, lower, u) = min (max lower u) upper


(* Relay *)

let node relay(son, soff, von, voff, u) = r where
    rec automaton
    | On -> do r = von unless (soff -. u > 0.) then Off
    | Off -> do r = voff unless (u -. son > 0.) then On
    end

(* Rising edge. *)
(* forbids two consecutive hits, e.g., x(n-2) < 0.0, x(n-1) = 0.0, x(n) > 0.0 *)
let node rising x =
  let rec hit = false -> if x > 0.0 then pre(x) <= 0.0 && not (pre hit)
                else if x = 0.0 then pre x < 0.0 else false in
  hit

let node falling x =
  let rec hit = false -> if x < 0.0 then pre x >= 0.0 && not (pre hit)
                         else if x = 0.0 then pre x > 0.0 else false in
  hit
      
let node either x =
  let rec hit = false -> if x > 0.0 then pre x <= 0.0
                         else if x = 0.0 then pre x < 0.0
                         else if x < 0.0 then pre x >= 0.0
		         else false in
  hit
		    
let node dead_zone(ll, ul, u) = y where
  y = if u <= ll then u -. ll
      else if u >= ul then u -. ul
      else 0.0

let node wrap_to_zero(threshold, u) = y where
  y = if u > threshold then 0.0 else u
  
let node backlash (width, y0, (u, u')) = y where
  rec half_width = width /. 2.0
  and init y = y0
  and automaton
      | Disengaged ->
           do unless (u -.  (last y +. half_width) >= 0.) then Engaged_positive
           else (u -. (last y -. half_width) <= 0.) then Engaged_negative
      | Engaged_positive ->
          do y = u -. half_width
          unless (u' < 0.) then Disengaged
      | Engaged_negative ->
          do y = u +. half_width
          unless (u' > 0.) then Disengaged
      end

let node memory(i, x) = i fby x