//! The concrete 3D / `f32` controller `#[pyclass]`-es: the kinematic
//! character controller, the PID/PD family, and the 3D-only raycast vehicle
//! controller, plus the `register_controllers` function used by `#[pymodule]`.
//!
//! This module used to be generated by a `(Real, DIM)`-parameterized macro so
//! that several cdylibs could each expand their own concrete types. There is
//! now a single concrete target — 3D, `f32` — so the types are written out
//! directly, with no macros.

use crate::*;
use rapier3d as rapier;

use crate::pyo3::exceptions::PyTypeError;
use crate::pyo3::prelude::*;
use crate::pyo3::pyclass::CompareOp;

// ============================================================================
// Shared: CharacterLength, CharacterAutostep, EffectiveCharacterMovement,
// CharacterCollision, KinematicCharacterController, AxesMask, PdErrors,
// PidCorrection.
// ============================================================================

// ---------------------------------------------------------------
// CharacterLength tagged union
//
// We don't expose this as a `#[pyclass] enum` because the upstream
// variants carry a `Real` payload. Instead we model it as a small
// frozen struct with a `kind` discriminant and a `value` payload,
// plus `.absolute(v)` / `.relative(v)` static constructors. Matches
// the API documented in `08-controllers.md`.
// ---------------------------------------------------------------
/// A length used by the kinematic character controller.
///
/// Tagged union with two variants — ``absolute`` (raw world-space
/// units) and ``relative`` (a fraction of the controlled shape's
/// height). Use the :meth:`absolute` / :meth:`relative` static
/// constructors instead of the keyword constructor when possible.
///
/// :ivar kind: ``"absolute"`` or ``"relative"``.
/// :ivar value: The numeric length, interpreted per ``kind``.
#[pyclass(name = "CharacterLength", module = "rapier", frozen)]
#[derive(Debug, Clone, Copy)]
pub struct CharacterLength {
    #[pyo3(get)]
    pub kind: &'static str,
    #[pyo3(get)]
    pub value: Real,
}

#[pymethods]
impl CharacterLength {
    /// Construct a ``CharacterLength`` from an explicit ``(value, kind)``.
    ///
    /// :param value: Numeric length.
    /// :param kind: ``"absolute"`` or ``"relative"``.
    /// :raises TypeError: if ``kind`` is not one of the two
    ///     accepted strings.
    #[new]
    #[pyo3(signature = (value=0.0 as Real, kind="absolute"))]
    fn new(value: Real, kind: &str) -> PyResult<Self> {
        match kind {
            "absolute" => Ok(Self {
                kind: "absolute",
                value,
            }),
            "relative" => Ok(Self {
                kind: "relative",
                value,
            }),
            _ => Err(PyTypeError::new_err(
                "CharacterLength kind must be \"absolute\" or \"relative\"",
            )),
        }
    }
    /// Build an absolute length expressed in world units.
    ///
    /// :param value: Length in the simulation's distance units.
    #[staticmethod]
    fn absolute(value: Real) -> Self {
        Self {
            kind: "absolute",
            value,
        }
    }
    /// Build a relative length expressed as a fraction of the shape height.
    ///
    /// :param value: Multiplier in ``[0, 1]`` of the controlled
    ///     shape's bounding height.
    #[staticmethod]
    fn relative(value: Real) -> Self {
        Self {
            kind: "relative",
            value,
        }
    }
    /// Return ``CharacterLength.absolute(v)`` / ``relative(v)`` repr.
    fn __repr__(&self) -> String {
        format!(
            "CharacterLength.{}({})",
            if self.kind == "absolute" {
                "absolute"
            } else {
                "relative"
            },
            self.value
        )
    }
    /// Equality (``==`` / ``!=``). Other comparisons raise ``TypeError``.
    fn __richcmp__(&self, other: &Self, op: CompareOp) -> PyResult<bool> {
        match op {
            CompareOp::Eq => Ok(self.kind == other.kind && self.value == other.value),
            CompareOp::Ne => Ok(!(self.kind == other.kind && self.value == other.value)),
            _ => Err(PyTypeError::new_err(
                "CharacterLength supports only == and !=",
            )),
        }
    }
}

impl CharacterLength {
    #[inline]
    pub(crate) fn to_rapier(self) -> rapier::control::CharacterLength {
        if self.kind == "relative" {
            rapier::control::CharacterLength::Relative(self.value)
        } else {
            rapier::control::CharacterLength::Absolute(self.value)
        }
    }
    #[inline]
    pub(crate) fn from_rapier(v: rapier::control::CharacterLength) -> Self {
        match v {
            rapier::control::CharacterLength::Relative(x) => Self {
                kind: "relative",
                value: x,
            },
            rapier::control::CharacterLength::Absolute(x) => Self {
                kind: "absolute",
                value: x,
            },
        }
    }
}

// Accept a `(value, "kind")` tuple, a `CharacterLength` instance, or
// a bare float (interpreted as Absolute).
fn _extract_char_length(obj: &Bound<'_, PyAny>) -> PyResult<CharacterLength> {
    if let Ok(c) = obj.extract::<CharacterLength>() {
        return Ok(c);
    }
    if let Ok(f) = obj.extract::<Real>() {
        return Ok(CharacterLength {
            kind: "absolute",
            value: f,
        });
    }
    Err(PyTypeError::new_err(
        "expected a CharacterLength, a float (interpreted as absolute), or use CharacterLength.absolute/.relative",
    ))
}

// ---------------------------------------------------------------
// CharacterAutostep
// ---------------------------------------------------------------
/// Configuration for the kinematic character controller auto-step.
///
/// Auto-step lets the character climb small ledges (stairs, kerbs)
/// automatically without falling off when the obstacle is shorter
/// than ``max_height`` and the landing platform is wider than
/// ``min_width``. Pass ``None`` (or :meth:`disabled`) to
/// :class:`KinematicCharacterController` to disable.
///
/// :ivar max_height: Max obstacle height to step over.
/// :ivar min_width: Min platform width above the step.
/// :ivar include_dynamic_bodies: If ``True``, step up onto dynamic
///     bodies too (not just statics/kinematics).
#[pyclass(name = "CharacterAutostep", module = "rapier")]
#[derive(Debug, Clone, Copy)]
pub struct CharacterAutostep {
    #[pyo3(get, set)]
    pub max_height: CharacterLength,
    #[pyo3(get, set)]
    pub min_width: CharacterLength,
    #[pyo3(get, set)]
    pub include_dynamic_bodies: bool,
}

#[pymethods]
impl CharacterAutostep {
    /// Build a ``CharacterAutostep`` with optional overrides.
    ///
    /// :param max_height: :class:`CharacterLength` or bare float
    ///     (interpreted as absolute). Defaults to upstream value.
    /// :param min_width: :class:`CharacterLength` or bare float.
    /// :param include_dynamic_bodies: Whether dynamic surfaces
    ///     count as valid step targets.
    #[new]
    #[pyo3(signature = (max_height=None, min_width=None, include_dynamic_bodies=true))]
    fn new(
        max_height: Option<&Bound<'_, PyAny>>,
        min_width: Option<&Bound<'_, PyAny>>,
        include_dynamic_bodies: bool,
    ) -> PyResult<Self> {
        let default = rapier::control::CharacterAutostep::default();
        let mh = match max_height {
            None => CharacterLength::from_rapier(default.max_height),
            Some(o) => _extract_char_length(o)?,
        };
        let mw = match min_width {
            None => CharacterLength::from_rapier(default.min_width),
            Some(o) => _extract_char_length(o)?,
        };
        Ok(Self {
            max_height: mh,
            min_width: mw,
            include_dynamic_bodies,
        })
    }
    /// Return ``None``, the sentinel to disable auto-step on the controller.
    ///
    /// Idiomatic: ``controller.autostep = CharacterAutostep.disabled()``.
    #[staticmethod]
    fn disabled() -> Option<Self> {
        None
    }
    /// Return the ``CharacterAutostep(...)`` repr.
    fn __repr__(&self) -> String {
        format!(
            "CharacterAutostep(max_height={:?}, min_width={:?}, include_dynamic_bodies={})",
            self.max_height, self.min_width, self.include_dynamic_bodies
        )
    }
}

impl CharacterAutostep {
    #[inline]
    pub(crate) fn to_rapier(self) -> rapier::control::CharacterAutostep {
        rapier::control::CharacterAutostep {
            max_height: self.max_height.to_rapier(),
            min_width: self.min_width.to_rapier(),
            include_dynamic_bodies: self.include_dynamic_bodies,
        }
    }
}

