//! Virtual speed from measured power (§5.7 of REQUIREMENTS.md).
//!
//! The app owns the physics rather than trusting the trainer's reported speed
//! (FR-7.1). This makes ride behaviour reproducible in tests, independent of
//! the trainer's internal mass assumptions, and is a prerequisite for virtual
//! gearing later.
//!
//! Per tick:
//! ```text
//! F_propulsive = (P × drivetrain_efficiency) / max(v, v_min)
//! F_gravity    = m × g × sin(atan(gradient))
//! F_rolling    = m × g × Crr × cos(atan(gradient))
//! F_aero       = ½ × ρ × CdA × v²
//! a            = (F_propulsive − F_gravity − F_rolling − F_aero) / m
//! v           += a × Δt      (clamped at ≥ 0)
//! ```

use crate::types::RiderConfig;

pub const GRAVITY: f32 = 9.80665;

/// Speed floor used to keep `P / v` finite at a standstill. Also the speed
/// below which the rider is considered stopped.
pub const MIN_SPEED_MPS: f32 = 0.5;

/// Absolute ceiling on virtual speed, ~144 km/h. Aerodynamic drag bounds the
/// model well below this for any plausible input; the cap exists so that
/// absurd configuration (CdA of zero, a 90% descent) still cannot run away.
pub const MAX_SPEED_MPS: f32 = 40.0;

/// Ceiling on *measured* power fed to the model. FTMS Instantaneous Power is a
/// sint16, so a glitched packet can legitimately decode to 32767 W — which the
/// force balance faithfully turns into a 144 km/h ride. No human produces more
/// than ~2500 W even for a single track-sprint pedal stroke, so anything above
/// this is a bad reading, not a rider.
///
/// This is deliberately *not* [`crate::types::SafetyLimits::max_power_w`]: that
/// one bounds the ERG target we *command*, this one bounds the power we
/// *believe*.
pub const MAX_MEASURED_POWER_W: f32 = 2500.0;

/// Longest tick the integrator will honour. A caller that stalls for a minute
/// must not be allowed to teleport the rider down a mountain.
const MAX_DT_S: f32 = 10.0;

/// The integrator sub-divides the caller's `dt` to this resolution. Forward
/// Euler on `P/v` is stiff at low speed, so the result would otherwise depend
/// on how often the caller happens to tick; sub-stepping makes a 1 Hz tick and
/// a 4 Hz tick agree.
const SUBSTEP_S: f32 = 0.02;

/// Gradients beyond this are not physical roads and only appear as bad input.
const MAX_ABS_GRADIENT_PCT: f32 = 100.0;

/// Evolving physical state of the virtual rider.
#[derive(Debug, Clone, Copy, PartialEq, Default)]
pub struct PhysicsState {
    /// Virtual speed, metres per second.
    pub speed_mps: f32,
    /// Virtual distance travelled, metres.
    pub distance_m: f64,
    /// Cumulative elevation gained, metres.
    pub elevation_gain_m: f32,
}

impl PhysicsState {
    /// Advance the simulation by `dt` seconds under `power_w` at `gradient_pct`.
    ///
    /// Must model inertia (FR-7.3) — speed accelerates toward equilibrium
    /// rather than snapping to it — and must never produce negative speed,
    /// NaN, or unbounded values for any finite input.
    pub fn step(&mut self, power_w: f32, gradient_pct: f32, cfg: &RiderConfig, dt: f32) {
        let dt = sanitise(dt, 0.0).clamp(0.0, MAX_DT_S);
        if dt <= 0.0 {
            return;
        }

        let forces = Forces::new(power_w, gradient_pct, cfg);

        // Recover from a poisoned state rather than propagating it: a single
        // bad tick must not permanently wedge the ride.
        if !self.speed_mps.is_finite() {
            self.speed_mps = 0.0;
        }
        if !self.distance_m.is_finite() {
            self.distance_m = 0.0;
        }
        if !self.elevation_gain_m.is_finite() {
            self.elevation_gain_m = 0.0;
        }

        let steps = (dt / SUBSTEP_S).ceil().max(1.0);
        let h = dt / steps;
        let steps = steps as u32;

        for _ in 0..steps {
            let v0 = self.speed_mps.clamp(0.0, MAX_SPEED_MPS);
            let v1 = (v0 + forces.acceleration(v0) * h).clamp(0.0, MAX_SPEED_MPS);
            self.speed_mps = v1;

            // Trapezoidal: with forward Euler on velocity this is the exact
            // integral of the linear velocity ramp over the sub-step.
            let ds = (0.5 * (v0 + v1) * h) as f64;
            self.distance_m += ds;

            // `ds` is measured along the road surface, so the vertical
            // component is sin(θ). Only ascent counts (FR-7.6).
            let climb = ds as f32 * forces.sin_theta;
            if climb > 0.0 {
                self.elevation_gain_m += climb;
            }
        }

        if !self.speed_mps.is_finite() {
            self.speed_mps = 0.0;
        }
    }

