//! Virtual gears for a single-cog drivetrain (§5.4, FR-4.1).
//!
//! With a Zwift Cog there is one 14T sprocket and no way to shift, so the rider
//! has exactly one gear. That is tolerable on the flat and useless everywhere
//! else: on a climb they grind, and on a descent the trainer unloads, they spin
//! out against nothing, and their effort stops contributing at precisely the
//! moment they can see the speed rising.
//!
//! # How a gear is expressed
//!
//! Not as a tooth count — the rider should not have to know what chainring is
//! fitted — but as **development**: the metres travelled per crank revolution.
//! A 34×28 bottom gear on 700c is about 2.5 m; a 50×11 top gear about 9.5 m.
//! Development is the honest statement of what a gear *does*, and it needs only
//! the wheel circumference to be useful.
//!
//! # How a gear is made to feel real
//!
//! FTMS has no virtual-shifting op code — Zwift's implementation is proprietary
//! — so gearing is servoed rather than commanded. The causal chain is the same
//! as a real bike:
//!
//! ```text
//! gear  ->  resistance  ->  power  ->  speed
//! ```
//!
//! Shifting does not set the speed. It sets how hard the pedals are, which
//! decides the power the rider produces, which the physics model turns into
//! speed. Deriving speed straight from cadence × gear would be simpler and
//! wrong: it would let a rider spin up a 15% wall at 45 km/h without ever
//! producing the watts that requires, and gradient would become decoration.
//!
//! # What commands the trainer
//!
//! [`Gearing::load_gradient_pct`] — a direct computation, not a feedback loop.
//! A gear changes the leverage between crank and wheel, so the load it implies
//! is the road force scaled by the gear's development relative to the bike's
//! real one. Shifting therefore lands on the pedals the moment it happens.
//!
//! An earlier design servoed the load toward a cadence target instead. It was
//! abandoned because a servo can only find the right load by first being wrong:
//! at a gentle enough gain not to oscillate against the rider's own cadence
//! variation, a shift took seconds to be felt, which is not what a shift is.
//!
//! # What the cadence loop is still for
//!
//! The servo ([`Gearing::update`], [`Gearing::correction_pct`]) survives as a
//! *readout*, not a control input. For a given road speed the selected gear
//! implies a cadence:
//!
//! ```text
//! target_cadence = road_speed × 60 / development
//! ```
//!
//! and the signed distance between that and the rider's actual cadence says
//! whether they are spinning out or grinding — worth showing them, and worth
//! recording, but no longer added to what the trainer is asked for. Nothing
//! reads it into the control path; see `RideSession::tick`.
//!
//! Either way this had to wait for the Zwift-channel decode (§2.1.1) — FTMS on
//! this trainer reports no cadence at all.

use serde::{Deserialize, Serialize};

/// Widest load correction the servo may apply, in gradient percent. Generous
/// enough to recover a spun-out descent, bounded so a runaway loop cannot ask
/// for a cliff.
const MAX_CORRECTION_PCT: f32 = 8.0;
/// Widest load the gear model may command, in gradient percent.
///
/// Separate from the servo's bound, and deliberately so: this one caps what the
/// rider is *asked to push against*, not how far a feedback loop may wander. A
/// top gear on a steep climb legitimately reaches well past the servo's range,
/// and 16% is about the steepest thing worth reproducing under a rider before
/// it stops being training and starts being a wall.
const MAX_LOAD_PCT: f32 = 16.0;
/// Gradient percent applied per rpm of cadence error, per second. Deliberately
/// gentle: shifting should settle over a second or two, not snap, and an
/// aggressive gain oscillates against the rider's own cadence variation.
const GAIN_PCT_PER_RPM_S: f32 = 0.02;
/// Cadence error small enough to ignore, rpm. Real pedalling wanders by a few
/// rpm and chasing that would churn the control point for nothing.
const DEADBAND_RPM: f32 = 3.0;

