use rapier2d::prelude::*;

use crate::protocol::*;

const MAX_SIM_TIME: f32 = 30.0;
const REST_SPEED: f32 = 0.04;
const REST_ANGULAR_SPEED: f32 = 0.05;
const G: f32 = 9.80665;

/// Initial |ω| for full curl: 5 rotations over 14 s (rad/s).
/// Sign follows curl_sign; clockwise (curl>0) uses +ω0 in spawn (see apply_curl).
pub const INITIAL_OMEGA: f32 = 5.0 * 2.0 * std::f32::consts::PI / 14.0;

/// Target spin duration (s) matching the 5-rev / 14 s design.
/// Client animation only starts at the hog (~9 s in); damping must leave
/// tangible |ω| past that, or stones look frozen on screen.
pub const SPIN_HOLD_S: f32 = 14.0;

/// Scale for lateral speed: `v_lat = CURL_LAT_K * µ(speed) * friction_scalar` (m/s).
/// Applied ⊥ **instantaneous velocity** heading as continuous normal dynamics
/// (`a_n = v_lat / CURL_LAT_TAU`, integrated each substep) so we do not stack
/// a fixed geometric rotation of atan(v_lat/v) per 1/120 s tick.
/// Calibrated so a full-curl DRAW_VELOCITY throw to the tee drifts ≈ 4 feet.
/// Clockwise curl_sign > 0 → right of velocity (+x when moving +y).
pub const CURL_LAT_K: f32 = 0.683;

/// Time constant (s) mapping target v_lat → normal acceleration: a_n = v_lat / TAU.
pub const CURL_LAT_TAU: f32 = 1.0;

/// Target lateral displacement (m) for a full-curl draw to the tee line.
#[allow(dead_code)] // used by unit tests + docs; keeps calibration goal explicit
pub const CURL_DRAW_LATERAL_M: f32 = 4.0 * FEET_TO_METERS;

/// Calibrated initial speed (m/s) for a mid draw that stops near the tee line
/// with friction_scalar = 1.0, curl = 0, broom aimed at HOUSE_CENTER.
#[allow(dead_code)] // used by unit tests / clients; sim accepts arbitrary velocity
pub const DRAW_VELOCITY: f32 = 2.38;

/// Ice friction coefficient µ as a function of speed (m/s).
/// Piecewise-linear interpolation of the binding table; µ(v≥2.5) = 0.0081.
pub fn mu(v: f32) -> f32 {
    // Binding µ(v) table (v_m/s, µ)
    const KNOTS: [(f32, f32); 7] = [
        (0.0, 0.016),
        (0.1482, 0.014),
        (0.3005, 0.0116),
        (0.4486, 0.0098),
        (0.7371, 0.0079),
        (1.0098, 0.0073),
        (2.5, 0.0081),
    ];

    let speed = v.abs();
    if speed >= 2.5 {
        return 0.0081;
    }
    for i in 0..KNOTS.len() - 1 {
        let (v0, mu0) = KNOTS[i];
        let (v1, mu1) = KNOTS[i + 1];
        if speed >= v0 && speed <= v1 {
            let t = if (v1 - v0).abs() < f32::EPSILON {
                0.0
            } else {
                (speed - v0) / (v1 - v0)
            };
            return mu0 + t * (mu1 - mu0);
        }
    }
    0.016
}

pub struct PhysicsWorld {
    gravity: Vector,
    integration_parameters: IntegrationParameters,
    pipeline: PhysicsPipeline,
    islands: IslandManager,
    broad_phase: DefaultBroadPhase,
    narrow_phase: NarrowPhase,
    bodies: RigidBodySet,
    colliders: ColliderSet,
    impulse_joints: ImpulseJointSet,
    multibody_joints: MultibodyJointSet,
    ccd_solver: CCDSolver,
    /// Next stone number per team within the current end (1..=8).
    next_n_team1: u8,
    next_n_team2: u8,
    /// (id, handle, team, curl_sign, friction_scalar)
    stone_handles: Vec<(StoneId, RigidBodyHandle, Team, i8, f32)>,
}

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

impl PhysicsWorld {
    pub fn new() -> Self {
        let mut integration_parameters = IntegrationParameters::default();
        integration_parameters.dt = PHYSICS_DT;
        integration_parameters.num_solver_iterations = 8;

        let mut world = Self {
            gravity: Vector::new(0.0, 0.0),
            integration_parameters,
            pipeline: PhysicsPipeline::new(),
            islands: IslandManager::new(),
            broad_phase: DefaultBroadPhase::new(),
            narrow_phase: NarrowPhase::new(),
            bodies: RigidBodySet::new(),
            colliders: ColliderSet::new(),
            impulse_joints: ImpulseJointSet::new(),
            multibody_joints: MultibodyJointSet::new(),
            ccd_solver: CCDSolver::new(),
            next_n_team1: 1,
            next_n_team2: 1,
            stone_handles: Vec::new(),
        };
        world.build_sheet();
        world
    }

