//! Allocation-free flight-control primitives.
//!
//! This crate contains no board layout, peripheral, RTIC, or allocator
//! inversion logic. A vehicle provides reviewed static geometry and
//! precomputed mixer rows; the high-rate path only performs bounded products,
//! saturation, and slew limiting.

#![no_std]

use kaptan_math::{Frd, Ned, UnitQuaternion, Vector3, sqrt};

/// Body-frame angular rate in radians per second.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct BodyRate(Vector3<Frd>);
impl BodyRate {
    /// Creates a body rate in rad/s.
    pub const fn radians_per_second(roll: f32, pitch: f32, yaw: f32) -> Self {
        Self(Vector3::new(roll, pitch, yaw))
    }
    /// Returns the FRD vector.
    pub const fn vector(self) -> Vector3<Frd> {
        self.0
    }
}

/// Desired NED force vector in newtons.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct NedForce(Vector3<Ned>);
impl NedForce {
    /// Creates a NED force in newtons.
    pub const fn newtons(north: f32, east: f32, down: f32) -> Self {
        Self(Vector3::new(north, east, down))
    }
    /// Returns the NED force vector.
    pub const fn vector(self) -> Vector3<Ned> {
        self.0
    }
}

/// Desired horizontal heading direction in NED, rather than an ambiguous yaw scalar.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct HeadingNed(Vector3<Ned>);
impl HeadingNed {
    /// Validates and normalizes a horizontal NED heading.
    pub fn new(north: f32, east: f32) -> Option<Self> {
        let vector = Vector3::new(north, east, 0.0).normalized()?;
        Some(Self(vector))
    }
    /// Returns the normalized NED heading vector.
    pub const fn vector(self) -> Vector3<Ned> {
        self.0
    }
}

/// Force-to-attitude conversion failure.
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub enum AttitudeError {
    /// A zero or non-finite force has no defined thrust direction.
    ZeroForce,
    /// Requested heading is parallel to the thrust axis.
    SingularHeading,
}

/// Computes a body-to-NED attitude whose body-down axis opposes multicopter
/// thrust force and whose body-forward axis follows the requested heading as
/// closely as geometry allows.
pub fn force_to_attitude(
    force: NedForce,
    heading: HeadingNed,
) -> Result<UnitQuaternion<Frd, Ned>, AttitudeError> {
    let body_down = force
        .vector()
        .scale(-1.0)
        .normalized()
        .ok_or(AttitudeError::ZeroForce)?;
    let body_right = body_down
        .cross(heading.vector())
        .normalized()
        .ok_or(AttitudeError::SingularHeading)?;
    let body_forward = body_right
        .cross(body_down)
        .normalized()
        .ok_or(AttitudeError::SingularHeading)?;
    UnitQuaternion::from_body_axes_in_ned(body_forward, body_right, body_down)
        .map_err(|_| AttitudeError::SingularHeading)
}

/// Bounded scalar PID configuration.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct PidConfig {
    /// Proportional gain.
    pub proportional: f32,
    /// Integral gain.
    pub integral: f32,
    /// Derivative gain applied to supplied error derivative.
    pub derivative: f32,
    /// Absolute output bound.
    pub output_limit: f32,
    /// Absolute stored integral contribution bound.
    pub integral_limit: f32,
}

/// Invalid PID input/configuration.
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub enum PidError {
    /// Static PID gains or limits were non-finite or contradictory.
    InvalidConfiguration,
    /// Time step was non-finite, non-positive, or exceeded the 100-ms integration bound.
    InvalidTime,
    /// Error input was non-finite.
    NonFinite,
}

/// Scalar PID with conditional-integration anti-windup.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct Pid {
    config: PidConfig,
    integral_contribution: f32,
}

impl Pid {
    /// Creates a reset PID. Invalid limits intentionally make all updates fail
    /// rather than being silently repaired in the flight path.
    pub const fn new(config: PidConfig) -> Self {
        Self {
            config,
            integral_contribution: 0.0,
        }
    }
    /// Clears only the retained integral contribution.
    pub fn reset(&mut self) {
        self.integral_contribution = 0.0;
    }
    /// Returns the retained integral output contribution.
    pub const fn integral_contribution(&self) -> f32 {
        self.integral_contribution
    }

    fn validate(&self, error: f32, error_rate: f32, dt_seconds: f32) -> Result<(), PidError> {
        if !self.config.proportional.is_finite()
            | !self.config.integral.is_finite()
            | !self.config.derivative.is_finite()
            | !self.config.output_limit.is_finite()
            | (self.config.output_limit <= 0.0)
            | !self.config.integral_limit.is_finite()
            | (self.config.integral_limit < 0.0)
        {
            return Err(PidError::InvalidConfiguration);
        }
        if !dt_seconds.is_finite()
            | (dt_seconds <= 0.0)
            | (dt_seconds > MAXIMUM_CONTROL_STEP_SECONDS)
        {
            return Err(PidError::InvalidTime);
        }
        if !error.is_finite() || !error_rate.is_finite() {
            return Err(PidError::NonFinite);
        }
        Ok(())
    }

    /// Updates with error and error derivative. Conditional integration keeps
    /// the integrator from increasing saturation in its current direction.
    pub fn update(
        &mut self,
        error: f32,
        error_rate: f32,
        dt_seconds: f32,
    ) -> Result<f32, PidError> {
        self.validate(error, error_rate, dt_seconds)?;
        let proportional_derivative =
            self.config.proportional * error + self.config.derivative * error_rate;
        if !proportional_derivative.is_finite() {
            return Err(PidError::NonFinite);
        }
        let integral_delta = self.config.integral * error * dt_seconds;
        if !integral_delta.is_finite() {
            return Err(PidError::NonFinite);
        }
        let tentative_integral = self.integral_contribution + integral_delta;
        if !tentative_integral.is_finite() {
            return Err(PidError::NonFinite);
        }
        let candidate = clamp(
            tentative_integral,
            -self.config.integral_limit,
            self.config.integral_limit,
        );
        // `tentative_integral` and both clamp bounds were validated finite, so
        // the selected operand is finite as well.
        debug_assert!(candidate.is_finite());
        let candidate_output = proportional_derivative + candidate;
        if !candidate_output.is_finite() {
            return Err(PidError::NonFinite);
        }
        let limited = clamp(
            candidate_output,
            -self.config.output_limit,
            self.config.output_limit,
        );
        if candidate_output == limited || candidate_output.signum() != error.signum() {
            self.integral_contribution = candidate;
        }
        let output = clamp(
            proportional_derivative + self.integral_contribution,
            -self.config.output_limit,
            self.config.output_limit,
        );
        output
            .is_finite()
            .then_some(output)
            .ok_or(PidError::NonFinite)
    }
}

/// Three independent body-axis PIDs for the rate loop.
pub struct RatePid {
    roll: Pid,
    pitch: Pid,
    yaw: Pid,
    previous_error: Option<Vector3<Frd>>,
}
impl RatePid {
    /// Creates one fixed PID per FRD angular-rate axis.
    pub const fn new(roll: PidConfig, pitch: PidConfig, yaw: PidConfig) -> Self {
        Self {
            roll: Pid::new(roll),
            pitch: Pid::new(pitch),
            yaw: Pid::new(yaw),
            previous_error: None,
        }
    }

    /// Clears the retained integral and derivative history on every axis.
    pub fn reset(&mut self) {
        self.roll.reset();
        self.pitch.reset();
        self.yaw.reset();
        self.previous_error = None;
    }

    /// Discards only retained integral authority after an actuator limit.
    ///
    /// Derivative history is intentionally retained: a mixer limit must not
    /// manufacture a rate-error impulse on the next controller tick.
    pub fn reset_integrators(&mut self) {
        self.roll.reset();
        self.pitch.reset();
        self.yaw.reset();
    }

    /// Produces bounded roll/pitch/yaw torque requests from rate error.
    pub fn update(
        &mut self,
        target: BodyRate,
        measured: BodyRate,
        dt_seconds: f32,
    ) -> Result<Vector3<Frd>, PidError> {
        let error = target.vector().sub(measured.vector());
        if !dt_seconds.is_finite()
            | (dt_seconds <= 0.0)
            | (dt_seconds > MAXIMUM_CONTROL_STEP_SECONDS)
        {
            return Err(PidError::InvalidTime);
        }
        let error_rate = self
            .previous_error
            .map(|previous| error.sub(previous).scale(1.0 / dt_seconds))
            .unwrap_or_else(Vector3::zero);
        // Validate all axes before altering any retained state, so a malformed
        // per-axis configuration cannot leave a partial controller update.
        self.roll.validate(error.x(), error_rate.x(), dt_seconds)?;
        self.pitch.validate(error.y(), error_rate.y(), dt_seconds)?;
        self.yaw.validate(error.z(), error_rate.z(), dt_seconds)?;
        let mut roll = self.roll;
        let mut pitch = self.pitch;
        let mut yaw = self.yaw;
        let output = Vector3::new(
            roll.update(error.x(), error_rate.x(), dt_seconds)?,
            pitch.update(error.y(), error_rate.y(), dt_seconds)?,
            yaw.update(error.z(), error_rate.z(), dt_seconds)?,
        );
        self.roll = roll;
        self.pitch = pitch;
        self.yaw = yaw;
        self.previous_error = Some(error);
        Ok(output)
    }
}