/// A ladder of gears, expressed as development in metres per crank revolution.
#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)]
pub struct VirtualCassette {
    /// Metres per crank revolution, ascending (easiest first).
    development_m: Vec<f32>,
}

impl VirtualCassette {
    /// Evenly spaced gears between two developments.
    pub fn linear(gears: usize, easiest_m: f32, hardest_m: f32) -> Self {
        let gears = gears.max(1);
        if gears == 1 {
            return Self { development_m: vec![easiest_m.max(0.1)] };
        }
        let step = (hardest_m - easiest_m) / (gears - 1) as f32;
        Self {
            development_m: (0..gears)
                .map(|i| (easiest_m + step * i as f32).max(0.1))
                .collect(),
        }
    }

    pub fn len(&self) -> usize {
        self.development_m.len()
    }

    pub fn is_empty(&self) -> bool {
        self.development_m.is_empty()
    }

    /// Metres per crank revolution for a gear.
    pub fn development_m(&self, gear: usize) -> f32 {
        self.development_m
            .get(gear.min(self.development_m.len().saturating_sub(1)))
            .copied()
            .unwrap_or(1.0)
    }
}

impl Default for VirtualCassette {
    /// Twelve gears from 2.5 m to 9.5 m — roughly a 34/28 to 50/11 road setup,
    /// which is a sane range for terrain from a steep climb to a fast descent.
    fn default() -> Self {
        Self::linear(12, 2.5, 9.5)
    }
}

/// Gear selection plus the servo that makes the selection felt.
#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)]
pub struct Gearing {
    cassette: VirtualCassette,
    gear: usize,
    /// Load correction the servo has settled on, in gradient percent.
    correction_pct: f32,
}

impl Default for Gearing {
    fn default() -> Self {
        Self::new(VirtualCassette::default())
    }
}

impl Gearing {
    pub fn new(cassette: VirtualCassette) -> Self {
        let gear = cassette.len() / 2;
        Self { cassette, gear, correction_pct: 0.0 }
    }

    /// One-based, because riders count gears from one.
    pub fn gear(&self) -> usize {
        self.gear + 1
    }

    pub fn gear_count(&self) -> usize {
        self.cassette.len()
    }

    pub fn development_m(&self) -> f32 {
        self.cassette.development_m(self.gear)
    }

    /// The load correction currently applied, in gradient percent.
    pub fn correction_pct(&self) -> f32 {
        self.correction_pct
    }

    /// Cadence the selected gear implies at this road speed, rpm.
    pub fn target_cadence_rpm(&self, speed_mps: f32) -> f32 {
        let dev = self.development_m().max(0.1);
        (speed_mps.max(0.0) * 60.0 / dev).clamp(0.0, 250.0)
    }

    /// Advance the servo one tick and return the load correction to add to the
    /// commanded gradient.
    ///
    /// `cadence_rpm` is `None` when the trainer is not reporting it, in which
    /// case the correction decays toward zero rather than freezing — a stale
    /// correction is worse than none, because the rider cannot tell it is there.
    pub fn update(&mut self, cadence_rpm: Option<f32>, speed_mps: f32, dt: f32) -> f32 {
        let dt = if dt.is_finite() { dt.clamp(0.0, 1.0) } else { 0.0 };
        if dt <= 0.0 {
            return self.correction_pct;
        }

        match cadence_rpm.filter(|c| c.is_finite() && *c > 0.0) {
            Some(actual) => {
                let target = self.target_cadence_rpm(speed_mps);
                // Below walking pace the target is meaningless; a rider rolling
                // to a stop should not be handed a correction for it.
                if target < 20.0 {
                    self.decay(dt);
                    return self.correction_pct;
                }
                let error = actual - target;
                if error.abs() > DEADBAND_RPM {
                    self.correction_pct += error * GAIN_PCT_PER_RPM_S * dt;
                    self.correction_pct =
                        self.correction_pct.clamp(-MAX_CORRECTION_PCT, MAX_CORRECTION_PCT);
                } else {
                    // Inside the deadband, bleed off. Holding the last value
                    // instead would freeze whatever transient the rider passed
                    // through on the way to riding the gear correctly, and
                    // leave the readout claiming an error that is over.
                    self.decay(dt);
                }
            }
            None => self.decay(dt),
        }
        self.correction_pct
    }