    pub fn reset(&mut self) {
        self.bodies = RigidBodySet::new();
        self.colliders = ColliderSet::new();
        self.islands = IslandManager::new();
        self.broad_phase = DefaultBroadPhase::new();
        self.narrow_phase = NarrowPhase::new();
        self.impulse_joints = ImpulseJointSet::new();
        self.multibody_joints = MultibodyJointSet::new();
        self.ccd_solver = CCDSolver::new();
        self.stone_handles.clear();
        self.build_sheet();
    }

    /// Reset per-team stone numbers for a new end (n starts at 1 again).
    pub fn reset_stone_ids(&mut self) {
        self.next_n_team1 = 1;
        self.next_n_team2 = 1;
    }

    /// No wall colliders: stones leave play via prune_out_of_play only.
    fn build_sheet(&mut self) {
        // Intentionally empty — open boundaries (no left/right/back bounce).
    }

    fn alloc_stone_id(&mut self, team: Team) -> Result<StoneId, String> {
        let n = match team {
            Team::Team1 => self.next_n_team1,
            Team::Team2 => self.next_n_team2,
        };
        if n > STONES_PER_TEAM {
            return Err(format!("no stones remaining for {}", team));
        }
        match team {
            Team::Team1 => self.next_n_team1 = n.saturating_add(1),
            Team::Team2 => self.next_n_team2 = n.saturating_add(1),
        }
        Ok(StoneId { team, n })
    }

    /// Throw a stone with pure initial velocity (m/s).
    /// `friction_scalar` is clamped to 0.5..=1.5 and multiplies µ.
    pub fn throw(
        &mut self,
        team: Team,
        broom_x: f32,
        broom_y: f32,
        velocity: f32,
        curl: i8,
        friction_scalar: f32,
    ) -> Result<Vec<StonePath>, String> {
        let speed = velocity.max(0.0);
        let dx = broom_x;
        let dy = broom_y - HACK_Y;
        let len = (dx * dx + dy * dy).sqrt().max(0.01);
        let vx = dx / len * speed;
        let vy = dy / len * speed;

        let curl_sign = if curl < 0 {
            -1
        } else if curl > 0 {
            1
        } else {
            0
        };
        let friction_scalar = friction_scalar.clamp(0.5, 1.5);

        self.spawn_stone(team, 0.0, HACK_Y, vx, vy, curl_sign, friction_scalar)
    }

    fn spawn_stone(
        &mut self,
        team: Team,
        x: f32,
        y: f32,
        vx: f32,
        vy: f32,
        curl_sign: i8,
        friction_scalar: f32,
    ) -> Result<Vec<StonePath>, String> {
        let id = self.alloc_stone_id(team)?;

        // Clockwise curl (curl_sign > 0) → positive ω0; lateral model maps that to +x.
        let omega0 = curl_sign as f32 * INITIAL_OMEGA;
        let body = RigidBodyBuilder::dynamic()
            .translation(Vector::new(x, y))
            .linvel(Vector::new(vx, vy))
            .angvel(omega0)
            .linear_damping(0.0)
            .angular_damping(0.0)
            .ccd_enabled(true)
            .can_sleep(false)
            .build();

        let handle = self.bodies.insert(body);

        let collider = ColliderBuilder::ball(STONE_RADIUS)
            .friction(STONE_FRICTION)
            .friction_combine_rule(CoefficientCombineRule::Average)
            .restitution(STONE_RESTITUTION)
            .restitution_combine_rule(CoefficientCombineRule::Average)
            .density(STONE_MASS / (std::f32::consts::PI * STONE_RADIUS * STONE_RADIUS))
            .build();

        self.colliders
            .insert_with_parent(collider, handle, &mut self.bodies);

        self.stone_handles
            .push((id, handle, team, curl_sign, friction_scalar));

        self.simulate_until_rest(id)
    }

