from __future__ import annotations

from typing import Optional, Tuple

import numpy as np


def to_world_points(points_sensor: np.ndarray, pose: Optional[np.ndarray]) -> np.ndarray:
    """
    Convert sensor-frame points to world frame using pose (world_T_sensor).
    Falls back to input points when pose is unavailable/invalid.
    """
    if pose is None:
        return np.asarray(points_sensor, dtype=np.float32)
    try:
        pts = np.asarray(points_sensor, dtype=np.float32)
        pose_np = np.asarray(pose, dtype=np.float32)
        if pts.ndim != 2 or pts.shape[1] != 3 or pose_np.shape != (4, 4):
            return pts
        rot = pose_np[:3, :3]
        trans = pose_np[:3, 3]
        return (pts @ rot.T) + trans
    except Exception:
        return np.asarray(points_sensor, dtype=np.float32)


def apply_transform_points(points: Optional[np.ndarray], transform: np.ndarray) -> Optional[np.ndarray]:
    if points is None:
        return None
    pts = np.asarray(points, dtype=np.float32)
    if len(pts) == 0:
        return pts
    try:
        tf = np.asarray(transform, dtype=np.float32)
        if tf.shape != (4, 4):
            return pts
        rot = tf[:3, :3]
        trans = tf[:3, 3]
        return (pts @ rot.T) + trans
    except Exception:
        return pts


def tf_delta(prev_tf: np.ndarray, new_tf: np.ndarray) -> Tuple[float, float]:
    try:
        a = np.asarray(prev_tf, dtype=np.float64)
        b = np.asarray(new_tf, dtype=np.float64)
        if a.shape != (4, 4) or b.shape != (4, 4):
            return 0.0, 0.0
        dp = b[:3, 3] - a[:3, 3]
        trans = float(np.linalg.norm(dp))
        r = b[:3, :3] @ a[:3, :3].T
        ang = float(np.degrees(np.arccos(np.clip((np.trace(r) - 1.0) * 0.5, -1.0, 1.0))))
        return trans, ang
    except Exception:
        return 0.0, 0.0


def blend_rigid_tf(prev_tf: np.ndarray, new_tf: np.ndarray, alpha_t: float, alpha_r: float) -> np.ndarray:
    try:
        a = np.asarray(prev_tf, dtype=np.float64)
        b = np.asarray(new_tf, dtype=np.float64)
        if a.shape != (4, 4) or b.shape != (4, 4):
            return np.asarray(new_tf, dtype=np.float64)
        at = float(np.clip(alpha_t, 0.0, 1.0))
        ar = float(np.clip(alpha_r, 0.0, 1.0))
        out = np.eye(4, dtype=np.float64)
        out[:3, 3] = ((1.0 - at) * a[:3, 3]) + (at * b[:3, 3])
        rm = ((1.0 - ar) * a[:3, :3]) + (ar * b[:3, :3])
        u, _, vt = np.linalg.svd(rm, full_matrices=False)
        r = u @ vt
        if np.linalg.det(r) < 0:
            u[:, -1] *= -1.0
            r = u @ vt
        out[:3, :3] = r
        return out
    except Exception:
        return np.asarray(new_tf, dtype=np.float64)


def yaw_deg_from_tf(tf: np.ndarray) -> float:
    try:
        m = np.asarray(tf, dtype=np.float64)
        if m.shape != (4, 4):
            return 0.0
        yaw = np.degrees(np.arctan2(float(m[1, 0]), float(m[0, 0])))
        return float(yaw)
    except Exception:
        return 0.0


def tf_from_xyzyaw(x: float, y: float, z: float, yaw_deg: float) -> np.ndarray:
    yaw = np.deg2rad(float(yaw_deg))
    cy = float(np.cos(yaw))
    sy = float(np.sin(yaw))
    tf = np.eye(4, dtype=np.float64)
    tf[:3, :3] = np.array(
        [[cy, -sy, 0.0], [sy, cy, 0.0], [0.0, 0.0, 1.0]],
        dtype=np.float64,
    )
    tf[:3, 3] = np.array([float(x), float(y), float(z)], dtype=np.float64)
    return tf