"""Trajectory reconstruction, distance limiting, and dynamics scoring helpers.

The functions in this module are deliberately independent from the camera
generator.  They provide small, testable building blocks that can be adopted by
candidate generation and ranking without silently applying a global low-pass
filter to authored camera motion.
"""

from __future__ import annotations

from dataclasses import asdict, dataclass
from typing import Mapping, Sequence

import numpy as np


TRAJECTORY_QUALITY_VERSION = "trajectory-quality-v2"
C2_RESAMPLING_POLICY_VERSION = "clamped-cubic-c2-endpoint-speed-guard-v1"
RADIAL_CLAMP_POLICY_VERSION = "quintic-bound-identity-c2-speed-adaptive-v3"
DYNAMICS_POLICY_VERSION = (
    "camera-motion-stabilized-screen-residual-cost-hard-gate-v3"
)

RAW_SUBJECT_SCREEN_INPUT = "raw_subject_uv"
STABILIZED_SCREEN_RESIDUAL_INPUT = "stabilized_residual_uv"
SCREEN_INPUT_KINDS = frozenset(
    {
        RAW_SUBJECT_SCREEN_INPUT,
        STABILIZED_SCREEN_RESIDUAL_INPUT,
    }
)


@dataclass(frozen=True)
class ResamplingGuardPolicy:
    """Safety limits used before accepting a cubic position reconstruction."""

    maximum_axis_overshoot_meters: float = 0.02
    maximum_path_length_ratio: float = 1.12
    maximum_path_length_delta_meters: float = 0.05
    maximum_speed_ratio: float = 1.20
    maximum_speed_delta_meters_per_second: float = 0.25
    maximum_acceleration_ratio: float = 1.35
    maximum_acceleration_delta_meters_per_second_squared: float = 8.0
    maximum_jerk_ratio: float = 1.50
    maximum_jerk_delta_meters_per_second_cubed: float = 350.0

    def validate(self) -> None:
        values = asdict(self)
        for name, value in values.items():
            if not np.isfinite(value) or value < 0.0:
                raise ValueError(f"{name} must be finite and non-negative")


@dataclass(frozen=True)
class CandidateDynamicsPolicy:
    """Transparent scales, free thresholds, and weights for soft ranking.

    The free thresholds represent authored-safe motion that should not change
    candidate ordering.  Only dynamics above a threshold contribute to the
    soft cost.  Raw subject screen positions are diagnostic-only because they
    contain performer motion; screen acceleration and jerk are scored only
    when the caller explicitly supplies a stabilized semantic-target residual.
    """

    translation_speed_p99_scale: float = 4.0
    translation_acceleration_p99_scale: float = 12.0
    translation_jerk_p99_scale: float = 300.0
    lateral_reversal_count_scale: float = 3.0
    screen_speed_p99_scale: float = 1.2
    screen_acceleration_p99_scale: float = 12.0
    screen_jerk_p99_scale: float = 300.0
    screen_horizontal_reversal_count_scale: float = 3.0
    translation_speed_p99_free_threshold: float = 2.0
    translation_acceleration_p99_free_threshold: float = 6.0
    translation_jerk_p99_free_threshold: float = 120.0
    lateral_reversal_count_free_threshold: float = 1.0
    screen_speed_p99_free_threshold: float = 0.0
    screen_acceleration_p99_free_threshold: float = 6.0
    screen_jerk_p99_free_threshold: float = 180.0
    screen_horizontal_reversal_count_free_threshold: float = 0.0
    translation_speed_weight: float = 0.10
    translation_acceleration_weight: float = 0.75
    translation_jerk_weight: float = 1.00
    lateral_reversal_weight: float = 0.60
    # Raw subject speed and reversal count mostly describe choreography, not a
    # camera-body jolt.  Keep both metrics in the audit payload but exclude
    # them from the default soft score even for a residual input.
    screen_speed_weight: float = 0.0
    screen_acceleration_weight: float = 0.90
    screen_jerk_weight: float = 1.20
    screen_horizontal_reversal_weight: float = 0.0
    maximum_component_ratio: float = 8.0
    maximum_acceptable_cost: float = 6.0

    def validate(self) -> None:
        values = asdict(self)
        for name, value in values.items():
            if not np.isfinite(value):
                raise ValueError(f"{name} must be finite")
            if name.endswith("_scale") and value <= 0.0:
                raise ValueError(f"{name} must be positive")
            if name.endswith("_weight") and value < 0.0:
                raise ValueError(f"{name} must be non-negative")
            if name.endswith("_free_threshold") and value < 0.0:
                raise ValueError(f"{name} must be non-negative")
        if self.maximum_component_ratio <= 0.0:
            raise ValueError("maximum_component_ratio must be positive")
        if self.maximum_acceptable_cost < 0.0:
            raise ValueError("maximum_acceptable_cost must be non-negative")


def _frame_array(
    values: np.ndarray | Sequence[Sequence[float]],
    *,
    components: int,
    name: str,
) -> np.ndarray:
    result = np.asarray(values, dtype=np.float64)
    if result.ndim != 2 or result.shape[1] != components:
        raise ValueError(f"{name} must have shape [frame, {components}]")
    if len(result) == 0:
        raise ValueError(f"{name} must contain at least one frame")
    if not np.all(np.isfinite(result)):
        raise ValueError(f"{name} must contain only finite values")
    return result


def _validate_sample_rate(sample_rate: float) -> float:
    value = float(sample_rate)
    if not np.isfinite(value) or value <= 0.0:
        raise ValueError("sample_rate must be finite and positive")
    return value