    /// Pull the modelled speed toward one the trainer actually measured.
    ///
    /// The model knows what a bike *would* do for a given power and gradient;
    /// the trainer knows how fast its flywheel is really turning. Neither alone
    /// is right on a single-cog drivetrain: pure physics lets the rider "coast"
    /// downhill at 39 km/h while spinning out against no resistance, and pure
    /// trainer speed would cap descents at whatever cadence the one gear allows.
    ///
    /// `weight` is the fraction of the gap closed **per second**, so the result
    /// does not depend on tick rate — a 4 Hz and a 10 Hz loop converge the same.
    pub fn correct_toward(&mut self, measured_mps: f32, weight: f32, dt: f32) {
        if !measured_mps.is_finite() || measured_mps < 0.0 || !dt.is_finite() || dt <= 0.0 {
            return;
        }
        let w = weight.clamp(0.0, 1.0);
        if w == 0.0 {
            return;
        }
        // Fraction of the gap to close this tick, from the per-second rate.
        let alpha = 1.0 - (1.0 - w).powf(dt.min(MAX_DT_S));
        let corrected = self.speed_mps + (measured_mps - self.speed_mps) * alpha;
        if corrected.is_finite() {
            self.speed_mps = corrected.clamp(0.0, MAX_SPEED_MPS);
        }
    }

    pub fn speed_kph(&self) -> f32 {
        self.speed_mps * 3.6
    }

    pub fn is_moving(&self) -> bool {
        self.speed_mps > MIN_SPEED_MPS
    }
}

/// The speed-independent parts of the force balance, computed once per tick.
struct Forces {
    /// `P × efficiency`; divided by speed to give propulsive force.
    wheel_power_w: f32,
    sin_theta: f32,
    /// Gravity plus rolling resistance, newtons. Constant in speed.
    resistive_n: f32,
    /// `½ρ·CdA`; multiplied by v² to give drag.
    drag_k: f32,
    mass_kg: f32,
}

impl Forces {
    fn new(power_w: f32, gradient_pct: f32, cfg: &RiderConfig) -> Self {
        // Braking is not modelled, so negative power is treated as coasting.
        // The upper clamp is what keeps a glitched FTMS sample from driving the
        // ride at 144 km/h; the integrator is stable and drift-free on its own,
        // but it cannot tell an implausible input from a real one.
        let power = sanitise(power_w, 0.0).clamp(0.0, MAX_MEASURED_POWER_W);
        let gradient =
            sanitise(gradient_pct, 0.0).clamp(-MAX_ABS_GRADIENT_PCT, MAX_ABS_GRADIENT_PCT);
        let theta = (gradient / 100.0).atan();

        // A zero or negative mass would divide by zero; a config that broken
        // should degrade rather than produce NaN.
        let mass = sanitise(cfg.total_mass_kg(), 83.0).max(1.0);
        let efficiency = sanitise(cfg.drivetrain_efficiency, 1.0).clamp(0.0, 1.0);
        let crr = sanitise(cfg.crr, 0.0).max(0.0);
        let cda = sanitise(cfg.cda, 0.0).max(0.0);
        let rho = sanitise(cfg.air_density, 0.0).max(0.0);

        Self {
            wheel_power_w: power * efficiency,
            sin_theta: theta.sin(),
            resistive_n: mass * GRAVITY * (theta.sin() + crr * theta.cos()),
            drag_k: 0.5 * rho * cda,
            mass_kg: mass,
        }
    }