/// Position, velocity, and acceleration reference in the local NED frame.
///
/// The reference is generated continuously. A caller must not replace it with
/// a position step merely because an operator requested a distant goal.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct KinematicReference {
    /// Local NED position in metres.
    pub position: Vector3<Ned>,
    /// Local NED velocity in metres per second.
    pub velocity: Vector3<Ned>,
    /// Local NED acceleration in metres per second squared.
    pub acceleration: Vector3<Ned>,
}

/// Kinematic constraints for a local-position reference generator.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct KinematicLimits {
    /// Inclusive magnitude bound for horizontal velocity in m/s.
    pub maximum_horizontal_speed_mps: f32,
    /// Inclusive magnitude bound for vertical velocity in m/s.
    pub maximum_vertical_speed_mps: f32,
    /// Inclusive magnitude bound for horizontal acceleration in m/s².
    pub maximum_horizontal_acceleration_mps2: f32,
    /// Inclusive magnitude bound for vertical acceleration in m/s².
    pub maximum_vertical_acceleration_mps2: f32,
}

/// Rejection returned instead of silently repairing a goal or time interval.
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub enum TrajectoryError {
    /// A static speed/acceleration constraint is invalid.
    InvalidLimits,
    /// A goal coordinate is non-finite.
    NonFiniteGoal,
    /// The service interval was non-finite, non-positive, or exceeded the 100-ms integration bound.
    InvalidTime,
    /// A finite input produced a non-finite intermediate or candidate state.
    NumericalOverflow,
}

/// A reference generator must not turn a stalled task into a large implicit
/// integration step.  The scheduler must reset/reseed it after this bound.
const MAXIMUM_CONTROL_STEP_SECONDS: f32 = 0.1;

/// Allocation-free velocity-ramped trajectory for one local NED goal.
///
/// Horizontal motion is constrained as a vector, not independently per axis,
/// so a diagonal command cannot exceed the declared horizontal airspeed or
/// acceleration limit. Vertical motion has its own vehicle limit.
pub struct Trajectory {
    limits: KinematicLimits,
    reference: KinematicReference,
    goal: Vector3<Ned>,
}

impl Trajectory {
    /// Starts at a finite local position with zero velocity and acceleration.
    pub fn new(
        initial_position: Vector3<Ned>,
        limits: KinematicLimits,
    ) -> Result<Self, TrajectoryError> {
        if !valid_limits(limits) {
            return Err(TrajectoryError::InvalidLimits);
        }
        if !initial_position.is_finite() {
            return Err(TrajectoryError::NonFiniteGoal);
        }
        Ok(Self {
            limits,
            reference: KinematicReference {
                position: initial_position,
                velocity: Vector3::zero(),
                acceleration: Vector3::zero(),
            },
            goal: initial_position,
        })
    }

    /// Replaces the absolute local NED destination without resetting motion.
    pub fn set_goal(&mut self, goal: Vector3<Ned>) -> Result<(), TrajectoryError> {
        if !goal.is_finite() {
            return Err(TrajectoryError::NonFiniteGoal);
        }
        self.goal = goal;
        Ok(())
    }

    /// Returns the current absolute local NED destination.
    pub const fn goal(&self) -> Vector3<Ned> {
        self.goal
    }

    /// Returns the current continuous reference without advancing it.
    pub const fn reference(&self) -> KinematicReference {
        self.reference
    }

    /// Advances one bounded reference interval.
    pub fn update(&mut self, dt_seconds: f32) -> Result<KinematicReference, TrajectoryError> {
        if !dt_seconds.is_finite() || dt_seconds <= 0.0 || dt_seconds > MAXIMUM_CONTROL_STEP_SECONDS
        {
            return Err(TrajectoryError::InvalidTime);
        }

        let position_error = self.goal.sub(self.reference.position);
        if !position_error.is_finite() {
            return Err(TrajectoryError::NumericalOverflow);
        }
        let target_horizontal_velocity = braking_velocity(
            Vector3::new(position_error.x(), position_error.y(), 0.0),
            self.limits.maximum_horizontal_speed_mps,
            self.limits.maximum_horizontal_acceleration_mps2,
        )
        .ok_or(TrajectoryError::NumericalOverflow)?;
        let target_vertical_velocity = braking_velocity_axis(
            position_error.z(),
            self.limits.maximum_vertical_speed_mps,
            self.limits.maximum_vertical_acceleration_mps2,
        )
        .ok_or(TrajectoryError::NumericalOverflow)?;
        let current_horizontal_velocity = Vector3::new(
            self.reference.velocity.x(),
            self.reference.velocity.y(),
            0.0,
        );
        let horizontal_delta = limit_magnitude(
            target_horizontal_velocity.sub(current_horizontal_velocity),
            self.limits.maximum_horizontal_acceleration_mps2 * dt_seconds,
        )
        .ok_or(TrajectoryError::NumericalOverflow)?;
        let next_horizontal_velocity = current_horizontal_velocity.add(horizontal_delta);
        let vertical_delta = clamp(
            target_vertical_velocity - self.reference.velocity.z(),
            -self.limits.maximum_vertical_acceleration_mps2 * dt_seconds,
            self.limits.maximum_vertical_acceleration_mps2 * dt_seconds,
        );
        let mut next_velocity = Vector3::new(
            next_horizontal_velocity.x(),
            next_horizontal_velocity.y(),
            self.reference.velocity.z() + vertical_delta,
        );
        let mut next_position = self.reference.position.add(next_velocity.scale(dt_seconds));
        if !next_velocity.is_finite() | !next_position.is_finite() {
            return Err(TrajectoryError::NumericalOverflow);
        }

        // A discrete trajectory can cross the goal by less than one service
        // interval. Snap only when the current velocity can be stopped inside
        // this interval's declared acceleration envelope; otherwise let the
        // bounded braking law decelerate through the crossing normally.
        let current_horizontal_speed = current_horizontal_velocity.norm();
        let horizontal_error =
            Vector3::<Ned>::new(position_error.x(), position_error.y(), 0.0).norm();
        if (horizontal_error
            <= self.limits.maximum_horizontal_acceleration_mps2 * dt_seconds * dt_seconds)
            & (position_error.z().abs()
                <= self.limits.maximum_vertical_acceleration_mps2 * dt_seconds * dt_seconds)
            & (current_horizontal_speed
                <= self.limits.maximum_horizontal_acceleration_mps2 * dt_seconds)
            & (self.reference.velocity.z().abs()
                <= self.limits.maximum_vertical_acceleration_mps2 * dt_seconds)
        {
            next_position = self.goal;
            next_velocity = Vector3::zero();
        }

        let next_reference = KinematicReference {
            position: next_position,
            velocity: next_velocity,
            acceleration: next_velocity
                .sub(self.reference.velocity)
                .scale(1.0 / dt_seconds),
        };
        if !next_reference.acceleration.is_finite()
            | (Vector3::<Ned>::new(
                next_reference.velocity.x(),
                next_reference.velocity.y(),
                0.0,
            )
            .norm_squared()
                > self.limits.maximum_horizontal_speed_mps
                    * self.limits.maximum_horizontal_speed_mps
                    + 1.0e-4)
            | (next_reference.velocity.z().abs() > self.limits.maximum_vertical_speed_mps + 1.0e-4)
        {
            return Err(TrajectoryError::NumericalOverflow);
        }
        self.reference = next_reference;
        Ok(self.reference)
    }

    /// Returns whether this trajectory has reached its destination and stopped.
    pub fn is_complete(&self, position_tolerance_metres: f32, velocity_tolerance_mps: f32) -> bool {
        position_tolerance_metres.is_finite()
            & (position_tolerance_metres >= 0.0)
            & velocity_tolerance_mps.is_finite()
            & (velocity_tolerance_mps >= 0.0)
            & (self.goal.sub(self.reference.position).norm_squared()
                <= position_tolerance_metres * position_tolerance_metres)
            & (self.reference.velocity.norm_squared()
                <= velocity_tolerance_mps * velocity_tolerance_mps)
    }
}

fn valid_limits(limits: KinematicLimits) -> bool {
    limits.maximum_horizontal_speed_mps.is_finite()
        & (limits.maximum_horizontal_speed_mps > 0.0)
        & limits.maximum_vertical_speed_mps.is_finite()
        & (limits.maximum_vertical_speed_mps > 0.0)
        & limits.maximum_horizontal_acceleration_mps2.is_finite()
        & (limits.maximum_horizontal_acceleration_mps2 > 0.0)
        & limits.maximum_vertical_acceleration_mps2.is_finite()
        & (limits.maximum_vertical_acceleration_mps2 > 0.0)
}

fn braking_velocity(
    error: Vector3<Ned>,
    maximum_speed: f32,
    maximum_acceleration: f32,
) -> Option<Vector3<Ned>> {
    let distance = error.norm();
    if !distance.is_finite() || distance <= 1.0e-5 {
        return distance.is_finite().then_some(Vector3::zero());
    }
    let stopping_speed = sqrt(2.0 * maximum_acceleration * distance);
    if !stopping_speed.is_finite() {
        return None;
    }
    let velocity = error.scale(maximum_speed.min(stopping_speed) / distance);
    velocity.is_finite().then_some(velocity)
}