    fn simulate_until_rest(&mut self, _thrown_id: StoneId) -> Result<Vec<StonePath>, String> {
        // Path samples are [x, y, theta]. Client time is sample_index / SAMPLE_RATE_HZ.
        // All stones share the same sample clock from the thrown stone's release.
        let sample_step = 1.0 / SAMPLE_RATE_HZ as f32;
        let mut sample_accum: f32 = 0.0;
        let mut time: f32 = 0.0;

        // Pre-allocate a path buffer for every stone currently in the world.
        let mut paths: Vec<(StoneId, Team, RigidBodyHandle, Vec<[f32; 3]>)> = self
            .stone_handles
            .iter()
            .map(|(id, handle, team, _, _)| (*id, *team, *handle, Vec::new()))
            .collect();

        // Record the initial sample for every stone.
        for (id, _, handle, path) in &mut paths {
            if let Some(body) = self.bodies.get(*handle) {
                let pos = body.translation();
                let theta = body.rotation().angle();
                path.push([pos.x, pos.y, theta]);
            } else {
                return Err(format!("stone {:?}/{} has no rigid body", id.team, id.n));
            }
        }

        loop {
            self.step();
            self.apply_ice_friction();
            self.apply_curl();
            time += PHYSICS_DT;
            sample_accum += PHYSICS_DT;

            if sample_accum >= sample_step {
                sample_accum -= sample_step;
                for (_, _, handle, path) in &mut paths {
                    if let Some(body) = self.bodies.get(*handle) {
                        let pos = body.translation();
                        let theta = body.rotation().angle();
                        // Don't grow client animation paths once the stone is clearly off-sheet.
                        if Self::position_clearly_out_of_play(pos.x, pos.y) {
                            continue;
                        }
                        path.push([pos.x, pos.y, theta]);
                    }
                }
            }

            if self.all_stones_settled_or_out() || time > MAX_SIM_TIME {
                break;
            }
        }

        self.prune_out_of_play();

        Ok(paths
            .into_iter()
            .map(|(id, team, handle, trajectory)| {
                let rotation = self
                    .bodies
                    .get(handle)
                    .map(|b| b.rotation().angle())
                    .or_else(|| trajectory.last().map(|s| s[2]))
                    .unwrap_or(0.0);
                StonePath {
                    stone_id: id,
                    rotation,
                    team,
                    trajectory,
                }
            })
            .collect())
    }

    /// Apply a = −µ_eff * g * unit(v) after each physics step.
    /// µ_eff = mu(|v|) * friction_scalar. If velocity would reverse, stop.
    fn apply_ice_friction(&mut self) {
        for (_, handle, _, _, friction_scalar) in &self.stone_handles {
            let body = match self.bodies.get_mut(*handle) {
                Some(b) => b,
                None => continue,
            };

            let v = body.linvel();
            let speed = (v.x * v.x + v.y * v.y).sqrt();
            if speed < 1e-6 {
                body.set_linvel(Vector::new(0.0, 0.0), true);
                continue;
            }

            let mu_eff = mu(speed) * *friction_scalar;
            let a = mu_eff * G;
            let dv = a * PHYSICS_DT;
            if dv >= speed {
                body.set_linvel(Vector::new(0.0, 0.0), true);
            } else {
                let scale = (speed - dv) / speed;
                body.set_linvel(Vector::new(v.x * scale, v.y * scale), true);
            }
        }
    }

    /// Spin-curl model after drag:
    /// - Angular damping designed for ~SPIN_HOLD_S hold (not µmg/R, which killed
    ///   spin in ~4 s — before the client ever drew the stone past the hog)
    /// - Instantaneous velocity heading; right = CW perp (uy, -ux)
    /// - v_lat = curl_sign * CURL_LAT_K * µ(speed) * friction_scalar
    /// - Continuous normal dynamics: a_n = v_lat / CURL_LAT_TAU, v += a_n * right * dt
    /// - Clockwise curl_sign > 0 → right of velocity (+x when moving +y)
    fn apply_curl(&mut self) {
        const MIN_CURL_SPEED: f32 = 0.08;

        for (_, handle, _, curl_sign, friction_scalar) in &self.stone_handles {
            let body = match self.bodies.get_mut(*handle) {
                Some(b) => b,
                None => continue,
            };

            let v = body.linvel();
            let speed = (v.x * v.x + v.y * v.y).sqrt();

            // Decay |ω| so it lasts ~SPIN_HOLD_S at friction_scalar=1; scale by scalar.
            // Old α = µ g / R wiped spin pre-hog so FE never showed rotation.
            let omega = body.angvel();
            if speed < REST_SPEED {
                body.set_angvel(0.0, true);
            } else if omega.abs() > 1e-8 {
                let alpha = (INITIAL_OMEGA / SPIN_HOLD_S) * *friction_scalar;
                let domega = alpha * PHYSICS_DT;
                let new_omega = if domega >= omega.abs() {
                    0.0
                } else {
                    omega - omega.signum() * domega
                };
                body.set_angvel(new_omega, true);
            }

            if *curl_sign == 0 || speed < MIN_CURL_SPEED {
                continue;
            }

            // Instantaneous velocity heading and body-right (CW 90°).
            let ux = v.x / speed;
            let uy = v.y / speed;
            let rx = uy;
            let ry = -ux;

            // v_lat = k * µ(speed) * friction_scalar (same µ table as ice friction).
            let v_lat =
                (*curl_sign as f32) * CURL_LAT_K * mu(speed) * *friction_scalar;
            // Continuous: a_n = v_lat / τ → integrates without per-tick geometric stack.
            let a_n = v_lat / CURL_LAT_TAU;
            body.set_linvel(
                Vector::new(v.x + rx * a_n * PHYSICS_DT, v.y + ry * a_n * PHYSICS_DT),
                true,
            );
        }
    }