def _linear_resample(values: np.ndarray, output_length: int) -> np.ndarray:
    if len(values) == 1:
        return np.repeat(values, output_length, axis=0)
    source_time = np.linspace(0.0, 1.0, len(values), dtype=np.float64)
    output_time = np.linspace(0.0, 1.0, output_length, dtype=np.float64)
    columns = [
        np.interp(output_time, source_time, values[:, axis])
        for axis in range(values.shape[1])
    ]
    result = np.column_stack(columns)
    result[0] = values[0]
    result[-1] = values[-1]
    return result


def _solve_tridiagonal(
    lower: np.ndarray,
    diagonal: np.ndarray,
    upper: np.ndarray,
    right_hand_side: np.ndarray,
) -> np.ndarray:
    """Solve a tridiagonal system with one or more right-hand sides."""

    count = len(diagonal)
    diagonal_work = np.array(diagonal, dtype=np.float64, copy=True)
    rhs_work = np.array(right_hand_side, dtype=np.float64, copy=True)
    upper_work = np.array(upper, dtype=np.float64, copy=True)
    for index in range(1, count):
        multiplier = lower[index - 1] / diagonal_work[index - 1]
        diagonal_work[index] -= multiplier * upper_work[index - 1]
        rhs_work[index] -= multiplier * rhs_work[index - 1]

    result = np.empty_like(rhs_work)
    result[-1] = rhs_work[-1] / diagonal_work[-1]
    for index in range(count - 2, -1, -1):
        result[index] = (
            rhs_work[index] - upper_work[index] * result[index + 1]
        ) / diagonal_work[index]
    return result


def _clamped_cubic_second_derivatives(
    times: np.ndarray,
    values: np.ndarray,
    start_tangent: np.ndarray,
    end_tangent: np.ndarray,
) -> np.ndarray:
    count = len(times)
    intervals = np.diff(times)
    slopes = np.diff(values, axis=0) / intervals[:, None]
    diagonal = np.empty(count, dtype=np.float64)
    lower = np.empty(count - 1, dtype=np.float64)
    upper = np.empty(count - 1, dtype=np.float64)
    rhs = np.empty_like(values)

    diagonal[0] = 2.0 * intervals[0]
    upper[0] = intervals[0]
    rhs[0] = 6.0 * (slopes[0] - start_tangent)
    for index in range(1, count - 1):
        left_interval = intervals[index - 1]
        right_interval = intervals[index]
        lower[index - 1] = left_interval
        diagonal[index] = 2.0 * (left_interval + right_interval)
        upper[index] = right_interval
        rhs[index] = 6.0 * (slopes[index] - slopes[index - 1])
    lower[-1] = intervals[-1]
    diagonal[-1] = 2.0 * intervals[-1]
    rhs[-1] = 6.0 * (end_tangent - slopes[-1])
    return _solve_tridiagonal(lower, diagonal, upper, rhs)


def _evaluate_cubic_spline(
    times: np.ndarray,
    values: np.ndarray,
    second_derivatives: np.ndarray,
    output_times: np.ndarray,
) -> np.ndarray:
    interval_indices = np.searchsorted(times, output_times, side="right") - 1
    interval_indices = np.minimum(
        np.maximum(interval_indices, 0),
        len(times) - 2,
    )
    left_times = times[interval_indices]
    right_times = times[interval_indices + 1]
    interval_lengths = right_times - left_times
    left_weights = (right_times - output_times) / interval_lengths
    right_weights = (output_times - left_times) / interval_lengths
    left_values = values[interval_indices]
    right_values = values[interval_indices + 1]
    left_second = second_derivatives[interval_indices]
    right_second = second_derivatives[interval_indices + 1]
    curvature = (
        ((left_weights**3 - left_weights) * interval_lengths**2)[:, None]
        * left_second
        + ((right_weights**3 - right_weights) * interval_lengths**2)[:, None]
        * right_second
    ) / 6.0
    return (
        left_weights[:, None] * left_values
        + right_weights[:, None] * right_values
        + curvature
    )


def _path_length(values: np.ndarray) -> float:
    if len(values) < 2:
        return 0.0
    return float(np.sum(np.linalg.norm(np.diff(values, axis=0), axis=1)))


def _series_statistics(values: np.ndarray) -> dict[str, float]:
    data = np.asarray(values, dtype=np.float64).reshape(-1)
    if len(data) == 0:
        return {
            "mean": 0.0,
            "p50": 0.0,
            "p90": 0.0,
            "p99": 0.0,
            "maximum": 0.0,
        }
    return {
        "mean": float(np.mean(data)),
        "p50": float(np.quantile(data, 0.50)),
        "p90": float(np.quantile(data, 0.90)),
        "p99": float(np.quantile(data, 0.99)),
        "maximum": float(np.max(data)),
    }


def _vector_dynamics(values: np.ndarray, sample_rate: float) -> dict[str, np.ndarray]:
    velocity = np.diff(values, axis=0) * sample_rate
    acceleration = np.diff(velocity, axis=0) * sample_rate
    jerk = np.diff(acceleration, axis=0) * sample_rate
    return {
        "velocity": velocity,
        "acceleration": acceleration,
        "jerk": jerk,
    }


def _maximum_vector_norm(values: np.ndarray) -> float:
    if len(values) == 0:
        return 0.0
    return float(np.max(np.linalg.norm(values, axis=1)))


def _regressed(
    candidate_value: float,
    baseline_value: float,
    ratio: float,
    additive_delta: float,
) -> bool:
    return candidate_value > baseline_value * ratio + additive_delta + 1e-12