// ---------------------------------------------------------------
// EffectiveCharacterMovement
// ---------------------------------------------------------------
/// Result of :meth:`KinematicCharacterController.move_shape`.
///
/// Reports the actual translation applied to the character after
/// collision-aware sliding, auto-step, and ground snapping, plus a
/// ``grounded`` flag derived from the controller's contact state.
///
/// :ivar translation: World-space translation actually applied.
/// :ivar grounded: ``True`` if the character is in contact with a
///     surface this frame.
/// :ivar is_sliding_down_slope: ``True`` if the controller is
///     sliding the character down a too-steep slope.
#[pyclass(name = "EffectiveCharacterMovement", module = "rapier", frozen)]
#[derive(Debug, Clone, Copy)]
pub struct EffectiveCharacterMovement {
    #[pyo3(get)]
    pub translation: Vec3,
    #[pyo3(get)]
    pub grounded: bool,
    #[pyo3(get)]
    pub is_sliding_down_slope: bool,
}

#[pymethods]
impl EffectiveCharacterMovement {
    /// Return the ``EffectiveCharacterMovement(...)`` repr.
    fn __repr__(&self) -> String {
        format!(
            "EffectiveCharacterMovement(translation={:?}, grounded={}, is_sliding_down_slope={})",
            self.translation.0, self.grounded, self.is_sliding_down_slope
        )
    }
}

// ---------------------------------------------------------------
// CharacterCollision
// ---------------------------------------------------------------
/// A single collision event reported during character movement.
///
/// Instances are delivered to the ``events_callback`` of
/// :meth:`KinematicCharacterController.move_shape` and can later
/// be fed back to
/// :meth:`KinematicCharacterController.solve_character_collision_impulses`
/// to apply reactive impulses to the bodies the character bumped.
///
/// :ivar handle: Handle of the collider the character hit.
/// :ivar character_pos: World-space pose at the moment of contact.
/// :ivar translation_applied: Portion of the desired translation
///     successfully applied before the collision.
/// :ivar translation_remaining: Portion of translation still owed
///     (typically slid along the contact surface).
/// :ivar toi: ``ShapeCastHit`` with witness points and normals.
#[pyclass(name = "CharacterCollision", module = "rapier", frozen)]
#[derive(Debug, Clone, Copy)]
pub struct CharacterCollision {
    #[pyo3(get)]
    pub handle: ColliderHandle,
    #[pyo3(get)]
    pub character_pos: Isometry3,
    #[pyo3(get)]
    pub translation_applied: Vec3,
    #[pyo3(get)]
    pub translation_remaining: Vec3,
    #[pyo3(get)]
    pub toi: ShapeCastHit,
}

#[pymethods]
impl CharacterCollision {
    /// Return the ``CharacterCollision(handle=..., toi=...)`` repr.
    fn __repr__(&self) -> String {
        format!(
            "CharacterCollision(handle={:?}, toi={})",
            self.handle.0.into_raw_parts(),
            self.toi.time_of_impact
        )
    }
}

impl CharacterCollision {
    #[inline]
    pub(crate) fn from_rapier(c: rapier::control::CharacterCollision) -> Self {
        let trans_applied: crate::na::SVector<Real, 3> = c.translation_applied.into();
        let trans_remaining: crate::na::SVector<Real, 3> = c.translation_remaining.into();
        let iso: crate::na::Isometry<Real, _, 3> = c.character_pos.into();
        Self {
            handle: ColliderHandle(c.handle),
            character_pos: Isometry3(iso),
            translation_applied: Vec3(trans_applied),
            translation_remaining: Vec3(trans_remaining),
            toi: ShapeCastHit::from_parry(c.hit),
        }
    }
    #[inline]
    pub(crate) fn to_rapier(&self) -> rapier::control::CharacterCollision {
        let iso: rapier::math::Pose = self.character_pos.0.into();
        let ta: rapier::math::Vector = self.translation_applied.0.into();
        let tr: rapier::math::Vector = self.translation_remaining.0.into();
        rapier::control::CharacterCollision {
            handle: self.handle.0,
            character_pos: iso,
            translation_applied: ta,
            translation_remaining: tr,
            hit: rapier::parry::query::ShapeCastHit {
                time_of_impact: self.toi.time_of_impact,
                witness1: self.toi.witness1.0.coords.into(),
                witness2: self.toi.witness2.0.coords.into(),
                normal1: crate::na::Unit::new_unchecked(self.toi.normal1.0)
                    .into_inner()
                    .into(),
                normal2: crate::na::Unit::new_unchecked(self.toi.normal2.0)
                    .into_inner()
                    .into(),
                status: match self.toi.status {
                    ShapeCastStatus::OUT_OF_ITERATIONS => {
                        rapier::parry::query::ShapeCastStatus::OutOfIterations
                    }
                    ShapeCastStatus::CONVERGED => rapier::parry::query::ShapeCastStatus::Converged,
                    ShapeCastStatus::FAILED => rapier::parry::query::ShapeCastStatus::Failed,
                    ShapeCastStatus::PENETRATING_OR_WITHIN_TARGET_DIST => {
                        rapier::parry::query::ShapeCastStatus::PenetratingOrWithinTargetDist
                    }
                },
            },
        }
    }
}

// ---------------------------------------------------------------
// KinematicCharacterController
// ---------------------------------------------------------------
/// Kinematic character controller (collision-aware shape mover).
///
/// Drives a kinematic-position body / shape through a scene while
/// resolving collisions via sliding, auto-step (small obstacles),
/// and ground snapping (so the character sticks to descending
/// slopes). The controller itself owns no body — call
/// :meth:`move_shape` each frame with the desired translation and
/// then apply the returned :class:`EffectiveCharacterMovement`
/// to your body.
///
/// Example::
///
///     controller = KinematicCharacterController(offset=0.01)
///     mv = controller.move_shape(
///         dt, bodies, colliders, queries, shape, pose,
///         desired_translation
///     )
///     # then translate your kinematic body by mv.translation
#[pyclass(name = "KinematicCharacterController", module = "rapier")]
#[derive(Debug, Clone)]
pub struct KinematicCharacterController(pub rapier::control::KinematicCharacterController);

