(* Coulomb friction *)
let hybrid sgn(x) =
  present up(x) -> 1.0 | up(-. x) -> -1.0 init 0.0

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

let hybrid 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 hybrid 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 *)
let hybrid quantizer(q, u) = n where
  rec init n = q *. floor(u /. q)
  and nq = n *. q
  and present up(u -. nq -. q) -> do n = last n +. 1.0 done
            | up(nq -. u) -> do n = last n -. 1.0 done

(* Saturation block *)
let saturation(upper, lower, u) = min (max lower u) upper

(* Relay *)
let hybrid relay(son, so , von, vo , u) = r where
  rec automaton
      | On -> do r = von unless up(so -. u) then Off
      | Off -> do r = vo  unless up(u -. son) then On
      end

(* Comparison, rising, falling edges *)
type pos = Positive | Negative | Between

let hybrid compare(epsilon, x) = pos where
  rec automaton
      | BeetweenState ->
	  do pos = Between
	  unless up(x -. epsilon) then PositiveState
	  else up(-. x) then NegativeState
      | PositiveState ->
	  do pos = Positive unless up(-. x) then NegativeState
      | NegativeState ->
	  do pos = Negative unless up(x -. epsilon) then PositiveState
      end

(* Dead zone *)
let hybrid dead_zone(ll, ul, u) = y where
  rec epsilon = 0.0
  and y = match compare(epsilon, u -. ul +. epsilon) with
	  | Positive -> u -. ul
	  | Between | Negative ->
	      match compare(epsilon, u -. ll -. epsilon) with
	      | Negative -> u -. ll
	      | Positive | Between -> 0.0

(* Backlash *)
(*
let hybrid backlash (width, y0, (u, u')) = y where
  rec half_width = width /. 2.0
  and init y = y0
  and automaton
      | Disengaged ->
	  do unless up(u -. (y +. half_width)) then Engaged_positive
	  else up( -.u +. (y -. half_width)) then Engaged_negative
      | Engaged_positive ->
	  do y = u -. half_width unless up(-. u') then Disengaged
      | Engaged_negative ->
	  do y = u +. half_width unless up(u') then Disengaged
      end
      *)
  
let hybrid backlash (width, y0, (u, u')) = y where
  rec half_width = width /. 2.0
  and init y = y0
  and automaton
      | Disengaged ->
	  do unless up(u -. (last y +. half_width)) then Engaged_positive
	  else up( -.u +. (last y -. half_width)) then Engaged_negative
      | Engaged_positive ->
	  do y = u -. half_width unless up(-. u') then Disengaged
      | Engaged_negative ->
	  do y = u +. half_width unless up(u') then Disengaged
      end