    fn acceleration(&self, v: f32) -> f32 {
        let propulsive = self.wheel_power_w / v.max(MIN_SPEED_MPS);
        // Rolling resistance and gravity are folded together, so at a
        // standstill on the flat the net is a small negative that the ≥0 clamp
        // absorbs — the rider does not roll backwards.
        let net = propulsive - self.resistive_n - self.drag_k * v * v;
        let a = net / self.mass_kg;
        if a.is_finite() {
            a
        } else {
            0.0
        }
    }
}

fn sanitise(value: f32, fallback: f32) -> f32 {
    if value.is_finite() {
        value
    } else {
        fallback
    }
}

/// Steady-state speed for a given power and gradient — the speed at which
/// propulsive and resistive forces balance. Useful for tests and for sanity
/// checks on the resistance curve later.
///
/// The balance is a cubic in `v` (`P·η = F_const·v + k·v³`) with no clean
/// closed form once the `max(v, v_min)` floor is included, so it is solved by
/// bisection. Net force is non-increasing in `v`, which makes the bracket
/// unambiguous.
pub fn equilibrium_speed_mps(power_w: f32, gradient_pct: f32, cfg: &RiderConfig) -> f32 {
    let forces = Forces::new(power_w, gradient_pct, cfg);

    // Cannot get moving at all: the rider stalls on the climb.
    if forces.acceleration(0.0) <= 0.0 {
        return 0.0;
    }
    if forces.acceleration(MAX_SPEED_MPS) > 0.0 {
        return MAX_SPEED_MPS;
    }

    let mut lo = 0.0f32;
    let mut hi = MAX_SPEED_MPS;
    // 60 halvings takes the bracket far below f32 resolution.
    for _ in 0..60 {
        let mid = 0.5 * (lo + hi);
        if mid <= lo || mid >= hi {
            break;
        }
        if forces.acceleration(mid) > 0.0 {
            lo = mid;
        } else {
            hi = mid;
        }
    }
    0.5 * (lo + hi)
}