#[pymethods]
impl KinematicCharacterController {
    /// Build a controller with sensible defaults plus optional overrides.
    ///
    /// :param up: World-space up direction.
    /// :param offset: Skin-width kept between the character shape
    ///     and obstacles (:class:`CharacterLength` or raw float).
    /// :param slide: Whether to slide along obstacles on contact.
    /// :param autostep: :class:`CharacterAutostep` config, or
    ///     ``None`` to disable.
    /// :param max_slope_climb_angle: Slopes steeper than this
    ///     (radians) cannot be climbed.
    /// :param min_slope_slide_angle: Slopes steeper than this
    ///     (radians) cause the character to slide down.
    /// :param snap_to_ground: Distance below the character that
    ///     should be treated as still grounded after walking off
    ///     a small drop. ``None`` disables.
    /// :param normal_nudge_factor: Small bias along contact
    ///     normals to avoid getting stuck on geometry.
    #[new]
    #[pyo3(signature = (
        up=None,
        offset=None,
        slide=None,
        autostep=None,
        max_slope_climb_angle=None,
        min_slope_slide_angle=None,
        snap_to_ground=None,
        normal_nudge_factor=None,
    ))]
    #[allow(clippy::too_many_arguments)]
    fn new(
        py: Python<'_>,
        up: Option<PyVector>,
        offset: Option<&Bound<'_, PyAny>>,
        slide: Option<bool>,
        autostep: Option<&Bound<'_, PyAny>>,
        max_slope_climb_angle: Option<Real>,
        min_slope_slide_angle: Option<Real>,
        snap_to_ground: Option<&Bound<'_, PyAny>>,
        normal_nudge_factor: Option<Real>,
    ) -> PyResult<Self> {
        let _ = py;
        let mut inner = rapier::control::KinematicCharacterController::default();
        if let Some(u) = up {
            inner.up = u.0.into();
        }
        if let Some(o) = offset {
            inner.offset = _extract_char_length(o)?.to_rapier();
        }
        if let Some(s) = slide {
            inner.slide = s;
        }
        if let Some(a) = autostep {
            if a.is_none() {
                inner.autostep = None;
            } else {
                let auto: CharacterAutostep = a.extract()?;
                inner.autostep = Some(auto.to_rapier());
            }
        }
        if let Some(v) = max_slope_climb_angle {
            inner.max_slope_climb_angle = v;
        }
        if let Some(v) = min_slope_slide_angle {
            inner.min_slope_slide_angle = v;
        }
        if let Some(s) = snap_to_ground {
            if s.is_none() {
                inner.snap_to_ground = None;
            } else {
                inner.snap_to_ground = Some(_extract_char_length(s)?.to_rapier());
            }
        }
        if let Some(v) = normal_nudge_factor {
            inner.normal_nudge_factor = v;
        }
        Ok(Self(inner))
    }

    // ---- property getters / setters ----
    /// World-space "up" direction the controller uses to classify
    /// slopes, ground contacts, and the auto-step axis.
    #[getter]
    fn up(&self) -> Vec3 {
        let v: crate::na::SVector<Real, 3> = self.0.up.into();
        Vec3(v)
    }
    #[setter]
    /// Set the world-space up direction used by the controller.
    fn set_up(&mut self, v: PyVector) {
        self.0.up = v.0.into();
    }

    /// Skin width kept between the character and the environment.
    #[getter]
    fn offset(&self) -> CharacterLength {
        CharacterLength::from_rapier(self.0.offset)
    }
    #[setter]
    /// Set the controller's collision-skin offset.
    ///
    /// Accepts a :class:`CharacterLength` or a bare float
    /// (interpreted as absolute world units).
    fn set_offset(&mut self, py: Python<'_>, obj: Py<PyAny>) -> PyResult<()> {
        let b = obj.bind(py);
        self.0.offset = _extract_char_length(b)?.to_rapier();
        Ok(())
    }

    /// Whether to slide along contact surfaces.
    #[getter]
    fn slide(&self) -> bool {
        self.0.slide
    }
    #[setter]
    fn set_slide(&mut self, v: bool) {
        self.0.slide = v;
    }

    /// Optional :class:`CharacterAutostep` config (``None`` if disabled).
    #[getter]
    fn autostep(&self) -> Option<CharacterAutostep> {
        self.0.autostep.map(|a| CharacterAutostep {
            max_height: CharacterLength::from_rapier(a.max_height),
            min_width: CharacterLength::from_rapier(a.min_width),
            include_dynamic_bodies: a.include_dynamic_bodies,
        })
    }
    #[setter]
    /// Set or clear the :class:`CharacterAutostep` configuration.
    ///
    /// Pass ``None`` to disable auto-step.
    fn set_autostep(&mut self, py: Python<'_>, v: Option<Py<PyAny>>) -> PyResult<()> {
        self.0.autostep = match v {
            None => None,
            Some(obj) => {
                let b = obj.bind(py);
                if b.is_none() {
                    None
                } else {
                    let a: CharacterAutostep = b.extract()?;
                    Some(a.to_rapier())
                }
            }
        };
        Ok(())
    }

    /// Max slope angle (radians) the character can still climb.
    #[getter]
    fn max_slope_climb_angle(&self) -> Real {
        self.0.max_slope_climb_angle
    }
    #[setter]
    fn set_max_slope_climb_angle(&mut self, v: Real) {
        self.0.max_slope_climb_angle = v;
    }

    /// Min slope angle (radians) at which the character will slide
    /// downward (steeper than ``max_slope_climb_angle``).
    #[getter]
    fn min_slope_slide_angle(&self) -> Real {
        self.0.min_slope_slide_angle
    }
    #[setter]
    fn set_min_slope_slide_angle(&mut self, v: Real) {
        self.0.min_slope_slide_angle = v;
    }

    /// Distance below the character treated as still grounded for
    /// snap-down on stairs / descending slopes. ``None`` disables.
    #[getter]
    fn snap_to_ground(&self) -> Option<CharacterLength> {
        self.0.snap_to_ground.map(CharacterLength::from_rapier)
    }
    #[setter]
    /// Set or clear the snap-to-ground distance.
    ///
    /// Accepts a :class:`CharacterLength`, a bare float (absolute),
    /// or ``None`` to disable snapping.
    fn set_snap_to_ground(&mut self, py: Python<'_>, v: Option<Py<PyAny>>) -> PyResult<()> {
        self.0.snap_to_ground = match v {
            None => None,
            Some(obj) => {
                let b = obj.bind(py);
                if b.is_none() {
                    None
                } else {
                    Some(_extract_char_length(b)?.to_rapier())
                }
            }
        };
        Ok(())
    }

    /// Small nudge along contact normals to avoid sticking on geometry.
    #[getter]
    fn normal_nudge_factor(&self) -> Real {
        self.0.normal_nudge_factor
    }
    #[setter]
    fn set_normal_nudge_factor(&mut self, v: Real) {
        self.0.normal_nudge_factor = v;
    }

    // ---- methods ----

    /// Compute the effective collision-aware translation for one step.
    ///
    /// Performs sweep tests through ``queries``, applies sliding,
    /// optional auto-step and ground snapping, and returns the
    /// translation actually applicable to the kinematic body.
    ///
    /// The ``bodies`` / ``colliders`` arguments are accepted for
    /// API parity with the Rust API; the actual sweep uses the
    /// sets already referenced by ``queries``.
    ///
    /// :param dt: Time step in seconds.
    /// :param bodies: Rigid-body set (kept for API parity).
    /// :param colliders: Collider set (kept for API parity).
    /// :param queries: Up-to-date :class:`QueryPipeline`.
    /// :param shape: Character collision shape.
    /// :param shape_pos: World pose of the shape at frame start.
    /// :param desired_translation: Desired translation this step.
    /// :param filter: Optional query filter.
    /// :param events_callback: Optional callable receiving each
    ///     :class:`CharacterCollision` as it is detected.
    /// :returns: :class:`EffectiveCharacterMovement`.
    #[pyo3(signature = (
        dt, bodies, colliders, queries, shape, shape_pos, desired_translation,
        filter=None, events_callback=None
    ))]
    #[allow(clippy::too_many_arguments)]
    fn move_shape(
        &self,
        py: Python<'_>,
        dt: Real,
        bodies: &Bound<'_, PyAny>,
        colliders: &Bound<'_, PyAny>,
        queries: &QueryPipeline,
        shape: &SharedShape,
        shape_pos: PyIsometry,
        desired_translation: PyVector,
        filter: Option<&QueryFilter>,
        events_callback: Option<Py<PyAny>>,
    ) -> PyResult<EffectiveCharacterMovement> {
        let _ = (bodies, colliders); // parity-only; queries owns them
        let pose: rapier::math::Pose = shape_pos.0.into();
        let desired: rapier::math::Vector = desired_translation.0.into();
        let inner = &self.0;
        let cb_obj = events_callback.map(|o| o.clone_ref(py));
        let cb_err: std::cell::RefCell<Option<crate::pyo3::PyErr>> = std::cell::RefCell::new(None);

        let result = {
            // Borrow the world's sub-sets immutably (queries are read-only).
            let bp = queries.broad_phase.borrow(py);
            let np = queries.narrow_phase.borrow(py);
            let bodies = queries.bodies.borrow(py);
            let colliders = queries.colliders.borrow(py);

            let py_pred_obj: Option<Py<PyAny>> =
                filter.and_then(|f| f.predicate.as_ref().map(|p| p.clone_ref(py)));
            let pred_err = std::cell::RefCell::new(None::<crate::pyo3::PyErr>);
            let pred_closure =
                |h: rapier::geometry::ColliderHandle, co: &rapier::geometry::Collider| -> bool {
                    let _ = co;
                    if let Some(ref obj) = py_pred_obj {
                        let coll_py = Collider {
                            backing: ColliderBacking::InSet {
                                set: queries.colliders.clone_ref(py),
                                handle: h,
                            },
                        };
                        let h_py = ColliderHandle(h);
                        match obj.call1(py, (h_py, coll_py)) {
                            Ok(r) => r.extract::<bool>(py).unwrap_or(true),
                            Err(e) => {
                                *pred_err.borrow_mut() = Some(e);
                                true
                            }
                        }
                    } else {
                        true
                    }
                };
            let pred_dyn: Option<
                &dyn Fn(rapier::geometry::ColliderHandle, &rapier::geometry::Collider) -> bool,
            > = if py_pred_obj.is_some() {
                Some(&pred_closure)
            } else {
                None
            };

            let qf = filter.map(|f| f.as_rapier(pred_dyn)).unwrap_or_else(|| {
                let mut base = rapier::pipeline::QueryFilter::default();
                base.predicate = pred_dyn;
                base
            });

            let qp =
                bp.0.as_query_pipeline(np.0.query_dispatcher(), &bodies.0, &colliders.0, qf);

            let result = (|qp: rapier::pipeline::QueryPipeline<'_>| {
                let mv = inner.move_shape(dt, &qp, &*shape.0.0, &pose, desired, |collision| {
                    if let Some(ref obj) = cb_obj {
                        let py_col = CharacterCollision::from_rapier(collision);
                        if let Err(e) = obj.call1(py, (py_col,)) {
                            *cb_err.borrow_mut() = Some(e);
                        }
                    }
                });
                let t: crate::na::SVector<Real, 3> = mv.translation.into();
                Ok::<EffectiveCharacterMovement, crate::pyo3::PyErr>(EffectiveCharacterMovement {
                    translation: Vec3(t),
                    grounded: mv.grounded,
                    is_sliding_down_slope: mv.is_sliding_down_slope,
                })
            })(qp);
            if let Some(e) = pred_err.into_inner() {
                return Err(e);
            }
            result
        }?;
        if let Some(e) = cb_err.into_inner() {
            return Err(e);
        }
        Ok(result)
    }

    /// Push the dynamic bodies the character touched this frame.
    ///
    /// Iterates over the :class:`CharacterCollision` events
    /// previously delivered by :meth:`move_shape` and applies
    /// reactive impulses sized for ``character_mass`` onto any
    /// dynamic colliders found.
    ///
    /// :param dt: Time step in seconds.
    /// :param bodies: Rigid-body set (mutated).
    /// :param colliders: Collider set (mutated).
    /// :param queries: Up-to-date :class:`QueryPipeline`.
    /// :param character_shape: Character collision shape.
    /// :param character_pos: World pose (currently unused; kept for
    ///     forward-compat with upstream).
    /// :param character_mass: Mass used to scale impulses.
    /// :param collisions: List of :class:`CharacterCollision`.
    /// :param filter: Optional query filter.
    #[pyo3(signature = (
        dt, bodies, colliders, queries, character_shape, character_pos,
        character_mass, collisions, filter=None
    ))]
    #[allow(clippy::too_many_arguments)]
    fn solve_character_collision_impulses(
        &self,
        py: Python<'_>,
        dt: Real,
        bodies: Py<RigidBodySet>,
        colliders: Py<ColliderSet>,
        queries: &QueryPipeline,
        character_shape: &SharedShape,
        character_pos: PyIsometry,
        character_mass: Real,
        collisions: Vec<CharacterCollision>,
        filter: Option<&QueryFilter>,
    ) -> PyResult<()> {
        let _ = character_pos;
        // Build an owned QueryPipelineMut. Since QueryPipelineMut holds
        // `&mut RigidBodySet` and `&mut ColliderSet`, we borrow_mut the
        // Py<> handles.
        let bp = queries.broad_phase.borrow(py);
        let np = queries.narrow_phase.borrow(py);
        let mut bodies_ref = bodies.borrow_mut(py);
        let mut colliders_ref = colliders.borrow_mut(py);
        let qf = filter.map(|f| f.as_rapier(None)).unwrap_or_default();
        let mut qpmut = bp.0.as_query_pipeline_mut(
            np.0.query_dispatcher(),
            &mut bodies_ref.0,
            &mut colliders_ref.0,
            qf,
        );
        let collisions_rs: Vec<_> = collisions.iter().map(|c| c.to_rapier()).collect();
        self.0.solve_character_collision_impulses(
            dt,
            &mut qpmut,
            &*character_shape.0.0,
            character_mass,
            collisions_rs.iter(),
        );
        Ok(())
    }

    /// Return a debug string summarizing the controller config.
    fn __repr__(&self) -> String {
        format!(
            "KinematicCharacterController(up={:?}, offset={:?}, slide={}, autostep={}, max_slope_climb_angle={}, min_slope_slide_angle={}, snap_to_ground={})",
            self.0.up,
            self.0.offset,
            self.0.slide,
            if self.0.autostep.is_some() {
                "set"
            } else {
                "None"
            },
            self.0.max_slope_climb_angle,
            self.0.min_slope_slide_angle,
            if self.0.snap_to_ground.is_some() {
                "set"
            } else {
                "None"
            },
        )
    }
}