fn braking_velocity_axis(error: f32, maximum_speed: f32, maximum_acceleration: f32) -> Option<f32> {
    if error.abs() <= 1.0e-5 {
        error.is_finite().then_some(0.0)
    } else {
        let stopping_speed = sqrt(2.0 * maximum_acceleration * error.abs());
        if stopping_speed.is_finite() {
            Some(error.signum() * maximum_speed.min(stopping_speed))
        } else {
            None
        }
    }
}

fn limit_magnitude(vector: Vector3<Ned>, maximum: f32) -> Option<Vector3<Ned>> {
    if !vector.is_finite() | !maximum.is_finite() | (maximum < 0.0) {
        return None;
    }
    let scale = vector.x().abs().max(vector.y().abs()).max(vector.z().abs());
    if scale == 0.0 {
        return Some(vector);
    }
    let scaled = vector.scale(1.0 / scale);
    let magnitude = scale * sqrt(scaled.norm_squared());
    if !magnitude.is_finite() || magnitude > maximum {
        return vector
            .normalized()
            .map(|direction| direction.scale(maximum));
    }
    Some(vector)
}

/// Estimate consumed by the translational controller.
///
/// This is intentionally a value type. The controller does not accept a
/// simulator object or an aiding measurement; callers pass the estimator's
/// current state after its integrity policy has accepted it.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct TranslationalEstimate {
    /// Estimated local NED position in metres.
    pub position: Vector3<Ned>,
    /// Estimated local NED velocity in metres per second.
    pub velocity: Vector3<Ned>,
}

/// Static limits and gains for the position-to-acceleration loop.
///
/// TODO(configuration): this is supplied by a reviewed build-time vehicle
/// profile until persistent ground configuration exists. Gains and all limits
/// must be staged and validated as one controller revision, then activated
/// only while disarmed with controller state reset; field-by-field live tuning
/// is not a safe update protocol.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct TranslationalControlConfig {
    /// Position-error gain in s⁻².
    pub position_gain_per_second_squared: f32,
    /// Velocity-error gain in s⁻¹.
    pub velocity_gain_per_second: f32,
    /// Maximum horizontal commanded acceleration in m/s².
    pub maximum_horizontal_acceleration_mps2: f32,
    /// Maximum upward commanded acceleration in m/s² (negative NED down).
    pub maximum_upward_acceleration_mps2: f32,
    /// Maximum downward commanded acceleration in m/s² (positive NED down).
    pub maximum_downward_acceleration_mps2: f32,
}

/// Position-control configuration or input rejection.
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub enum TranslationalControlError {
    /// A gain or kinematic limit is invalid.
    InvalidConfiguration,
    /// A reference or estimator value is non-finite.
    NonFiniteState,
}

/// Stateless outer loop for a cascaded multicopter controller.
///
/// Integral action is deliberately absent here. Position aiding can disappear
/// or be rejected by the estimator; retaining an unobservable position
/// integral across that transition would be unsafe. Any future integral must
/// have an explicit estimator-validity reset and allocator-feedback policy.
pub struct TranslationalController {
    config: TranslationalControlConfig,
}

impl TranslationalController {
    /// Validates one vehicle's static outer-loop configuration.
    pub fn new(config: TranslationalControlConfig) -> Result<Self, TranslationalControlError> {
        if !config.position_gain_per_second_squared.is_finite()
            || config.position_gain_per_second_squared < 0.0
            || !config.velocity_gain_per_second.is_finite()
            || config.velocity_gain_per_second < 0.0
            || !config.maximum_horizontal_acceleration_mps2.is_finite()
            || config.maximum_horizontal_acceleration_mps2 <= 0.0
            || !config.maximum_upward_acceleration_mps2.is_finite()
            || config.maximum_upward_acceleration_mps2 <= 0.0
            || !config.maximum_downward_acceleration_mps2.is_finite()
            || config.maximum_downward_acceleration_mps2 <= 0.0
        {
            return Err(TranslationalControlError::InvalidConfiguration);
        }
        Ok(Self { config })
    }

    /// Produces a bounded local-NED acceleration request.
    pub fn update(
        &self,
        reference: KinematicReference,
        estimate: TranslationalEstimate,
    ) -> Result<Vector3<Ned>, TranslationalControlError> {
        if !reference.position.is_finite()
            || !reference.velocity.is_finite()
            || !reference.acceleration.is_finite()
            || !estimate.position.is_finite()
            || !estimate.velocity.is_finite()
        {
            return Err(TranslationalControlError::NonFiniteState);
        }
        let position_error = reference.position.sub(estimate.position);
        let velocity_error = reference.velocity.sub(estimate.velocity);
        let raw = reference
            .acceleration
            .add(position_error.scale(self.config.position_gain_per_second_squared))
            .add(velocity_error.scale(self.config.velocity_gain_per_second));
        if !raw.is_finite() {
            return Err(TranslationalControlError::NonFiniteState);
        }
        let horizontal = limit_magnitude(
            Vector3::new(raw.x(), raw.y(), 0.0),
            self.config.maximum_horizontal_acceleration_mps2,
        )
        .ok_or(TranslationalControlError::NonFiniteState)?;
        Ok(Vector3::new(
            horizontal.x(),
            horizontal.y(),
            clamp(
                raw.z(),
                -self.config.maximum_upward_acceleration_mps2,
                self.config.maximum_downward_acceleration_mps2,
            ),
        ))
    }
}

/// Physical conversion and attitude-envelope limits for one multirotor.
///
/// TODO(configuration): mass, thrust capability, and tilt limit are a single
/// vehicle-identification/calibration record. The future configuration path
/// must atomically bind this record to the matching mixer and reject output
/// until it is reviewed for the installed airframe.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct ThrustEnvelope {
    /// Vehicle mass in kilograms.
    pub mass_kg: f32,
    /// Maximum thrust available from all actuators in newtons.
    pub maximum_collective_thrust_newtons: f32,
    /// Maximum horizontal-to-vertical thrust ratio. For a 35° tilt limit this
    /// is `tan(35°)`, calculated and reviewed with the vehicle configuration.
    pub maximum_tilt_tangent: f32,
}

/// Thrust-envelope rejection.
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub enum ThrustEnvelopeError {
    /// A static physical parameter is invalid.
    InvalidConfiguration,
    /// A requested acceleration is non-finite or cannot be produced by
    /// positive multicopter thrust.
    InvalidAcceleration,
}

impl ThrustEnvelope {
    /// Validates a vehicle's reviewed force limits.
    pub fn validate(self) -> Result<(), ThrustEnvelopeError> {
        if !self.mass_kg.is_finite()
            || self.mass_kg <= 0.0
            || !self.maximum_collective_thrust_newtons.is_finite()
            || self.maximum_collective_thrust_newtons <= 0.0
            || !self.maximum_tilt_tangent.is_finite()
            || self.maximum_tilt_tangent <= 0.0
        {
            Err(ThrustEnvelopeError::InvalidConfiguration)
        } else {
            Ok(())
        }
    }

    /// Converts a bounded NED acceleration request to a feasible thrust force.
    ///
    /// The returned force is always upward or level in NED. Horizontal force
    /// is reduced first for tilt, then the full vector for collective thrust.
    /// A caller can expose the reduction as saturation and reset its rate-loop
    /// integrators before commanding the mixer.
    pub fn force_for_acceleration(
        self,
        acceleration: Vector3<Ned>,
        gravity_mps2: f32,
    ) -> Result<(NedForce, bool), ThrustEnvelopeError> {
        self.validate()?;
        if !acceleration.is_finite() || !gravity_mps2.is_finite() || gravity_mps2 <= 0.0 {
            return Err(ThrustEnvelopeError::InvalidAcceleration);
        }
        let vertical_force = self.mass_kg * (acceleration.z() - gravity_mps2);
        if !vertical_force.is_finite() || vertical_force >= 0.0 {
            return Err(ThrustEnvelopeError::InvalidAcceleration);
        }
        let requested_horizontal = Vector3::new(
            self.mass_kg * acceleration.x(),
            self.mass_kg * acceleration.y(),
            0.0,
        );
        if !vertical_force.is_finite() | !requested_horizontal.is_finite() {
            return Err(ThrustEnvelopeError::InvalidAcceleration);
        }
        let maximum_horizontal = -vertical_force * self.maximum_tilt_tangent;
        if !maximum_horizontal.is_finite() | (maximum_horizontal <= 0.0) {
            return Err(ThrustEnvelopeError::InvalidAcceleration);
        }
        let limited_horizontal = limit_magnitude(requested_horizontal, maximum_horizontal)
            .ok_or(ThrustEnvelopeError::InvalidAcceleration)?;
        let tilt_limited = limited_horizontal != requested_horizontal;
        let requested = Vector3::new(
            limited_horizontal.x(),
            limited_horizontal.y(),
            vertical_force,
        );
        let magnitude = requested.norm();
        let limited = limit_magnitude(requested, self.maximum_collective_thrust_newtons)
            .ok_or(ThrustEnvelopeError::InvalidAcceleration)?;
        // `limit_magnitude` returns `Some` only for a finite input vector and
        // finite bound, so a returned vector is finite by construction.
        if !magnitude.is_finite() {
            return Err(ThrustEnvelopeError::InvalidAcceleration);
        }
        Ok((
            NedForce::newtons(limited.x(), limited.y(), limited.z()),
            tilt_limited || magnitude > self.maximum_collective_thrust_newtons,
        ))
    }
}

