Core ride logic, FTMS client, FIT encoder and probe CLI

Adds backing state for Resistance and Erg control modes, which had no
value to hold and so could never satisfy FR-4.3/FR-4.6.

Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>

crates/core/src/physics.rs

@@ -23,6 +23,24 @@ pub const GRAVITY: f32 = 9.80665;
 /// 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;
+
+/// 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 {
@@ -41,8 +59,50 @@ impl PhysicsState {
     /// 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 _ = (power_w, gradient_pct, cfg, dt);
-        todo!("implemented in crates/core/src/physics.rs — see AGENT task A")
+        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;
+        }
     }
 
     pub fn speed_kph(&self) -> f32 {
@@ -54,10 +114,375 @@ impl PhysicsState {
     }
 }
 
+/// 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.
+        let power = sanitise(power_w, 0.0).max(0.0);
+        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 _ = (power_w, gradient_pct, cfg);
-    todo!("implemented in crates/core/src/physics.rs — see AGENT task A")
+    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)
+}
+
+#[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);
+    }
+
+    #[test]
+    fn speed_kph_conversion() {
+        let s = PhysicsState {
+            speed_mps: 10.0,
+            ..Default::default()
+        };
+        assert!((s.speed_kph() - 36.0).abs() < 1e-5);
+    }
 }