// ---------------------------------------------------------------
// AxesMask (controllers use this, distinct from LockedAxes).
//
// Upstream `AxesMask` is dim-aware: LIN_X, LIN_Y, LIN_Z, ANG_X,
// ANG_Y, ANG_Z (all present in 3D).
// ---------------------------------------------------------------
/// Bitflags selecting which DOFs a PID/PD controller drives.
///
/// In 3D the bits are ``LIN_X``, ``LIN_Y``, ``LIN_Z``, ``ANG_X``,
/// ``ANG_Y``, ``ANG_Z``. In 2D only ``LIN_X``, ``LIN_Y``, and
/// ``ANG_Z`` exist (other axes are absent from upstream and not
/// exposed as class attributes).
///
/// Combine bits with the usual ``|``, ``&``, ``^``, ``-``, ``~``
/// operators. Distinct from :class:`LockedAxes`, which freezes
/// DOFs on a rigid body.
#[pyclass(name = "AxesMask", module = "rapier", frozen)]
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct AxesMask(pub rapier::dynamics::AxesMask);

#[pymethods]
impl AxesMask {
    /// Build an ``AxesMask`` from a raw bit pattern.
    #[new]
    #[pyo3(signature = (bits = 0))]
    fn new(bits: u8) -> Self {
        Self(rapier::dynamics::AxesMask::from_bits_truncate(bits))
    }
    /// Return the empty mask (no axes).
    #[staticmethod]
    fn empty() -> Self {
        Self(rapier::dynamics::AxesMask::empty())
    }
    /// Return the mask with every dim-valid axis set.
    #[staticmethod]
    fn all_axes() -> Self {
        Self(rapier::dynamics::AxesMask::all())
    }
    // For consistency with LockedAxes:
    /// Alias of :meth:`all_axes` for parity with :class:`LockedAxes`.
    #[staticmethod]
    fn all() -> Self {
        Self(rapier::dynamics::AxesMask::all())
    }

    #[classattr]
    const LIN_X: AxesMask = AxesMask(rapier::dynamics::AxesMask::LIN_X);
    #[classattr]
    const LIN_Y: AxesMask = AxesMask(rapier::dynamics::AxesMask::LIN_Y);
    #[classattr]
    const ANG_Z: AxesMask = AxesMask(rapier::dynamics::AxesMask::ANG_Z);

    /// Raw bits as an unsigned int.
    #[getter]
    fn bits(&self) -> u8 {
        self.0.bits()
    }
    /// ``True`` if ``self`` is a superset of ``other``.
    fn contains(&self, other: &Self) -> bool {
        self.0.contains(other.0)
    }
    /// ``True`` if no bits are set.
    fn is_empty(&self) -> bool {
        self.0.is_empty()
    }
    /// Python ``in`` operator: alias of :meth:`contains`.
    fn __contains__(&self, other: &Self) -> bool {
        self.0.contains(other.0)
    }
    /// Bitwise OR: union of two masks.
    fn __or__(&self, other: &Self) -> Self {
        Self(self.0 | other.0)
    }
    /// Bitwise AND: intersection of two masks.
    fn __and__(&self, other: &Self) -> Self {
        Self(self.0 & other.0)
    }
    /// Bitwise XOR: symmetric difference of two masks.
    fn __xor__(&self, other: &Self) -> Self {
        Self(self.0 ^ other.0)
    }
    /// Bitwise NOT: complement within the valid axes for this dim.
    fn __invert__(&self) -> Self {
        Self(!self.0)
    }
    /// Set subtraction.
    fn __sub__(&self, other: &Self) -> Self {
        Self(self.0 - other.0)
    }
    /// ``False`` iff no bits are set.
    fn __bool__(&self) -> bool {
        !self.0.is_empty()
    }
    /// Equality (``==`` / ``!=``). Other comparisons raise ``TypeError``.
    fn __richcmp__(&self, other: &Self, op: CompareOp) -> PyResult<bool> {
        match op {
            CompareOp::Eq => Ok(self.0 == other.0),
            CompareOp::Ne => Ok(self.0 != other.0),
            _ => Err(PyTypeError::new_err("AxesMask supports only == and !=")),
        }
    }
    /// Hash by raw bit pattern (consistent with ``==``).
    fn __hash__(&self) -> u64 {
        self.0.bits() as u64
    }
    /// Return ``AxesMask(bits=0b...)`` repr.
    fn __repr__(&self) -> String {
        format!("AxesMask(bits={:#010b})", self.0.bits())
    }
}

// Dim-specific class attributes — multiple-pymethods is enabled, so
// we can have a second `impl AxesMask` block here.
#[pymethods]
impl AxesMask {
    #[classattr]
    const LIN_Z: AxesMask = AxesMask(rapier::dynamics::AxesMask::LIN_Z);
    #[classattr]
    const ANG_X: AxesMask = AxesMask(rapier::dynamics::AxesMask::ANG_X);
    #[classattr]
    const ANG_Y: AxesMask = AxesMask(rapier::dynamics::AxesMask::ANG_Y);
}

// ============================================================================
// PID/PD controller (uses dim-specific angular type).
// ============================================================================

// PdErrors — position or velocity error pair.
/// Linear + angular error pair fed to :class:`PidController` /
/// :class:`PdController`.
///
/// Used both as a "position error" (desired minus current pose)
/// and as a "velocity error" (desired minus current velocity).
/// In 3D ``angular`` is a 3-vector; in 2D it is a single ``float``.
///
/// :ivar linear: Linear error vector.
/// :ivar angular: Angular error (vector in 3D, scalar in 2D).
#[pyclass(name = "PdErrors", module = "rapier")]
#[derive(Debug, Clone, Copy)]
pub struct PdErrors {
    #[pyo3(get, set)]
    pub linear: Vec3,
    #[pyo3(get, set)]
    pub angular: Vec3,
}

