import numpy as np


def compute_face_normal(corners: np.ndarray) -> np.ndarray:
    """
    Compute the normal vector of a face defined by its corners.
    Assumes corners are in order (e.g., clockwise or counter-clockwise).

    Args:
        corners: (N, 3) array of corner coordinates.

    Returns:
        (3,) normalized normal vector.
    """
    if corners.shape[0] < 3:
        raise ValueError("At least 3 corners are required to compute a normal.")

    # Use the cross product of two edges
    v1 = corners[1] - corners[0]
    v2 = corners[2] - corners[0]

    normal = np.cross(v1, v2)
    norm = np.linalg.norm(normal)

    if norm < 1e-10:
        raise ValueError("Corners are collinear or degenerate; cannot compute normal.")

    return normal / norm


def rotation_align_vectors(from_vec: np.ndarray, to_vec: np.ndarray) -> np.ndarray:
    """
    Compute the 3x3 rotation matrix that aligns from_vec to to_vec.

    Args:
        from_vec: (3,) source vector.
        to_vec: (3,) target vector.

    Returns:
        (3, 3) rotation matrix.
    """
    # Normalize inputs
    a = from_vec / np.linalg.norm(from_vec)
    b = to_vec / np.linalg.norm(to_vec)

    v = np.cross(a, b)
    c = np.dot(a, b)
    s = np.linalg.norm(v)

    # Handle parallel case
    if s < 1e-10:
        if c > 0:
            return np.eye(3)
        else:
            # Anti-parallel case: 180 degree rotation around an orthogonal axis
            # Find an orthogonal axis
            if abs(a[0]) < 0.9:
                ortho = np.array([1, 0, 0])
            else:
                ortho = np.array([0, 1, 0])

            axis = np.cross(a, ortho)
            axis /= np.linalg.norm(axis)

            # Rodrigues formula for 180 degrees
            K = np.array(
                [[0, -axis[2], axis[1]], [axis[2], 0, -axis[0]], [-axis[1], axis[0], 0]]
            )
            return np.eye(3) + 2 * (K @ K)

    # General case using Rodrigues' rotation formula
    # R = I + [v]_x + [v]_x^2 * (1-c)/s^2
    vx = np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])

    R = np.eye(3) + vx + (vx @ vx) * ((1 - c) / (s**2))
    return R


def apply_alignment_to_pose(T: np.ndarray, R_align: np.ndarray) -> np.ndarray:
    """
    Apply an alignment rotation to a 4x4 pose matrix.
    The alignment is applied in the global frame (pre-multiplication of rotation).

    Args:
        T: (4, 4) homogeneous transformation matrix.
        R_align: (3, 3) alignment rotation matrix.

    Returns:
        (4, 4) aligned transformation matrix.
    """
    if T.shape != (4, 4):
        raise ValueError(f"Expected 4x4 matrix, got {T.shape}")
    if R_align.shape != (3, 3):
        raise ValueError(f"Expected 3x3 matrix, got {R_align.shape}")

    T_align = np.eye(4)
    T_align[:3, :3] = R_align

    return T_align @ T