    fn decay(&mut self, dt: f32) {
        // ~2 s time constant, so an unexplained correction fades rather than
        // lingering under the pedals.
        self.correction_pct *= 1.0 - (0.5 * dt).min(1.0);
        if self.correction_pct.abs() < 0.01 {
            self.correction_pct = 0.0;
        }
    }

    /// Shift to a harder gear. Clamps at the top — never wraps (FR-4.1.3),
    /// because wrapping from hardest to easiest mid-climb would be violent.
    pub fn shift_up(&mut self) -> bool {
        if self.gear + 1 < self.cassette.len() {
            self.gear += 1;
            true
        } else {
            false
        }
    }

    /// Shift to an easier gear. Clamps at the bottom.
    pub fn shift_down(&mut self) -> bool {
        if self.gear > 0 {
            self.gear -= 1;
            true
        } else {
            false
        }
    }

    /// Select a gear directly, one-based. Out-of-range values clamp.
    pub fn set_gear(&mut self, one_based: usize) {
        let idx = one_based.saturating_sub(1);
        self.gear = idx.min(self.cassette.len().saturating_sub(1));
    }
}

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

    fn settle(g: &mut Gearing, cadence: f32, speed_mps: f32, seconds: f32) -> f32 {
        let dt = 0.25;
        for _ in 0..((seconds / dt) as u32) {
            g.update(Some(cadence), speed_mps, dt);
        }
        g.correction_pct()
    }

    #[test]
    fn a_harder_gear_demands_a_lower_cadence() {
        let mut g = Gearing::default();
        let v = 8.0; // ~29 km/h
        let easy = {
            g.set_gear(1);
            g.target_cadence_rpm(v)
        };
        g.set_gear(12);
        let hard = g.target_cadence_rpm(v);
        assert!(easy > hard, "bottom gear should spin faster: {easy} vs {hard}");
    }

    #[test]
    fn spinning_out_adds_load() {
        // The descent failure: rider at 110 rpm when the gear implies far less.
        let mut g = Gearing::default();
        g.set_gear(6);
        let v = 8.0;
        let target = g.target_cadence_rpm(v);
        assert!(target < 110.0);
        let correction = settle(&mut g, 110.0, v, 6.0);
        assert!(correction > 0.5, "should add load, got {correction}");
    }

    #[test]
    fn grinding_sheds_load() {
        let mut g = Gearing::default();
        g.set_gear(6);
        let v = 8.0;
        let target = g.target_cadence_rpm(v);
        assert!(target > 40.0);
        let correction = settle(&mut g, 40.0, v, 6.0);
        assert!(correction < -0.5, "should shed load, got {correction}");
    }

    #[test]
    fn a_cadence_matching_the_gear_is_left_alone() {
        let mut g = Gearing::default();
        g.set_gear(6);
        let v = 8.0;
        let target = g.target_cadence_rpm(v);
        let correction = settle(&mut g, target, v, 6.0);
        assert_eq!(correction, 0.0, "no error means no correction");
    }

    #[test]
    fn correction_is_bounded_however_long_the_error_persists() {
        let mut g = Gearing::default();
        g.set_gear(6);
        let correction = settle(&mut g, 200.0, 8.0, 120.0);
        assert!(
            correction <= MAX_CORRECTION_PCT,
            "runaway correction: {correction}"
        );
    }

    #[test]
    fn losing_cadence_decays_the_correction_rather_than_freezing_it() {
        let mut g = Gearing::default();
        g.set_gear(6);
        settle(&mut g, 110.0, 8.0, 6.0);
        assert!(g.correction_pct() > 0.5);
        for _ in 0..40 {
            g.update(None, 8.0, 0.25);
        }
        assert!(
            g.correction_pct().abs() < 0.1,
            "stale correction lingered: {}",
            g.correction_pct()
        );
    }

    #[test]
    fn shifting_is_monotonic_and_clamps_at_both_ends() {
        let mut g = Gearing::new(VirtualCassette::linear(5, 2.5, 9.5));
        while g.shift_down() {}
        assert_eq!(g.gear(), 1);
        assert!(!g.shift_down(), "must not wrap past the bottom");

        let mut previous = g.development_m();
        while g.shift_up() {
            let now = g.development_m();
            assert!(now > previous, "each shift up must lengthen the gear");
            previous = now;
        }
        assert_eq!(g.gear(), 5);
        assert!(!g.shift_up(), "must not wrap past the top");
    }

    #[test]
    fn a_stationary_rider_is_not_given_a_correction() {
        let mut g = Gearing::default();
        g.set_gear(6);
        let correction = settle(&mut g, 90.0, 0.0, 4.0);
        assert_eq!(correction, 0.0);
    }
}