#[pymethods]
impl PdErrors {
    /// Build a ``PdErrors`` from optional ``linear`` / ``angular`` values.
    ///
    /// :param linear: Linear error (defaults to zero).
    /// :param angular: Angular error (defaults to zero).
    #[new]
    #[pyo3(signature = (linear=None, angular=None))]
    fn new(linear: Option<PyVector>, angular: Option<&Bound<'_, PyAny>>) -> PyResult<Self> {
        let lin = linear
            .map(|v| v.0)
            .unwrap_or_else(crate::na::SVector::<Real, 3>::zeros);
        let ang = {
            match angular {
                None => Ok::<Vec3, crate::pyo3::PyErr>(Vec3(crate::na::Vector3::zeros())),
                Some(o) => {
                    let pv: PyVector = o.extract()?;
                    Ok(Vec3(pv.0))
                }
            }
        }?;
        Ok(Self {
            linear: Vec3(lin),
            angular: ang,
        })
    }
    /// Return the ``PdErrors(linear=..., angular=...)`` repr.
    fn __repr__(&self) -> String {
        format!(
            "PdErrors(linear={:?}, angular={:?})",
            self.linear.0, self.angular
        )
    }
}

impl PdErrors {
    #[inline]
    pub(crate) fn to_rapier(&self) -> rapier::control::PdErrors {
        rapier::control::PdErrors {
            linear: self.linear.0.into(),
            angular: {
                let av: rapier::math::AngVector = self.angular.0.into();
                av
            },
        }
    }
    #[inline]
    #[allow(dead_code)] // symmetry with to_rapier; kept for completeness
    pub(crate) fn from_rapier(p: rapier::control::PdErrors) -> Self {
        let l: crate::na::SVector<Real, 3> = p.linear.into();
        Self {
            linear: Vec3(l),
            angular: {
                let r: crate::na::Vector3<Real> = p.angular.into();
                Vec3(r)
            },
        }
    }
}

// PidCorrection — linvel/angvel update produced by a controller.
/// Linear + angular velocity correction returned by PID/PD updates.
///
/// The caller is expected to apply this as an impulse / velocity
/// delta to the controlled rigid body each step.
///
/// :ivar linear: Linear velocity correction.
/// :ivar angular: Angular velocity correction (vector in 3D,
///     scalar in 2D).
#[pyclass(name = "PidCorrection", module = "rapier", frozen)]
#[derive(Debug, Clone, Copy)]
pub struct PidCorrection {
    #[pyo3(get)]
    pub linear: Vec3,
    #[pyo3(get)]
    pub angular: Vec3,
}

#[pymethods]
impl PidCorrection {
    /// Return the ``PidCorrection(linear=..., angular=...)`` repr.
    fn __repr__(&self) -> String {
        format!(
            "PidCorrection(linear={:?}, angular={:?})",
            self.linear.0, self.angular
        )
    }
}

impl PidCorrection {
    #[inline]
    fn from_velocity(v: rapier::dynamics::RigidBodyVelocity<Real>) -> Self {
        let lin: crate::na::SVector<Real, 3> = v.linvel.into();
        Self {
            linear: Vec3(lin),
            angular: {
                let r: crate::na::Vector3<Real> = v.angvel.into();
                Vec3(r)
            },
        }
    }
}

// PdController — immutable PD.
/// Proportional-derivative controller (PD).
///
/// Like :class:`PidController` but without the integral term —
/// cheaper per step, no integral wind-up, but cannot eliminate
/// steady-state error. Choose this when target tracking is
/// frequent and the loop is well-damped.
#[pyclass(name = "PdController", module = "rapier")]
#[derive(Debug, Clone, Copy)]
pub struct PdController(pub rapier::control::PdController);

#[pymethods]
impl PdController {
    /// Build a PD controller with optional per-axis ``Kp`` / ``Kd``.
    ///
    /// :param axes: :class:`AxesMask` selecting which DOFs to
    ///     control (defaults to all).
    /// :param Kp: Proportional gain — scalar (broadcast) or a
    ///     vector matching linear-axis count.
    /// :param Kd: Derivative gain — same shape as ``Kp``.
    #[new]
    #[pyo3(signature = (axes=None, Kp=None, Kd=None))]
    #[allow(non_snake_case)]
    fn new(
        axes: Option<&AxesMask>,
        Kp: Option<&Bound<'_, PyAny>>,
        Kd: Option<&Bound<'_, PyAny>>,
    ) -> PyResult<Self> {
        let am = axes
            .map(|a| a.0)
            .unwrap_or_else(rapier::dynamics::AxesMask::all);
        let mut inner = rapier::control::PdController::new(60.0 as Real, 0.8 as Real, am);
        if let Some(k) = Kp {
            let v: PyVector = k.extract()?;
            inner.lin_kp = v.0.into();
            let ang_kp = {
                // Accept a (x,y,z) tuple/list/Vec3 or a single float (broadcast).
                let av: rapier::math::AngVector = if let Ok(f) = k.extract::<Real>() {
                    rapier::math::AngVector::splat(f)
                } else {
                    let pv: PyVector = k.extract()?;
                    pv.0.into()
                };
                Ok::<rapier::math::AngVector, crate::pyo3::PyErr>(av)
            }?;
            inner.ang_kp = ang_kp;
        }
        if let Some(k) = Kd {
            let v: PyVector = k.extract()?;
            inner.lin_kd = v.0.into();
            let ang_kd = {
                // Accept a (x,y,z) tuple/list/Vec3 or a single float (broadcast).
                let av: rapier::math::AngVector = if let Ok(f) = k.extract::<Real>() {
                    rapier::math::AngVector::splat(f)
                } else {
                    let pv: PyVector = k.extract()?;
                    pv.0.into()
                };
                Ok::<rapier::math::AngVector, crate::pyo3::PyErr>(av)
            }?;
            inner.ang_kd = ang_kd;
        }
        Ok(Self(inner))
    }

    /// :class:`AxesMask` selecting which DOFs are driven.
    #[getter]
    fn axes(&self) -> AxesMask {
        AxesMask(self.0.axes)
    }
    #[setter]
    /// Replace the :class:`AxesMask` selecting controlled DOFs.
    fn set_axes(&mut self, v: &AxesMask) {
        self.0.axes = v.0;
    }

    /// Set the per-linear-axis proportional gain vector.
    fn set_axes_kp(&mut self, v: PyVector) {
        self.0.lin_kp = v.0.into();
    }
    /// Set the per-linear-axis derivative gain vector.
    fn set_axes_kd(&mut self, v: PyVector) {
        self.0.lin_kd = v.0.into();
    }

    /// Velocity correction toward ``target_pose`` (zero target velocity).
    ///
    /// :param body: :class:`RigidBody` to drive.
    /// :param target_pose: Desired world pose.
    /// :returns: :class:`PidCorrection` to apply as a velocity delta.
    #[pyo3(signature = (body, target_pose))]
    fn rigid_body_correction(
        &self,
        py: Python<'_>,
        body: &Bound<'_, PyAny>,
        target_pose: PyIsometry,
    ) -> PyResult<PidCorrection> {
        let _ = py;
        let rb = body.extract::<PyRef<'_, RigidBody>>()?;
        let target_pose: rapier::math::Pose = target_pose.0.into();
        let zero_vels = rapier::dynamics::RigidBodyVelocity::<Real>::zero();
        let owned = rb.to_owned_body();
        let corr = self.0.rigid_body_correction(&owned, target_pose, zero_vels);
        Ok(PidCorrection::from_velocity(corr))
    }

    /// Velocity correction from explicit pose / velocity errors.
    ///
    /// :param pose_errors: Position error pair.
    /// :param vel_errors: Velocity error pair.
    /// :returns: :class:`PidCorrection`.
    fn correction(&self, pose_errors: &PdErrors, vel_errors: &PdErrors) -> PidCorrection {
        let pe = pose_errors.to_rapier();
        let ve = vel_errors.to_rapier();
        let c = self.0.correction(&pe, &ve);
        PidCorrection::from_velocity(c)
    }
}

// PidController — full PID.
/// Proportional-integral-derivative controller (PID).
///
/// Drives a rigid body toward a target pose along the DOFs picked
/// by an :class:`AxesMask`. Gains ``Kp`` / ``Ki`` / ``Kd`` may be
/// specified per axis (vector) or as a scalar broadcast to every
/// driven axis. Use :meth:`rigid_body_correction` per step to get
/// the velocity correction to apply.
#[pyclass(name = "PidController", module = "rapier")]
#[derive(Debug, Clone, Copy)]
pub struct PidController(pub rapier::control::PidController);