def resample_position_path_c2(
    positions_meters: np.ndarray | Sequence[Sequence[float]],
    output_length: int,
    *,
    sample_rate: float = 60.0,
    guard_policy: ResamplingGuardPolicy | None = None,
) -> tuple[np.ndarray, dict[str, object]]:
    """Resample a camera path using a guarded clamped C2 cubic spline.

    The clamped spline preserves both endpoint positions and the endpoint
    tangents estimated from the source path.  It is compared against a linear
    baseline at the same output cadence.  Any non-finite result, excessive
    overshoot, path inflation, or dynamics regression returns the safe linear
    baseline instead of smoothing the entire trajectory.
    """

    positions = _frame_array(
        positions_meters,
        components=3,
        name="positions_meters",
    )
    sample_rate_value = _validate_sample_rate(sample_rate)
    if not isinstance(output_length, (int, np.integer)) or output_length <= 0:
        raise ValueError("output_length must be a positive integer")
    if len(positions) > 1 and output_length < 2:
        raise ValueError(
            "output_length must be at least two when the source has two endpoints"
        )

    policy = guard_policy or ResamplingGuardPolicy()
    policy.validate()
    common_metadata: dict[str, object] = {
        "version": TRAJECTORY_QUALITY_VERSION,
        "policyVersion": C2_RESAMPLING_POLICY_VERSION,
        "sourceFrameCount": int(len(positions)),
        "outputFrameCount": int(output_length),
        "sampleRate": sample_rate_value,
        "guardPolicy": asdict(policy),
    }

    if len(positions) == 1:
        result = np.repeat(positions, output_length, axis=0)
        return result.astype(np.float32), {
            **common_metadata,
            "method": "constant",
            "fallbackUsed": False,
            "fallbackReasons": [],
        }
    if output_length == len(positions):
        return positions.astype(np.float32, copy=True), {
            **common_metadata,
            "method": "identity",
            "fallbackUsed": False,
            "fallbackReasons": [],
        }

    linear = _linear_resample(positions, output_length)
    if len(positions) < 3:
        return linear.astype(np.float32), {
            **common_metadata,
            "method": "linear",
            "fallbackUsed": True,
            "fallbackReasons": ["insufficient_points_for_cubic"],
        }

    source_times = np.linspace(0.0, 1.0, len(positions), dtype=np.float64)
    output_times = np.linspace(0.0, 1.0, output_length, dtype=np.float64)
    source_tangents = np.gradient(
        positions,
        source_times,
        axis=0,
        edge_order=2,
    )
    start_tangent = source_tangents[0]
    end_tangent = source_tangents[-1]
    second_derivatives = _clamped_cubic_second_derivatives(
        source_times,
        positions,
        start_tangent,
        end_tangent,
    )
    cubic = _evaluate_cubic_spline(
        source_times,
        positions,
        second_derivatives,
        output_times,
    )
    cubic[0] = positions[0]
    cubic[-1] = positions[-1]

    source_minimum = np.min(positions, axis=0)
    source_maximum = np.max(positions, axis=0)
    below = np.maximum(source_minimum - cubic, 0.0)
    above = np.maximum(cubic - source_maximum, 0.0)
    maximum_axis_overshoot = float(np.max(np.maximum(below, above)))
    linear_dynamics = _vector_dynamics(linear, sample_rate_value)
    cubic_dynamics = _vector_dynamics(cubic, sample_rate_value)
    linear_path_length = _path_length(linear)
    cubic_path_length = _path_length(cubic)
    linear_maximum_speed = _maximum_vector_norm(linear_dynamics["velocity"])
    cubic_maximum_speed = _maximum_vector_norm(cubic_dynamics["velocity"])
    linear_maximum_acceleration = _maximum_vector_norm(
        linear_dynamics["acceleration"]
    )
    cubic_maximum_acceleration = _maximum_vector_norm(
        cubic_dynamics["acceleration"]
    )
    linear_maximum_jerk = _maximum_vector_norm(linear_dynamics["jerk"])
    cubic_maximum_jerk = _maximum_vector_norm(cubic_dynamics["jerk"])

    fallback_reasons: list[str] = []
    if not np.all(np.isfinite(cubic)):
        fallback_reasons.append("non_finite_cubic_result")
    if maximum_axis_overshoot > policy.maximum_axis_overshoot_meters + 1e-12:
        fallback_reasons.append("axis_overshoot")
    if _regressed(
        cubic_path_length,
        linear_path_length,
        policy.maximum_path_length_ratio,
        policy.maximum_path_length_delta_meters,
    ):
        fallback_reasons.append("path_length_regression")
    if _regressed(
        cubic_maximum_speed,
        linear_maximum_speed,
        policy.maximum_speed_ratio,
        policy.maximum_speed_delta_meters_per_second,
    ):
        fallback_reasons.append("speed_regression")
    if _regressed(
        cubic_maximum_acceleration,
        linear_maximum_acceleration,
        policy.maximum_acceleration_ratio,
        policy.maximum_acceleration_delta_meters_per_second_squared,
    ):
        fallback_reasons.append("acceleration_regression")
    if _regressed(
        cubic_maximum_jerk,
        linear_maximum_jerk,
        policy.maximum_jerk_ratio,
        policy.maximum_jerk_delta_meters_per_second_cubed,
    ):
        fallback_reasons.append("jerk_regression")

    accepted = not fallback_reasons
    selected = cubic if accepted else linear
    metadata = {
        **common_metadata,
        "method": "clamped_cubic_c2" if accepted else "linear",
        "fallbackUsed": not accepted,
        "fallbackReasons": fallback_reasons,
        "maximumAxisOvershootMeters": maximum_axis_overshoot,
        "linearPathLengthMeters": linear_path_length,
        "cubicPathLengthMeters": cubic_path_length,
        "linearMaximumSpeedMetersPerSecond": linear_maximum_speed,
        "cubicMaximumSpeedMetersPerSecond": cubic_maximum_speed,
        "linearMaximumAccelerationMetersPerSecondSquared": (
            linear_maximum_acceleration
        ),
        "cubicMaximumAccelerationMetersPerSecondSquared": (
            cubic_maximum_acceleration
        ),
        "linearMaximumJerkMetersPerSecondCubed": linear_maximum_jerk,
        "cubicMaximumJerkMetersPerSecondCubed": cubic_maximum_jerk,
        "sourceEndpointTangentsPerNormalizedProgress": [
            start_tangent.tolist(),
            end_tangent.tolist(),
        ],
        "endpointTangentConstraintMaximumError": 0.0,
    }
    return selected.astype(np.float32), metadata


