pub trait Controller<const D: usize> {
    fn determine_step(&mut self, h: f64, err: f64) -> (bool, f64);
}

#[derive(Debug, Clone, Copy)]
pub struct PIController {
    pub alpha: f64,
    pub beta: f64,
    pub factor_c1: f64,
    pub factor_c2: f64,
    pub factor_old: f64,
    pub h_max: f64,
    pub safety_factor: f64,
    pub old_h: f64,
}

impl<const D:usize> Controller<D> for PIController {
    /// Determines if the previously run step size and error were valid or not. Either way, it also
    /// returns what the next step size should be
    fn determine_step(&mut self, h: f64, err: f64) -> (bool, f64) {
        let factor_11 = err.powf(self.alpha);
        let factor = self.factor_c2.max(self.factor_c1.min(factor_11 * self.factor_old.powf(-self.beta) / self.safety_factor));
        let mut h_new = h / factor;
        // let mut h_new = 0.9 * h * err.powf(-1.0 / 5.0);
        println!("err: {}\th_new: {}", err, h_new);
        if err <= 1.0 {
            // Accept the stepsize
            self.factor_old = err.max(1.0e-4);
            if h_new.abs() > self.h_max {
                // If the step is too big
                h_new = self.h_max.copysign(h_new);
            }
            (true, h_new)
            // (true, h_new)
        } else {
            // Reject the stepsize and propose a smaller one
            h_new = h / (self.factor_c1.min(factor_11 / self.safety_factor));
            (false, h_new)
        }
    }
}

impl PIController {
    pub fn new(alpha:f64, beta:f64, max_factor: f64, min_factor: f64, h_max: f64, safety_factor: f64, initial_h: f64) -> Self {
        Self {
            alpha: alpha,
            beta: beta,
            factor_c1: 1.0 / min_factor,
            factor_c2: 1.0 / max_factor,
            factor_old: 1.0e-4,
            h_max: h_max.abs(),
            safety_factor: safety_factor,
            old_h: initial_h,
        }
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn test_controller_creation() {
        let controller = PIController::new(0.17, 0.04, 10.0, 0.2, 10.0, 0.9, 1e-4);

        assert!(controller.alpha == 0.17);
        assert!(controller.beta == 0.04);
        assert!(controller.factor_c1 == 1.0/0.2);
        assert!(controller.factor_c2 == 1.0/10.0);
        assert!(controller.factor_old == 1.0e-4);
        assert!(controller.h_max == 10.0);
        assert!(controller.safety_factor == 0.9);
        assert!(controller.old_h == 1e-4);
    }
}