#[pymethods]
impl PidController {
    /// Build a PID with optional per-axis ``Kp`` / ``Ki`` / ``Kd``.
    ///
    /// :param axes: :class:`AxesMask` selecting controlled DOFs.
    /// :param Kp: Proportional gain (scalar or vector).
    /// :param Ki: Integral gain (scalar or vector).
    /// :param Kd: Derivative gain (scalar or vector).
    #[new]
    #[pyo3(signature = (axes=None, Kp=None, Ki=None, Kd=None))]
    #[allow(non_snake_case)]
    fn new(
        axes: Option<&AxesMask>,
        Kp: Option<&Bound<'_, PyAny>>,
        Ki: Option<&Bound<'_, PyAny>>,
        Kd: Option<&Bound<'_, PyAny>>,
    ) -> PyResult<Self> {
        let am = axes
            .map(|a| a.0)
            .unwrap_or_else(rapier::dynamics::AxesMask::all);
        let mut inner =
            rapier::control::PidController::new(60.0 as Real, 1.0 as Real, 0.8 as Real, am);
        if let Some(k) = Kp {
            let v: PyVector = k.extract()?;
            inner.pd.lin_kp = v.0.into();
            inner.pd.ang_kp = {
                // Accept a (x,y,z) tuple/list/Vec3 or a single float (broadcast).
                let av: rapier::math::AngVector = if let Ok(f) = k.extract::<Real>() {
                    rapier::math::AngVector::splat(f)
                } else {
                    let pv: PyVector = k.extract()?;
                    pv.0.into()
                };
                Ok::<rapier::math::AngVector, crate::pyo3::PyErr>(av)
            }?;
        }
        if let Some(k) = Kd {
            let v: PyVector = k.extract()?;
            inner.pd.lin_kd = v.0.into();
            inner.pd.ang_kd = {
                // Accept a (x,y,z) tuple/list/Vec3 or a single float (broadcast).
                let av: rapier::math::AngVector = if let Ok(f) = k.extract::<Real>() {
                    rapier::math::AngVector::splat(f)
                } else {
                    let pv: PyVector = k.extract()?;
                    pv.0.into()
                };
                Ok::<rapier::math::AngVector, crate::pyo3::PyErr>(av)
            }?;
        }
        if let Some(k) = Ki {
            let v: PyVector = k.extract()?;
            inner.lin_ki = v.0.into();
            inner.ang_ki = {
                // Accept a (x,y,z) tuple/list/Vec3 or a single float (broadcast).
                let av: rapier::math::AngVector = if let Ok(f) = k.extract::<Real>() {
                    rapier::math::AngVector::splat(f)
                } else {
                    let pv: PyVector = k.extract()?;
                    pv.0.into()
                };
                Ok::<rapier::math::AngVector, crate::pyo3::PyErr>(av)
            }?;
        }
        Ok(Self(inner))
    }

    /// :class:`AxesMask` selecting which DOFs are driven.
    #[getter]
    fn axes(&self) -> AxesMask {
        AxesMask(self.0.axes())
    }
    #[setter]
    /// Replace the :class:`AxesMask` selecting controlled DOFs.
    fn set_axes_attr(&mut self, v: &AxesMask) {
        self.0.set_axes(v.0);
    }

    /// Set the per-linear-axis proportional gain vector.
    fn set_axes_kp(&mut self, v: PyVector) {
        self.0.pd.lin_kp = v.0.into();
    }
    /// Set the per-linear-axis integral gain vector.
    fn set_axes_ki(&mut self, v: PyVector) {
        self.0.lin_ki = v.0.into();
    }
    /// Set the per-linear-axis derivative gain vector.
    fn set_axes_kd(&mut self, v: PyVector) {
        self.0.pd.lin_kd = v.0.into();
    }

    /// Reset the integral accumulators to zero.
    fn reset(&mut self) {
        self.0.reset_integrals();
    }

    /// One PID step: velocity correction toward ``target_pose``.
    ///
    /// :param dt: Time step in seconds.
    /// :param body: :class:`RigidBody` to drive.
    /// :param target_pose: Desired world pose.
    /// :returns: :class:`PidCorrection`.
    #[pyo3(signature = (dt, body, target_pose))]
    fn rigid_body_correction(
        &mut self,
        dt: Real,
        body: &Bound<'_, PyAny>,
        target_pose: PyIsometry,
    ) -> PyResult<PidCorrection> {
        let rb = body.extract::<PyRef<'_, RigidBody>>()?;
        let pose: rapier::math::Pose = target_pose.0.into();
        let zero_vels = rapier::dynamics::RigidBodyVelocity::<Real>::zero();
        let owned = rb.to_owned_body();
        let corr = self.0.rigid_body_correction(dt, &owned, pose, zero_vels);
        Ok(PidCorrection::from_velocity(corr))
    }

    /// PID correction from two poses (no rigid-body needed).
    ///
    /// Useful for offline / off-graph usage where you only have
    /// the current and target world poses. Assumes zero current
    /// and target velocities.
    ///
    /// :param dt: Time step in seconds.
    /// :param position: Current world pose.
    /// :param target_position: Desired world pose.
    /// :returns: :class:`PidCorrection`.
    #[pyo3(signature = (dt, position, target_position))]
    fn position_correction(
        &mut self,
        dt: Real,
        position: PyIsometry,
        target_position: PyIsometry,
    ) -> PyResult<PidCorrection> {
        let pos: rapier::math::Pose = position.0.into();
        let tgt: rapier::math::Pose = target_position.0.into();
        // Compute pose errors manually using the same formula upstream
        // uses (RigidBodyPosition::pose_errors with COM = origin).
        let rb_pos = rapier::dynamics::RigidBodyPosition {
            position: pos,
            next_position: tgt,
        };
        let pose_errors = rb_pos.pose_errors(rapier::math::Vector::ZERO);
        let zero_vels: rapier::control::PdErrors =
            rapier::dynamics::RigidBodyVelocity::<Real>::zero().into();
        let corr = self.0.correction(dt, &pose_errors, &zero_vels);
        Ok(PidCorrection::from_velocity(corr))
    }

    /// Run one PID update step with explicit pose/velocity errors.
    ///
    /// :param dt: Time step in seconds.
    /// :param pose_errors: Position error pair.
    /// :param vel_errors: Velocity error pair.
    /// :returns: :class:`PidCorrection`.
    fn update(&mut self, dt: Real, pose_errors: &PdErrors, vel_errors: &PdErrors) -> PidCorrection {
        let pe = pose_errors.to_rapier();
        let ve = vel_errors.to_rapier();
        let c = self.0.correction(dt, &pe, &ve);
        PidCorrection::from_velocity(c)
    }
}

// ============================================================================
// 3D-only: vehicle controller.
// ============================================================================

// ---------------------------------------------------------------
// WheelTuning
// ---------------------------------------------------------------
/// Spring / friction tuning parameters for a vehicle wheel.
///
/// Used when adding a wheel via
/// :meth:`DynamicRayCastVehicleController.add_wheel`.
///
/// .. note::
///    :meth:`default` returns rapier's upstream defaults which are
///    typically *too soft* for most real-world setups — wheels tend
///    to bottom out and drift sideways. Expect to tighten
///    ``suspension_stiffness``, ``friction_slip``, and
///    ``side_friction_stiffness`` substantially.
#[pyclass(name = "WheelTuning", module = "rapier")]
#[derive(Debug, Clone, Copy)]
pub struct WheelTuning(pub rapier::control::WheelTuning);

#[pymethods]
impl WheelTuning {
    /// Build a ``WheelTuning`` with optional overrides.
    ///
    /// :param suspension_stiffness: Spring constant.
    /// :param suspension_compression: Compression damping.
    /// :param suspension_damping: Relaxation damping.
    /// :param max_suspension_travel: Max compression travel.
    /// :param side_friction_stiffness: Lateral tire grip.
    /// :param friction_slip: Longitudinal friction coefficient.
    /// :param max_suspension_force: Force cap on the spring.
    #[new]
    #[pyo3(signature = (suspension_stiffness=None, suspension_compression=None, suspension_damping=None, max_suspension_travel=None, side_friction_stiffness=None, friction_slip=None, max_suspension_force=None))]
    #[allow(clippy::too_many_arguments)]
    fn new(
        suspension_stiffness: Option<Real>,
        suspension_compression: Option<Real>,
        suspension_damping: Option<Real>,
        max_suspension_travel: Option<Real>,
        side_friction_stiffness: Option<Real>,
        friction_slip: Option<Real>,
        max_suspension_force: Option<Real>,
    ) -> Self {
        let mut t = rapier::control::WheelTuning::default();
        if let Some(v) = suspension_stiffness {
            t.suspension_stiffness = v;
        }
        if let Some(v) = suspension_compression {
            t.suspension_compression = v;
        }
        if let Some(v) = suspension_damping {
            t.suspension_damping = v;
        }
        if let Some(v) = max_suspension_travel {
            t.max_suspension_travel = v;
        }
        if let Some(v) = side_friction_stiffness {
            t.side_friction_stiffness = v;
        }
        if let Some(v) = friction_slip {
            t.friction_slip = v;
        }
        if let Some(v) = max_suspension_force {
            t.max_suspension_force = v;
        }
        Self(t)
    }

    /// Return rapier's default ``WheelTuning``.
    ///
    /// Beware — the upstream defaults are intentionally soft and
    /// often produce skating / bottoming-out behavior. Tune for
    /// your vehicle.
    #[staticmethod]
    fn default() -> Self {
        Self(rapier::control::WheelTuning::default())
    }