def quintic_c2_transition(
    normalized_progress: np.ndarray | Sequence[float] | float,
) -> np.ndarray:
    """Evaluate the monotonic bound-to-identity C2 transition polynomial.

    ``h(t) = 3t^5 - 8t^4 + 6t^3`` has value/first/second derivative
    ``(0, 0, 0)`` at the hard bound and ``(1, 1, 0)`` at the identity edge.
    """

    progress = np.asarray(normalized_progress, dtype=np.float64)
    if not np.all(np.isfinite(progress)):
        raise ValueError("normalized_progress must contain only finite values")
    if np.any(progress < 0.0) or np.any(progress > 1.0):
        raise ValueError("normalized_progress must be inside [0, 1]")
    return (
        3.0 * progress**5
        - 8.0 * progress**4
        + 6.0 * progress**3
    )


def _effective_transition_widths(
    lower_bound: float,
    upper_bound: float,
    lower_width: float,
    upper_width: float,
) -> tuple[float, float]:
    span = upper_bound - lower_bound
    requested_sum = lower_width + upper_width
    if requested_sum <= span:
        return lower_width, upper_width
    scale = span / requested_sum
    return lower_width * scale, upper_width * scale


def _boundary_radial_speed(
    radii: np.ndarray,
    *,
    bound: float,
    transition_width: float,
    sample_rate: float,
    lower_bound: bool,
) -> float:
    """Return the fastest radial step that reaches a bound's local band.

    Only intervals touching the requested transition band participate.  A fast
    move elsewhere in the shot must not needlessly consume the available
    distance span, while a frame pair that jumps across the band is included.
    """

    if len(radii) < 2:
        return 0.0
    left = radii[:-1]
    right = radii[1:]
    if lower_bound:
        local = np.minimum(left, right) <= bound + transition_width
    else:
        local = np.maximum(left, right) >= bound - transition_width
    if not np.any(local):
        return 0.0
    return float(np.max(np.abs(right[local] - left[local])) * sample_rate)


def _fill_missing_directions(
    directions: np.ndarray,
    valid: np.ndarray,
    fallback_direction: np.ndarray,
) -> tuple[np.ndarray, int]:
    missing_count = int(np.count_nonzero(~valid))
    if missing_count == 0:
        return directions, 0
    filled = np.array(directions, copy=True)
    if np.any(valid):
        indices = np.arange(len(directions), dtype=np.float64)
        valid_indices = indices[valid]
        for axis in range(3):
            filled[~valid, axis] = np.interp(
                indices[~valid],
                valid_indices,
                filled[valid, axis],
            )
        interpolated_norms = np.linalg.norm(filled[~valid], axis=1)
        unresolved = interpolated_norms <= 1e-12
        if np.any(~unresolved):
            missing_indices = np.flatnonzero(~valid)
            resolved_indices = missing_indices[~unresolved]
            filled[resolved_indices] /= interpolated_norms[~unresolved, None]
        if np.any(unresolved):
            missing_indices = np.flatnonzero(~valid)
            filled[missing_indices[unresolved]] = fallback_direction
    else:
        filled[:] = fallback_direction
    return filled, missing_count


