"""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",))

    # Normalize columns.
    col_norm = np.linalg.norm(j_r, axis=0) + 1e-12
    j_r_n = j_r / col_norm
    singular = np.linalg.svd(j_r_n, compute_uv=False)
    cond_r = float(singular[0] / max(singular[-1], 1e-12))
    rotation_ok = cond_r < condition_threshold and singular[-1] > 1e-3

    # Translation observability proxy: diversity of rotation axes and presence of translation in B.
    axes = []
    translations = []
    for pair in usable:
        axis = so3_log(pair.R_B)
        n = np.linalg.norm(axis)
        if n > 1e-8:
            axes.append(axis / n)
        if pair.t_B_m is not None:
            translations.append(pair.t_B_m)
    axis_rank = 0
    if axes:
        axis_mat = np.asarray(axes, dtype=float)
        axis_rank = int(np.linalg.matrix_rank(axis_mat, tol=0.1))
    trans_span = 0.0
    if translations:
        tmat = np.asarray(translations, dtype=float)
        trans_span = float(np.linalg.norm(np.std(tmat, axis=0)))
    # For planar yaw-mostly motion, translation z is typically weak.
    translation_ok = axis_rank >= 2 and trans_span > 0.2 and len(translations) >= 5
    cond_t = 1e9 if not translation_ok else float(max(3, 10 - axis_rank * 2) * (0.5 / max(trans_span, 1e-3)))

    if not rotation_ok:
        notes.append(f"rotation condition {cond_r:.1f} exceeds threshold {condition_threshold}")
    else:
        notes.append(f"rotation condition {cond_r:.1f}")
    if not translation_ok:
        notes.append(
            f"translation not observable (axis_rank={axis_rank}, trans_span={trans_span:.3f} m); "
            "V1 will reject full SE3 without strong priors"
        )
    return ObservabilityReport(
        rotation_observable=rotation_ok,
        translation_observable=translation_ok,
        condition_rotation=cond_r,
        condition_translation=cond_t,
        notes=tuple(notes),
    )