pub struct Hysteresis {
    fs: f32,
    t: f32,

    m_s: f32,
    a: f32,
    c: f32,

    // Save calculations
    nc: f32,
    m_s_oa: f32,
    m_s_oa_tc: f32,
    m_s_oa_tc_talpha: f32,

    // state variables
    m_n1: f32,
    h_n1: f32,
    h_d_n1: f32,
}

impl Hysteresis {
    const ALPHA: f32 = 1.6e-3;
    const D_ALPHA: f32 = 0.9;
    const UPPER_LIM: f32 = 20.0;
    const K: f32 = 0.47875;

    pub fn new() -> Self {
        let m_s = 1.0;
        let a = m_s / 4.0;
        let c = 1.7e-1;

        let fs = 48000.0;
        let t = 1.0 / fs;
        Self {
            fs,
            t,

            m_s,
            a,
            c,

            // Save calculations
            nc: 1.0 - c,
            m_s_oa: m_s / a,
            m_s_oa_tc: c * m_s / a,
            m_s_oa_tc_talpha: Self::ALPHA * c * m_s / a,

            // state variables
            m_n1: 0.0,
            h_n1: 0.0,
            h_d_n1: 0.0,
        }
    }

    pub fn set_sample_rate(&mut self, sample_rate: f32) {
        // Sample rate
        self.fs = sample_rate;
        self.t = 1.0 / self.fs;
    }

    pub fn process(&mut self, drive: f32, width: f32, sat: f32, h: f32) -> f32 {
        self.m_s = 0.5 + 1.5 * (1.0 - sat);
        self.a = self.m_s / (0.01 + 6.0 * drive);
        self.c = (1.0 - width).sqrt() - 0.01;

        self.nc = 1.0 - self.c;
        self.m_s_oa = self.m_s / self.a;
        self.m_s_oa_tc = self.c * self.m_s_oa;
        self.m_s_oa_tc_talpha = Self::ALPHA * self.m_s_oa_tc;

        let h_d = self.deriv(h, self.h_n1, self.h_d_n1);
        let mut m = self.solver(h, h_d);

        if m.is_nan() || m > Self::UPPER_LIM {
            m = 0.0;
        }

        self.m_n1 = m;
        self.m_n1 = h;
        self.h_d_n1 = h_d;

        m
    }

    fn deriv(&self, x_n: f32, x_n1: f32, x_d_n1: f32) -> f32 {
        (((1.0 + Self::D_ALPHA) / self.t) * (x_n - x_n1)) - Self::D_ALPHA * x_d_n1
    }

    fn solver(&self, h: f32, h_d: f32) -> f32 {
        // Corresponds to RK2
        let k1 = self.t * self.hysteresis_func(self.m_n1, self.h_n1, self.h_d_n1);
        let k2 = self.t
            * self.hysteresis_func(
                self.m_n1 + (k1 / 2.0),
                (h + self.h_n1) / 2.0,
                (h_d + self.h_d_n1) / 2.0,
            );

        self.m_n1 + k2
    }

    fn hysteresis_func(&self, m: f32, h: f32, h_d: f32) -> f32 {
        let q = (h + Self::ALPHA * m) / self.a;
        let coth = 1.0 / q.tanh();
        let near_zero = q.abs() < 0.001;

        let m_diff = self.m_s * langevin(coth, q, near_zero) - m;

        let delta: f32 = if h_d >= 0.0 { 1.0 } else { -1.0 };
        let delta_m = if delta.signum() == m_diff.signum() {
            1.0
        } else {
            0.0
        };

        let l_prime = langevin_d(coth, q, near_zero);

        let kap1 = self.nc * delta_m;
        let f1_denom = self.nc * delta * Self::K - Self::ALPHA * m_diff;
        let f1 = kap1 * m_diff / f1_denom;
        let f2 = self.m_s_oa_tc * l_prime;
        let f3 = 1.0 - (self.m_s_oa_tc_talpha * l_prime);

        h_d * (f1 + f2) / f3
    }
}

fn langevin(coth: f32, x: f32, near_zero: bool) -> f32 {
    if near_zero {
        x / 3.0
    } else {
        coth - (1.0 / x)
    }
}

fn langevin_d(coth: f32, x: f32, near_zero: bool) -> f32 {
    if near_zero {
        1.0 / 3.0
    } else {
        (1.0 / (x * x)) - (coth * coth) + 1.0
    }
}