def smooth_radial_distance_clamp(
    relative_positions_meters: np.ndarray | Sequence[Sequence[float]],
    lower_bound_meters: float,
    upper_bound_meters: float,
    *,
    lower_transition_meters: float = 0.35,
    upper_transition_meters: float = 0.75,
    sample_rate: float = 60.0,
    minimum_transition_seconds: float = 0.75,
    fallback_direction: np.ndarray | Sequence[float] = (0.0, 0.0, 1.0),
) -> tuple[np.ndarray, dict[str, object]]:
    """Apply exact radial bounds with speed-adaptive C2 transitions.

    Distances outside a bound sit exactly on that bound.  The transition band
    uses :func:`quintic_c2_transition`, so it joins the bound plateau and the
    unchanged interior with matching value, first derivative, and second
    derivative.  Its width grows with the radial crossing speed, giving the
    blend a minimum temporal duration instead of forcing a fast camera through
    a fixed 35 cm spatial band.  No hard ``numpy.clip`` is used on the distance
    curve.
    """

    positions = _frame_array(
        relative_positions_meters,
        components=3,
        name="relative_positions_meters",
    )
    lower_bound = float(lower_bound_meters)
    upper_bound = float(upper_bound_meters)
    lower_width = float(lower_transition_meters)
    upper_width = float(upper_transition_meters)
    sample_rate_value = _validate_sample_rate(sample_rate)
    transition_seconds = float(minimum_transition_seconds)
    scalar_values = (
        lower_bound,
        upper_bound,
        lower_width,
        upper_width,
    )
    if not all(np.isfinite(value) for value in scalar_values):
        raise ValueError("bounds and transition widths must be finite")
    if lower_bound < 0.0 or upper_bound <= lower_bound:
        raise ValueError("bounds must satisfy 0 <= lower_bound < upper_bound")
    if lower_width < 0.0 or upper_width <= 0.0:
        raise ValueError(
            "lower_transition_meters must be non-negative and "
            "upper_transition_meters must be positive"
        )
    if lower_bound > 0.0 and lower_width <= 0.0:
        raise ValueError(
            "lower_transition_meters must be positive for a positive lower bound"
        )
    if not np.isfinite(transition_seconds) or transition_seconds < 0.0:
        raise ValueError(
            "minimum_transition_seconds must be finite and non-negative"
        )

    fallback = np.asarray(fallback_direction, dtype=np.float64)
    if fallback.shape != (3,) or not np.all(np.isfinite(fallback)):
        raise ValueError("fallback_direction must be a finite three-vector")
    fallback_norm = float(np.linalg.norm(fallback))
    if fallback_norm <= 1e-12:
        raise ValueError("fallback_direction must be non-zero")
    fallback /= fallback_norm

    radii = np.linalg.norm(positions, axis=1)
    lower_radial_speed = _boundary_radial_speed(
        radii,
        bound=lower_bound,
        transition_width=lower_width,
        sample_rate=sample_rate_value,
        lower_bound=True,
    )
    upper_radial_speed = _boundary_radial_speed(
        radii,
        bound=upper_bound,
        transition_width=upper_width,
        sample_rate=sample_rate_value,
        lower_bound=False,
    )
    adaptive_lower_width = max(
        lower_width,
        lower_radial_speed * transition_seconds,
    )
    adaptive_upper_width = max(
        upper_width,
        upper_radial_speed * transition_seconds,
    )
    effective_lower_width, effective_upper_width = _effective_transition_widths(
        lower_bound,
        upper_bound,
        adaptive_lower_width,
        adaptive_upper_width,
    )
    mapped_radii = np.array(radii, copy=True)

    lower_plateau = radii <= lower_bound
    lower_transition = np.zeros(len(radii), dtype=bool)
    if effective_lower_width > 0.0:
        lower_transition = (
            (radii > lower_bound)
            & (radii < lower_bound + effective_lower_width)
        )
        progress = (
            radii[lower_transition] - lower_bound
        ) / effective_lower_width
        mapped_radii[lower_transition] = lower_bound + effective_lower_width * (
            quintic_c2_transition(progress)
        )
    mapped_radii[lower_plateau] = lower_bound

    upper_plateau = radii >= upper_bound
    upper_transition = (
        (radii > upper_bound - effective_upper_width)
        & (radii < upper_bound)
    )
    progress = (
        upper_bound - radii[upper_transition]
    ) / effective_upper_width
    mapped_radii[upper_transition] = upper_bound - effective_upper_width * (
        quintic_c2_transition(progress)
    )
    mapped_radii[upper_plateau] = upper_bound

    nonzero = radii > 1e-12
    directions = np.zeros_like(positions)
    directions[nonzero] = positions[nonzero] / radii[nonzero, None]
    direction_fallback_count = 0
    if lower_bound > 0.0:
        directions, direction_fallback_count = _fill_missing_directions(
            directions,
            nonzero,
            fallback,
        )
    result = directions * mapped_radii[:, None]
    output_radii = np.linalg.norm(result, axis=1)
    tolerance = max(1e-9, upper_bound * 1e-9)
    if np.any(output_radii < lower_bound - tolerance) or np.any(
        output_radii > upper_bound + tolerance
    ):
        raise RuntimeError("smooth radial clamp failed to enforce its bounds")

    # The generated camera is published as float32.  An oblique vector that is
    # exactly on a radial plateau in float64 can acquire a norm a few ULPs past
    # that plateau after its components are rounded independently.  Audit and
    # repair the actual published representation, not only the temporary
    # float64 curve.  The inward margin is microscopic (tens of micrometres at
    # the largest supported radii) while remaining comfortably larger than the
    # worst three-component float32 norm error.
    published_result = result.astype(np.float32)
    published_radii = np.linalg.norm(
        published_result.astype(np.float64),
        axis=1,
    )
    bound_span = upper_bound - lower_bound
    publication_margin = min(
        bound_span * 0.25,
        max(
            1e-6,
            16.0
            * float(np.finfo(np.float32).eps)
            * max(1.0, abs(lower_bound), abs(upper_bound)),
        ),
    )
    lower_publication_violation = (
        (published_radii < lower_bound) if lower_bound > 0.0 else np.zeros(
            len(published_radii),
            dtype=bool,
        )
    )
    upper_publication_violation = published_radii > upper_bound
    if np.any(lower_publication_violation):
        published_result[lower_publication_violation] = (
            directions[lower_publication_violation]
            * (lower_bound + publication_margin)
        ).astype(np.float32)
    if np.any(upper_publication_violation):
        published_result[upper_publication_violation] = (
            directions[upper_publication_violation]
            * (upper_bound - publication_margin)
        ).astype(np.float32)
    publication_projection = (
        lower_publication_violation | upper_publication_violation
    )
    published_radii = np.linalg.norm(
        published_result.astype(np.float64),
        axis=1,
    )
    if np.any(published_radii < lower_bound - tolerance) or np.any(
        published_radii > upper_bound + tolerance
    ):
        raise RuntimeError(
            "smooth radial clamp failed to enforce float32 publication bounds"
        )

    metadata: dict[str, object] = {
        "version": TRAJECTORY_QUALITY_VERSION,
        "policyVersion": RADIAL_CLAMP_POLICY_VERSION,
        "lowerBoundMeters": lower_bound,
        "upperBoundMeters": upper_bound,
        "requestedLowerTransitionMeters": lower_width,
        "requestedUpperTransitionMeters": upper_width,
        "adaptiveLowerTransitionMeters": adaptive_lower_width,
        "adaptiveUpperTransitionMeters": adaptive_upper_width,
        "effectiveLowerTransitionMeters": effective_lower_width,
        "effectiveUpperTransitionMeters": effective_upper_width,
        "sampleRate": sample_rate_value,
        "minimumTransitionSeconds": transition_seconds,
        "lowerBoundaryMaximumRadialSpeedMetersPerSecond": (
            lower_radial_speed
        ),
        "upperBoundaryMaximumRadialSpeedMetersPerSecond": (
            upper_radial_speed
        ),
        "lowerPlateauFrameCount": int(np.count_nonzero(lower_plateau)),
        "lowerTransitionFrameCount": int(np.count_nonzero(lower_transition)),
        "upperTransitionFrameCount": int(np.count_nonzero(upper_transition)),
        "upperPlateauFrameCount": int(np.count_nonzero(upper_plateau)),
        "directionFallbackFrameCount": direction_fallback_count,
        "minimumInputDistanceMeters": float(np.min(radii)),
        "maximumInputDistanceMeters": float(np.max(radii)),
        "minimumOutputDistanceMeters": float(np.min(published_radii)),
        "maximumOutputDistanceMeters": float(np.max(published_radii)),
        "float32BoundaryProjectionFrameCount": int(
            np.count_nonzero(publication_projection)
        ),
        "float32BoundaryProjectionMarginMeters": publication_margin,
        "maximumRadialAdjustmentMeters": float(
            np.max(np.abs(published_radii - radii))
        ),
    }
    return published_result, metadata


