use kcan::{LowPassFilterConfig, PIDConfig, PIDController, SecondOrderLowPassFilter, SolverType};

fn assert_close(left: f64, right: f64, tolerance: f64) {
    assert!(
        (left - right).abs() <= tolerance,
        "left={left} right={right} tolerance={tolerance}"
    );
}

#[test]
fn pid_first_output_is_zero() {
    let mut pid = PIDController::new(PIDConfig::default(), LowPassFilterConfig::default());

    assert_eq!(pid.compute_output(1.0, 1.0, 0.0), 0.0);
}

#[test]
fn pid_proportional_term_only() {
    let mut pid = PIDController::new(
        PIDConfig {
            proportional_gain: 2.0,
            integral_gain: 0.0,
            derivative_gain: 0.0,
            output_limits: Some((-100.0, 100.0)),
        },
        LowPassFilterConfig::default(),
    );

    pid.compute_output(0.0, 0.0, 0.0);
    let output = pid.compute_output(1.0, 1.0, 0.5);

    assert_close(output, 1.0, f64::EPSILON);
}

#[test]
fn pid_clamps_large_error_to_sixty_degrees() {
    let mut pid = PIDController::new(
        PIDConfig {
            proportional_gain: 1.0,
            integral_gain: 0.0,
            derivative_gain: 0.0,
            output_limits: Some((-100.0, 100.0)),
        },
        LowPassFilterConfig::default(),
    );

    pid.compute_output(0.0, 0.0, 0.0);
    let output = pid.compute_output(1.0, 500_f64.to_radians(), 0.0);

    assert_close(output, 60_f64.to_radians(), 1e-12);
}

#[test]
fn pid_output_is_clamped() {
    let mut pid = PIDController::new(
        PIDConfig {
            proportional_gain: 1000.0,
            integral_gain: 0.0,
            derivative_gain: 0.0,
            output_limits: Some((-5.0, 5.0)),
        },
        LowPassFilterConfig::default(),
    );

    pid.compute_output(0.0, 0.0, 0.0);
    assert_eq!(pid.compute_output(1.0, 1.0, 0.0), 5.0);
}

#[test]
fn pid_reset_clears_internal_state() {
    let mut pid = PIDController::default();
    pid.compute_output(0.0, 1.0, 0.0);
    pid.compute_output(1.0, 1.0, 0.0);

    pid.reset();

    assert_eq!(pid.integral(), 0.0);
    assert_eq!(pid.previous_velocity(), 0.0);
}

#[test]
fn low_pass_filter_converges_to_constant_input() {
    let mut filter = SecondOrderLowPassFilter::new(LowPassFilterConfig {
        cut_off_frequency_rad_per_sec: 20.0,
        damping_ratio: 1.0,
        initial_condition: 0.0,
        solver_type: SolverType::RungeKutta,
    });

    let mut output = 0.0;
    for _ in 0..500 {
        (output, _) = filter.step(1.0, 0.01);
    }

    assert_close(output, 1.0, 1e-2);
}

#[test]
fn low_pass_filter_reset_restores_initial_condition() {
    let mut filter = SecondOrderLowPassFilter::new(LowPassFilterConfig {
        cut_off_frequency_rad_per_sec: 20.0,
        damping_ratio: 1.0,
        initial_condition: 2.5,
        solver_type: SolverType::ForwardEuler,
    });

    filter.step(10.0, 0.01);
    filter.reset();

    assert_close(filter.state().filtered_output, 2.5, f64::EPSILON);
    assert_close(filter.state().feedback_state, 2.5, f64::EPSILON);
}
