"""Object axis estimation via PCA.

Given Nx3 object points (in any frame), compute:
  - centroid
  - 3 principal axes (unit vectors) with corresponding extents
  - right-handed frame (axes columns form a rotation matrix)
"""

from __future__ import annotations

from dataclasses import dataclass

import numpy as np


@dataclass
class ObjectAxes:
    centroid: np.ndarray              # (3,)
    axes: np.ndarray                  # (3,3) columns = unit vectors, sorted largest extent first
    extents: np.ndarray               # (3,) along each axis (meters)

    @property
    def long_axis(self) -> np.ndarray:
        return self.axes[:, 0]

    @property
    def mid_axis(self) -> np.ndarray:
        return self.axes[:, 1]

    @property
    def short_axis(self) -> np.ndarray:
        return self.axes[:, 2]

    def to_dict(self) -> dict:
        return {
            "centroid": self.centroid.tolist(),
            "axes": self.axes.tolist(),
            "extents_m": self.extents.tolist(),
        }


def compute_axes(points_xyz: np.ndarray) -> ObjectAxes:
    pts = np.asarray(points_xyz, dtype=np.float64)
    if pts.ndim != 2 or pts.shape[1] != 3 or len(pts) < 3:
        raise ValueError(f"need at least 3 3D points, got shape {pts.shape}")

    c = pts.mean(axis=0)
    centered = pts - c
    cov = np.cov(centered.T)
    eigvals, eigvecs = np.linalg.eigh(cov)
    # eigh returns ascending — reverse for largest-first
    order = np.argsort(eigvals)[::-1]
    eigvals = eigvals[order]
    eigvecs = eigvecs[:, order]

    # Ensure right-handed coordinate system
    if np.linalg.det(eigvecs) < 0:
        eigvecs[:, 2] = -eigvecs[:, 2]

    # Extent along each axis = range of projected coordinates
    proj = centered @ eigvecs  # Nx3
    extents = proj.max(axis=0) - proj.min(axis=0)
    return ObjectAxes(centroid=c, axes=eigvecs, extents=extents)