def _persistent_direction_reversals(
    scalar_velocity: np.ndarray,
    *,
    sample_rate: float,
    minimum_speed: float,
    persistence_seconds: float,
) -> tuple[int, list[int]]:
    if minimum_speed < 0.0 or not np.isfinite(minimum_speed):
        raise ValueError("minimum_speed must be finite and non-negative")
    if persistence_seconds < 0.0 or not np.isfinite(persistence_seconds):
        raise ValueError("persistence_seconds must be finite and non-negative")
    minimum_run = max(1, int(round(persistence_seconds * sample_rate)))
    signs = np.zeros(len(scalar_velocity), dtype=np.int8)
    signs[scalar_velocity > minimum_speed] = 1
    signs[scalar_velocity < -minimum_speed] = -1

    runs: list[tuple[int, int, int]] = []
    index = 0
    while index < len(signs):
        sign = int(signs[index])
        if sign == 0:
            index += 1
            continue
        end = index + 1
        while end < len(signs) and int(signs[end]) == sign:
            end += 1
        if end - index >= minimum_run:
            runs.append((sign, index, end))
        index = end

    reversal_frames: list[int] = []
    for previous, current in zip(runs, runs[1:]):
        if previous[0] != current[0]:
            reversal_frames.append(int(current[1] + 1))
    return len(reversal_frames), reversal_frames


def translation_dynamics_metrics(
    positions_meters: np.ndarray | Sequence[Sequence[float]],
    *,
    sample_rate: float = 60.0,
    minimum_lateral_speed_meters_per_second: float = 0.03,
    reversal_persistence_seconds: float = 0.08,
) -> dict[str, object]:
    """Measure world-space translation dynamics without changing the path."""

    positions = _frame_array(
        positions_meters,
        components=3,
        name="positions_meters",
    )
    sample_rate_value = _validate_sample_rate(sample_rate)
    dynamics = _vector_dynamics(positions, sample_rate_value)
    velocity = dynamics["velocity"]
    acceleration = dynamics["acceleration"]
    jerk = dynamics["jerk"]
    speed = np.linalg.norm(velocity, axis=1)
    acceleration_magnitude = np.linalg.norm(acceleration, axis=1)
    jerk_magnitude = np.linalg.norm(jerk, axis=1)
    horizontal_speed = (
        np.linalg.norm(velocity[:, (0, 2)], axis=1)
        if len(velocity)
        else np.empty(0, dtype=np.float64)
    )
    vertical_speed = (
        np.abs(velocity[:, 1])
        if len(velocity)
        else np.empty(0, dtype=np.float64)
    )
    lateral_speed = (
        np.abs(velocity[:, 0])
        if len(velocity)
        else np.empty(0, dtype=np.float64)
    )
    lateral_acceleration = (
        np.abs(acceleration[:, 0])
        if len(acceleration)
        else np.empty(0, dtype=np.float64)
    )
    lateral_jerk = (
        np.abs(jerk[:, 0])
        if len(jerk)
        else np.empty(0, dtype=np.float64)
    )
    reversal_count, reversal_frames = _persistent_direction_reversals(
        velocity[:, 0] if len(velocity) else np.empty(0, dtype=np.float64),
        sample_rate=sample_rate_value,
        minimum_speed=minimum_lateral_speed_meters_per_second,
        persistence_seconds=reversal_persistence_seconds,
    )
    displacement = positions[-1] - positions[0]
    return {
        "version": TRAJECTORY_QUALITY_VERSION,
        "metricSpace": "world_translation_meters",
        "frameCount": int(len(positions)),
        "sampleRate": sample_rate_value,
        "durationSeconds": float((len(positions) - 1) / sample_rate_value),
        "pathLengthMeters": _path_length(positions),
        "netDisplacementMeters": float(np.linalg.norm(displacement)),
        "speedMetersPerSecond": _series_statistics(speed),
        "accelerationMetersPerSecondSquared": _series_statistics(
            acceleration_magnitude
        ),
        "jerkMetersPerSecondCubed": _series_statistics(jerk_magnitude),
        "horizontalSpeedMetersPerSecond": _series_statistics(horizontal_speed),
        "verticalSpeedMetersPerSecond": _series_statistics(vertical_speed),
        "lateralSpeedMetersPerSecond": _series_statistics(lateral_speed),
        "lateralAccelerationMetersPerSecondSquared": _series_statistics(
            lateral_acceleration
        ),
        "lateralJerkMetersPerSecondCubed": _series_statistics(lateral_jerk),
        "persistentLateralReversalCount": reversal_count,
        "persistentLateralReversalFrames": reversal_frames,
        "reversalMinimumSpeedMetersPerSecond": float(
            minimum_lateral_speed_meters_per_second
        ),
        "reversalPersistenceSeconds": float(reversal_persistence_seconds),
    }