    /// Spring stiffness constant.
    #[getter]
    fn suspension_stiffness(&self) -> Real {
        self.0.suspension_stiffness
    }
    #[setter]
    /// Assign the suspension spring stiffness constant.
    fn set_suspension_stiffness(&mut self, v: Real) {
        self.0.suspension_stiffness = v;
    }
    /// Damping while the spring is compressing.
    #[getter]
    fn suspension_compression(&self) -> Real {
        self.0.suspension_compression
    }
    #[setter]
    /// Assign the compression-phase damping coefficient.
    fn set_suspension_compression(&mut self, v: Real) {
        self.0.suspension_compression = v;
    }
    /// Damping while the spring is relaxing.
    #[getter]
    fn suspension_damping(&self) -> Real {
        self.0.suspension_damping
    }
    #[setter]
    /// Assign the relaxation-phase damping coefficient.
    fn set_suspension_damping(&mut self, v: Real) {
        self.0.suspension_damping = v;
    }
    /// Max travel distance for the suspension.
    #[getter]
    fn max_suspension_travel(&self) -> Real {
        self.0.max_suspension_travel
    }
    #[setter]
    /// Assign the maximum suspension travel distance.
    fn set_max_suspension_travel(&mut self, v: Real) {
        self.0.max_suspension_travel = v;
    }
    /// Lateral (sideways) tire stiffness.
    #[getter]
    fn side_friction_stiffness(&self) -> Real {
        self.0.side_friction_stiffness
    }
    #[setter]
    /// Assign the lateral tire stiffness coefficient.
    fn set_side_friction_stiffness(&mut self, v: Real) {
        self.0.side_friction_stiffness = v;
    }
    /// Longitudinal friction coefficient.
    #[getter]
    fn friction_slip(&self) -> Real {
        self.0.friction_slip
    }
    #[setter]
    /// Assign the longitudinal friction coefficient.
    fn set_friction_slip(&mut self, v: Real) {
        self.0.friction_slip = v;
    }
    /// Maximum suspension spring force.
    #[getter]
    fn max_suspension_force(&self) -> Real {
        self.0.max_suspension_force
    }
    #[setter]
    /// Assign the cap on suspension spring force.
    fn set_max_suspension_force(&mut self, v: Real) {
        self.0.max_suspension_force = v;
    }

    /// Return the ``WheelTuning(...)`` repr.
    fn __repr__(&self) -> String {
        format!(
            "WheelTuning(suspension_stiffness={}, friction_slip={})",
            self.0.suspension_stiffness, self.0.friction_slip
        )
    }
}

// ---------------------------------------------------------------
// RayCastInfo
// ---------------------------------------------------------------
/// Read-only per-wheel raycast hit info for the vehicle controller.
///
/// Snapshot taken after the latest
/// :meth:`DynamicRayCastVehicleController.update_vehicle` call.
///
/// :ivar contact_normal_ws: World-space normal at the contact.
/// :ivar contact_point_ws: World-space contact point.
/// :ivar suspension_length: Current spring length (compressed).
/// :ivar hard_point_ws: World-space wheel attachment point on chassis.
/// :ivar is_in_contact: ``True`` if the ray hit anything.
/// :ivar ground_object: Hit collider handle, if any.
#[pyclass(name = "RayCastInfo", module = "rapier", frozen)]
#[derive(Debug, Clone, Copy)]
pub struct RayCastInfo {
    #[pyo3(get)]
    pub contact_normal_ws: Vec3,
    #[pyo3(get)]
    pub contact_point_ws: Vec3,
    #[pyo3(get)]
    pub suspension_length: Real,
    #[pyo3(get)]
    pub hard_point_ws: Vec3,
    #[pyo3(get)]
    pub is_in_contact: bool,
    #[pyo3(get)]
    pub ground_object: Option<ColliderHandle>,
}

#[pymethods]
impl RayCastInfo {
    /// Return the ``RayCastInfo(...)`` repr.
    fn __repr__(&self) -> String {
        format!(
            "RayCastInfo(is_in_contact={}, suspension_length={})",
            self.is_in_contact, self.suspension_length
        )
    }
}

impl RayCastInfo {
    #[inline]
    fn from_wheel(w: &rapier::control::Wheel) -> Self {
        let r = w.raycast_info();
        let cn: crate::na::Vector3<Real> = r.contact_normal_ws.into();
        let cp: crate::na::Vector3<Real> = r.contact_point_ws.into();
        let hp: crate::na::Vector3<Real> = r.hard_point_ws.into();
        Self {
            contact_normal_ws: Vec3(cn),
            contact_point_ws: Vec3(cp),
            suspension_length: r.suspension_length,
            hard_point_ws: Vec3(hp),
            is_in_contact: r.is_in_contact,
            ground_object: r.ground_object.map(ColliderHandle),
        }
    }
}

// ---------------------------------------------------------------
// Wheel — a snapshot/proxy. The upstream controller owns the
// backing `Vec<Wheel>`; we project read+write fields into a Python
// class. Mutations on a `Wheel` instance returned by
// `.wheels()` / `.wheel(i)` do NOT propagate back to the
// controller — they are detached copies. Use the controller's
// `set_brake` / `set_steering` / `apply_engine_force` for
// mutations that need to take effect.
// ---------------------------------------------------------------
/// Detached snapshot of one vehicle wheel.
///
/// .. warning::
///    Instances returned by
///    :meth:`DynamicRayCastVehicleController.wheels` /
///    :meth:`DynamicRayCastVehicleController.wheel` are **copies**.
///    Mutating the fields here does **not** propagate back to the
///    controller. Use
///    :meth:`DynamicRayCastVehicleController.set_brake`,
///    :meth:`set_steering`, and :meth:`apply_engine_force` to
///    actually drive the wheel.
#[pyclass(name = "Wheel", module = "rapier")]
#[derive(Debug, Clone, Copy)]
pub struct Wheel {
    #[pyo3(get, set)]
    pub chassis_connection_point_cs: Vec3,
    #[pyo3(get, set)]
    pub direction_cs: Vec3,
    #[pyo3(get, set)]
    pub axle_cs: Vec3,
    #[pyo3(get, set)]
    pub suspension_rest_length: Real,
    #[pyo3(get, set)]
    pub max_suspension_travel: Real,
    #[pyo3(get, set)]
    pub radius: Real,
    #[pyo3(get, set)]
    pub suspension_stiffness: Real,
    #[pyo3(get, set)]
    pub damping_compression: Real,
    #[pyo3(get, set)]
    pub damping_relaxation: Real,
    #[pyo3(get, set)]
    pub friction_slip: Real,
    #[pyo3(get, set)]
    pub side_friction_stiffness: Real,
    #[pyo3(get, set)]
    pub rotation: Real,
    #[pyo3(get, set)]
    pub max_suspension_force: Real,
    #[pyo3(get, set)]
    pub forward_impulse: Real,
    #[pyo3(get, set)]
    pub side_impulse: Real,
    #[pyo3(get, set)]
    pub steering: Real,
    #[pyo3(get, set)]
    pub engine_force: Real,
    #[pyo3(get, set)]
    pub brake: Real,
    #[pyo3(get, set)]
    pub wheel_suspension_force: Real,
    #[pyo3(get)]
    pub raycast_info: RayCastInfo,
}

#[pymethods]
impl Wheel {
    /// Return the ``Wheel(...)`` repr.
    fn __repr__(&self) -> String {
        format!(
            "Wheel(engine_force={}, brake={}, steering={})",
            self.engine_force, self.brake, self.steering
        )
    }
}

impl Wheel {
    #[inline]
    pub(crate) fn from_rapier(w: &rapier::control::Wheel) -> Self {
        let ccp: crate::na::Vector3<Real> = w.chassis_connection_point_cs.into();
        let dir: crate::na::Vector3<Real> = w.direction_cs.into();
        let axle: crate::na::Vector3<Real> = w.axle_cs.into();
        Self {
            chassis_connection_point_cs: Vec3(ccp),
            direction_cs: Vec3(dir),
            axle_cs: Vec3(axle),
            suspension_rest_length: w.suspension_rest_length,
            max_suspension_travel: w.max_suspension_travel,
            radius: w.radius,
            suspension_stiffness: w.suspension_stiffness,
            damping_compression: w.damping_compression,
            damping_relaxation: w.damping_relaxation,
            friction_slip: w.friction_slip,
            side_friction_stiffness: w.side_friction_stiffness,
            rotation: w.rotation,
            max_suspension_force: w.max_suspension_force,
            forward_impulse: w.forward_impulse,
            side_impulse: w.side_impulse,
            steering: w.steering,
            engine_force: w.engine_force,
            brake: w.brake,
            wheel_suspension_force: w.wheel_suspension_force,
            raycast_info: RayCastInfo::from_wheel(w),
        }
    }
}

