#![doc = include_str!("../Readme.md")]

mod generic_float;

use crate::generic_float::Scalar;
use std::ops::{Add, AddAssign, Div, Mul, Sub};

/// An intuitive description of the spring system from which [SpringParameters] can be built (Using [Into::into])
pub struct SpringDescription<Scalar> {
    /// `f`, the natural frequency
    ///
    /// Corresponds to:
    /// - The speed at which the system responds to changes in input
    /// - The frequency at which the system will vibrate
    pub frequency: Scalar,
    /// `Zeta`, the damping coefficient
    ///
    /// Describes how fast the system settles to its target:
    /// - `0` (undamped): vibration never dies
    /// - `0 < Zeta <= 1` (underdamped): vibration dies
    /// - `Zeta > 1`: system does not vibrate, but approaches its target
    pub damping: Scalar,
    /// `r`, the initial response
    ///
    /// - Negative: system anticipates motion by going in the opposite direction shortly
    /// - `0`: system takes time to begin accelerating from rest
    /// - Positive: system immediately reacts to changes
    /// - `r > 1`: system overshoots target at first
    pub initial_response: Scalar,
}

impl<S> From<SpringDescription<S>> for SpringParameters<S>
where
    S: Copy + Scalar + Add<Output = S> + Sub<Output = S> + Mul<Output = S> + Div<Output = S>,
{
    fn from(description: SpringDescription<S>) -> SpringParameters<S> {
        let SpringDescription {
            frequency: f,
            damping: z,
            initial_response: r,
        } = description;

        let two_pi_f = S::TWO * S::PI * f;

        let k_1 = z / (S::PI * f);
        let k_2 = S::ONE / (two_pi_f * two_pi_f);
        let k_3 = r * z / two_pi_f;
        Self {
            k_1,
            k_2,
            k_3,
            max_safe_time_delta: (S::FOUR * k_2 + k_1 * k_1).square_root() - k_1,
        }
    }
}

/// Parameters that determine the behavior of the system
///
/// Use [SpringDescription] to construct these
pub struct SpringParameters<Scalar> {
    k_1: Scalar,
    k_2: Scalar,
    k_3: Scalar,
    max_safe_time_delta: Scalar,
}

struct SpringState<T> {
    position: T,
    velocity: T,
}

/// A spring system for spring-mechanic-inspired animations
pub struct SpringSystem<T = f32, Scalar = f32> {
    parameters: SpringParameters<Scalar>,
    state: SpringState<T>,
    previous_target_position: T,
}

impl<T: Copy, Scalar> SpringSystem<T, Scalar> {
    /// Construct a system with the specified parameters and initial position/velocity
    pub fn new(parameters: impl Into<SpringParameters<Scalar>>, position: T, velocity: T) -> Self {
        Self {
            parameters: parameters.into(),
            state: SpringState { position, velocity },
            previous_target_position: position,
        }
    }
}

fn partial_min<T: PartialOrd>(a: T, b: T) -> T {
    if a < b {
        a
    } else {
        b
    }
}

impl<T, S> SpringSystem<T, S>
where
    T: Copy
        + Add<T, Output = T>
        + Sub<T, Output = T>
        + Mul<S, Output = T>
        + Div<S, Output = T>
        + AddAssign<T>,
    S: Copy + PartialOrd,
{
    /// Perform a step of the simulation and return the new position
    ///
    /// If the `time_delta` is too large to safely use without losing stability, it will be clamped to a safe maximum value
    pub fn step_clamped(&mut self, time_delta: S, target: T) -> T {
        let estimated_velocity = target - self.previous_target_position;
        let clamped_delta = partial_min(time_delta, self.parameters.max_safe_time_delta);

        self.step_with_target_velocity(clamped_delta, target, estimated_velocity);
        self.previous_target_position = target;

        self.state.position
    }

    fn step_with_target_velocity(&mut self, time_delta: S, target: T, target_velocity: T) {
        let SpringParameters { k_1, k_2, k_3, .. } = self.parameters;

        self.state.position += self.state.velocity * time_delta;

        let acceleration =
            (target + target_velocity * k_3 - self.state.position - self.state.velocity * k_1)
                / k_2;
        self.state.velocity += acceleration * time_delta;
    }
}