impl Gearing {
    /// The gradient to command so the pedals feel like the road does.
    ///
    /// This is the whole point of the module, and it is a direct computation
    /// rather than a feedback loop. `resistive_n` is what the road is doing at
    /// the current speed — gravity down the slope, rolling resistance, and
    /// aerodynamic drag rising with v² — from [`crate::physics::resistive_force_n`].
    ///
    /// A gear changes the *leverage* between crank and wheel, so the torque the
    /// rider feels for a given wheel force scales with development. Expressing
    /// the selected gear as a multiple of the bike's real one gives
    ///
    /// ```text
    /// commanded_force = road_force × (virtual_development / physical_development)
    /// ```
    ///
    /// and converting that force back to the gradient the trainer understands is
    /// just `F / (m·g)`. A longer gear therefore asks the trainer for a steeper
    /// hill, which is exactly what a longer gear feels like.
    ///
    /// Note what this does *not* do: on a real descent, gravity exceeds drag and
    /// the net road force is negative. The honest result is little or no load,
    /// because a rider on a real descent freewheels. Selecting a longer gear
    /// scales that up, so there is more to push against, but it cannot conjure
    /// resistance that the road is not providing.
    pub fn load_gradient_pct(&self, resistive_n: f32, mass_kg: f32, physical_development_m: f32) -> f32 {
        let mass = if mass_kg.is_finite() { mass_kg.max(1.0) } else { 1.0 };
        let physical = if physical_development_m.is_finite() && physical_development_m > 0.1 {
            physical_development_m
        } else {
            5.1
        };
        let f = if resistive_n.is_finite() { resistive_n } else { 0.0 };
        let scaled = f * (self.development_m() / physical);
        let pct = scaled / (mass * crate::physics::GRAVITY) * 100.0;
        if pct.is_finite() {
            pct.clamp(-MAX_LOAD_PCT, MAX_LOAD_PCT)
        } else {
            0.0
        }
    }
}