// ---------------------------------------------------------------
// DynamicRayCastVehicleController
// ---------------------------------------------------------------
/// Raycast-based vehicle controller attached to a chassis body (3D only).
///
/// Each "wheel" is a raycast from a chassis-space attachment point
/// along ``direction_cs`` for ``suspension_rest_length`` units. The
/// controller resolves spring/damper forces from each ray hit and
/// applies them to the chassis :class:`RigidBody` to simulate a
/// driving vehicle without needing actual wheel colliders.
#[pyclass(name = "DynamicRayCastVehicleController", module = "rapier")]
pub struct DynamicRayCastVehicleController(pub rapier::control::DynamicRayCastVehicleController);

#[pymethods]
impl DynamicRayCastVehicleController {
    /// Construct a vehicle controller for the given chassis body.
    ///
    /// :param chassis_handle: :class:`RigidBodyHandle` of the chassis
    ///     dynamic body.
    #[new]
    fn new(chassis_handle: &RigidBodyHandle) -> Self {
        Self(rapier::control::DynamicRayCastVehicleController::new(
            chassis_handle.0,
        ))
    }

    /// Append a new wheel to this controller.
    ///
    /// :param chassis_connection_cs: Wheel attachment point in
    ///     chassis-local coordinates.
    /// :param direction_cs: Suspension axis (typically ``-up``).
    /// :param axle_cs: Wheel spin axis in chassis-local coords.
    /// :param suspension_rest_length: Spring rest length.
    /// :param radius: Wheel radius (for raycast clipping).
    /// :param tuning: :class:`WheelTuning` spring/friction params.
    /// :returns: The index of the new wheel.
    #[pyo3(signature = (chassis_connection_cs, direction_cs, axle_cs, suspension_rest_length, radius, tuning))]
    #[allow(clippy::too_many_arguments)]
    fn add_wheel(
        &mut self,
        chassis_connection_cs: PyVector,
        direction_cs: PyVector,
        axle_cs: PyVector,
        suspension_rest_length: Real,
        radius: Real,
        tuning: &WheelTuning,
    ) -> usize {
        let ccs: rapier::math::Vector = chassis_connection_cs.0.into();
        let dir: rapier::math::Vector = direction_cs.0.into();
        let axl: rapier::math::Vector = axle_cs.0.into();
        self.0
            .add_wheel(ccs, dir, axl, suspension_rest_length, radius, &tuning.0);
        self.0.wheels().len() - 1
    }

    /// Step the vehicle controller for one tick.
    ///
    /// Performs per-wheel raycasts through ``queries`` and updates
    /// the chassis :class:`RigidBody` with the resulting forces and
    /// torques. Call this before stepping the physics world.
    ///
    /// Requires a fresh :class:`QueryPipeline` — typically call
    /// ``world.update_query_pipeline()`` first.
    ///
    /// The chassis body is excluded from the suspension raycasts (their origins
    /// sit on its own collider), unless ``filter`` already excludes a body.
    ///
    /// :param dt: Time step in seconds.
    /// :param bodies: Rigid-body set (mutated).
    /// :param colliders: Collider set (mutated).
    /// :param queries: Up-to-date :class:`QueryPipeline`.
    /// :param filter: Optional query filter.
    #[pyo3(signature = (dt, bodies, colliders, queries, filter=None))]
    fn update_vehicle(
        &mut self,
        py: Python<'_>,
        dt: Real,
        bodies: Py<RigidBodySet>,
        colliders: Py<ColliderSet>,
        queries: &QueryPipeline,
        filter: Option<&QueryFilter>,
    ) -> PyResult<()> {
        let bp = queries.broad_phase.borrow(py);
        let np = queries.narrow_phase.borrow(py);
        let mut bodies_ref = bodies.borrow_mut(py);
        let mut colliders_ref = colliders.borrow_mut(py);
        let mut qf = filter.map(|f| f.as_rapier(None)).unwrap_or_default();
        qf.exclude_rigid_body = qf.exclude_rigid_body.or(Some(self.0.chassis));
        let qpmut = bp.0.as_query_pipeline_mut(
            np.0.query_dispatcher(),
            &mut bodies_ref.0,
            &mut colliders_ref.0,
            qf,
        );
        self.0.update_vehicle(dt, qpmut);
        Ok(())
    }

    /// Set the brake force on wheel ``idx``.
    ///
    /// :raises TypeError: if ``idx`` is out of range.
    fn set_brake(&mut self, idx: usize, brake: Real) -> PyResult<()> {
        let wheels = self.0.wheels_mut();
        if idx >= wheels.len() {
            return Err(PyTypeError::new_err(format!(
                "wheel index {} out of range (len={})",
                idx,
                wheels.len()
            )));
        }
        wheels[idx].brake = brake;
        Ok(())
    }

    /// Set the steering angle (radians) on wheel ``idx``.
    ///
    /// :raises TypeError: if ``idx`` is out of range.
    fn set_steering(&mut self, idx: usize, steering: Real) -> PyResult<()> {
        let wheels = self.0.wheels_mut();
        if idx >= wheels.len() {
            return Err(PyTypeError::new_err(format!(
                "wheel index {} out of range (len={})",
                idx,
                wheels.len()
            )));
        }
        wheels[idx].steering = steering;
        Ok(())
    }

    /// Set the engine drive force on wheel ``idx``.
    ///
    /// :raises TypeError: if ``idx`` is out of range.
    fn apply_engine_force(&mut self, idx: usize, force: Real) -> PyResult<()> {
        let wheels = self.0.wheels_mut();
        if idx >= wheels.len() {
            return Err(PyTypeError::new_err(format!(
                "wheel index {} out of range (len={})",
                idx,
                wheels.len()
            )));
        }
        wheels[idx].engine_force = force;
        Ok(())
    }

    /// Return a fresh list of :class:`Wheel` snapshots (detached copies).
    fn wheels(&self) -> Vec<Wheel> {
        self.0.wheels().iter().map(Wheel::from_rapier).collect()
    }

    /// Return a snapshot of the single wheel at ``idx``.
    ///
    /// :raises TypeError: if ``idx`` is out of range.
    fn wheel(&self, idx: usize) -> PyResult<Wheel> {
        let wheels = self.0.wheels();
        wheels.get(idx).map(Wheel::from_rapier).ok_or_else(|| {
            PyTypeError::new_err(format!(
                "wheel index {} out of range (len={})",
                idx,
                wheels.len()
            ))
        })
    }

    /// Current forward speed in km/h.
    fn current_speed_km_hour(&self) -> Real {
        // upstream's `current_vehicle_speed` is in m/s; convert to km/h.
        self.0.current_vehicle_speed * (3.6 as Real)
    }

    /// Current forward speed in m/s along the chassis forward axis.
    #[getter]
    fn current_vehicle_speed(&self) -> Real {
        self.0.current_vehicle_speed
    }

    /// :class:`RigidBodyHandle` of the chassis body.
    fn chassis(&self) -> RigidBodyHandle {
        RigidBodyHandle(self.0.chassis)
    }

    /// Index of the up-axis in chassis-local coords (``0=X``, ``1=Y``, ``2=Z``).
    #[getter]
    fn index_up_axis(&self) -> usize {
        self.0.index_up_axis
    }
    #[setter]
    /// Set the chassis-local up-axis index (``0=X``, ``1=Y``, ``2=Z``).
    fn set_index_up_axis(&mut self, v: usize) {
        self.0.index_up_axis = v;
    }
    /// Index of the forward-axis in chassis-local coords (``0=X``, ``1=Y``, ``2=Z``).
    #[getter]
    fn index_forward_axis(&self) -> usize {
        self.0.index_forward_axis
    }
    #[setter]
    /// Set the chassis-local forward-axis index (``0=X``, ``1=Y``, ``2=Z``).
    fn set_index_forward_axis(&mut self, v: usize) {
        self.0.index_forward_axis = v;
    }

    /// Return a debug string with the wheel count and forward speed.
    fn __repr__(&self) -> String {
        format!(
            "DynamicRayCastVehicleController(wheels={}, speed={} m/s)",
            self.0.wheels().len(),
            self.0.current_vehicle_speed
        )
    }
}

// ============================================================================
// Registration.
// ============================================================================

pub fn register_controllers(
    _py: crate::pyo3::Python<'_>,
    m: &crate::pyo3::Bound<'_, crate::pyo3::types::PyModule>,
) -> crate::pyo3::PyResult<()> {
    use crate::pyo3::prelude::*;
    m.add_class::<CharacterLength>()?;
    m.add_class::<CharacterAutostep>()?;
    m.add_class::<EffectiveCharacterMovement>()?;
    m.add_class::<CharacterCollision>()?;
    m.add_class::<KinematicCharacterController>()?;
    m.add_class::<AxesMask>()?;
    m.add_class::<PdErrors>()?;
    m.add_class::<PidCorrection>()?;
    m.add_class::<PdController>()?;
    m.add_class::<PidController>()?;
    m.add_class::<WheelTuning>()?;
    m.add_class::<RayCastInfo>()?;
    m.add_class::<Wheel>()?;
    m.add_class::<DynamicRayCastVehicleController>()?;
    Ok(())
}
