//! Local port of `KinematicCharacterController::solve_character_collision_impulses`.
//!
//! Rapier's version (0.35.2, still present on master) accumulates every nearby collider's
//! contact manifolds into one shared `Vec` and then slices it with `manifolds[prev_len..]`.
//! That assumes `contact_manifolds` appends, but parry's composite-shape paths (compound,
//! polyline, trimesh, ...) `mem::take` the output vec and rebuild it for the current pair
//! only. With two or more dynamic bodies inside the character's AABB the second call empties
//! the vec and the slice panics with "range start index 1 out of range for slice of length 0".
//! The character's collider here is a convex decomposition (a compound), so we hit it.
//!
//! This port uses a fresh scratch vec per collider, which is what rapier's own code intended.

use rapier2d::{
    control::{CharacterCollision, CharacterLength, KinematicCharacterController},
    geometry::{ContactManifold, Shape},
    math::{Pose, Real},
    parry::{
        bounding_volume::BoundingVolume,
        query::{DefaultQueryDispatcher, PersistentQueryDispatcher},
    },
    pipeline::QueryPipelineMut,
};

/// Apply approximate impulses to the dynamic bodies a kinematic character ran into.
pub fn solve_character_collision_impulses<'a>(
    controller: &KinematicCharacterController,
    dt: Real,
    queries: &mut QueryPipelineMut,
    character_shape: &dyn Shape,
    character_mass: Real,
    collisions: impl IntoIterator<Item = &'a CharacterCollision>,
) {
    for collision in collisions {
        solve_single_character_collision_impulse(
            controller,
            dt,
            queries,
            character_shape,
            character_mass,
            collision,
        );
    }
}

fn eval_length(length: CharacterLength, value: Real) -> Real {
    match length {
        CharacterLength::Relative(x) => value * x,
        CharacterLength::Absolute(x) => x,
    }
}

fn solve_single_character_collision_impulse(
    controller: &KinematicCharacterController,
    dt: Real,
    queries: &mut QueryPipelineMut,
    character_shape: &dyn Shape,
    character_mass: Real,
    collision: &CharacterCollision,
) {
    let extents = character_shape.compute_local_aabb().extents();
    let up_extent = extents.dot(controller.up.abs());
    let movement_to_transfer =
        collision.hit.normal1 * collision.translation_remaining.dot(collision.hit.normal1);
    // `KinematicCharacterController::predict_ground` is private; this is its body.
    let prediction = eval_length(controller.offset, up_extent) + 0.05;

    let dispatcher = DefaultQueryDispatcher;

    let mut manifolds: Vec<ContactManifold> = Vec::new();
    // World pose of the collider each manifold was computed against: the `local_p2`
    // points are in the collider's frame, which differs from its body's when offset.
    let mut manifold_collider_poses: Vec<Pose> = Vec::new();
    let character_aabb = character_shape
        .compute_aabb(&collision.character_pos)
        .loosened(prediction);

    for (_, collider) in queries.as_ref().intersect_aabb_conservative(character_aabb) {
        let Some(parent) = collider.parent() else {
            continue;
        };
        let Some(body) = queries.bodies.get(parent) else {
            continue;
        };
        if !body.is_dynamic() {
            continue;
        }

        let pos12 = collision.character_pos.inv_mul(collider.position());
        // Fresh vec per pair: parry may take/replace it rather than append.
        let mut pair_manifolds: Vec<ContactManifold> = Vec::new();
        let _ = dispatcher.contact_manifolds(
            &pos12,
            character_shape,
            collider.shape(),
            prediction,
            &mut pair_manifolds,
            &mut None,
        );

        for mut m in pair_manifolds {
            m.data.rigid_body2 = Some(parent);
            m.data.normal = collision.character_pos.rotation * m.local_n1;
            manifolds.push(m);
            manifold_collider_poses.push(*collider.position());
        }
    }

    let inv_dt = if dt != 0.0 { 1.0 / dt } else { 0.0 };
    let velocity_to_transfer = movement_to_transfer * inv_dt;

    for (manifold, collider_pos) in manifolds.iter().zip(manifold_collider_poses.iter()) {
        let Some(body_handle) = manifold.data.rigid_body2 else {
            continue;
        };
        let Some(body) = queries.bodies.get_mut(body_handle) else {
            continue;
        };

        for pt in &manifold.points {
            if pt.dist <= prediction {
                let body_mass = body.mass();
                let contact_point = *collider_pos * pt.local_p2;
                let delta_vel_per_contact = (velocity_to_transfer
                    - body.velocity_at_point(contact_point))
                .dot(manifold.data.normal);
                let mass_ratio = body_mass * character_mass / (body_mass + character_mass);

                body.apply_impulse_at_point(
                    manifold.data.normal * delta_vel_per_contact.max(0.0) * mass_ratio,
                    contact_point,
                    true,
                );
            }
        }
    }
}