    fn prune_out_of_play(&mut self) {
        let mut keep = Vec::new();
        for (id, handle, team, curl, friction_scalar) in self.stone_handles.drain(..) {
            if let Some(body) = self.bodies.get(handle) {
                let pos = body.translation();
                let beyond_back = pos.y > BACK_LINE_Y;
                let short_of_hog = pos.y < HOG_LINE_Y;
                let outside = pos.x.abs() > SHEET_WIDTH / 2.0;
                if beyond_back || short_of_hog || outside {
                    self.bodies.remove(
                        handle,
                        &mut self.islands,
                        &mut self.colliders,
                        &mut self.impulse_joints,
                        &mut self.multibody_joints,
                        true,
                    );
                } else {
                    keep.push((id, handle, team, curl, friction_scalar));
                }
            }
        }
        self.stone_handles = keep;
    }

    fn step(&mut self) {
        self.pipeline.step(
            self.gravity,
            &self.integration_parameters,
            &mut self.islands,
            &mut self.broad_phase,
            &mut self.narrow_phase,
            &mut self.bodies,
            &mut self.colliders,
            &mut self.impulse_joints,
            &mut self.multibody_joints,
            &mut self.ccd_solver,
            &(),
            &(),
        );
    }

    fn position_clearly_out_of_play(x: f32, y: f32) -> bool {
        y > BACK_LINE_Y || x.abs() > SHEET_WIDTH / 2.0
    }

    /// End sim when every stone is at rest or already past back/sidelines.
    /// Avoids MAX_SIM_TIME client animations for long overthrows.
    fn all_stones_settled_or_out(&self) -> bool {
        for (_, handle, _, _, _) in &self.stone_handles {
            if let Some(body) = self.bodies.get(*handle) {
                let pos = body.translation();
                if Self::position_clearly_out_of_play(pos.x, pos.y) {
                    continue;
                }
                let v = body.linvel();
                let speed = (v.x * v.x + v.y * v.y).sqrt();
                if speed > REST_SPEED || body.angvel().abs() > REST_ANGULAR_SPEED {
                    return false;
                }
            }
        }
        true
    }

    pub fn current_stones(&self) -> Vec<StoneState> {
        let mut states = Vec::new();
        for (id, handle, team, _, _) in &self.stone_handles {
            if let Some(body) = self.bodies.get(*handle) {
                let pos = body.translation();
                states.push(StoneState {
                    id: *id,
                    team: *team,
                    x: pos.x,
                    y: pos.y,
                    rotation: body.rotation().angle(),
                });
            }
        }
        states
    }