impl Gearing {
    /// The watts the road is asking for at this speed — what to command when
    /// the load is expressed as power rather than slope.
    ///
    /// Simply `F × v`, because that is what power is. Note there is no gear
    /// term: at a *given speed* the power required is the same in every gear,
    /// which is not a flaw in the model but the definition of a gear. The gear
    /// enters through the speed, since speed is cadence × development — so
    /// shifting up at the same cadence raises the speed and with it the demand,
    /// while what changes at the pedal is the force (see
    /// [`Gearing::pedal_force_n`]) and the cadence needed to hold it.
    ///
    /// On a trainer whose power target is a ceiling rather than a setpoint,
    /// commanding this makes the ride self-correcting: the rider accelerates
    /// when they exceed it and slows when they fall short, and next tick it is
    /// recomputed at the new speed.
    pub fn load_power_w(&self, resistive_n: f32, speed_mps: f32) -> f32 {
        let f = if resistive_n.is_finite() { resistive_n } else { 0.0 };
        let v = if speed_mps.is_finite() { speed_mps.max(0.0) } else { 0.0 };
        let w = f * v;
        // A descent asks for negative power, which no brake can supply — the
        // honest floor is zero, and the safety limits raise it to whatever the
        // trainer's minimum really is.
        if w.is_finite() { w.max(0.0) } else { 0.0 }
    }

    /// Force the rider's leg feels at the pedal, newtons.
    ///
    /// Work is force × distance on both sides of the crank. Over one crank
    /// revolution the pedal travels `2πr` and the bike travels `development`,
    /// and the work done is the same quantity seen twice:
    ///
    /// ```text
    /// F_pedal × 2πr  =  F_road × development
    /// ```
    ///
    /// Note which development appears. The trainer is asked for
    /// `F_road × (virtual / physical)` at the wheel, and the rider's crank
    /// turns through the *physical* gear, so the two physical terms cancel and
    /// what the leg feels is `F_road × virtual / 2πr` — precisely what a real
    /// bike in that gear would feel. That the virtual gear lands on the pedal
    /// exactly as a real one would is the check that the whole scheme is
    /// mechanically honest, not merely plausible.
    ///
    /// Purely a readout: nothing downstream of the wheel changes what is
    /// commanded. What it is *for* is judging a gear ladder — 40 N is freewheel
    /// -light and 600 N is a gear nobody can turn, and neither is visible from
    /// a gradient.
    pub fn pedal_force_n(&self, resistive_n: f32, crank_length_m: f32) -> f32 {
        let crank = if crank_length_m.is_finite() && crank_length_m > 0.01 {
            crank_length_m
        } else {
            0.1725
        };
        let f = if resistive_n.is_finite() { resistive_n } else { 0.0 };
        let pedal = f * self.development_m() / (std::f32::consts::TAU * crank);
        if pedal.is_finite() {
            pedal
        } else {
            0.0
        }
    }
}

#[cfg(test)]
mod pedal_tests {
    use super::*;
    use crate::physics::resistive_force_n;
    use crate::types::RiderConfig;

    #[test]
    fn a_longer_gear_is_heavier_at_the_pedal() {
        let c = RiderConfig::default();
        let f = resistive_force_n(8.0, 3.0, &c);
        let mut g = Gearing::default();
        g.set_gear(1);
        let easy = g.pedal_force_n(f, c.crank_length_m);
        g.set_gear(12);
        let hard = g.pedal_force_n(f, c.crank_length_m);
        assert!(hard > easy * 3.0, "top gear must be far heavier: {hard} vs {easy}");
    }

    #[test]
    fn longer_cranks_lighten_the_pedal() {
        // More leverage, less force, same work — the whole reason crank length
        // is a number worth knowing.
        let c = RiderConfig::default();
        let f = resistive_force_n(8.0, 3.0, &c);
        let g = Gearing::default();
        let short = g.pedal_force_n(f, 0.165);
        let long = g.pedal_force_n(f, 0.175);
        assert!(long < short, "longer cranks must feel lighter: {long} vs {short}");
        // Inverse in the radius, so the ratio is exact.
        assert!((short / long - 0.175 / 0.165).abs() < 0.001);
    }

