"""
_aruco_utils.py — Pure-Python helpers for AprilTag detection (no ROS2 deps).

Importable without rclpy for unit testing.
"""

from __future__ import annotations

import math
import numpy as np
import cv2


_FAMILY = 'DICT_APRILTAG_36h11'


def rvec_tvec_to_pose(rvec: np.ndarray, tvec: np.ndarray):
    """
    Convert OpenCV rotation + translation vectors to geometry_msgs/Pose.
    Returns a plain namespace object to avoid importing geometry_msgs here.
    """
    R, _ = cv2.Rodrigues(rvec)
    m = R

    t = m[0, 0] + m[1, 1] + m[2, 2]
    if t > 0:
        s  = 0.5 / math.sqrt(t + 1.0)
        qw = 0.25 / s
        qx = (m[2, 1] - m[1, 2]) * s
        qy = (m[0, 2] - m[2, 0]) * s
        qz = (m[1, 0] - m[0, 1]) * s
    elif m[0, 0] > m[1, 1] and m[0, 0] > m[2, 2]:
        s  = 2.0 * math.sqrt(1.0 + m[0, 0] - m[1, 1] - m[2, 2])
        qw = (m[2, 1] - m[1, 2]) / s
        qx = 0.25 * s
        qy = (m[0, 1] + m[1, 0]) / s
        qz = (m[0, 2] + m[2, 0]) / s
    elif m[1, 1] > m[2, 2]:
        s  = 2.0 * math.sqrt(1.0 + m[1, 1] - m[0, 0] - m[2, 2])
        qw = (m[0, 2] - m[2, 0]) / s
        qx = (m[0, 1] + m[1, 0]) / s
        qy = 0.25 * s
        qz = (m[1, 2] + m[2, 1]) / s
    else:
        s  = 2.0 * math.sqrt(1.0 + m[2, 2] - m[0, 0] - m[1, 1])
        qw = (m[1, 0] - m[0, 1]) / s
        qx = (m[0, 2] + m[2, 0]) / s
        qy = (m[1, 2] + m[2, 1]) / s
        qz = 0.25 * s

    return (
        float(tvec.flat[0]), float(tvec.flat[1]), float(tvec.flat[2]),
        float(qx), float(qy), float(qz), float(qw),
    )


class ArucoBackend:
    """Wraps cv2.aruco, handling the 4.6 vs 4.7+ API difference."""

    def __init__(self, logger=None):
        self._log = logger
        dictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_APRILTAG_36h11)
        params = cv2.aruco.DetectorParameters()

        try:
            self._detector = cv2.aruco.ArucoDetector(dictionary, params)
            self._new_api  = True
            if logger:
                logger.info('[apriltag] ArucoDetector (OpenCV ≥ 4.7)')
        except AttributeError:
            self._dict     = dictionary
            self._params   = params
            self._new_api  = False
            if logger:
                logger.info('[apriltag] legacy aruco.detectMarkers (OpenCV 4.6)')

    def detect(self, gray: np.ndarray):
        """Returns (corners, ids); ids may be None."""
        if self._new_api:
            corners, ids, _ = self._detector.detectMarkers(gray)
        else:
            corners, ids, _ = cv2.aruco.detectMarkers(
                gray, self._dict, parameters=self._params
            )
        return corners, ids
