use crate::Scalar;
pub(crate) fn pi_controller_raw<T: Scalar>(
error_norm: T,
prev_error_norm: Option<T>,
pi_integral: T,
pi_proportional: T,
eff_order: usize,
) -> T {
let order_f = T::from_usize(eff_order).unwrap();
let ki = pi_integral / order_f;
if pi_proportional == T::zero() {
error_norm.pow(-ki)
} else {
match &prev_error_norm {
Some(prev) => {
let kp = pi_proportional / order_f;
let e_iexp = error_norm.pow(-(ki + kp));
let e_pexp = prev.pow(kp);
e_iexp * e_pexp
}
None => error_norm.pow(-ki),
}
}
}