    #[test]
    fn the_work_balance_holds_across_the_crank() {
        // The identity the function encodes: over one crank revolution the
        // rider does the same work whichever side of the crank you measure.
        let c = RiderConfig::default();
        let mut g = Gearing::default();
        g.set_gear(8);
        let road_n = resistive_force_n(8.0, 4.0, &c);
        let pedal_n = g.pedal_force_n(road_n, c.crank_length_m);

        let at_the_pedal = pedal_n * std::f32::consts::TAU * c.crank_length_m;
        let at_the_road = road_n * g.development_m();
        assert!(
            (at_the_pedal - at_the_road).abs() < at_the_road * 1e-4,
            "{at_the_pedal} J at the pedal vs {at_the_road} J at the road"
        );
    }

    #[test]
    fn a_flat_road_in_a_middling_gear_is_a_force_a_person_can_produce() {
        // Sanity on the absolute scale, which is the only thing that makes this
        // readout worth showing: easy flat riding should be tens of newtons.
        let c = RiderConfig::default();
        let g = Gearing::default();
        let pedal = g.pedal_force_n(resistive_force_n(8.0, 0.0, &c), c.crank_length_m);
        assert!(
            (20.0..250.0).contains(&pedal),
            "flat at 29 km/h should be light but not nothing: {pedal} N"
        );
    }

    #[test]
    fn absurd_cranks_do_not_produce_absurd_forces() {
        let g = Gearing::default();
        assert!(g.pedal_force_n(100.0, 0.0).is_finite());
        assert!(g.pedal_force_n(100.0, f32::NAN).is_finite());
        assert_eq!(g.pedal_force_n(f32::NAN, 0.1725), 0.0);
    }
}

#[cfg(test)]
mod load_tests {
    use super::*;
    use crate::physics::resistive_force_n;
    use crate::types::RiderConfig;

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

    #[test]
    fn a_longer_gear_asks_for_more_load() {
        let c = cfg();
        let f = resistive_force_n(8.0, 0.0, &c);
        let mut g = Gearing::default();
        g.set_gear(1);
        let easy = g.load_gradient_pct(f, c.total_mass_kg(), 5.1);
        g.set_gear(12);
        let hard = g.load_gradient_pct(f, c.total_mass_kg(), 5.1);
        assert!(hard > easy, "top gear must load more: {hard} vs {easy}");
    }

    #[test]
    fn a_climb_loads_more_than_the_flat() {
        let c = cfg();
        let g = Gearing::default();
        let flat = g.load_gradient_pct(resistive_force_n(8.0, 0.0, &c), c.total_mass_kg(), 5.1);
        let climb = g.load_gradient_pct(resistive_force_n(8.0, 6.0, &c), c.total_mass_kg(), 5.1);
        assert!(climb > flat, "a hill must be harder: {climb} vs {flat}");
    }

    #[test]
    fn air_resistance_shows_up_as_load_at_speed() {
        // Flat road: the only thing that grows with speed is drag, so the
        // commanded load must grow with it too.
        let c = cfg();
        let g = Gearing::default();
        let slow = g.load_gradient_pct(resistive_force_n(4.0, 0.0, &c), c.total_mass_kg(), 5.1);
        let fast = g.load_gradient_pct(resistive_force_n(14.0, 0.0, &c), c.total_mass_kg(), 5.1);
        assert!(fast > slow, "drag must load at speed: {fast} vs {slow}");
    }

    #[test]
    fn a_steep_descent_honestly_offers_little_load() {
        // Not a bug: gravity exceeds drag, so a real rider freewheels.
        let c = cfg();
        let g = Gearing::default();
        let load = g.load_gradient_pct(resistive_force_n(10.0, -8.0, &c), c.total_mass_kg(), 5.1);
        assert!(load < 0.0, "a steep descent should not demand work: {load}");
    }

    #[test]
    fn absurd_inputs_do_not_produce_absurd_targets() {
        let g = Gearing::default();
        assert_eq!(g.load_gradient_pct(f32::NAN, 100.0, 5.1), 0.0);
        assert!(g.load_gradient_pct(1e9, 100.0, 5.1).is_finite());
        assert!(g.load_gradient_pct(100.0, 0.0, 0.0).is_finite());
    }
}
