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;
const OOB_SENSOR_TAG: u128 = 1;

/// Initial |ω| for full curl (rad/s). Sign follows curl_sign.
pub const INITIAL_OMEGA: f32 = 0.5 * 2.0 * std::f32::consts::PI;

/// Lateral scale: `v_lat = CURL_LAT_K * µ(speed)` (m/s), applied ⊥ heading.
/// Calibrated so a full-curl tee-line draw drifts ≈ 4 ft.
pub const CURL_LAT_K: f32 = 0.683;

/// Rapier angular linear damping (1/s): dω/dt ≈ −ANGULAR_DAMPING · ω.
/// Kept low so spin stays visible through a draw.
pub const ANGULAR_DAMPING: f32 = 0.05;

/// Target lateral displacement (m) for a full-curl draw to the tee line.
#[allow(dead_code)]
pub const CURL_DRAW_LATERAL_M: f32 = 4.0 * FEET_TO_METERS;

/// Ice friction coefficient µ as a function of speed (m/s).
pub fn mu(v: f32) -> f32 {
    const KNOTS: [(f32, f32); 7] = [
        (0.0, 0.018),
        (0.1482, 0.016),
        (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
}

/// Edge-touch out-of-bounds (sides / back). Hog is checked only at rest.
pub fn edge_out_of_bounds(x: f32, y: f32) -> bool {
    let half = SHEET_WIDTH / 2.0;
    x.abs() + STONE_RADIUS >= half || y + STONE_RADIUS >= BACK_LINE_Y
}

/// Hog rule: stone must completely clear the hog (trailing edge past the line).
pub fn short_of_hog(y: f32) -> bool {
    y - STONE_RADIUS <= HOG_LINE_Y
}

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_n_team1: u8,
    next_n_team2: u8,
    stone_handles: Vec<(StoneId, RigidBodyHandle, Team, i8)>,
    /// When false (calibration), no OOB sensors / pruning — open ice.
    bounds_enabled: bool,
}

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

impl PhysicsWorld {
    pub fn new() -> Self {
        Self::with_bounds(true)
    }

    /// Open sheet for stop-distance calibration (sensors off, no prune).
    pub fn new_open() -> Self {
        Self::with_bounds(false)
    }

    fn with_bounds(bounds_enabled: bool) -> 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(),
            bounds_enabled,
        };
        world.build_sheet();
        world
    }

    pub fn reset(&mut self) {
        let bounds_enabled = self.bounds_enabled;
        *self = Self::with_bounds(bounds_enabled);
    }

    pub fn reset_stone_ids(&mut self) {
        self.next_n_team1 = 1;
        self.next_n_team2 = 1;
    }

    /// Sensor colliders just outside the playable edge-touch room (no bounce).
    fn build_sheet(&mut self) {
        if !self.bounds_enabled {
            return;
        }

        let half = SHEET_WIDTH / 2.0;
        let r = STONE_RADIUS;
        // Center becomes OOB when edge touches lines → sensor starts at half - r / BACK - r.
        let side_in = half - r;
        let back_in = BACK_LINE_Y - r;
        let mid_y = (HOG_LINE_Y + BACK_LINE_Y) * 0.5;
        let tall = 80.0_f32;
        let thick = 8.0_f32;

        let sensors = [
            // right
            (side_in + thick * 0.5, mid_y, thick * 0.5, tall * 0.5),
            // left
            (-(side_in + thick * 0.5), mid_y, thick * 0.5, tall * 0.5),
            // beyond backline
            (0.0, back_in + thick * 0.5, half + thick, thick * 0.5),
        ];

        for (tx, ty, hx, hy) in sensors {
            let collider = ColliderBuilder::cuboid(hx, hy)
                .translation(Vector::new(tx, ty))
                .sensor(true)
                .user_data(OOB_SENSOR_TAG)
                .build();
            self.colliders.insert(collider);
        }
    }

    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 })
    }

    pub fn throw(
        &mut self,
        team: Team,
        broom_x: f32,
        broom_y: f32,
        velocity: f32,
        curl: i8,
    ) -> 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
        };
        self.spawn_stone(team, 0.0, HACK_Y, vx, vy, curl_sign)
    }

    fn spawn_stone(
        &mut self,
        team: Team,
        x: f32,
        y: f32,
        vx: f32,
        vy: f32,
        curl_sign: i8,
    ) -> Result<Vec<StonePath>, String> {
        let id = self.alloc_stone_id(team)?;
        // Trajectory uses curl_sign for lateral drift; ω sign is opposite for
        // correct on-ice visual (CW handle → CW granite spin).
        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(ANGULAR_DAMPING)
            .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::Max)
            .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));
        self.simulate_until_rest()
    }

    fn simulate_until_rest(&mut self) -> Result<Vec<StonePath>, String> {
        let sample_step = 1.0 / SAMPLE_RATE_HZ as f32;
        let mut sample_accum: f32 = 0.0;
        let mut time: f32 = 0.0;

        let mut paths: Vec<(StoneId, Team, RigidBodyHandle, Vec<[f32; 3]>)> = self
            .stone_handles
            .iter()
            .map(|(id, handle, team, _)| (*id, *team, *handle, Vec::new()))
            .collect();

        for (_, _, handle, path) in &mut paths {
            if let Some(body) = self.bodies.get(*handle) {
                let pos = body.translation();
                path.push([pos.x, pos.y, body.rotation().angle()]);
            } else {
                return Err("stone missing rigid body".into());
            }
        }

        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;
                // Always sample every stone for the full shared clock — even when OOB —
                // so multi-stone takes don't desync or snap back after prune.
                for (_, _, handle, path) in &mut paths {
                    if let Some(body) = self.bodies.get(*handle) {
                        let pos = body.translation();
                        path.push([pos.x, pos.y, body.rotation().angle()]);
                    }
                }
            }

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

        if self.bounds_enabled {
            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())
    }

    fn apply_ice_friction(&mut self) {
        for (_, handle, _, _) 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 a = mu(speed) * 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);
            }
        }
    }

    fn apply_curl(&mut self) {
        const MIN_CURL_SPEED: f32 = 0.08;

        for (_, handle, _, curl_sign) 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();

            // Spin decay is Rapier angular_damping only; zero at rest for clean settle.
            if speed < REST_SPEED {
                body.set_angvel(0.0, true);
            }

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

            let ux = v.x / speed;
            let uy = v.y / speed;
            // Body-left of heading = (−uy, ux). Clockwise curl drifts left when moving +y.
            let lx = -uy;
            let ly = ux;
            let v_lat = (*curl_sign as f32) * CURL_LAT_K * mu(speed);
            body.set_linvel(
                Vector::new(v.x + lx * v_lat * PHYSICS_DT, v.y + ly * v_lat * PHYSICS_DT),
                true,
            );
        }
    }

    fn body_hits_oob_sensor(&self, handle: RigidBodyHandle) -> bool {
        let Some(body) = self.bodies.get(handle) else {
            return false;
        };
        for &ch in body.colliders() {
            for (a, b, intersecting) in self.narrow_phase.intersection_pairs_with(ch) {
                if !intersecting {
                    continue;
                }
                let other = if a == ch { b } else { a };
                if let Some(col) = self.colliders.get(other) {
                    if col.user_data == OOB_SENSOR_TAG {
                        return true;
                    }
                }
            }
        }
        false
    }

    fn prune_out_of_play(&mut self) {
        let candidates: Vec<_> = self.stone_handles.drain(..).collect();
        let mut keep = Vec::new();
        for (id, handle, team, curl) in candidates {
            let remove = if let Some(body) = self.bodies.get(handle) {
                let pos = body.translation();
                edge_out_of_bounds(pos.x, pos.y)
                    || short_of_hog(pos.y)
                    || self.body_hits_oob_sensor(handle)
            } else {
                true
            };
            if remove {
                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));
            }
        }
        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 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.bounds_enabled
                    && (edge_out_of_bounds(pos.x, pos.y) || self.body_hits_oob_sensor(*handle))
                {
                    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 stop_y(velocity: f32) -> f32 {
        let mut world = PhysicsWorld::new_open();
        world
            .throw(Team::Team1, 0.0, HOUSE_CENTER.1, velocity, 0)
            .unwrap();
        world.current_stones()[0].y
    }

    fn final_y_in_play(velocity: f32) -> Option<f32> {
        let mut world = PhysicsWorld::new();
        world
            .throw(Team::Team1, 0.0, HOUSE_CENTER.1, velocity, 0)
            .unwrap();
        world.current_stones().first().map(|s| s.y)
    }

    #[test]
    fn mu_table_endpoints() {
        assert!((mu(0.0) - 0.018).abs() < 1e-6);
        assert!((mu(2.5) - 0.0081).abs() < 1e-6);
    }

    /// Calibrates categorical speeds 1–10 + hack to stop offsets from the tee.
    /// Board/control/normal/peel are fixed m/s (look-up only).
    #[test]
    fn speed_table_lands_at_categorical_distances() {
        let ft = FEET_TO_METERS;
        let tee = HOUSE_CENTER.1;
        let mut distance_labels: Vec<(String, f32, f32)> = WEIGHT_STOP_OFFSET_FT
            .iter()
            .enumerate()
            .map(|(i, off)| {
                (
                    (i + 1).to_string(),
                    WEIGHT_SPEEDS[i],
                    tee + off * ft,
                )
            })
            .collect();
        distance_labels.push((
            "hack".into(),
            HACK_SPEED,
            tee + HACK_STOP_OFFSET_FT * ft,
        ));
        distance_labels.extend([
            (
                "board".into(),
                BOARD_SPEED,
                tee + BOARD_STOP_OFFSET_FT * ft,
            ),
            (
                "control".into(),
                CONTROL_SPEED,
                tee + CONTROL_STOP_OFFSET_FT * ft,
            ),
            (
                "normal".into(),
                NORMAL_SPEED,
                tee + NORMAL_STOP_OFFSET_FT * ft,
            ),
            (
                "peel".into(),
                PEEL_SPEED,
                tee + PEEL_STOP_OFFSET_FT * ft,
            ),
        ]);

        for (label, speed, target) in &distance_labels {
            let y = stop_y(*speed);
            let err_ft = (y - target) / ft;
            assert!(
                err_ft.abs() < 0.35,
                "{label}: speed={speed} stop_y={y} target={target} err_ft={err_ft:.2}"
            );
        }

        assert!((DRAW_VELOCITY - WEIGHT_SPEEDS[6]).abs() < 1e-5);
        assert!((BOARD_STOP_OFFSET_FT - (HACK_STOP_OFFSET_FT + 6.0)).abs() < 1e-5);
        assert!((CONTROL_STOP_OFFSET_FT - (HACK_STOP_OFFSET_FT + 15.0)).abs() < 1e-5);
        assert!((NORMAL_STOP_OFFSET_FT - (HACK_STOP_OFFSET_FT + 25.0)).abs() < 1e-5);
        assert!((PEEL_STOP_OFFSET_FT - (HACK_STOP_OFFSET_FT + 35.0)).abs() < 1e-5);
    }

    #[test]
    fn edge_touch_backline_is_out() {
        assert!(edge_out_of_bounds(0.0, BACK_LINE_Y - STONE_RADIUS + 0.001));
        assert!(!edge_out_of_bounds(0.0, BACK_LINE_Y - STONE_RADIUS - 0.01));
    }

    #[test]
    fn hog_edge_rule() {
        assert!(short_of_hog(HOG_LINE_Y + STONE_RADIUS - 0.001));
        assert!(!short_of_hog(HOG_LINE_Y + STONE_RADIUS + 0.01));
    }

    #[test]
    fn backline_touches_house_ring() {
        assert!(
            (BACK_LINE_Y - (HOUSE_CENTER.1 + HOUSE_RADIUS)).abs() < 1e-5,
            "backline should sit on outer house edge"
        );
    }

    #[test]
    fn sensor_bounds_prune_overshoot() {
        assert!(final_y_in_play(PEEL_SPEED).is_none());
    }

    #[test]
    fn draw_and_curl_stay_in_play() {
        let mut world = PhysicsWorld::new();
        world
            .throw(Team::Team1, 0.0, HOUSE_CENTER.1, DRAW_VELOCITY, 1)
            .unwrap();
        assert_eq!(world.current_stones().len(), 1);
    }

    #[test]
    fn tee_line_full_curl_drifts_about_four_feet() {
        let mut world = PhysicsWorld::new();
        world
            .throw(Team::Team1, 0.0, HOUSE_CENTER.1, DRAW_VELOCITY, 1)
            .unwrap();
        let s = &world.current_stones()[0];
        assert!(
            (s.y - HOUSE_CENTER.1).abs() < 1.0,
            "should stop near tee, y={}",
            s.y
        );
        // CW curl → body-left (negative x when moving +y).
        assert!(
            (s.x + CURL_DRAW_LATERAL_M).abs() < 0.3,
            "full curl should drift ~-4 ft, got x={} m ({:.2} ft)",
            s.x,
            s.x / FEET_TO_METERS
        );
    }

    #[test]


    fn collision_moves_sitting_stone() {
        let mut world = PhysicsWorld::new();
        world
            .throw(Team::Team1, 0.0, HOUSE_CENTER.1, DRAW_VELOCITY, 0)
            .unwrap();
        let sit = world.current_stones()[0].clone();
        let paths = world
            .throw(Team::Team2, sit.x, sit.y, PEEL_SPEED, 0)
            .unwrap();
        assert!(paths.len() >= 2);
        let moved = paths
            .iter()
            .find(|p| p.stone_id.team == Team::Team1)
            .unwrap();
        let d = {
            let a = moved.trajectory.first().unwrap();
            let b = moved.trajectory.last().unwrap();
            ((b[0] - a[0]).powi(2) + (b[1] - a[1]).powi(2)).sqrt()
        };
        assert!(d > 0.2, "struck stone should move, d={d}");
    }
}