def screen_space_dynamics_metrics(
    screen_positions_uv: np.ndarray | Sequence[Sequence[float]],
    *,
    sample_rate: float = 60.0,
    minimum_horizontal_speed_uv_per_second: float = 0.01,
    reversal_persistence_seconds: float = 0.08,
) -> dict[str, object]:
    """Measure image-plane motion in normalized UV coordinates."""

    positions = _frame_array(
        screen_positions_uv,
        components=2,
        name="screen_positions_uv",
    )
    sample_rate_value = _validate_sample_rate(sample_rate)
    dynamics = _vector_dynamics(positions, sample_rate_value)
    velocity = dynamics["velocity"]
    acceleration = dynamics["acceleration"]
    jerk = dynamics["jerk"]
    speed = np.linalg.norm(velocity, axis=1)
    acceleration_magnitude = np.linalg.norm(acceleration, axis=1)
    jerk_magnitude = np.linalg.norm(jerk, axis=1)
    horizontal_speed = (
        np.abs(velocity[:, 0])
        if len(velocity)
        else np.empty(0, dtype=np.float64)
    )
    horizontal_acceleration = (
        np.abs(acceleration[:, 0])
        if len(acceleration)
        else np.empty(0, dtype=np.float64)
    )
    horizontal_jerk = (
        np.abs(jerk[:, 0])
        if len(jerk)
        else np.empty(0, dtype=np.float64)
    )
    reversal_count, reversal_frames = _persistent_direction_reversals(
        velocity[:, 0] if len(velocity) else np.empty(0, dtype=np.float64),
        sample_rate=sample_rate_value,
        minimum_speed=minimum_horizontal_speed_uv_per_second,
        persistence_seconds=reversal_persistence_seconds,
    )
    return {
        "version": TRAJECTORY_QUALITY_VERSION,
        "metricSpace": "normalized_screen_uv",
        "frameCount": int(len(positions)),
        "sampleRate": sample_rate_value,
        "durationSeconds": float((len(positions) - 1) / sample_rate_value),
        "pathLengthUv": _path_length(positions),
        "speedUvPerSecond": _series_statistics(speed),
        "accelerationUvPerSecondSquared": _series_statistics(
            acceleration_magnitude
        ),
        "jerkUvPerSecondCubed": _series_statistics(jerk_magnitude),
        "horizontalSpeedUvPerSecond": _series_statistics(horizontal_speed),
        "horizontalAccelerationUvPerSecondSquared": _series_statistics(
            horizontal_acceleration
        ),
        "horizontalJerkUvPerSecondCubed": _series_statistics(horizontal_jerk),
        "persistentHorizontalReversalCount": reversal_count,
        "persistentHorizontalReversalFrames": reversal_frames,
        "reversalMinimumSpeedUvPerSecond": float(
            minimum_horizontal_speed_uv_per_second
        ),
        "reversalPersistenceSeconds": float(reversal_persistence_seconds),
    }


def _required_metric(
    metrics: Mapping[str, object],
    group: str,
    statistic: str,
) -> float:
    group_value = metrics.get(group)
    if not isinstance(group_value, Mapping) or statistic not in group_value:
        raise ValueError(f"metrics are missing {group}.{statistic}")
    value = float(group_value[statistic])
    if not np.isfinite(value) or value < 0.0:
        raise ValueError(f"metrics value {group}.{statistic} is invalid")
    return value


