"""Audited suspended-settle diagnostics for the pre-policy frozen pose."""

from __future__ import annotations

import numpy as np

from .h2_contract import H2_DOF, HARDWARE_JOINT_NAMES


# Only shoulder and elbow joints use the wider frozen-pose diagnostic band. The
# 2026-09-04 suspended commissioning run measured a stable 0.442 rad arm error
# with the ONNX arm stiffness (14.25 Nm/rad); fixed-base inverse dynamics gives
# 0.34--0.41 rad static offsets for these joints.  Wrists, torso, legs and head
# remain on the common diagnostic band. Position error is not an L1 admission
# gate, matching the G1 deployment's direct policy start semantics. The tether
# can create large static PD offsets before the balance policy begins.
FROZEN_GRAVITY_LOADED_ARM_IDS = (15, 16, 17, 18, 22, 23, 24, 25)


def position_tolerances_hw(common_rad: float, arm_rad: float) -> np.ndarray:
    tolerance = np.full(H2_DOF, float(common_rad), dtype=np.float64)
    tolerance[list(FROZEN_GRAVITY_LOADED_ARM_IDS)] = float(arm_rad)
    if not np.isfinite(tolerance).all() or np.any(tolerance <= 0.0):
        raise ValueError("frozen-pose position tolerances must be finite and positive")
    return tolerance


def tracking_status(
    q_hw: np.ndarray,
    ready_q_hw: np.ndarray,
    tolerance_hw: np.ndarray,
) -> tuple[bool, float, int, np.ndarray]:
    q = np.asarray(q_hw, dtype=np.float64).reshape(H2_DOF)
    ready = np.asarray(ready_q_hw, dtype=np.float64).reshape(H2_DOF)
    tolerance = np.asarray(tolerance_hw, dtype=np.float64).reshape(H2_DOF)
    if not np.isfinite(q).all() or not np.isfinite(ready).all():
        raise ValueError("frozen-pose tracking inputs must be finite")
    error = np.abs(q - ready)
    normalized = error / tolerance
    worst_id = int(np.argmax(normalized))
    return bool(np.all(normalized <= 1.0)), float(error[worst_id]), worst_id, error


def suspended_settle_status(
    q_hw: np.ndarray,
    dq_hw: np.ndarray,
    ready_q_hw: np.ndarray,
    tolerance_hw: np.ndarray,
    velocity_tolerance_rad_s: float,
) -> tuple[bool, bool, float, int, float]:
    """Require a quiet finite suspension state; report pose error without gating it."""

    q = np.asarray(q_hw, dtype=np.float64).reshape(H2_DOF)
    dq = np.asarray(dq_hw, dtype=np.float64).reshape(H2_DOF)
    if not np.isfinite(q).all() or not np.isfinite(dq).all():
        raise ValueError("suspended-settle state must be finite")
    velocity_limit = float(velocity_tolerance_rad_s)
    if not np.isfinite(velocity_limit) or velocity_limit <= 0.0:
        raise ValueError("suspended-settle velocity tolerance must be finite and positive")
    position_ok, worst_error, worst_id, _ = tracking_status(
        q, ready_q_hw, tolerance_hw
    )
    velocity_error = float(np.max(np.abs(dq)))
    return (
        velocity_error <= velocity_limit,
        position_ok,
        worst_error,
        worst_id,
        velocity_error,
    )


def error_summary(
    q_hw: np.ndarray,
    ready_q_hw: np.ndarray,
    tolerance_hw: np.ndarray,
    *,
    limit: int = 4,
) -> str:
    q = np.asarray(q_hw, dtype=np.float64).reshape(H2_DOF)
    ready = np.asarray(ready_q_hw, dtype=np.float64).reshape(H2_DOF)
    tolerance = np.asarray(tolerance_hw, dtype=np.float64).reshape(H2_DOF)
    error = np.abs(q - ready)
    order = np.argsort(-(error / tolerance))[: max(1, int(limit))]
    return ", ".join(
        f"id={index:02d}:{HARDWARE_JOINT_NAMES[index]} "
        f"q={q[index]:+.3f} target={ready[index]:+.3f} "
        f"err={error[index]:.3f}/{tolerance[index]:.3f}rad"
        for index in order
    )


__all__ = [
    "FROZEN_GRAVITY_LOADED_ARM_IDS",
    "error_summary",
    "position_tolerances_hw",
    "suspended_settle_status",
    "tracking_status",
]