"""Normalized-Jacobian observability analysis for rotation / SE(3) gates."""

from __future__ import annotations

from dataclasses import dataclass

import numpy as np

from .contracts import MotionPair
from .geometry import skew, so3_log


@dataclass(frozen=True)
class ObservabilityReport:
    rotation_observable: bool
    translation_observable: bool
    condition_rotation: float
    condition_translation: float
    notes: tuple[str, ...] = ()


def _rotation_jacobian(pairs: list[MotionPair], r_x: np.ndarray) -> np.ndarray:
    rows = []
    for pair in pairs:
        # Residual r = log(R_x^T R_A R_x R_B^T); approximate J w.r.t. left perturbation of R_x.
        # Use finite-difference columns for robustness in V1.
        base = so3_log(r_x.T @ pair.R_A @ r_x @ pair.R_B.T)
        cols = []
        eps = 1e-5
        for axis in range(3):
            delta = np.zeros(3)
            delta[axis] = eps
            r_pert = r_x @ (np.eye(3) + skew(delta))
            # Orthonormalize lightly
            u, _, vt = np.linalg.svd(r_pert)
            r_pert = u @ vt
            pert = so3_log(r_pert.T @ pair.R_A @ r_pert @ pair.R_B.T)
            cols.append((pert - base) / eps)
        rows.append(np.column_stack(cols))
    return np.vstack(rows) if rows else np.zeros((0, 3))


def analyze_observability(
    pairs: list[MotionPair] | tuple[MotionPair, ...],
    r_x: np.ndarray,
    *,
    condition_threshold: float = 100.0,
) -> ObservabilityReport:
    """Gate whether rotation-only or full SE(3) should be accepted."""

    usable = list(pairs)
    notes: list[str] = []
    if len(usable) < 3:
        return ObservabilityReport(False, False, 1e9, 1e9, ("insufficient pairs",))

    j_r = _rotation_jacobian(usable, np.asarray(r_x, dtype=float))
    if j_r.size == 0:
        return ObservabilityReport(False, False, 1e9, 1e9, ("empty rotation jacobian",))

    singular = np.linalg.svd(j_r, compute_uv=False)
    cond_r = float(singular[0] / max(singular[-1], 1e-12))
    rotation_information = float(singular[-1] / np.sqrt(max(len(usable), 1)))
    rotation_ok = (
        cond_r < condition_threshold
        and rotation_information > 1e-3
        and singular[-1] > 1e-6
    )

    # Translation lever arm is observable through stacked (R_A - I). Pure
    # planar yaw leaves its vertical column in the nullspace and must fail.
    translation_rows = [
        np.asarray(pair.R_A, dtype=float).reshape(3, 3) - np.eye(3)
        for pair in usable
        if pair.t_B_m is not None
    ]
    if translation_rows:
        j_t = np.vstack(translation_rows)
        singular_t = np.linalg.svd(j_t, compute_uv=False)
        cond_t = float(singular_t[0] / max(singular_t[-1], 1e-12))
        translation_information = float(
            singular_t[-1] / np.sqrt(max(len(translation_rows), 1))
        )
    else:
        cond_t = 1e9
        translation_information = 0.0
    translation_ok = (
        len(translation_rows) >= 5
        and cond_t < condition_threshold
        and translation_information > 0.02
    )

    if not rotation_ok:
        notes.append(
            f"rotation not observable: condition={cond_r:.1f}, "
            f"min_information={rotation_information:.3e}"
        )
    else:
        notes.append(
            f"rotation observable: condition={cond_r:.1f}, "
            f"min_information={rotation_information:.3e}"
        )
    if not translation_ok:
        notes.append(
            f"translation not observable: condition={cond_t:.1f}, "
            f"min_information={translation_information:.3e}; "
            "full SE3 will be rejected"
        )
    return ObservabilityReport(
        rotation_observable=rotation_ok,
        translation_observable=translation_ok,
        condition_rotation=cond_r,
        condition_translation=cond_t,
        notes=tuple(notes),
    )