/// Runtime command limits for one statically ordered actuator.
///
/// Physical positions, thrust axes, and reaction coefficients belong to the
/// vehicle's reviewed offline allocation calculation. Keeping a second copy
/// here would create an authority that the rate-path allocator cannot use.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct ActuatorLimits {
    /// Inclusive lower command bound.
    pub minimum: f32,
    /// Inclusive upper command bound.
    pub maximum: f32,
    /// Largest command movement per second.
    pub slew_per_second: f32,
}

/// One reviewed allocation output. Coefficients are ordered as roll torque,
/// pitch torque, yaw torque, then collective thrust.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct MixerOutput {
    /// Precomputed allocation row produced from the vehicle geometry offline.
    pub coefficients: [f32; 4],
    /// Physical command constraints for this same actuator.
    pub limits: ActuatorLimits,
}

/// Reviewed, precomputed allocation map in static actuator order.
///
/// Pairing each row with its command limits prevents independently indexed
/// geometry and matrix tables from drifting out of correspondence.
///
/// TODO(configuration): persist and activate this only as an entire,
/// vehicle-bound allocation profile, with actuator output inhibited and
/// allocation history reset. Individual mixer-row editing would break the
/// static-order and physical-review guarantees of this type.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct PrecomputedMixer<const ACTUATORS: usize> {
    outputs: [MixerOutput; ACTUATORS],
}
impl<const ACTUATORS: usize> PrecomputedMixer<ACTUATORS> {
    /// Creates a reviewed static allocation map.
    pub const fn new(outputs: [MixerOutput; ACTUATORS]) -> Self {
        Self { outputs }
    }
}

/// Requested body wrench in vehicle-specific normalized allocation units.
/// Units remain explicit in the vehicle's reviewed mixer calibration; generic
/// allocation never pretends torque and thrust share a physical unit.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct BodyWrench {
    /// Roll torque request.
    pub roll: f32,
    /// Pitch torque request.
    pub pitch: f32,
    /// Yaw torque request.
    pub yaw: f32,
    /// Collective thrust request.
    pub collective: f32,
}

/// Allocation construction failure.
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub enum AllocatorError {
    /// One static allocation output has non-finite or contradictory limits.
    InvalidActuator,
    /// Mixer contains non-finite values.
    InvalidMixer,
    /// Time step was non-finite, non-positive, or exceeded the 100-ms integration bound.
    InvalidTime,
    /// Requested wrench contains a non-finite component.
    InvalidWrench,
    /// The requested actuator index is outside the static allocation map.
    UnknownActuator,
}

/// Allocation output in the static actuator order.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct AllocationResult<const ACTUATORS: usize> {
    /// Command for each statically ordered actuator.
    pub commands: [f32; ACTUATORS],
    /// At least one command was limited by range, reversal, slew, or failure.
    pub saturated: bool,
}

/// Fixed-map actuator allocator with no dynamic inversion.
pub struct Allocator<const ACTUATORS: usize> {
    mixer: PrecomputedMixer<ACTUATORS>,
    previous: [f32; ACTUATORS],
    failed: [bool; ACTUATORS],
}

impl<const ACTUATORS: usize> Allocator<ACTUATORS> {
    /// Validates the reviewed static allocation map.
    pub fn new(mixer: PrecomputedMixer<ACTUATORS>) -> Result<Self, AllocatorError> {
        validate_mixer_outputs(&mixer.outputs)?;
        let mut previous = [0.0; ACTUATORS];
        for (previous, output) in previous.iter_mut().zip(mixer.outputs) {
            *previous = output.limits.minimum;
        }
        Ok(Self {
            mixer,
            previous,
            failed: [false; ACTUATORS],
        })
    }

    /// Marks one actuator failed. A failed actuator is forced to its reviewed
    /// minimum command on the next allocation; it cannot silently disappear
    /// from the mix or retain thrust after a declared fault.
    pub fn set_failed(&mut self, index: usize, failed: bool) -> Result<(), AllocatorError> {
        if index >= ACTUATORS {
            return Err(AllocatorError::UnknownActuator);
        }
        self.failed[index] = failed;
        Ok(())
    }

    /// Resets command history to the reviewed minimum command without clearing
    /// any actuator-failure latch.  Call on disarm, failsafe, mode changes, or
    /// estimator reset before outputs can be authorized again.
    pub fn reset_commands(&mut self) {
        for (previous, output) in self.previous.iter_mut().zip(self.mixer.outputs) {
            *previous = output.limits.minimum;
        }
    }

    /// Performs fixed-cost mix, limits, and slew control in static motor order.
    pub fn allocate(
        &mut self,
        wrench: BodyWrench,
        dt_seconds: f32,
    ) -> Result<AllocationResult<ACTUATORS>, AllocatorError> {
        if !valid_control_step(dt_seconds) {
            return Err(AllocatorError::InvalidTime);
        }
        let requested = [wrench.roll, wrench.pitch, wrench.yaw, wrench.collective];
        if !wrench_components_are_finite(wrench) {
            return Err(AllocatorError::InvalidWrench);
        }
        let mut commands = self.previous;
        let mut saturated = false;
        for (index, output) in commands.iter_mut().enumerate() {
            let mut command = 0.0;
            let output_config = self.mixer.outputs[index];
            for (coefficient, request) in output_config.coefficients.iter().zip(requested) {
                command += coefficient * request;
            }
            if !command.is_finite() {
                return Err(AllocatorError::InvalidWrench);
            }
            if self.failed[index] {
                command = output_config.limits.minimum;
                saturated = true;
            } else {
                let bounded = clamp(
                    command,
                    output_config.limits.minimum,
                    output_config.limits.maximum,
                );
                saturated |= bounded != command;
                command = bounded;
                let maximum_delta = output_config.limits.slew_per_second * dt_seconds;
                // Both factors are finite and bounded: validated control time
                // is at most 0.1 s, so a finite reviewed slew limit cannot
                // overflow this product.
                debug_assert!(maximum_delta.is_finite());
                let slewed = clamp(
                    command,
                    self.previous[index] - maximum_delta,
                    self.previous[index] + maximum_delta,
                );
                saturated |= slewed != command;
                command = slewed;
            }
            *output = command;
        }
        self.previous = commands;
        Ok(AllocationResult {
            commands,
            saturated,
        })
    }
}

fn validate_mixer_outputs(outputs: &[MixerOutput]) -> Result<(), AllocatorError> {
    if outputs.is_empty() {
        return Err(AllocatorError::InvalidMixer);
    }
    for output in outputs {
        let limits = output.limits;
        if !limits.minimum.is_finite()
            | !limits.maximum.is_finite()
            | (limits.minimum > limits.maximum)
            | !limits.slew_per_second.is_finite()
            | (limits.slew_per_second < 0.0)
        {
            return Err(AllocatorError::InvalidActuator);
        }
        if !mixer_coefficients_are_finite(output.coefficients) {
            return Err(AllocatorError::InvalidMixer);
        }
    }
    Ok(())
}

fn mixer_coefficients_are_finite(coefficients: [f32; 4]) -> bool {
    coefficients[0].is_finite()
        & coefficients[1].is_finite()
        & coefficients[2].is_finite()
        & coefficients[3].is_finite()
}

fn valid_control_step(dt_seconds: f32) -> bool {
    dt_seconds.is_finite() & (dt_seconds > 0.0) & (dt_seconds <= MAXIMUM_CONTROL_STEP_SECONDS)
}

fn wrench_components_are_finite(wrench: BodyWrench) -> bool {
    wrench.roll.is_finite()
        & wrench.pitch.is_finite()
        & wrench.yaw.is_finite()
        & wrench.collective.is_finite()
}