    pub fn stone_states_for_scoring(&self) -> Vec<(StoneId, Team, f32, f32)> {
        let mut out = Vec::new();
        for (id, handle, team, _, _) in &self.stone_handles {
            if let Some(body) = self.bodies.get(*handle) {
                let pos = body.translation();
                out.push((*id, *team, pos.x, pos.y));
            }
        }
        out
    }
}

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

    fn final_y(world: &PhysicsWorld, id: StoneId) -> f32 {
        world
            .stone_handles
            .iter()
            .find(|(sid, _, _, _, _)| *sid == id)
            .map(|(_, h, _, _, _)| {
                let b = &world.bodies[*h];
                b.translation().y
            })
            .unwrap_or(f32::NAN)
    }

    fn final_x(world: &PhysicsWorld, id: StoneId) -> f32 {
        world
            .stone_handles
            .iter()
            .find(|(sid, _, _, _, _)| *sid == id)
            .map(|(_, h, _, _, _)| {
                let b = &world.bodies[*h];
                b.translation().x
            })
            .unwrap_or(f32::NAN)
    }

    fn last_thrown_id(world: &PhysicsWorld, team: Team) -> StoneId {
        world
            .stone_handles
            .iter()
            .rev()
            .find(|(_, _, t, _, _)| *t == team)
            .map(|(id, _, _, _, _)| *id)
            .unwrap_or_else(|| {
                // Pruned: reconstruct from counters (last allocated n - 1)
                let n = match team {
                    Team::Team1 => world.next_n_team1.saturating_sub(1),
                    Team::Team2 => world.next_n_team2.saturating_sub(1),
                };
                StoneId { team, n }
            })
    }

    #[test]
    fn mu_at_rest_is_0_016() {
        assert!((mu(0.0) - 0.016).abs() < 1e-6);
    }

    #[test]
    fn mu_interpolates_between_knots() {
        // Midpoint between 0.1482 (0.014) and 0.3005 (0.0116)
        let v = (0.1482 + 0.3005) / 2.0;
        let expected = (0.014 + 0.0116) / 2.0;
        let got = mu(v);
        assert!(
            (got - expected).abs() < 1e-5,
            "mu({}) = {}, expected ~{}",
            v,
            got,
            expected
        );
        // High-speed plateau
        assert!((mu(2.5) - 0.0081).abs() < 1e-6);
        assert!((mu(5.0) - 0.0081).abs() < 1e-6);
        // Exact knot
        assert!((mu(1.0098) - 0.0073).abs() < 1e-6);
    }

    #[test]
    fn velocity_draw_lands_on_tee_line() {
        let mut world = PhysicsWorld::new();
        // curl=0 so lateral drift does not push the stone OOB before rest.
        world
            .throw(Team::Team1, 0.0, HOUSE_CENTER.1, DRAW_VELOCITY, 0, 1.0)
            .unwrap();
        let id = last_thrown_id(&world, Team::Team1);
        let y = final_y(&world, id);
        println!("DRAW_VELOCITY={} final y={}", DRAW_VELOCITY, y);
        assert!(
            (y - HOUSE_CENTER.1).abs() <= 0.8,
            "draw shot should finish near tee line, got y={} (tee={})",
            y,
            HOUSE_CENTER.1
        );
    }

    #[test]
    fn high_friction_or_low_v_prunes_before_hog() {
        // Low velocity + high friction_scalar ⇒ short of hog, pruned.
        let mut world = PhysicsWorld::new();
        world
            .throw(Team::Team1, 0.0, HOUSE_CENTER.1, 1.0, 0, 1.5)
            .unwrap();
        let stones = world.current_stones();
        assert!(
            stones.is_empty(),
            "low-v high-friction throw should be pruned short of hog"
        );
    }

    #[test]
    fn sideline_aim_goes_out_not_bounce() {
        // Aim so the stone crosses |x| > SHEET_WIDTH/2; with open boundaries it
        // must be pruned (not bounce off a wall and remain in play).
        let mut world = PhysicsWorld::new();
        world
            .throw(Team::Team1, 4.0, 15.0, DRAW_VELOCITY, 0, 1.0)
            .unwrap();
        let stones = world.current_stones();
        assert!(
            stones.is_empty(),
            "sideline-bound stone should be pruned, not bounce; remaining={:?}",
            stones
                .iter()
                .map(|s| (s.x, s.y))
                .collect::<Vec<_>>()
        );
    }

    #[test]
    fn curl_direction_mirrors_x_offset() {
        // Use trajectory last sample (pre-prune): strong curl can exit the sheet.
        let mut right = PhysicsWorld::new();
        let right_traj = right
            .throw(Team::Team1, 0.0, HOUSE_CENTER.1, DRAW_VELOCITY, 1, 1.0)
            .unwrap();
        let right_x = right_traj[0]
            .trajectory
            .last()
            .map(|p| p[0])
            .unwrap_or(f32::NAN);

        let mut left = PhysicsWorld::new();
        let left_traj = left
            .throw(Team::Team1, 0.0, HOUSE_CENTER.1, DRAW_VELOCITY, -1, 1.0)
            .unwrap();
        let left_x = left_traj[0]
            .trajectory
            .last()
            .map(|p| p[0])
            .unwrap_or(f32::NAN);

        println!("right curl final x={} left curl final x={}", right_x, left_x);
        assert!(
            right_x > left_x + 0.05,
            "right curl should finish to the right of left curl: right={} left={}",
            right_x,
            left_x
        );
    }

    #[test]
    fn initial_angvel_magnitude_matches_5_rot_per_14s() {
        let expected = 5.0 * 2.0 * std::f32::consts::PI / 14.0;
        assert!(
            (INITIAL_OMEGA - expected).abs() < 1e-5,
            "INITIAL_OMEGA={} expected {}",
            INITIAL_OMEGA,
            expected
        );

        // Early path dθ/dt should be near |ω0| before damping eats much spin.
        let mut world = PhysicsWorld::new();
        let traj = world
            .throw(Team::Team1, 0.0, HOUSE_CENTER.1, DRAW_VELOCITY, 1, 1.0)
            .unwrap();
        let path = &traj[0].trajectory;
        assert!(path.len() >= 3, "need samples to estimate ω");
        let dt = 1.0 / SAMPLE_RATE_HZ as f32;
        let omega_est = (path[1][2] - path[0][2]) / dt;
        assert!(
            (omega_est.abs() - expected).abs() < expected * 0.35,
            "early |ω|≈{} should be near {} (5 rot / 14s)",
            omega_est.abs(),
            expected
        );
    }

    #[test]
    fn clockwise_curl_moves_right() {
        // Clockwise curl (curl > 0) must finish to the right of counterclockwise.
        let mut cw = PhysicsWorld::new();
        let cw_traj = cw
            .throw(Team::Team1, 0.0, HOUSE_CENTER.1, DRAW_VELOCITY, 1, 1.0)
            .unwrap();
        let right_x = cw_traj[0]
            .trajectory
            .last()
            .map(|p| p[0])
            .unwrap_or(f32::NAN);

        let mut ccw = PhysicsWorld::new();
        let ccw_traj = ccw
            .throw(Team::Team1, 0.0, HOUSE_CENTER.1, DRAW_VELOCITY, -1, 1.0)
            .unwrap();
        let left_x = ccw_traj[0]
            .trajectory
            .last()
            .map(|p| p[0])
            .unwrap_or(f32::NAN);

        println!(
            "clockwise final x={} counterclockwise final x={}",
            right_x, left_x
        );
        assert!(
            right_x > left_x + 0.05,
            "clockwise curl should move right: right_x={} left_x={}",
            right_x,
            left_x
        );
    }

    #[test]
    fn path_samples_include_nonzero_theta_when_spinning() {
        let mut world = PhysicsWorld::new();
        let traj = world
            .throw(Team::Team1, 0.0, HOUSE_CENTER.1, DRAW_VELOCITY, 1, 1.0)
            .unwrap();
        let path = &traj[0].trajectory;
        let max_abs_theta = path
            .iter()
            .map(|s| s[2].abs())
            .fold(0.0_f32, f32::max);
        assert!(
            max_abs_theta > 0.05,
            "spinning stone path should include nonzero theta, max|θ|={}",
            max_abs_theta
        );
    }

    /// FE trims trajectories to the hog; spin must still change θ after that.
    #[test]
    fn theta_keeps_changing_after_hog_when_curling() {
        let mut world = PhysicsWorld::new();
        let traj = world
            .throw(Team::Team1, 0.0, HOUSE_CENTER.1, DRAW_VELOCITY, 1, 1.0)
            .unwrap();
        let path = &traj[0].trajectory;
        let post_hog: Vec<[f32; 3]> = path
            .iter()
            .copied()
            .filter(|s| s[1] >= HOG_LINE_Y)
            .collect();
        assert!(
            post_hog.len() > 10,
            "need a post-hog path to animate, got {}",
            post_hog.len()
        );

        // Unwrap sample-to-sample Δθ (Rapier angle is in [-π, π]).
        let mut travel = 0.0_f32;
        let mut prev = post_hog[0][2];
        for s in post_hog.iter().skip(1) {
            let mut d = s[2] - prev;
            if d > std::f32::consts::PI {
                d -= 2.0 * std::f32::consts::PI;
            }
            if d < -std::f32::consts::PI {
                d += 2.0 * std::f32::consts::PI;
            }
            travel += d.abs();
            prev = s[2];
        }
        assert!(
            travel > 0.75,
            "stone should rotate past the hog (client-visible), |Δθ|sum={travel} rad"
        );
    }

    #[test]
    fn stones_persist_after_multiple_throws() {
        let mut world = PhysicsWorld::new();
        world
            .throw(Team::Team1, 0.0, HOUSE_CENTER.1, DRAW_VELOCITY, 0, 1.0)
            .unwrap();
        // Slight lateral aim so stones don't stack identically; still in-bounds.
        world
            .throw(Team::Team1, 0.3, HOUSE_CENTER.1, DRAW_VELOCITY, 0, 1.0)
            .unwrap();

        let stones = world.current_stones();
        assert_eq!(stones.len(), 2, "both stones should remain in the physics world");
        assert_eq!(
            stones[0].id,
            StoneId {
                team: Team::Team1,
                n: 1
            }
        );
        assert_eq!(
            stones[1].id,
            StoneId {
                team: Team::Team1,
                n: 2
            }
        );
    }

    #[test]
    fn stone_ids_are_per_team_and_reset_each_end() {
        let mut world = PhysicsWorld::new();
        world
            .throw(Team::Team1, 0.0, HOUSE_CENTER.1, DRAW_VELOCITY, 0, 1.0)
            .unwrap();
        world
            .throw(Team::Team2, 0.2, HOUSE_CENTER.1, DRAW_VELOCITY, 0, 1.0)
            .unwrap();
        let stones = world.current_stones();
        let t1 = stones.iter().find(|s| s.team == Team::Team1).unwrap();
        let t2 = stones.iter().find(|s| s.team == Team::Team2).unwrap();
        assert_eq!(t1.id.n, 1);
        assert_eq!(t2.id.n, 1);

        world.reset();
        world.reset_stone_ids();
        world
            .throw(Team::Team1, 0.0, HOUSE_CENTER.1, DRAW_VELOCITY, 0, 1.0)
            .unwrap();
        let again = world.current_stones();
        assert_eq!(again[0].id.n, 1, "stone numbers reset each end");
    }

    #[test]
    fn out_of_play_stone_is_pruned() {
        // A very slow, high-friction throw should stop short of the hog line and be removed.
        let mut world = PhysicsWorld::new();
        world
            .throw(Team::Team1, 0.0, HOUSE_CENTER.1, 0.8, 0, 1.5)
            .unwrap();
        let stones = world.current_stones();
        assert!(
            stones.is_empty(),
            "stones short of the hog line should be pruned"
        );
    }

    #[test]
    fn collision_records_trajectories_for_both_stones() {
        // Place a stationary stone on the center line and throw a second stone
        // straight at it so they collide. Both stones must have sampled paths.
        let takeout_v = DRAW_VELOCITY * 1.4;

        let mut world = PhysicsWorld::new();
        world
            .throw(Team::Team1, 0.0, HOUSE_CENTER.1, DRAW_VELOCITY, 0, 1.0)
            .unwrap();
        let first_id = last_thrown_id(&world, Team::Team1);

        let target_y = final_y(&world, first_id);
        let target_x = final_x(&world, first_id);
        world
            .throw(Team::Team2, target_x, target_y, takeout_v, 0, 1.0)
            .unwrap();
        let second_id = last_thrown_id(&world, Team::Team2);

        // Re-run the collision throw and capture trajectories.
        let mut world = PhysicsWorld::new();
        world
            .throw(Team::Team1, 0.0, HOUSE_CENTER.1, DRAW_VELOCITY, 0, 1.0)
            .unwrap();
        let first_id = last_thrown_id(&world, Team::Team1);
        let target_y = final_y(&world, first_id);
        let target_x = final_x(&world, first_id);
        let trajectories = world
            .throw(Team::Team2, target_x, target_y, takeout_v, 0, 1.0)
            .unwrap();

        let by_id: std::collections::HashMap<StoneId, Vec<[f32; 3]>> = trajectories
            .into_iter()
            .map(|st| (st.stone_id, st.trajectory))
            .collect();

        assert!(
            by_id.contains_key(&first_id),
            "trajectories should contain the first stone (id={:?})",
            first_id
        );
        assert!(
            by_id.contains_key(&second_id),
            "trajectories should contain the thrown stone (id={:?})",
            second_id
        );

        let first_path = by_id.get(&first_id).unwrap();
        let second_path = by_id.get(&second_id).unwrap();
        assert!(
            first_path.len() > 1,
            "first stone path should have multiple samples, got {}",
            first_path.len()
        );
        assert!(
            second_path.len() > 1,
            "thrown stone path should have multiple samples, got {}",
            second_path.len()
        );

        // Paths are [x, y, theta]; both stones must have a sample at release (index 0).
        assert!(
            first_path[0][2].is_finite(),
            "first stone path should include finite theta"
        );
        assert!(
            second_path[0][2].is_finite(),
            "thrown stone path should include finite theta"
        );
    }

    /// Head-on takeout with nearly elastic restitution must launch the sitters
    /// and keep both moving along the impact (down-sheet) direction — not a
    /// plastic "stick and dump" limp.
    #[test]
    fn near_elastic_takeout_launches_both_downsheet() {
        let takeout_v = DRAW_VELOCITY * 1.6;

        let mut world = PhysicsWorld::new();
        world
            .throw(Team::Team1, 0.0, HOUSE_CENTER.1, DRAW_VELOCITY, 0, 1.0)
            .unwrap();
        let first_id = last_thrown_id(&world, Team::Team1);
        let rest_x = final_x(&world, first_id);
        let rest_y = final_y(&world, first_id);

        let trajectories = world
            .throw(Team::Team2, rest_x, rest_y, takeout_v, 0, 1.0)
            .unwrap();
        let second_id = last_thrown_id(&world, Team::Team2);

        let by_id: std::collections::HashMap<StoneId, Vec<[f32; 3]>> = trajectories
            .into_iter()
            .map(|st| (st.stone_id, st.trajectory))
            .collect();

        let first_path = by_id.get(&first_id).expect("struck stone path");
        let second_path = by_id.get(&second_id).expect("shooter path");

        let first_start_y = first_path[0][1];
        let first_max_y = first_path.iter().map(|s| s[1]).fold(f32::NEG_INFINITY, f32::max);
        let first_launch = first_max_y - first_start_y;

        // Inelastic e≈0.05 only nudges the sitters; nearly elastic takes them meters.
        assert!(
            first_launch > 1.5,
            "struck stone should be launched down-sheet, launch={first_launch} rest_y={rest_y}"
        );

        // Both should still be moving +y at some point after contact (sample peak
        // leftmost/rightmost velocity proxy: later samples farther down than early).
        let second_start_y = second_path[0][1];
        let second_max_y = second_path.iter().map(|s| s[1]).fold(f32::NEG_INFINITY, f32::max);
        assert!(
            second_max_y > second_start_y + 10.0,
            "shooter must travel down-sheet, Δy={}",
            second_max_y - second_start_y
        );

        // Impact direction is primarily +y; struck stone's net lateral drift after
        // a head-on should stay small compared to longitudinal launch.
        let first_end = first_path.last().expect("non-empty struck path");
        let lateral = (first_end[0] - rest_x).abs();
        assert!(
            lateral < first_launch * 0.5,
            "head-on should keep both mostly along impact axis: lateral={lateral} launch={first_launch}"
        );
    }

    #[test]
    fn stone_path_samples_are_xyz_arrays() {
        let mut world = PhysicsWorld::new();
        let paths = world
            .throw(Team::Team1, 0.0, HOUSE_CENTER.1, DRAW_VELOCITY, 0, 1.0)
            .unwrap();
        assert_eq!(paths[0].stone_id.n, 1);
        assert_eq!(paths[0].team, Team::Team1);
        assert!(!paths[0].trajectory.is_empty());
        let sample = paths[0].trajectory[0];
        assert_eq!(sample.len(), 3);
        assert!(sample[0].is_finite() && sample[1].is_finite() && sample[2].is_finite());
    }

    #[test]
    fn weight5_ui_velocity_leaves_stone_in_play() {
        let v = crate::protocol::MIN_SPEED
            + 4.0 / 9.0 * (crate::protocol::MAX_SPEED - crate::protocol::MIN_SPEED);
        assert!(
            (v - DRAW_VELOCITY).abs() < 0.02,
            "weight-5 velocity {v} should ≈ DRAW_VELOCITY {DRAW_VELOCITY}"
        );
        let mut world = PhysicsWorld::new();
        world
            .throw(Team::Team1, 0.0, HOUSE_CENTER.1, v, 0, 1.0)
            .unwrap();
        assert_eq!(world.current_stones().len(), 1);
    }

    #[test]
    fn min_ui_speed_reaches_past_hog() {
        let mut world = PhysicsWorld::new();
        world
            .throw(
                Team::Team1,
                0.0,
                HOUSE_CENTER.1,
                crate::protocol::MIN_SPEED,
                0,
                1.0,
            )
            .unwrap();
        let stones = world.current_stones();
        assert_eq!(stones.len(), 1);
        assert!(stones[0].y >= HOG_LINE_Y);
    }

    #[test]
    fn full_curl_draw_stays_on_sheet() {
        // UI default was curl=±1; old k/v model pruned every curled throw.
        let v = crate::protocol::MIN_SPEED
            + 4.0 / 9.0 * (crate::protocol::MAX_SPEED - crate::protocol::MIN_SPEED);
        for curl in [1i8, -1] {
            let mut world = PhysicsWorld::new();
            world
                .throw(Team::Team1, 0.0, HOUSE_CENTER.1, v, curl, 1.0)
                .unwrap();
            let stones = world.current_stones();
            assert_eq!(
                stones.len(),
                1,
                "curl={curl} must leave a stone in play, got {}",
                stones.len()
            );
            assert!(stones[0].y >= HOG_LINE_Y && stones[0].y <= BACK_LINE_Y);
            assert!(stones[0].x.abs() <= SHEET_WIDTH / 2.0);
        }
    }

    #[test]
    fn draw_to_tee_full_curl_drifts_four_feet() {
        // v_lat = CURL_LAT_K * µ(v); k calibrated so |x| ≈ 4 ft on a tee-line draw.
        let mut world = PhysicsWorld::new();
        world
            .throw(Team::Team1, 0.0, HOUSE_CENTER.1, DRAW_VELOCITY, 1, 1.0)
            .unwrap();
        let stones = world.current_stones();
        assert_eq!(stones.len(), 1);
        let s = &stones[0];
        assert!(
            (s.y - HOUSE_CENTER.1).abs() < 1.0,
            "should stop near tee line, y={}",
            s.y
        );
        assert!(
            (s.x - CURL_DRAW_LATERAL_M).abs() < 0.25,
            "full curl should drift ~4 ft ({} m), got x={} m ({:.2} ft)",
            CURL_DRAW_LATERAL_M,
            s.x,
            s.x / FEET_TO_METERS
        );
        // Opposite curl is mirror-image.
        let mut world2 = PhysicsWorld::new();
        world2
            .throw(Team::Team1, 0.0, HOUSE_CENTER.1, DRAW_VELOCITY, -1, 1.0)
            .unwrap();
        let s2 = &world2.current_stones()[0];
        assert!(
            (s2.x + CURL_DRAW_LATERAL_M).abs() < 0.25,
            "ccw curl should drift ~-4 ft, got x={}",
            s2.x
        );
    }

    #[test]
    fn fast_overshoot_does_not_run_full_max_sim_path() {
        let mut world = PhysicsWorld::new();
        let paths = world
            .throw(Team::Team1, 0.0, HOUSE_CENTER.1, 4.0, 0, 1.0)
            .unwrap();
        let n = paths[0].trajectory.len();
        assert!(
            n < 900,
            "overshoot path should end when past back line, got {n} samples"
        );
        assert!(world.current_stones().is_empty());
    }
}