def candidate_dynamics_cost(
    translation_metrics: Mapping[str, object],
    screen_space_metrics: Mapping[str, object] | None = None,
    *,
    policy: CandidateDynamicsPolicy | None = None,
    screen_input_kind: str = RAW_SUBJECT_SCREEN_INPUT,
) -> tuple[float, dict[str, object]]:
    """Return a transparent dynamics cost for a generated camera candidate.

    High-frequency camera acceleration and jerk receive more weight than
    authored-safe low-frequency travel.  ``screen_space_metrics`` is scored
    only when ``screen_input_kind`` is ``stabilized_residual_uv``.  That input
    must be the projected semantic focus minus its stabilized desired UV path,
    not the raw projected performer position.  Raw subject UV remains in the
    returned audit but contributes no soft cost, so choreography is not
    mistaken for camera shake.  This function does not change any downstream
    hard safety gate or the metrics those gates consume.
    """

    selected_policy = policy or CandidateDynamicsPolicy()
    selected_policy.validate()
    if screen_input_kind not in SCREEN_INPUT_KINDS:
        raise ValueError(
            "screen_input_kind must be raw_subject_uv or "
            "stabilized_residual_uv"
        )
    raw_components = [
        (
            "translationSpeedP99",
            _required_metric(
                translation_metrics,
                "speedMetersPerSecond",
                "p99",
            ),
            selected_policy.translation_speed_p99_scale,
            selected_policy.translation_speed_weight,
            selected_policy.translation_speed_p99_free_threshold,
            False,
        ),
        (
            "translationAccelerationP99",
            _required_metric(
                translation_metrics,
                "accelerationMetersPerSecondSquared",
                "p99",
            ),
            selected_policy.translation_acceleration_p99_scale,
            selected_policy.translation_acceleration_weight,
            selected_policy.translation_acceleration_p99_free_threshold,
            False,
        ),
        (
            "translationJerkP99",
            _required_metric(
                translation_metrics,
                "jerkMetersPerSecondCubed",
                "p99",
            ),
            selected_policy.translation_jerk_p99_scale,
            selected_policy.translation_jerk_weight,
            selected_policy.translation_jerk_p99_free_threshold,
            False,
        ),
        (
            "persistentLateralReversalCount",
            float(translation_metrics.get("persistentLateralReversalCount", 0)),
            selected_policy.lateral_reversal_count_scale,
            selected_policy.lateral_reversal_weight,
            selected_policy.lateral_reversal_count_free_threshold,
            False,
        ),
    ]
    if screen_space_metrics is not None:
        raw_components.extend(
            [
                (
                    "screenSpeedP99",
                    _required_metric(
                        screen_space_metrics,
                        "speedUvPerSecond",
                        "p99",
                    ),
                    selected_policy.screen_speed_p99_scale,
                    selected_policy.screen_speed_weight,
                    selected_policy.screen_speed_p99_free_threshold,
                    True,
                ),
                (
                    "screenAccelerationP99",
                    _required_metric(
                        screen_space_metrics,
                        "accelerationUvPerSecondSquared",
                        "p99",
                    ),
                    selected_policy.screen_acceleration_p99_scale,
                    selected_policy.screen_acceleration_weight,
                    selected_policy.screen_acceleration_p99_free_threshold,
                    True,
                ),
                (
                    "screenJerkP99",
                    _required_metric(
                        screen_space_metrics,
                        "jerkUvPerSecondCubed",
                        "p99",
                    ),
                    selected_policy.screen_jerk_p99_scale,
                    selected_policy.screen_jerk_weight,
                    selected_policy.screen_jerk_p99_free_threshold,
                    True,
                ),
                (
                    "persistentHorizontalReversalCount",
                    float(
                        screen_space_metrics.get(
                            "persistentHorizontalReversalCount",
                            0,
                        )
                    ),
                    selected_policy.screen_horizontal_reversal_count_scale,
                    selected_policy.screen_horizontal_reversal_weight,
                    (
                        selected_policy
                        .screen_horizontal_reversal_count_free_threshold
                    ),
                    True,
                ),
            ]
        )

    components: dict[str, dict[str, float | bool]] = {}
    total_cost = 0.0
    score_screen_residual = (
        screen_input_kind == STABILIZED_SCREEN_RESIDUAL_INPUT
    )
    for name, value, scale, configured_weight, free_threshold, is_screen in (
        raw_components
    ):
        if not np.isfinite(value) or value < 0.0:
            raise ValueError(f"candidate dynamics component {name} is invalid")
        weight = (
            configured_weight
            if not is_screen or score_screen_residual
            else 0.0
        )
        excess = max(0.0, value - free_threshold)
        normalized = min(
            excess / scale,
            selected_policy.maximum_component_ratio,
        )
        contribution = weight * normalized**2
        components[name] = {
            "value": float(value),
            "scale": float(scale),
            "weight": float(weight),
            "configuredWeight": float(configured_weight),
            "freeThreshold": float(free_threshold),
            "excessAboveFreeThreshold": float(excess),
            "normalized": float(normalized),
            "cost": float(contribution),
            "screenInputSuppressed": bool(is_screen and not score_screen_residual),
        }
        total_cost += contribution

    details: dict[str, object] = {
        "version": TRAJECTORY_QUALITY_VERSION,
        "policyVersion": DYNAMICS_POLICY_VERSION,
        "cost": float(total_cost),
        "passed": bool(total_cost <= selected_policy.maximum_acceptable_cost),
        "maximumAcceptableCost": float(
            selected_policy.maximum_acceptable_cost
        ),
        "components": components,
        "screenSpaceIncluded": screen_space_metrics is not None,
        "screenSpaceInputKind": (
            screen_input_kind if screen_space_metrics is not None else None
        ),
        "screenSpaceSoftCostEnabled": bool(
            screen_space_metrics is not None and score_screen_residual
        ),
        "screenSpaceInputContract": (
            "Score only stabilized semantic-focus residual UV: "
            "projected_focus_uv - stabilized_desired_uv. Raw subject UV is "
            "diagnostic-only and is excluded from soft candidate ranking."
        ),
        "policy": asdict(selected_policy),
    }
    return float(total_cost), details


def evaluate_candidate_dynamics(
    positions_meters: np.ndarray | Sequence[Sequence[float]],
    screen_positions_uv: np.ndarray | Sequence[Sequence[float]] | None = None,
    *,
    sample_rate: float = 60.0,
    policy: CandidateDynamicsPolicy | None = None,
    screen_input_kind: str = RAW_SUBJECT_SCREEN_INPUT,
) -> dict[str, object]:
    """Measure and score a candidate in one call.

    ``screen_positions_uv`` defaults to ``raw_subject_uv`` and is then kept
    only for diagnostics and downstream hard-gate auditing.  A caller that
    wants screen-space soft scoring must pass a stabilized semantic-focus
    residual and set ``screen_input_kind="stabilized_residual_uv"``.
    """

    translation = translation_dynamics_metrics(
        positions_meters,
        sample_rate=sample_rate,
    )
    screen = None
    if screen_positions_uv is not None:
        screen = screen_space_dynamics_metrics(
            screen_positions_uv,
            sample_rate=sample_rate,
        )
        if screen["frameCount"] != translation["frameCount"]:
            raise ValueError(
                "positions_meters and screen_positions_uv must have equal frame counts"
            )
    _, cost_details = candidate_dynamics_cost(
        translation,
        screen,
        policy=policy,
        screen_input_kind=screen_input_kind,
    )
    return {
        **cost_details,
        "translationMetrics": translation,
        "screenSpaceMetrics": screen,
    }


__all__ = [
    "C2_RESAMPLING_POLICY_VERSION",
    "DYNAMICS_POLICY_VERSION",
    "RADIAL_CLAMP_POLICY_VERSION",
    "RAW_SUBJECT_SCREEN_INPUT",
    "STABILIZED_SCREEN_RESIDUAL_INPUT",
    "TRAJECTORY_QUALITY_VERSION",
    "CandidateDynamicsPolicy",
    "ResamplingGuardPolicy",
    "candidate_dynamics_cost",
    "evaluate_candidate_dynamics",
    "quintic_c2_transition",
    "resample_position_path_c2",
    "screen_space_dynamics_metrics",
    "smooth_radial_distance_clamp",
    "translation_dynamics_metrics",
]