const fn clamp(value: f32, low: f32, high: f32) -> f32 {
    if value < low {
        low
    } else if value > high {
        high
    } else {
        value
    }
}

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

    fn trajectory_limits() -> KinematicLimits {
        KinematicLimits {
            maximum_horizontal_speed_mps: 4.0,
            maximum_vertical_speed_mps: 2.0,
            maximum_horizontal_acceleration_mps2: 3.0,
            maximum_vertical_acceleration_mps2: 1.5,
        }
    }

    #[test]
    fn distant_goal_stays_bounded() {
        let limits = trajectory_limits();
        let mut trajectory = Trajectory::new(Vector3::zero(), limits).unwrap();
        trajectory
            .set_goal(Vector3::new(150.0, 200.0, -15.0))
            .unwrap();

        for _ in 0..10_000 {
            let reference = trajectory.update(0.01).unwrap();
            let horizontal_speed =
                Vector3::<Ned>::new(reference.velocity.x(), reference.velocity.y(), 0.0).norm();
            let horizontal_acceleration =
                Vector3::<Ned>::new(reference.acceleration.x(), reference.acceleration.y(), 0.0)
                    .norm();
            assert!(horizontal_speed <= limits.maximum_horizontal_speed_mps + 1.0e-4);
            assert!(
                horizontal_acceleration <= limits.maximum_horizontal_acceleration_mps2 + 1.0e-3
            );
            assert!(reference.velocity.z().abs() <= limits.maximum_vertical_speed_mps + 1.0e-4);
            assert!(
                reference.acceleration.z().abs()
                    <= limits.maximum_vertical_acceleration_mps2 + 1.0e-3
            );
            if trajectory.is_complete(1.0e-4, 1.0e-4) {
                break;
            }
        }

        assert!(trajectory.is_complete(1.0e-3, 1.0e-3));
        let final_error = trajectory
            .reference()
            .position
            .sub(Vector3::new(150.0, 200.0, -15.0));
        assert!(final_error.x().abs() < 1.0e-3);
        assert!(final_error.y().abs() < 1.0e-3);
        assert!(final_error.z().abs() < 1.0e-3);
    }

    #[test]
    fn trajectory_validation() {
        let mut bad_limits = trajectory_limits();
        bad_limits.maximum_horizontal_speed_mps = 0.0;
        assert!(matches!(
            Trajectory::new(Vector3::zero(), bad_limits),
            Err(TrajectoryError::InvalidLimits)
        ));

        let mut trajectory = Trajectory::new(Vector3::zero(), trajectory_limits()).unwrap();
        assert_eq!(
            trajectory.set_goal(Vector3::new(f32::NAN, 0.0, 0.0)),
            Err(TrajectoryError::NonFiniteGoal)
        );
        assert_eq!(trajectory.update(0.0), Err(TrajectoryError::InvalidTime));
        assert_eq!(trajectory.update(0.2), Err(TrajectoryError::InvalidTime));

        trajectory
            .set_goal(Vector3::new(f32::MAX, 0.0, 0.0))
            .unwrap();
        let before = trajectory.reference();
        assert_eq!(
            trajectory.update(0.01),
            Err(TrajectoryError::NumericalOverflow)
        );
        assert_eq!(trajectory.reference(), before);
    }

    #[test]
    fn outer_loop_respects_limits() {
        let controller = TranslationalController::new(TranslationalControlConfig {
            position_gain_per_second_squared: 1.0,
            velocity_gain_per_second: 1.0,
            maximum_horizontal_acceleration_mps2: 4.0,
            maximum_upward_acceleration_mps2: 3.0,
            maximum_downward_acceleration_mps2: 2.0,
        })
        .unwrap();
        let acceleration = controller
            .update(
                KinematicReference {
                    position: Vector3::new(100.0, 100.0, -10.0),
                    velocity: Vector3::zero(),
                    acceleration: Vector3::zero(),
                },
                TranslationalEstimate {
                    position: Vector3::zero(),
                    velocity: Vector3::zero(),
                },
            )
            .unwrap();
        assert!(Vector3::<Ned>::new(acceleration.x(), acceleration.y(), 0.0).norm() <= 4.0001);
        assert!((acceleration.z() + 3.0).abs() < 1.0e-6);

        let envelope = ThrustEnvelope {
            mass_kg: 1.2,
            maximum_collective_thrust_newtons: 18.0,
            maximum_tilt_tangent: 0.1,
        };
        let (force, saturated) = envelope
            .force_for_acceleration(acceleration, 9.806_65)
            .unwrap();
        let vector = force.vector();
        let horizontal = Vector3::<Ned>::new(vector.x(), vector.y(), 0.0).norm();
        assert!(horizontal <= -vector.z() * 0.1001);
        assert!(vector.norm() <= 18.0001);
        assert!(saturated);
    }

    #[test]
    fn feed_forward_stays_finite() {
        let controller = TranslationalController::new(TranslationalControlConfig {
            position_gain_per_second_squared: 0.0,
            velocity_gain_per_second: 0.0,
            maximum_horizontal_acceleration_mps2: 4.0,
            maximum_upward_acceleration_mps2: 3.0,
            maximum_downward_acceleration_mps2: 2.0,
        })
        .unwrap();
        let acceleration = controller
            .update(
                KinematicReference {
                    position: Vector3::zero(),
                    velocity: Vector3::zero(),
                    acceleration: Vector3::new(f32::MAX, -f32::MAX, f32::MAX),
                },
                TranslationalEstimate {
                    position: Vector3::zero(),
                    velocity: Vector3::zero(),
                },
            )
            .unwrap();
        assert!(acceleration.is_finite());
        assert!(Vector3::<Ned>::new(acceleration.x(), acceleration.y(), 0.0).norm() <= 4.0001);
        assert_eq!(acceleration.z(), 2.0);

        let envelope = ThrustEnvelope {
            mass_kg: 1.0,
            maximum_collective_thrust_newtons: 20.0,
            maximum_tilt_tangent: 0.7,
        };
        let (force, _) = envelope
            .force_for_acceleration(Vector3::new(f32::MAX, 0.0, 0.0), 9.806_65)
            .unwrap();
        assert!(force.vector().is_finite());
        assert!(force.vector().norm() <= 20.0001);
    }

    #[test]
    fn pid_anti_windup() {
        let mut pid = Pid::new(PidConfig {
            proportional: 2.0,
            integral: 3.0,
            derivative: 0.0,
            output_limit: 1.0,
            integral_limit: 0.5,
        });
        for _ in 0..100 {
            assert_eq!(pid.update(1.0, 0.0, 0.01).unwrap(), 1.0);
        }
        assert!(pid.integral_contribution() <= 0.5);
        assert!(pid.update(-1.0, 0.0, 0.01).unwrap() < 0.0);
    }

    #[test]
    fn pid_overflow_preserves_state() {
        let mut pid = Pid::new(PidConfig {
            proportional: f32::MAX,
            integral: 1.0,
            derivative: 0.0,
            output_limit: 1.0,
            integral_limit: 0.5,
        });
        assert_eq!(pid.update(2.0, 0.0, 0.01), Err(PidError::NonFinite));
        assert_eq!(pid.integral_contribution(), 0.0);
    }

    #[test]
    fn rate_pid_uses_history() {
        let config = PidConfig {
            proportional: 0.0,
            integral: 0.0,
            derivative: 1.0,
            output_limit: 20.0,
            integral_limit: 0.0,
        };
        let mut pid = RatePid::new(config, config, config);
        assert_eq!(
            pid.update(
                BodyRate::radians_per_second(0.0, 0.0, 0.0),
                BodyRate::radians_per_second(0.0, 0.0, 0.0),
                0.1,
            )
            .unwrap()
            .x(),
            0.0
        );
        assert_eq!(
            pid.update(
                BodyRate::radians_per_second(1.0, 0.0, 0.0),
                BodyRate::radians_per_second(0.0, 0.0, 0.0),
                0.1,
            )
            .unwrap()
            .x(),
            10.0
        );
    }

    #[test]
    fn deadline_rejection_preserves_state() {
        let config = PidConfig {
            proportional: 0.0,
            integral: 1.0,
            derivative: 0.0,
            output_limit: 2.0,
            integral_limit: 1.0,
        };
        let mut pid = Pid::new(config);
        pid.update(1.0, 0.0, 0.1).unwrap();
        let pid_before = pid.integral_contribution();
        assert_eq!(pid.update(1.0, 0.0, 0.101), Err(PidError::InvalidTime));
        assert_eq!(pid.integral_contribution(), pid_before);

        let mut rate = RatePid::new(config, config, config);
        rate.update(
            BodyRate::radians_per_second(1.0, -1.0, 0.5),
            BodyRate::radians_per_second(0.0, 0.0, 0.0),
            0.1,
        )
        .unwrap();
        let rate_before = (
            rate.roll.integral_contribution(),
            rate.pitch.integral_contribution(),
            rate.yaw.integral_contribution(),
            rate.previous_error,
        );
        assert_eq!(
            rate.update(
                BodyRate::radians_per_second(0.0, 0.0, 0.0),
                BodyRate::radians_per_second(0.0, 0.0, 0.0),
                0.101,
            ),
            Err(PidError::InvalidTime)
        );
        assert_eq!(
            (
                rate.roll.integral_contribution(),
                rate.pitch.integral_contribution(),
                rate.yaw.integral_contribution(),
                rate.previous_error,
            ),
            rate_before
        );

        let mut trajectory = Trajectory::new(Vector3::zero(), trajectory_limits()).unwrap();
        trajectory.set_goal(Vector3::new(1.0, 0.0, 0.0)).unwrap();
        trajectory.update(0.1).unwrap();
        let trajectory_before = trajectory.reference();
        assert_eq!(trajectory.update(0.101), Err(TrajectoryError::InvalidTime));
        assert_eq!(trajectory.reference(), trajectory_before);

        let mut allocator =
            Allocator::new(PrecomputedMixer::new([output([0.0, 0.0, 0.0, 1.0])])).unwrap();
        allocator
            .allocate(
                BodyWrench {
                    roll: 0.0,
                    pitch: 0.0,
                    yaw: 0.0,
                    collective: 0.5,
                },
                0.1,
            )
            .unwrap();
        let allocator_before = allocator.previous;
        assert_eq!(
            allocator.allocate(
                BodyWrench {
                    roll: 0.0,
                    pitch: 0.0,
                    yaw: 0.0,
                    collective: 1.0,
                },
                0.101,
            ),
            Err(AllocatorError::InvalidTime)
        );
        assert_eq!(allocator.previous, allocator_before);
    }

    #[test]
    fn rate_pid_failure_is_atomic() {
        let finite = PidConfig {
            proportional: 0.0,
            integral: 1.0,
            derivative: 0.0,
            output_limit: 2.0,
            integral_limit: 1.0,
        };
        let invalid_yaw = PidConfig {
            proportional: f32::MAX,
            ..finite
        };
        let mut rate = RatePid::new(finite, finite, invalid_yaw);
        assert_eq!(
            rate.update(
                BodyRate::radians_per_second(1.0, 1.0, 2.0),
                BodyRate::radians_per_second(0.0, 0.0, 0.0),
                0.1,
            ),
            Err(PidError::NonFinite)
        );
        assert_eq!(rate.roll.integral_contribution(), 0.0);
        assert_eq!(rate.pitch.integral_contribution(), 0.0);
        assert_eq!(rate.yaw.integral_contribution(), 0.0);
        assert!(rate.previous_error.is_none());
    }

    #[test]
    fn level_heading() {
        let attitude = force_to_attitude(
            NedForce::newtons(0.0, 0.0, -10.0),
            HeadingNed::new(1.0, 0.0).unwrap(),
        )
        .unwrap();
        let forward = attitude.body_to_ned(Vector3::<Frd>::new(1.0, 0.0, 0.0));
        let down = attitude.body_to_ned(Vector3::<Frd>::new(0.0, 0.0, 1.0));
        assert!(forward.x() > 0.999);
        assert!(down.z() > 0.999);
    }

    fn output(coefficients: [f32; 4]) -> MixerOutput {
        MixerOutput {
            coefficients,
            limits: ActuatorLimits {
                minimum: 0.0,
                maximum: 1.0,
                slew_per_second: 100.0,
            },
        }
    }

    #[test]
    fn allocator_order_and_saturation() {
        let mixer =
            PrecomputedMixer::new([output([0.0, 0.0, 0.0, 1.0]), output([1.0, 0.0, 0.0, 1.0])]);
        let mut allocator = Allocator::new(mixer).unwrap();
        let output = allocator
            .allocate(
                BodyWrench {
                    roll: 0.5,
                    pitch: 0.0,
                    yaw: 0.0,
                    collective: 0.8,
                },
                0.1,
            )
            .unwrap();
        assert_eq!(output.commands, [0.8, 1.0]);
        assert!(output.saturated);
        allocator.set_failed(1, true).unwrap();
        assert_eq!(
            allocator
                .allocate(
                    BodyWrench {
                        roll: 0.0,
                        pitch: 0.0,
                        yaw: 0.0,
                        collective: 0.8
                    },
                    0.1
                )
                .unwrap()
                .commands[1],
            0.0
        );
    }

    #[test]
    fn allocator_fault_latch() {
        let limits = ActuatorLimits {
            minimum: 0.0,
            maximum: 1.0,
            slew_per_second: 100.0,
        };
        let mixer = PrecomputedMixer::new([
            MixerOutput {
                coefficients: [f32::MAX, 0.0, 0.0, 0.0],
                limits,
            },
            MixerOutput {
                coefficients: [0.0, 0.0, 0.0, 1.0],
                limits,
            },
        ]);
        let mut allocator = Allocator::new(mixer).unwrap();
        assert_eq!(
            allocator.allocate(
                BodyWrench {
                    roll: f32::MAX,
                    pitch: 0.0,
                    yaw: 0.0,
                    collective: 0.0,
                },
                0.01,
            ),
            Err(AllocatorError::InvalidWrench)
        );
        allocator.set_failed(1, true).unwrap();
        allocator.reset_commands();
        let output = allocator
            .allocate(
                BodyWrench {
                    roll: 0.0,
                    pitch: 0.0,
                    yaw: 0.0,
                    collective: 0.5,
                },
                0.01,
            )
            .unwrap();
        assert_eq!(output.commands, [0.0, 0.0]);
        assert!(output.saturated);
    }

    #[test]
    fn replaced_goals_stay_bounded() {
        let limits = trajectory_limits();
        let mut trajectory = Trajectory::new(Vector3::zero(), limits).unwrap();
        let mut seed = 0x1234_5678_u32;
        for step in 0..2_000 {
            seed = seed.wrapping_mul(1_664_525).wrapping_add(1_013_904_223);
            if step % 17 == 0 {
                let north = (seed as i32 as f32) / i32::MAX as f32 * 500.0;
                seed = seed.wrapping_mul(1_664_525).wrapping_add(1_013_904_223);
                let east = (seed as i32 as f32) / i32::MAX as f32 * 500.0;
                seed = seed.wrapping_mul(1_664_525).wrapping_add(1_013_904_223);
                let down = (seed as i32 as f32) / i32::MAX as f32 * 30.0;
                trajectory
                    .set_goal(Vector3::new(north, east, down))
                    .unwrap();
            }
            let reference = trajectory.update(0.01).unwrap();
            let horizontal_speed =
                Vector3::<Ned>::new(reference.velocity.x(), reference.velocity.y(), 0.0).norm();
            let horizontal_acceleration =
                Vector3::<Ned>::new(reference.acceleration.x(), reference.acceleration.y(), 0.0)
                    .norm();
            assert!(reference.position.is_finite());
            assert!(horizontal_speed <= limits.maximum_horizontal_speed_mps + 1.0e-4);
            assert!(
                horizontal_acceleration <= limits.maximum_horizontal_acceleration_mps2 + 1.0e-3
            );
            assert!(reference.velocity.z().abs() <= limits.maximum_vertical_speed_mps + 1.0e-4);
            assert!(
                reference.acceleration.z().abs()
                    <= limits.maximum_vertical_acceleration_mps2 + 1.0e-3
            );
        }
    }

    #[test]
    fn pid_rejects_invalid_input() {
        let valid = PidConfig {
            proportional: 1.0,
            integral: 1.0,
            derivative: 1.0,
            output_limit: 2.0,
            integral_limit: 1.0,
        };
        let invalid = [
            PidConfig {
                proportional: f32::NAN,
                ..valid
            },
            PidConfig {
                integral: f32::NAN,
                ..valid
            },
            PidConfig {
                derivative: f32::NAN,
                ..valid
            },
            PidConfig {
                output_limit: 0.0,
                ..valid
            },
            PidConfig {
                output_limit: f32::NAN,
                ..valid
            },
            PidConfig {
                integral_limit: -1.0,
                ..valid
            },
            PidConfig {
                integral_limit: f32::NAN,
                ..valid
            },
        ];
        for config in invalid {
            assert_eq!(
                Pid::new(config).update(0.0, 0.0, 0.01),
                Err(PidError::InvalidConfiguration)
            );
        }
        let mut pid = Pid::new(valid);
        assert_eq!(pid.update(0.0, 0.0, 0.0), Err(PidError::InvalidTime));
        assert_eq!(pid.update(f32::NAN, 0.0, 0.01), Err(PidError::NonFinite));
        assert_eq!(pid.update(0.0, f32::NAN, 0.01), Err(PidError::NonFinite));
        pid.integral_contribution = f32::MAX;
        pid.config.integral = f32::MAX;
        assert_eq!(pid.update(1.0, 0.0, 0.1), Err(PidError::NonFinite));
        pid.reset();
        assert_eq!(pid.integral_contribution(), 0.0);
        let mut opposite_sign_recovery = Pid::new(PidConfig {
            proportional: 0.0,
            integral: 0.0,
            derivative: 0.0,
            output_limit: 0.5,
            integral_limit: 1.0,
        });
        opposite_sign_recovery.integral_contribution = 1.0;
        assert_eq!(opposite_sign_recovery.update(-1.0, 0.0, 0.1), Ok(0.5));

        let mut rate = RatePid::new(valid, valid, valid);
        assert_eq!(
            rate.update(
                BodyRate::radians_per_second(0.0, 0.0, 0.0),
                BodyRate::radians_per_second(0.0, 0.0, 0.0),
                f32::NAN,
            ),
            Err(PidError::InvalidTime)
        );
        rate.update(
            BodyRate::radians_per_second(1.0, 0.0, 0.0),
            BodyRate::radians_per_second(0.0, 0.0, 0.0),
            0.1,
        )
        .unwrap();
        rate.reset_integrators();
        assert_eq!(rate.roll.integral_contribution(), 0.0);
        rate.reset();
        assert!(rate.previous_error.is_none());
    }

    #[test]
    fn trajectory_edges() {
        assert!(matches!(
            Trajectory::new(Vector3::new(f32::NAN, 0.0, 0.0), trajectory_limits()),
            Err(TrajectoryError::NonFiniteGoal)
        ));
        let mut limits = trajectory_limits();
        limits.maximum_vertical_speed_mps = f32::NAN;
        assert!(matches!(
            Trajectory::new(Vector3::zero(), limits),
            Err(TrajectoryError::InvalidLimits)
        ));
        for limits in [
            KinematicLimits {
                maximum_horizontal_speed_mps: f32::NAN,
                ..trajectory_limits()
            },
            KinematicLimits {
                maximum_vertical_speed_mps: 0.0,
                ..trajectory_limits()
            },
            KinematicLimits {
                maximum_horizontal_acceleration_mps2: f32::NAN,
                ..trajectory_limits()
            },
            KinematicLimits {
                maximum_vertical_acceleration_mps2: 0.0,
                ..trajectory_limits()
            },
            KinematicLimits {
                maximum_vertical_acceleration_mps2: f32::NAN,
                ..trajectory_limits()
            },
        ] {
            assert!(matches!(
                Trajectory::new(Vector3::zero(), limits),
                Err(TrajectoryError::InvalidLimits)
            ));
        }

        let mut trajectory = Trajectory::new(Vector3::zero(), trajectory_limits()).unwrap();
        assert_eq!(trajectory.goal(), Vector3::zero());
        assert!(!trajectory.is_complete(f32::NAN, 0.0));
        assert!(!trajectory.is_complete(0.0, -1.0));
        assert!(!trajectory.is_complete(0.0, f32::NAN));
        assert!(trajectory.is_complete(0.0, 0.0));
        assert_eq!(trajectory.update(0.01).unwrap().position, Vector3::zero());

        assert_eq!(
            braking_velocity(Vector3::zero(), 1.0, 1.0),
            Some(Vector3::zero())
        );
        assert_eq!(
            braking_velocity(Vector3::new(f32::NAN, 0.0, 0.0), 1.0, 1.0),
            None
        );
        assert_eq!(braking_velocity_axis(0.0, 1.0, 1.0), Some(0.0));
        assert_eq!(braking_velocity_axis(f32::NAN, 1.0, 1.0), None);
        assert_eq!(limit_magnitude(Vector3::zero(), 1.0), Some(Vector3::zero()));
        assert_eq!(limit_magnitude(Vector3::new(f32::NAN, 0.0, 0.0), 1.0), None);
        assert_eq!(limit_magnitude(Vector3::new(1.0, 0.0, 0.0), -1.0), None);
    }

    #[test]
    fn outer_loop_validation() {
        let config = TranslationalControlConfig {
            position_gain_per_second_squared: 1.0,
            velocity_gain_per_second: 1.0,
            maximum_horizontal_acceleration_mps2: 4.0,
            maximum_upward_acceleration_mps2: 3.0,
            maximum_downward_acceleration_mps2: 2.0,
        };
        for invalid in [
            TranslationalControlConfig {
                position_gain_per_second_squared: -1.0,
                ..config
            },
            TranslationalControlConfig {
                position_gain_per_second_squared: f32::NAN,
                ..config
            },
            TranslationalControlConfig {
                velocity_gain_per_second: -1.0,
                ..config
            },
            TranslationalControlConfig {
                velocity_gain_per_second: f32::NAN,
                ..config
            },
            TranslationalControlConfig {
                maximum_horizontal_acceleration_mps2: 0.0,
                ..config
            },
            TranslationalControlConfig {
                maximum_horizontal_acceleration_mps2: f32::NAN,
                ..config
            },
            TranslationalControlConfig {
                maximum_upward_acceleration_mps2: 0.0,
                ..config
            },
            TranslationalControlConfig {
                maximum_upward_acceleration_mps2: f32::NAN,
                ..config
            },
            TranslationalControlConfig {
                maximum_downward_acceleration_mps2: 0.0,
                ..config
            },
            TranslationalControlConfig {
                maximum_downward_acceleration_mps2: f32::NAN,
                ..config
            },
        ] {
            assert!(matches!(
                TranslationalController::new(invalid),
                Err(TranslationalControlError::InvalidConfiguration)
            ));
        }
        let controller = TranslationalController::new(config).unwrap();
        let invalid_reference = KinematicReference {
            position: Vector3::new(f32::NAN, 0.0, 0.0),
            velocity: Vector3::zero(),
            acceleration: Vector3::zero(),
        };
        let estimate = TranslationalEstimate {
            position: Vector3::zero(),
            velocity: Vector3::zero(),
        };
        assert_eq!(
            controller.update(invalid_reference, estimate),
            Err(TranslationalControlError::NonFiniteState)
        );
        for reference in [
            KinematicReference {
                position: Vector3::zero(),
                velocity: Vector3::new(f32::NAN, 0.0, 0.0),
                acceleration: Vector3::zero(),
            },
            KinematicReference {
                position: Vector3::zero(),
                velocity: Vector3::zero(),
                acceleration: Vector3::new(f32::NAN, 0.0, 0.0),
            },
        ] {
            assert_eq!(
                controller.update(reference, estimate),
                Err(TranslationalControlError::NonFiniteState)
            );
        }
        for invalid_estimate in [
            TranslationalEstimate {
                position: Vector3::new(f32::NAN, 0.0, 0.0),
                velocity: Vector3::zero(),
            },
            TranslationalEstimate {
                position: Vector3::zero(),
                velocity: Vector3::new(f32::NAN, 0.0, 0.0),
            },
        ] {
            assert_eq!(
                controller.update(
                    KinematicReference {
                        position: Vector3::zero(),
                        velocity: Vector3::zero(),
                        acceleration: Vector3::zero()
                    },
                    invalid_estimate
                ),
                Err(TranslationalControlError::NonFiniteState)
            );
        }
        let overflowing = KinematicReference {
            position: Vector3::new(f32::MAX, 0.0, 0.0),
            velocity: Vector3::zero(),
            acceleration: Vector3::zero(),
        };
        let huge_gain = TranslationalController::new(TranslationalControlConfig {
            position_gain_per_second_squared: f32::MAX,
            ..config
        })
        .unwrap();
        assert_eq!(
            huge_gain.update(overflowing, estimate),
            Err(TranslationalControlError::NonFiniteState)
        );

        let envelope = ThrustEnvelope {
            mass_kg: 1.0,
            maximum_collective_thrust_newtons: 100.0,
            maximum_tilt_tangent: 1.0,
        };
        assert_eq!(
            ThrustEnvelope {
                mass_kg: 0.0,
                ..envelope
            }
            .validate(),
            Err(ThrustEnvelopeError::InvalidConfiguration)
        );
        for invalid in [
            ThrustEnvelope {
                mass_kg: f32::NAN,
                ..envelope
            },
            ThrustEnvelope {
                maximum_collective_thrust_newtons: 0.0,
                ..envelope
            },
            ThrustEnvelope {
                maximum_collective_thrust_newtons: f32::NAN,
                ..envelope
            },
            ThrustEnvelope {
                maximum_tilt_tangent: 0.0,
                ..envelope
            },
            ThrustEnvelope {
                maximum_tilt_tangent: f32::NAN,
                ..envelope
            },
        ] {
            assert_eq!(
                invalid.validate(),
                Err(ThrustEnvelopeError::InvalidConfiguration)
            );
        }
        assert_eq!(
            envelope.force_for_acceleration(Vector3::new(f32::NAN, 0.0, 0.0), 9.8),
            Err(ThrustEnvelopeError::InvalidAcceleration)
        );
        assert_eq!(
            envelope.force_for_acceleration(Vector3::zero(), 0.0),
            Err(ThrustEnvelopeError::InvalidAcceleration)
        );
        assert_eq!(
            envelope.force_for_acceleration(Vector3::zero(), f32::NAN),
            Err(ThrustEnvelopeError::InvalidAcceleration)
        );
        assert_eq!(
            envelope.force_for_acceleration(Vector3::new(0.0, 0.0, 10.0), 9.8),
            Err(ThrustEnvelopeError::InvalidAcceleration)
        );
        assert!(
            !envelope
                .force_for_acceleration(Vector3::new(1.0, 0.0, 0.0), 9.8)
                .unwrap()
                .1
        );
        assert!(
            envelope
                .force_for_acceleration(Vector3::new(20.0, 0.0, 0.0), 9.8)
                .unwrap()
                .1
        );
    }

    #[test]
    fn allocator_rejects_invalid_data() {
        assert!(matches!(
            Allocator::<0>::new(PrecomputedMixer::new([])),
            Err(AllocatorError::InvalidMixer)
        ));
        let mut invalid = output([0.0; 4]);
        invalid.limits.minimum = 2.0;
        invalid.limits.maximum = 1.0;
        assert!(matches!(
            Allocator::new(PrecomputedMixer::new([invalid])),
            Err(AllocatorError::InvalidActuator)
        ));
        for limits in [
            ActuatorLimits {
                minimum: f32::NAN,
                ..output([0.0; 4]).limits
            },
            ActuatorLimits {
                maximum: f32::NAN,
                ..output([0.0; 4]).limits
            },
            ActuatorLimits {
                slew_per_second: f32::NAN,
                ..output([0.0; 4]).limits
            },
            ActuatorLimits {
                slew_per_second: -1.0,
                ..output([0.0; 4]).limits
            },
        ] {
            assert!(matches!(
                Allocator::new(PrecomputedMixer::new([MixerOutput {
                    coefficients: [0.0; 4],
                    limits
                }])),
                Err(AllocatorError::InvalidActuator)
            ));
        }
        assert!(matches!(
            Allocator::new(PrecomputedMixer::new([output([f32::NAN, 0.0, 0.0, 0.0])])),
            Err(AllocatorError::InvalidMixer)
        ));

        let mut allocator =
            Allocator::new(PrecomputedMixer::new([output([0.0, 0.0, 0.0, 1.0])])).unwrap();
        assert_eq!(
            allocator.set_failed(1, true),
            Err(AllocatorError::UnknownActuator)
        );
        assert_eq!(
            allocator.allocate(
                BodyWrench {
                    roll: 0.0,
                    pitch: 0.0,
                    yaw: 0.0,
                    collective: 0.0
                },
                0.0
            ),
            Err(AllocatorError::InvalidTime)
        );
        assert_eq!(
            allocator.allocate(
                BodyWrench {
                    roll: f32::NAN,
                    pitch: 0.0,
                    yaw: 0.0,
                    collective: 0.0
                },
                0.1
            ),
            Err(AllocatorError::InvalidWrench)
        );
        for wrench in [
            BodyWrench {
                roll: 0.0,
                pitch: f32::NAN,
                yaw: 0.0,
                collective: 0.0,
            },
            BodyWrench {
                roll: 0.0,
                pitch: 0.0,
                yaw: f32::NAN,
                collective: 0.0,
            },
            BodyWrench {
                roll: 0.0,
                pitch: 0.0,
                yaw: 0.0,
                collective: f32::NAN,
            },
        ] {
            assert_eq!(
                allocator.allocate(wrench, 0.1),
                Err(AllocatorError::InvalidWrench)
            );
        }
        assert_eq!(
            allocator.allocate(
                BodyWrench {
                    roll: 0.0,
                    pitch: 0.0,
                    yaw: 0.0,
                    collective: 0.5
                },
                2.0
            ),
            Err(AllocatorError::InvalidTime)
        );
        allocator.mixer.outputs[0].limits.slew_per_second = 100.0;
        allocator
            .allocate(
                BodyWrench {
                    roll: 0.0,
                    pitch: 0.0,
                    yaw: 0.0,
                    collective: 0.5,
                },
                0.1,
            )
            .unwrap();
        allocator.set_failed(0, true).unwrap();
        assert_eq!(
            allocator
                .allocate(
                    BodyWrench {
                        roll: 0.0,
                        pitch: 0.0,
                        yaw: 0.0,
                        collective: 1.0
                    },
                    0.1
                )
                .unwrap()
                .commands,
            [0.0]
        );
    }

    #[test]
    fn numerical_guard_paths_reject_without_committing_state() {
        let mut pid = Pid::new(PidConfig {
            proportional: 0.0,
            integral: f32::MAX,
            derivative: 0.0,
            output_limit: 1.0,
            integral_limit: 1.0,
        });
        assert_eq!(pid.update(2.0, 0.0, 0.1), Err(PidError::NonFinite));
        assert_eq!(pid.integral_contribution(), 0.0);

        let mut trajectory = Trajectory::new(Vector3::zero(), trajectory_limits()).unwrap();
        trajectory.goal = Vector3::new(f32::MAX, 0.0, 0.0);
        trajectory.reference.position = Vector3::new(f32::MAX, 0.0, 0.0);
        trajectory.reference.velocity = Vector3::new(f32::MAX, 0.0, 0.0);
        assert_eq!(
            trajectory.update(0.1),
            Err(TrajectoryError::NumericalOverflow)
        );

        let mut bounded_but_inconsistent =
            Trajectory::new(Vector3::zero(), trajectory_limits()).unwrap();
        bounded_but_inconsistent.reference.velocity = Vector3::new(5.0, 0.0, 0.0);
        assert_eq!(
            bounded_but_inconsistent.update(0.01),
            Err(TrajectoryError::NumericalOverflow)
        );

        let envelope = ThrustEnvelope {
            mass_kg: 1.0,
            maximum_collective_thrust_newtons: f32::MAX,
            maximum_tilt_tangent: 1.0,
        };
        assert_eq!(
            envelope.force_for_acceleration(Vector3::new(f32::MAX, 0.0, 0.0), f32::MAX / 2.0),
            Err(ThrustEnvelopeError::InvalidAcceleration)
        );
    }

    #[test]
    fn numeric_faults_fail_closed() {
        let valid = PidConfig {
            proportional: 0.0,
            integral: 0.0,
            derivative: 0.0,
            output_limit: f32::MAX,
            integral_limit: f32::MAX,
        };
        let mut derivative_overflow = Pid::new(PidConfig {
            derivative: f32::MAX,
            ..valid
        });
        assert_eq!(
            derivative_overflow.update(0.0, 2.0, 0.1),
            Err(PidError::NonFinite)
        );
        let mut integral_nan = Pid::new(valid);
        integral_nan.integral_contribution = f32::NAN;
        assert_eq!(integral_nan.update(0.0, 0.0, 0.1), Err(PidError::NonFinite));
        let mut output_overflow = Pid::new(PidConfig {
            proportional: f32::MAX,
            ..valid
        });
        output_overflow.integral_contribution = f32::MAX;
        assert_eq!(
            output_overflow.update(1.0, 0.0, 0.1),
            Err(PidError::NonFinite)
        );

        let mut trajectory = Trajectory::new(Vector3::zero(), trajectory_limits()).unwrap();
        trajectory.goal = Vector3::new(f32::MAX, 0.0, 0.0);
        trajectory.reference.position = Vector3::new(-f32::MAX, 0.0, 0.0);
        assert_eq!(
            trajectory.update(0.01),
            Err(TrajectoryError::NumericalOverflow)
        );
        assert_eq!(
            braking_velocity(Vector3::new(1.0, 0.0, 0.0), f32::MAX, f32::MAX),
            None
        );

        let horizontal_overflow = ThrustEnvelope {
            mass_kg: 2.0,
            maximum_collective_thrust_newtons: 10.0,
            maximum_tilt_tangent: 1.0,
        };
        assert_eq!(
            horizontal_overflow.force_for_acceleration(Vector3::new(f32::MAX, 0.0, 0.0), 9.8),
            Err(ThrustEnvelopeError::InvalidAcceleration)
        );
        let tilt_overflow = ThrustEnvelope {
            mass_kg: 1.0,
            maximum_collective_thrust_newtons: 10.0,
            maximum_tilt_tangent: f32::MAX,
        };
        assert_eq!(
            tilt_overflow.force_for_acceleration(Vector3::zero(), 9.8),
            Err(ThrustEnvelopeError::InvalidAcceleration)
        );
        let vertical_overflow = ThrustEnvelope {
            mass_kg: f32::MAX,
            maximum_collective_thrust_newtons: 10.0,
            maximum_tilt_tangent: 1.0,
        };
        assert_eq!(
            vertical_overflow.force_for_acceleration(Vector3::zero(), 9.8),
            Err(ThrustEnvelopeError::InvalidAcceleration)
        );
    }

    #[test]
    fn boundary_numeric_guards_reject_each_late_stage_without_state_commit() {
        let mut trajectory = Trajectory::new(Vector3::zero(), trajectory_limits()).unwrap();
        let before = trajectory.reference();
        assert_eq!(
            trajectory.update(f32::NAN),
            Err(TrajectoryError::InvalidTime)
        );
        assert_eq!(trajectory.reference(), before);

        // Each force component is finite and passes the tilt limiter, but the
        // requested vector norm overflows. The collective limiter must reject
        // it rather than manufacturing a finite-looking thrust command.
        let collective_norm_overflow = ThrustEnvelope {
            mass_kg: 1.0,
            maximum_collective_thrust_newtons: f32::MAX,
            maximum_tilt_tangent: 1.0,
        };
        assert_eq!(
            collective_norm_overflow
                .force_for_acceleration(Vector3::new(f32::MAX, 0.0, -f32::MAX / 2.0), 1.0),
            Err(ThrustEnvelopeError::InvalidAcceleration)
        );

        let mut allocator =
            Allocator::new(PrecomputedMixer::new([output([1.0, 0.0, 0.0, 0.0])])).unwrap();
        let history = allocator.previous;
        assert_eq!(
            allocator.allocate(
                BodyWrench {
                    roll: 0.0,
                    pitch: 0.0,
                    yaw: 0.0,
                    collective: 0.0,
                },
                f32::NAN,
            ),
            Err(AllocatorError::InvalidTime)
        );
        assert_eq!(allocator.previous, history);

        // A finite request can still overflow one precomputed mixer product.
        // The allocator must reject atomically, preserving its previous output.
        allocator.mixer.outputs[0].coefficients[0] = f32::MAX;
        assert_eq!(
            allocator.allocate(
                BodyWrench {
                    roll: f32::MAX,
                    pitch: 0.0,
                    yaw: 0.0,
                    collective: 0.0,
                },
                0.01,
            ),
            Err(AllocatorError::InvalidWrench)
        );
        assert_eq!(allocator.previous, history);
    }
}