(* Discrete-time PID *)

let node filter1(n)(h)(k, u) = Dint.derivative(h)(k, u)

(* Apply a low pass filter on the input (see Astrom & Muray's book, 2008). *)
let node filter(n)(h)(k, u) = udot where
  rec udot = n *. (k *. u -. f)
  and f = Dint.int(h)(n, 0.0, udot)

(* a generic filter parameterized by the integration function *)
let node generic_filter(n)(h)(int)(k, u) = udot where
  rec udot = n *. (k *. u -. f)
  and f = run (int h)(n, 0.0, udot)

let node filter_forward(n)(h)(k, u) =
  generic_filter(n)(h)(Dint.forward_euler)(k, u)

(* A function that is not causaly correct *)
(*
let node filter_backward(n)(h)(k, u) =
  generic_filter(n)(h)(Dint.backward_euler)(k, u)
*)
    
(* p is the proportional gain;
 * i the integral gain;
 * d the derivative gain;
 * n the filter coefficient *)
let node pid_par(h)(n)(p, i, d, u) = c where
  rec c_p = p *. u
  and i_p = Dint.int(h)(i, 0.0, u)
  and c_d = filter(n)(h)(d, u)
  and c = c_p +. i_p +. c_d
      

(* A generic PID parameterized by the integration and filtering function *)
let node generic_pid(int)(filter)(h)(p, i, d, u) = c where
  rec c_p = p *. u
  and i_p = run (int h)(i, 0.0, u)
  and c_d = run (filter h)(d, u)
  and c = c_p +. i_p +. c_d

let node pid_forward_no_filter(h)(p, i, d, u) =
  generic_pid(Dint.forward_euler)
    (Dint.derivative)(h)(p, i, d, u)

let node pid_backward_no_filter(h)(p, i, d, u) =
  generic_pid(Dint.backward_euler)(Dint.derivative)(h)(p, i, d, u)