use num_traits::Float;
use strafe_type::{FloatConstraint, Positive64, Real64};
use crate::traits::DPQ;
pub fn dnorm<R1: Into<Real64>, R2: Into<Real64>, P: Into<Positive64>>(
x: R1,
mu: R2,
sigma: P,
log: bool,
) -> Real64 {
let mut x = x.into().unwrap();
let mu = mu.into().unwrap();
let sigma = sigma.into().unwrap();
if !sigma.is_finite() {
return f64::d_0(log).into();
}
if !x.is_finite() && mu == x {
return f64::nan().into();
}
if sigma == 0.0 {
return if x == mu {
f64::infinity()
} else {
f64::d_0(log)
}
.into();
}
x = (x - mu) / sigma;
if !x.is_finite() {
return f64::d_0(log).into();
}
x = x.abs();
if x >= 2.0 * f64::MAX.sqrt() {
return f64::d_0(log).into();
}
if log {
return (-(strafe_consts::LN_SQRT_2TPI + 0.5 * x * x + sigma.ln())).into();
}
if x < 5.0 {
return (strafe_consts::_1DSQRT_2TPI * (-0.5 * x * x).exp() / sigma).into();
} else {
if x > (-2.0
* std::f64::consts::LN_2
* (f64::MIN_EXP as f64 + 1.0 - std::f64::MANTISSA_DIGITS as f64))
.sqrt()
{
return 0.0.into();
}
let x1 = x.ldexp(16.0).round().ldexp(-16.0);
let x2 = x - x1;
let ret = strafe_consts::_1DSQRT_2TPI / sigma
* ((-0.5 * x1 * x1).exp() * ((-0.5 * x2 - x1) * x2).exp());
ret.into()
}
}