"""SE(3) helpers: compose, invert, conversions rvec/quat/euler/matrix."""

from __future__ import annotations

import math

import numpy as np


def rvec_to_R(rvec: np.ndarray) -> np.ndarray:
    import cv2
    R, _ = cv2.Rodrigues(np.asarray(rvec, dtype=np.float64).reshape(3, 1))
    return R


def R_to_rvec(R: np.ndarray) -> np.ndarray:
    import cv2
    rvec, _ = cv2.Rodrigues(np.asarray(R, dtype=np.float64))
    return rvec.flatten()


def quat_xyzw_to_R(q: np.ndarray) -> np.ndarray:
    x, y, z, w = [float(v) for v in q]
    n = x * x + y * y + z * z + w * w
    if n == 0.0:
        return np.eye(3)
    s = 2.0 / n
    return np.array([
        [1 - s * (y * y + z * z), s * (x * y - z * w),     s * (x * z + y * w)],
        [s * (x * y + z * w),     1 - s * (x * x + z * z), s * (y * z - x * w)],
        [s * (x * z - y * w),     s * (y * z + x * w),     1 - s * (x * x + y * y)],
    ])


def R_to_quat_xyzw(R: np.ndarray) -> np.ndarray:
    m = np.asarray(R, dtype=np.float64)
    t = m[0, 0] + m[1, 1] + m[2, 2]
    if t > 0:
        s = np.sqrt(t + 1.0) * 2
        w = 0.25 * s
        x = (m[2, 1] - m[1, 2]) / s
        y = (m[0, 2] - m[2, 0]) / s
        z = (m[1, 0] - m[0, 1]) / s
    elif m[0, 0] > m[1, 1] and m[0, 0] > m[2, 2]:
        s = np.sqrt(1.0 + m[0, 0] - m[1, 1] - m[2, 2]) * 2
        w = (m[2, 1] - m[1, 2]) / s
        x = 0.25 * s
        y = (m[0, 1] + m[1, 0]) / s
        z = (m[0, 2] + m[2, 0]) / s
    elif m[1, 1] > m[2, 2]:
        s = np.sqrt(1.0 + m[1, 1] - m[0, 0] - m[2, 2]) * 2
        w = (m[0, 2] - m[2, 0]) / s
        x = (m[0, 1] + m[1, 0]) / s
        y = 0.25 * s
        z = (m[1, 2] + m[2, 1]) / s
    else:
        s = np.sqrt(1.0 + m[2, 2] - m[0, 0] - m[1, 1]) * 2
        w = (m[1, 0] - m[0, 1]) / s
        x = (m[0, 2] + m[2, 0]) / s
        y = (m[1, 2] + m[2, 1]) / s
        z = 0.25 * s
    return np.array([x, y, z, w])


def normalize_quaternion(q: list[float]) -> list[float]:
    """Normalizza un quaternion a lunghezza unitaria. Solleva ValueError se
    la norma e' troppo piccola per essere normalizzata in modo sensato
    (es. quaternion zero, sotto 1e-6)."""
    n = math.sqrt(sum(float(x)*float(x) for x in q))
    if n < 1e-6:
        raise ValueError(f"quaternion is degenerate (norm={n:.3e})")
    return [float(x) / n for x in q]


def make_T(R: np.ndarray, t: np.ndarray) -> np.ndarray:
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = np.asarray(t, dtype=np.float64).flatten()
    return T


def invert_T(T: np.ndarray) -> np.ndarray:
    R = T[:3, :3]
    t = T[:3, 3]
    Ti = np.eye(4)
    Ti[:3, :3] = R.T
    Ti[:3, 3] = -R.T @ t
    return Ti


def T_from_pos_quat(pos: np.ndarray, quat_xyzw: np.ndarray) -> np.ndarray:
    return make_T(quat_xyzw_to_R(np.asarray(quat_xyzw)), np.asarray(pos))


def pos_quat_from_T(T: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
    return T[:3, 3].copy(), R_to_quat_xyzw(T[:3, :3])


def T_from_rvec_tvec(rvec: np.ndarray, tvec: np.ndarray) -> np.ndarray:
    return make_T(rvec_to_R(rvec), np.asarray(tvec).flatten())


def R_to_euler_xyz(R: np.ndarray) -> np.ndarray:
    """Extrinsic XYZ euler (roll, pitch, yaw) in radians."""
    sy = np.sqrt(R[0, 0] ** 2 + R[1, 0] ** 2)
    singular = sy < 1e-6
    if not singular:
        x = np.arctan2(R[2, 1], R[2, 2])
        y = np.arctan2(-R[2, 0], sy)
        z = np.arctan2(R[1, 0], R[0, 0])
    else:
        x = np.arctan2(-R[1, 2], R[1, 1])
        y = np.arctan2(-R[2, 0], sy)
        z = 0.0
    return np.array([x, y, z])


def transform_points(T: np.ndarray, pts: np.ndarray) -> np.ndarray:
    """Apply T (4x4) to Nx3 points."""
    pts = np.asarray(pts, dtype=np.float64)
    R = T[:3, :3]
    t = T[:3, 3]
    return pts @ R.T + t


def rpy_deg_to_quaternion_xyzw(roll_deg: float, pitch_deg: float, yaw_deg: float) -> np.ndarray:
    """Extrinsic XYZ Euler (deg) -> quaternion xyzw. Inverse of `quaternion_xyzw_to_rpy_deg`.
    Convenzione coerente con `R_to_euler_xyz` (XYZ extrinsic, in radianti)."""
    cr, sr = np.cos(np.radians(roll_deg) * 0.5), np.sin(np.radians(roll_deg) * 0.5)
    cp, sp = np.cos(np.radians(pitch_deg) * 0.5), np.sin(np.radians(pitch_deg) * 0.5)
    cy, sy = np.cos(np.radians(yaw_deg) * 0.5), np.sin(np.radians(yaw_deg) * 0.5)
    qw = cr * cp * cy + sr * sp * sy
    qx = sr * cp * cy - cr * sp * sy
    qy = cr * sp * cy + sr * cp * sy
    qz = cr * cp * sy - sr * sp * cy
    return np.array([qx, qy, qz, qw], dtype=np.float64)


def quaternion_xyzw_to_rpy_deg(quat_xyzw) -> tuple[float, float, float]:
    """Quaternion xyzw -> extrinsic XYZ Euler (deg). Convention matches `R_to_euler_xyz`."""
    R = quat_xyzw_to_R(np.asarray(quat_xyzw, dtype=np.float64))
    rpy_rad = R_to_euler_xyz(R)
    rpy_deg = np.degrees(rpy_rad)
    return float(rpy_deg[0]), float(rpy_deg[1]), float(rpy_deg[2])