/// Total resistive force at a given speed and gradient, newtons.
///
/// This is what the road is doing to the rider: gravity down the slope, rolling
/// resistance, and aerodynamic drag rising with the square of speed. It is the
/// force a trainer must reproduce at the wheel for the ride to feel real, and
/// therefore the basis for virtual gearing (see `crate::gearing`).
pub fn resistive_force_n(speed_mps: f32, gradient_pct: f32, cfg: &RiderConfig) -> f32 {
    let forces = Forces::new(0.0, gradient_pct, cfg);
    let v = if speed_mps.is_finite() {
        speed_mps.clamp(0.0, MAX_SPEED_MPS)
    } else {
        0.0
    };
    let f = forces.resistive_n + forces.drag_k * v * v;
    if f.is_finite() {
        f
    } else {
        0.0
    }
}

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

    fn cfg() -> RiderConfig {
        RiderConfig::default()
    }

    /// Run the integrator to steady state and return the state.
    fn settle(power_w: f32, gradient_pct: f32, seconds: f32) -> PhysicsState {
        let mut s = PhysicsState::default();
        let cfg = cfg();
        let dt = 0.25;
        let ticks = (seconds / dt) as u32;
        for _ in 0..ticks {
            s.step(power_w, gradient_pct, &cfg, dt);
        }
        s
    }

    #[test]
    fn equilibrium_is_a_fixed_point_of_the_integrator() {
        for (power, gradient) in [(200.0, 0.0), (300.0, 5.0), (150.0, -2.0), (400.0, 8.0)] {
            let target = equilibrium_speed_mps(power, gradient, &cfg());
            let settled = settle(power, gradient, 900.0).speed_mps;
            assert!(
                (settled - target).abs() < 0.05,
                "P={power} g={gradient}: integrator settled at {settled}, equilibrium says {target}"
            );
        }
    }

    #[test]
    fn equilibrium_matches_hand_computed_flat_case() {
        // 250 W on the flat with the default rider: solve P·η = F_roll·v + k·v³.
        let c = cfg();
        let v = equilibrium_speed_mps(250.0, 0.0, &c);
        let m = c.total_mass_kg();
        let f_roll = m * GRAVITY * c.crr;
        let drag = 0.5 * c.air_density * c.cda;
        let balance = 250.0 * c.drivetrain_efficiency - (f_roll * v + drag * v * v * v);
        assert!(balance.abs() < 0.5, "residual force {balance} N at v={v}");
        // Sanity: a 75 kg rider at 250 W on the flat sits around 40 km/h.
        assert!((35.0..45.0).contains(&(v * 3.6)), "{} km/h", v * 3.6);
    }

    #[test]
    fn speed_approaches_equilibrium_rather_than_snapping() {
        let c = cfg();
        let target = equilibrium_speed_mps(250.0, 0.0, &c);
        let mut s = PhysicsState::default();

        s.step(250.0, 0.0, &c, 1.0);
        let after_one_second = s.speed_mps;
        assert!(
            after_one_second < target * 0.75,
            "one second reached {after_one_second} of {target} — no inertia"
        );
        assert!(after_one_second > 0.0);

        for _ in 0..600 {
            s.step(250.0, 0.0, &c, 1.0);
        }
        assert!((s.speed_mps - target).abs() < 0.05);
    }

    #[test]
    fn tick_rate_does_not_change_the_outcome() {
        let c = cfg();
        let mut coarse = PhysicsState::default();
        let mut fine = PhysicsState::default();
        for _ in 0..60 {
            coarse.step(300.0, 3.0, &c, 1.0);
        }
        for _ in 0..600 {
            fine.step(300.0, 3.0, &c, 0.1);
        }
        assert!((coarse.speed_mps - fine.speed_mps).abs() < 0.02);
        assert!((coarse.distance_m - fine.distance_m).abs() < 1.0);
    }

    #[test]
    fn zero_power_coasts_to_a_stop_on_the_flat() {
        let c = cfg();
        let mut s = PhysicsState {
            speed_mps: 11.0,
            ..Default::default()
        };
        let start = s.speed_mps;
        s.step(0.0, 0.0, &c, 1.0);
        assert!(s.speed_mps < start, "coasting must decelerate");

        for _ in 0..600 {
            s.step(0.0, 0.0, &c, 1.0);
        }
        assert_eq!(s.speed_mps, 0.0, "should have come to rest");
        assert!(!s.is_moving());
        assert!(s.distance_m > 0.0 && s.distance_m < 2000.0);
    }

    #[test]
    fn stationary_with_no_power_never_goes_backwards() {
        let c = cfg();
        let mut s = PhysicsState::default();
        for _ in 0..100 {
            s.step(0.0, 0.0, &c, 1.0);
            assert_eq!(s.speed_mps, 0.0);
        }
        assert_eq!(s.distance_m, 0.0);
    }

    #[test]
    fn steep_climb_stalls_but_stays_non_negative() {
        let c = cfg();
        let mut s = PhysicsState::default();
        for _ in 0..300 {
            s.step(60.0, 20.0, &c, 1.0);
            assert!(s.speed_mps >= 0.0);
        }
        assert!(s.speed_mps < 1.0, "60 W up 20% should barely move");
        assert_eq!(equilibrium_speed_mps(60.0, 20.0, &c), 0.0);
    }

    #[test]
    fn steep_descent_accelerates_to_a_bounded_terminal_speed() {
        let c = cfg();
        let mut s = PhysicsState::default();
        for _ in 0..600 {
            s.step(0.0, -12.0, &c, 1.0);
        }
        let terminal = equilibrium_speed_mps(0.0, -12.0, &c);
        assert!(terminal > 10.0, "should freewheel downhill, got {terminal}");
        assert!(terminal < MAX_SPEED_MPS);
        assert!((s.speed_mps - terminal).abs() < 0.1);
        assert_eq!(s.elevation_gain_m, 0.0, "descending gains no elevation");
    }

    #[test]
    fn more_power_always_means_more_speed() {
        let c = cfg();
        let mut previous = -1.0;
        for power in [0.0, 50.0, 100.0, 200.0, 300.0, 500.0, 1000.0] {
            let v = equilibrium_speed_mps(power, 0.0, &c);
            assert!(
                v > previous,
                "{power} W gave {v} m/s, not more than {previous}"
            );
            previous = v;
        }
    }

    #[test]
    fn steeper_gradient_always_means_less_speed() {
        let c = cfg();
        let mut previous = f32::INFINITY;
        for gradient in [-10.0, -5.0, 0.0, 2.0, 5.0, 10.0, 15.0] {
            let v = equilibrium_speed_mps(300.0, gradient, &c);
            assert!(
                v < previous,
                "{gradient}% gave {v} m/s, not less than {previous}"
            );
            previous = v;
        }
    }

    #[test]
    fn distance_and_elevation_accumulate_consistently() {
        let s = settle(250.0, 5.0, 600.0);
        assert!(s.distance_m > 0.0);
        // 5% grade: vertical is sin(atan(0.05)) ≈ 0.0499 of distance travelled.
        let expected = s.distance_m as f32 * (0.05f32.atan()).sin();
        assert!(
            (s.elevation_gain_m - expected).abs() < expected * 0.01,
            "gain {} vs expected {expected}",
            s.elevation_gain_m
        );
    }

    #[test]
    fn elevation_gain_counts_only_ascent() {
        let c = cfg();
        let mut s = PhysicsState::default();
        for _ in 0..300 {
            s.step(250.0, 5.0, &c, 1.0);
        }
        let after_climb = s.elevation_gain_m;
        assert!(after_climb > 10.0);
        for _ in 0..300 {
            s.step(250.0, -5.0, &c, 1.0);
        }
        assert_eq!(
            s.elevation_gain_m, after_climb,
            "descent must not reduce gain"
        );
    }

    #[test]
    fn hostile_inputs_never_produce_nan_or_negatives() {
        let mut c = cfg();
        let hostile = [
            f32::NAN,
            f32::INFINITY,
            f32::NEG_INFINITY,
            -1.0e30,
            1.0e30,
            0.0,
            -0.0,
        ];
        for &power in &hostile {
            for &gradient in &hostile {
                for &dt in &hostile {
                    let mut s = PhysicsState::default();
                    s.step(power, gradient, &c, dt);
                    s.step(power, gradient, &c, 1.0);
                    assert!(
                        s.speed_mps.is_finite(),
                        "speed NaN for {power}/{gradient}/{dt}"
                    );
                    assert!(s.speed_mps >= 0.0, "negative speed {}", s.speed_mps);
                    assert!(s.speed_mps <= MAX_SPEED_MPS);
                    assert!(s.distance_m.is_finite() && s.distance_m >= 0.0);
                    assert!(s.elevation_gain_m.is_finite() && s.elevation_gain_m >= 0.0);
                }
            }
        }

        // A degenerate rider config must degrade, not explode.
        c.rider_kg = 0.0;
        c.bike_kg = 0.0;
        c.cda = 0.0;
        c.air_density = 0.0;
        c.crr = f32::NAN;
        let mut s = PhysicsState::default();
        for _ in 0..100 {
            s.step(500.0, -30.0, &c, 1.0);
        }
        assert!(s.speed_mps.is_finite() && (0.0..=MAX_SPEED_MPS).contains(&s.speed_mps));
        assert!(equilibrium_speed_mps(500.0, -30.0, &c).is_finite());
    }

    #[test]
    fn poisoned_state_is_recovered() {
        let c = cfg();
        let mut s = PhysicsState {
            speed_mps: f32::NAN,
            distance_m: f64::NAN,
            elevation_gain_m: f32::NAN,
        };
        s.step(200.0, 0.0, &c, 1.0);
        assert!(s.speed_mps.is_finite());
        assert!(s.distance_m.is_finite());
        assert!(s.elevation_gain_m.is_finite());
    }

    #[test]
    fn zero_and_negative_dt_are_no_ops() {
        let c = cfg();
        let mut s = PhysicsState {
            speed_mps: 8.0,
            ..Default::default()
        };
        let before = s;
        s.step(300.0, 0.0, &c, 0.0);
        s.step(300.0, 0.0, &c, -5.0);
        assert_eq!(s, before);
    }

    /// The integrator must not creep. Forward Euler's discrete fixed point is
    /// exactly the root of `a(v)`, i.e. the continuous equilibrium, so a steady
    /// effort held for hours must not accumulate its way to a higher speed. A
    /// higher-order scheme would not improve this — it shares the same fixed
    /// point — so this test, not the integration order, is the guarantee.
    #[test]
    fn a_long_steady_ride_does_not_drift_upwards() {
        let c = cfg();
        let target = equilibrium_speed_mps(250.0, 0.0, &c);
        let mut s = PhysicsState::default();

        // Settle first, then hold for six hours of ride time.
        for _ in 0..2_400 {
            s.step(250.0, 0.0, &c, 0.25);
        }
        let after_settling = s.speed_mps;
        for _ in 0..86_400 {
            s.step(250.0, 0.0, &c, 0.25);
        }

        assert!(
            (s.speed_mps - after_settling).abs() < 1.0e-3,
            "speed crept from {after_settling} to {} over six hours",
            s.speed_mps
        );
        assert!(
            s.speed_mps <= target + 1.0e-3,
            "settled {} above equilibrium {target}",
            s.speed_mps
        );
    }

    /// Equilibrium is a fixed point *exactly*, not approximately: stepping from
    /// it must not move. This is the property that makes drift impossible.
    #[test]
    fn stepping_from_equilibrium_does_not_move() {
        let c = cfg();
        for (power, gradient) in [(200.0, 0.0), (300.0, 5.0), (150.0, -2.0)] {
            let v = equilibrium_speed_mps(power, gradient, &c);
            let mut s = PhysicsState {
                speed_mps: v,
                ..Default::default()
            };
            s.step(power, gradient, &c, 1.0);
            assert!(
                (s.speed_mps - v).abs() < 1.0e-4,
                "P={power} g={gradient}: {v} -> {}",
                s.speed_mps
            );
        }
    }

    /// A glitched FTMS sample is a sint16, so it can decode to 32767 W. That
    /// must not become a 144 km/h ride.
    #[test]
    fn implausible_power_cannot_drive_an_implausible_speed() {
        let c = cfg();
        let sane = settle(MAX_MEASURED_POWER_W, 0.0, 300.0).speed_mps;

        for absurd in [3_000.0, 10_000.0, 32_767.0] {
            let s = settle(absurd, 0.0, 300.0);
            assert!(
                (s.speed_mps - sane).abs() < 1.0e-3,
                "{absurd} W settled at {} m/s, above the {sane} m/s ceiling",
                s.speed_mps
            );
            assert!(
                s.speed_mps < MAX_SPEED_MPS,
                "{absurd} W pinned the speed at the absolute clamp"
            );
        }

        // Real efforts, including a hard sprint, must be untouched by the clamp.
        for real in [250.0, 600.0, 1_200.0, 2_000.0] {
            let s = settle(real, 0.0, 300.0);
            let expected = equilibrium_speed_mps(real, 0.0, &c);
            assert!(
                (s.speed_mps - expected).abs() < 0.05,
                "{real} W was clamped: {} vs {expected}",
                s.speed_mps
            );
        }
    }

    #[test]
    fn speed_kph_conversion() {
        let s = PhysicsState {
            speed_mps: 10.0,
            ..Default::default()
        };
        assert!((s.speed_kph() - 36.0).abs() < 1e-5);
    }
}