"""
CoMPAS Navlib - SO(3) Module

This module provides functions to work in the special orthogonal group SO(3) and its Lie algebra so(3).

Functions:
    rot_inv(rot): Compute the inverse of a rotation matrix.
    hpr2quat(hpr): Convert Heading, Pitch and Roll (HPR) angles to quaternion using the ZYX Euler angles convention.
    rph2quat(rph): Convert Roll, Pitch, Heading (RPH) angles to quaternion using the ZYX Euler angles convention.
    quat2hpr(quat): Convert quaternion to Heading, Pitch and Roll (HPR) angles using the ZYX Euler angles convention.
    quat2rph(quat): Convert quaternion to Roll, Pitch, Heading (RPH) angles using the ZYX Euler angles convention.
    hpr2rot(hpr): Convert Heading, Pitch and Roll (HPR) angles to SO(3) rotation matrix using the ZYX Euler angles
                  convention.
    rph2rot(rph): Convert Roll, Pitch, Heading (RPH) angles to SO(3) rotation matrix using the ZYX Euler angles
                  convention.
    quat2rot(quat): Convert quaternion to a rotation matrix in SO(3) using the ZYX Euler angles convention.
    rot2hpr(rot): Convert SO(3) rotation matrix to Roll, Pitch, Heading (RPH) angles using the ZYX Euler angles
                  convention.
    rot2rph(rot): Convert SO(3) rotation matrix to Roll, Pitch, Heading (RPH) angles using the ZYX Euler angles
                  convention.
    rot2quat(rot): Convert SO(3) rotation matrix to quaternion using the ZYX Euler angles convention.
    rot_diff(rot_1, rot_2): Compute the relative rotation between two matrices.
    vec_to_so3(vec): Compute the skew-symmetric representation for a vector of R3.
    so3_to_vec(mat): Convert a so3 matrix into a R3 vector.
    axis_ang3(vec): Converts a R3 representing a rotation into a exponential coordinates parametrization.
    matrix_exp3(mat): Compute the matrix exponential of a matrix in so(3) representing the exponential coordinates
                      for rotation.
    matrix_log3(mat): Compute the matrix logarithm of a matrix in SO(3) representing the exponential coordinates
                        for rotation.
    project_to_so3(mat): Project a matrix in R3 to SO(3).
    distance_so3(mat_1, mat_2): Compute the distance between two matrices in SO(3).
    wrap360(angles): Wrap an array or list of angles in degrees to the range [-360, 360).
    wrap180(angles): Wrap an array or list of angles in degrees to the range [-180, 180).
    wrap2pi(angles): Wrap an array or list of angles in radians to the range [-2pi, 2pi).
    wrap1pi(angles): Wrap an array or list of angles in radians to the range [-pi, pi).
    unwrap2pi(angles): Unwrap an array or list of angles in radians from the range [-2pi, 2pi).
    unwrap1pi(angles): Unwrap an array or list of angles in radians from the range [-pi, pi).
    unwrap360(angles): Unwrap an array or list of angles in degrees from the range [-360, 360).
    unwrap180(angles): Unwrap an array or list of angles in degrees from the range [-180, 180).
    wrapunwrap360(angles): Wrap to and unwrap an array or list of angles in degrees to the range [-360, 360).
    wrapunwrap (angles): Wrap to and unwrap an array or list of angles in radians to the range [-2pi, 2pi).

Authors: Sebastián Rodríguez-Martínez and Giancarlo Troni
Contact: srodriguez@mbari.org
Last Update: 2023/10/22
"""

__all__ = ["rph2rot", "hpr2rot", "rot2hpr", "rot2rph", "rot_diff", "vec_to_so3", "so3_to_vec",
           "axis_ang3", "matrix_exp3", "matrix_log3", "rot_inv", "project_to_SO3",
           "distance_to_SO3", "rph2quat", "hpr2quat", "quat2rph", "quat2hpr", "quat2rot",
           "rot2quat", "wrap360", "wrap180", "wrap2pi", "wrap1pi", "unwrap2pi", "unwrap1pi",
           "unwrap360", "unwrap180", "wrapunwrap", "wrapunwrap360"]

import numpy as np
import math
from typing import Union, Iterable, Tuple


def rot_inv(rot: np.ndarray) -> np.ndarray:
    """
    Compute the inverse of a rotation matrix.

    Args:
        rot (np.ndarray): The rotation matrix as a (3,3) numpy array.

    Returns:
        np.ndarray: The inverse of the rotation matrix as a (3,3) numpy array.

    Raises:
        ValueError: If the input rotation matrix is not a (3,3) numpy array.

    Examples:
        >>> rot_mat = np.array([[0.93629336, -0.27509585, 0.21835066],
                                [0.28962948, 0.95642509, -0.03695701],
                                [-0.19866933, 0.0978434, 0.97517033]])
        >>> rot_inv = rot_inv(rot_mat)
        >>> print(rot_inv)
        [[ 0.93629336  0.28962948 -0.19866933]
         [-0.27509585  0.95642509  0.0978434 ]
         [ 0.21835066 -0.03695701  0.97517033]]

    Notes:
        The input rotation matrix must be orthonormal and have a determinant of +1 to represent a valid rotation.
    """
    if not isinstance(rot, np.ndarray) and not isinstance(rot, list):
        raise ValueError("Usage: The rotation matrix must be a numpy array or a list.")

    rot = np.array(rot) if isinstance(rot, list) else rot

    if rot.shape != (3, 3):
        raise ValueError("Usage: The rotation matrix must have shape (3, 3)")

    return np.einsum("ij -> ji", rot)


def hpr2quat(hpr: Union[np.ndarray, Iterable[float]]) -> np.ndarray:
    """
    Convert Heading, Pitch and Roll (HPR) angles to quaternion using the ZYX Euler angles convention.

    Args:
        hpr (Union[np.ndarray, Iterable[float]]): hpr angles in radians as a numpy array or as a list.

    Returns:
        np.ndarray: The quaternion using hpr with the convention ZYX in order (x, y, z, w)

    Raises:
        ValueError: If the input HPR angles are not a 3-element numpy array or list.

    Examples:
        >>> hpr = np.array([0.3, 0.2, 0.1])
        >>> quat = hpr2quat(hpr)
        >>> print(quat)
        [0.93629336 0.28962948 0.19866933 0.0978434 ]

    Notes:
        The HPR angles represent rotations around the Z, Y, and X axes, respectively.
        The resulting quaternion represents a rotation of the input HPR angles using the ZYX Euler angles convention.
    """
    hpr = np.array(hpr).flatten() if isinstance(hpr, list) else hpr.flatten()
    if hpr.shape[0] != 3:
        raise ValueError("Usage: quat=hpr2quat([angH angP angR])")

    angH = hpr[0]
    angP = hpr[1]
    angR = hpr[2]

    ch = math.cos(angH / 2)
    sh = math.sin(angH / 2)
    cp = math.cos(angP / 2)
    sp = math.sin(angP / 2)
    cr = math.cos(angR / 2)
    sr = math.sin(angR / 2)

    quat = np.array(
        [
            ch * cp * sr - sh * sp * cr,
            ch * sp * cr + sh * cp * sr,
            sh * cp * cr - ch * sp * sr,
            ch * cp * cr + sh * sp * sr,
        ]
    )

    return quat


def rph2quat(rph: Union[np.ndarray, Iterable[float]]) -> np.ndarray:
    """
    Convert Roll, Pitch, Heading (RPH) angles to quaternion using the ZYX Euler angles convention.

    Args:
        rph (Union[np.ndarray, Iterable[float]]): rph angles in radians as a numpy array or as a list.

    Returns:
        np.ndarray: The quaternion using rph with the convention ZYX in order (x, y, z, w)

    Raises:
        ValueError: If the input RPH angles are not a 3-element numpy array or list.

    Examples:
        >>> rph = np.array([0.1, 0.2, 0.3])
        >>> quat = rph2quat(rph)
        >>> print(quat)
        [0.93629336 0.28962948 0.19866933 0.0978434 ]

    Notes:
        The RPH angles represent rotations around the X, Y, and Z axes, respectively.
        The resulting quaternion represents a rotation of the input RPH angles using the ZYX Euler angles convention.
    """
    rph = np.array(rph).flatten() if isinstance(rph, list) else rph.flatten()
    if rph.shape[0] != 3:
        raise ValueError("Usage: quat=rph2quat([angH angP angR])")

    angR = rph[0]
    angP = rph[1]
    angH = rph[2]

    ch = math.cos(angH / 2)
    sh = math.sin(angH / 2)
    cp = math.cos(angP / 2)
    sp = math.sin(angP / 2)
    cr = math.cos(angR / 2)
    sr = math.sin(angR / 2)

    quat = np.array(
        [
            ch * cp * sr - sh * sp * cr,
            ch * sp * cr + sh * cp * sr,
            sh * cp * cr - ch * sp * sr,
            ch * cp * cr + sh * sp * sr,
        ]
    )

    return quat


def quat2hpr(quat: Union[np.ndarray, Iterable[float]]) -> np.ndarray:
    """
    Convert quaternion to Heading, Pitch and Roll (HPR) angles using the ZYX Euler angles convention.

    Args:
        quat (Union[np.ndarray, Iterable[float]]): The quaternion as a numpy array or as a list.

    Returns:
        np.ndarray: The HPR angles in radians as a numpy array with the convention ZYX.

    Raises:
        ValueError: If the input quaternion is not a 4-element numpy array or list.
        ValueError: If the input quaternion is not normalized.

    Examples:
        >>> quat = np.array([0.93629336, 0.28962948, 0.19866933, 0.0978434])
        >>> hpr = quat2hpr(quat)
        >>> print(hpr)
        [0.3 0.2 0.1]

    Notes:
        The resulting HPR angles represent rotations around the Z, Y, and X axes, respectively.
        The input quaternion must be normalized to represent a valid rotation.
    """
    quat = np.array(quat).flatten() if isinstance(quat, list) else quat.flatten()
    if quat.shape[0] != 4:
        raise ValueError("Usage: hpr=quat2hpr([x y z w])")

    if abs(np.linalg.norm(quat) - 1.0) >= 1e-6:
        raise ValueError("Usage: The quaternion must be normalized.")

    q0 = quat[3]
    q1 = quat[0]
    q2 = quat[1]
    q3 = quat[2]

    angH = math.atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3))
    angP = math.asin(2 * (q0 * q2 - q3 * q1))
    angR = math.atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2))

    return np.array([angH, angP, angR])


def quat2rph(quat: Union[np.ndarray, Iterable[float]]) -> np.ndarray:
    """
    Convert quaternion to Roll, Pitch, Heading (RPH) angles using the ZYX Euler angles convention.

    Args:
        quat (Union[np.ndarray, Iterable[float]]): The quaternion as a numpy array or as a list.

    Returns:
        np.ndarray: The RPH angles in radians as a numpy array with the convention ZYX.

    Raises:
        ValueError: If the input quaternion is not a 4-element numpy array or list.
        ValueError: If the input quaternion is not normalized.

    Examples:
        >>> quat = np.array([0.93629336, 0.28962948, 0.19866933, 0.0978434])
        >>> rph = quat2rph(quat)
        >>> print(rph)
        [0.1 0.2 0.3]

    Notes:
        The resulting RPH angles represent rotations around the X, Y, and Z axes, respectively.
        The input quaternion must be normalized to represent a valid rotation.
    """
    quat = np.array(quat).flatten() if isinstance(quat, list) else quat.flatten()
    if quat.shape[0] != 4:
        raise ValueError("Usage: rph=quat2rph([x y z w])")

    if abs(np.linalg.norm(quat) - 1.0) >= 1e-6:
        raise ValueError("Usage: The quaternion must be normalized.")

    q0 = quat[3]
    q1 = quat[0]
    q2 = quat[1]
    q3 = quat[2]

    angR = math.atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2))
    angP = math.asin(2 * (q0 * q2 - q3 * q1))
    angH = math.atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3))

    return np.array([angR, angP, angH])


def hpr2rot(hpr: Union[np.ndarray, Iterable[float]]) -> np.ndarray:
    """
    Convert Heading, Pitch and Roll (HPR) angles to SO(3) rotation matrix using the ZYX Euler angles convention.

    Args:
        hpr (Union[np.ndarray, Iterable[float]]): hpr angles in radians as a numpy array or as a list.

    Returns:
        np.ndarray: The rotation matrix using hpr with the convention ZYX

    Raises:
        ValueError: If the input HPR angles are not a 3-element numpy array or list.

    Examples:
        >>> hpr = np.array([0.3, 0.2, 0.1])
        >>> rot_mat = hpr2rot(hpr)
        >>> print(rot_mat)
        [[ 0.93629336 -0.27509585  0.21835066]
         [ 0.28962948  0.95642509 -0.03695701]
         [-0.19866933  0.0978434   0.97517033]]

    Notes:
        The HPR angles represent rotations around the Z, Y, and X axes, respectively.
        The resulting rotation matrix represents a rotation of the input HPR angles using the ZYX Euler angles
        convention.
    """
    hpr = np.array(hpr).flatten() if isinstance(hpr, list) else hpr.flatten()
    if hpr.shape[0] != 3:
        raise ValueError("Usage: RotMat=hpr2rot([angH angP angR])")

    angH = hpr[0]
    angP = hpr[1]
    angR = hpr[2]

    c1 = math.cos(angH)
    s1 = math.sin(angH)
    c2 = math.cos(angP)
    s2 = math.sin(angP)
    c3 = math.cos(angR)
    s3 = math.sin(angR)

    RotMat = np.array(
        [
            [c1 * c2, c1 * s2 * s3 - c3 * s1, s1 * s3 + c1 * c3 * s2],
            [c2 * s1, c1 * c3 + s1 * s2 * s3, c3 * s1 * s2 - c1 * s3],
            [-s2, c2 * s3, c2 * c3],
        ]
    )

    return RotMat


def rph2rot(rph: Union[np.ndarray, Iterable[float]]) -> np.ndarray:
    """
    Convert Roll, Pitch, Heading (RPH) angles to SO(3) rotation matrix using the ZYX Euler angles convention.

    Args:
        rph (Union[np.ndarray, Iterable[float]]): rph angles in radians as a numpy array or as a list.

    Returns:
        np.ndarray: The rotation matrix using rph with the convention ZYX

    Raises:
        ValueError: If the input RPH angles are not a 3-element numpy array or list.

    Examples:
        >>> rph = np.array([0.1, 0.2, 0.3])
        >>> rot_mat = rph2rot(rph)
        >>> print(rot_mat)
        [[ 0.93629336 -0.27509585  0.21835066]
         [ 0.28962948  0.95642509 -0.03695701]
         [-0.19866933  0.0978434   0.97517033]]

    Notes:
        The RPH angles represent rotations around the X, Y, and Z axes, respectively.
        The resulting rotation matrix represents a rotation of the input RPH angles using the ZYX Euler angles
        convention.
    """
    rph = np.array(rph).flatten() if isinstance(rph, list) else rph.flatten()
    if rph.shape[0] != 3:
        raise ValueError("Usage: RotMat=rph2rot([angH angP angR])")

    angR = rph[0]
    angP = rph[1]
    angH = rph[2]

    c1 = math.cos(angH)
    s1 = math.sin(angH)
    c2 = math.cos(angP)
    s2 = math.sin(angP)
    c3 = math.cos(angR)
    s3 = math.sin(angR)

    RotMat = np.array(
        [
            [c1 * c2, c1 * s2 * s3 - c3 * s1, s1 * s3 + c1 * c3 * s2],
            [c2 * s1, c1 * c3 + s1 * s2 * s3, c3 * s1 * s2 - c1 * s3],
            [-s2, c2 * s3, c2 * c3],
        ]
    )

    return RotMat


def quat2rot(quat: Union[np.ndarray, Iterable[float]]) -> np.ndarray:
    """
    Convert quaternion to a rotation matrix in SO(3) using the ZYX Euler angles convention.

    Args:
        quat (Union[np.ndarray, Iterable[float]]): The quaternion as a numpy array or as a list.

    Returns:
        np.ndarray: The RPH angles in radians as a numpy array with the convention ZYX.

    Raises:
        ValueError: If the input quaternion is not a 4-element numpy array or list.
        ValueError: If the input quaternion is not normalized.

    Examples:
        >>> quat = np.array([0.93629336, 0.28962948, 0.19866933, 0.0978434])
        >>> rotmat = quat2rot(quat)
        >>> print(rotmat)
        [[ 0.93629336 -0.27509585  0.21835066]
         [ 0.28962948  0.95642509 -0.03695701]
         [-0.19866933  0.0978434   0.97517033]]

    Notes:
        The input quaternion must be normalized to represent a valid rotation.
    """
    quat = np.array(quat).flatten() if isinstance(quat, list) else quat.flatten()
    if quat.shape[0] != 4:
        raise ValueError("Usage: rph=quat2rph([x y z w])")

    if abs(np.linalg.norm(quat) - 1.0) >= 1e-6:
        raise ValueError("Usage: The quaternion must be normalized.")

    rph = quat2rph(quat)
    rot = rph2rot(rph)

    return rot


def rot2hpr(rot: np.ndarray) -> np.ndarray:
    """
    Convert SO(3) rotation matrix to Roll, Pitch, Heading (RPH) angles using the ZYX Euler angles convention.

    Args:
        rot (np.ndarray): The rotation matrix as a (3,3) numpy array.

    Returns:
        np.ndarray: The RPH angles in radians as a numpy array with the convention ZYX.

    Raises:
        ValueError: If the input rotation matrix is not a (3,3) numpy array or list.

    Examples:
        >>> rot_mat = np.array([[0.93629336, -0.27509585, 0.21835066],
                                [0.28962948, 0.95642509, -0.03695701],
                                [-0.19866933, 0.0978434, 0.97517033]])
        >>> hpr = rot2hpr(rot_mat)
        >>> print(hpr)
        [0.3 0.2 0.1]

    Notes:
        The resulting RPH angles represent rotations around the X, Y, and Z axes, respectively.
        The input rotation matrix must be orthonormal and have a determinant of +1 to represent a valid rotation.
    """
    if (not isinstance(rot, np.ndarray)) and (not isinstance(rot, list)):
        raise ValueError("Usage: The rotation matrix must be a numpy array or a list.")

    rot = np.array(rot) if isinstance(rot, list) else rot

    if rot.shape != (3, 3):
        raise ValueError("Usage: The rotation matrix must have shape (3, 3)")

    h = math.atan2(rot[1, 0], rot[0, 0])
    p = math.atan2(-rot[2, 0], math.sqrt(math.pow(rot[2, 1], 2) + math.pow(rot[2, 2], 2)))
    r = math.atan2(rot[2, 1], rot[2, 2])

    return np.array([h, p, r])


def rot2rph(rot: np.ndarray) -> np.ndarray:
    """
    Convert SO(3) rotation matrix to Roll, Pitch, Heading (RPH) angles using the ZYX Euler angles convention.

    Args:
        rot (np.ndarray): The rotation matrix as a (3,3) numpy array.

    Returns:
        np.ndarray: The RPH angles in radians as a numpy array with the convention ZYX.

    Raises:
        ValueError: If the input rotation matrix is not a (3,3) numpy array.

    Examples:
        >>> rot_mat = np.array([[0.93629336, -0.27509585, 0.21835066],
                                [0.28962948, 0.95642509, -0.03695701],
                                [-0.19866933, 0.0978434, 0.97517033]])
        >>> rph = rot2rph(rot_mat)
        >>> print(rph)
        [0.1 0.2 0.3]

    Notes:
        The resulting RPH angles represent rotations around the X, Y, and Z axes, respectively.
        The input rotation matrix must be orthonormal and have a determinant of +1 to represent a valid rotation.
    """
    if (not isinstance(rot, np.ndarray)) and (not isinstance(rot, list)):
        raise ValueError("Usage: The rotation matrix must be a numpy array or a list.")

    rot = np.array(rot) if isinstance(rot, list) else rot

    if rot.shape != (3, 3):
        raise ValueError("Usage: The rotation matrix must have shape (3, 3)")

    h = math.atan2(rot[1, 0], rot[0, 0])
    p = math.atan2(-rot[2, 0], math.sqrt(math.pow(rot[2, 1], 2) + math.pow(rot[2, 2], 2)))
    r = math.atan2(rot[2, 1], rot[2, 2])

    return np.array([r, p, h])


def rot2quat(rot: np.ndarray) -> np.ndarray:
    """
    Convert SO(3) rotation matrix to quaternion using the ZYX Euler angles convention.

    Args:
        rot (np.ndarray): The rotation matrix as a (3,3) numpy array.

    Returns:
        np.ndarray: The quaternion as a numpy array with the convention ZYX in order (x, y, z, w)

    Raises:
        ValueError: If the input rotation matrix is not a (3,3) numpy array.

    Examples:
        >>> rot_mat = np.array([[0.50000000, -0.1464466,  0.8535534],
                                [0.50000000,  0.8535534, -0.1464466],
                                [-0.7071068,  0.5000000,  0.5000000]])
        >>> quat = rot2quat(rot_mat)
        >>> print(quat)
        [0.1913417, 0.4619398, 0.1913417, 0.8446232]

    Notes:
        The input rotation matrix must be orthonormal and have a determinant of +1 to represent a valid rotation.
    """
    if (not isinstance(rot, np.ndarray)) and (not isinstance(rot, list)):
        raise ValueError("Usage: The rotation matrix must be a numpy array or a list.")

    rot = np.array(rot) if isinstance(rot, list) else rot

    if rot.shape != (3, 3):
        raise ValueError("Usage: The rotation matrix must have shape (3, 3)")

    rph = rot2rph(rot)
    quat = rph2quat(rph)

    return quat


def rot_diff(rot_1: np.ndarray, rot_2: np.ndarray) -> np.ndarray:
    """
    Compute the relative rotation between two matrices.

    The returned matrix is the attitude of 2 with respect to 1: R_12 = R_1w @ R_w2 = inv(R_w1) @ R_w2

    Args:
        rot_1 (np.ndarray): Rotation matrix for attitude 1 as a numpy array.
        rot_2 (np.ndarray): Rotation matrix for attitude 2 as a numpy array.

    Returns:
        np.ndarray: The attitude of 2 with respect to 1 as a numpy array.

    Raises:
        ValueError: If the input rotation matrices are not (3,3) numpy arrays.

    Examples:
        >>> rot_mat_1 = np.array([[0.93629336, -0.27509585, 0.21835066],
                                [0.28962948, 0.95642509, -0.03695701],
                                [-0.19866933, 0.0978434, 0.97517033]])
        >>> rot_mat_2 = np.array([[0.82298387, -0.41998535, 0.38175244],
                                [0.56975033, 0.81758362, -0.08209422],
                                [-0.0, 0.3939193, 0.91914503]])
        >>> rot_diff = rotDiff(rot_mat_1, rot_mat_2)
        >>> print(rot_diff)
        [[ 0.93557083, -0.23469286,  0.15104944],
         [ 0.31852406,  0.93603611, -0.09360322],
         [ 0.1586428 ,  0.26221888,  0.98271281]]

    Notes:
        The input rotation matrices must be orthonormal and have a determinant of +1 to represent valid rotations.
    """
    if not isinstance(rot_1, np.ndarray) or not isinstance(rot_2, np.ndarray):
        raise ValueError("Usage: The rotation matrices must be numpy arrays.")
    if rot_1.shape != (3, 3) or rot_2.shape != (3, 3):
        raise ValueError("Usage: The rotation matrix must have shape (3, 3)")
    return np.einsum("ij -> ji", rot_1) @ rot_2


def vec_to_so3(vec: Union[np.ndarray, Iterable[float]]) -> np.ndarray:
    """
    Compute the skew-symmetric representation for a vector of R3.

    Args:
        vec (Union[np.ndarray, Iterable[float]]): The vector as a numpy array or a list.

    Returns:
        np.ndarray: The skew-symmetric matrix as a numpy array.

    Raises:
        ValueError: If the input vector is not a numpy array or a list.
        ValueError: If the input vector does not have 3 components.

    Examples:
        >>> vec = np.array([1, 2, 3])
        >>> skew_sym = vec_to_so3(vec)
        >>> print(skew_sym)
        [[ 0. -3.  2.]
         [ 3.  0. -1.]
         [-2.  1.  0.]]
    """
    if (not isinstance(vec, np.ndarray)) and (not isinstance(vec, list)):
        raise ValueError("Type Error: The vector must be a numpy array or a list.")
    if isinstance(vec, np.ndarray):
        vec = vec.flatten()
    if len(vec) != 3:
        raise ValueError("Shape Error: The vector must have only 3 components.")
    matrix = np.zeros((3, 3))
    matrix[0, 1], matrix[0, 2], matrix[1, 2] = -vec[2], vec[1], -vec[0]
    return matrix - matrix.T


def so3_to_vec(mat: np.ndarray) -> np.ndarray:
    """
    Convert a so3 matrix into a R3 vector.

    Args:
        mat (np.ndarray): The skew-symmetric matrix as a (3,3) numpy array.

    Returns:
        np.ndarray: The vector as a (3, ) numpy array.

    Raises:
        ValueError: If the input matrix is not a (3,3) numpy array.

    Examples:
        >>> skew_sym = np.array([[0, -3, 2], [3, 0, -1], [-2, 1, 0]])
        >>> vec = so3_to_vec(skew_sym)
        >>> print(vec)
        [1. 2. 3.]
    """
    if not isinstance(mat, np.ndarray) and not isinstance(mat, list):
        raise ValueError("Type Error: The matrix must be a numpy array or a list.")

    mat = np.array(mat) if isinstance(mat, list) else mat

    if mat.shape != (3, 3):
        raise ValueError("Shape Error: The matrix must have shape (3, 3).")

    return np.array([mat[2][1], mat[0][2], mat[1][0]])


def axis_ang3(vec: Union[Iterable[float], np.ndarray]) -> Tuple[np.ndarray, float]:
    """
    Converts a R3 representing a rotation into a exponential coordinates
    parametrization, i.e., a unit vector representing the axis (w) and an
    angle theta.

    Args:
        vec (Union[Iterable[float], np.ndarray]): R3 vector representing a rotation in so3.

    Returns:
        Tuple[np.ndarray, float]: Axis-Angle representation for the rotation as a tuple with the unit axis
        and the angle of rotation.

    Raises:
        ValueError: If the input vector is not a numpy array or a list.
        ValueError: If the input vector does not have 3 components.

    Examples:
        >>> vec = np.array([1, 2, 3])
        >>> axis, angle = axis_ang_3(vec)
        >>> print(axis)
        [0.26726124 0.53452248 0.80178373]
        >>> print(angle)
        3.7416573867739413
    """
    if (not isinstance(vec, np.ndarray)) and (not isinstance(vec, list)):
        raise ValueError("Type Error: The vector must be a numpy array or a list.")

    vec = np.array(vec).flatten() if isinstance(vec, list) else vec.flatten()

    if len(vec) != 3:
        raise ValueError("Shape Error: The vector must have only 3 components.")

    if np.allclose(vec, np.zeros(3)):
        return np.zeros(3), 0

    vec_norm = np.linalg.norm(vec)
    return vec / vec_norm, vec_norm


def matrix_exp3(mat: np.ndarray) -> np.ndarray:
    """
    Compute the matrix exponential of a matrix in so(3) representing the
    exponential coordinates for rotation, i.e., [w]*theta, where [w] is the
    skew-symmetric matrix of the unitary rotation axis. To compute the exponential
    matrix, the Rodrigues' formula is used.

    Args:
        mat (np.ndarray): Skew-symmetric matrix representing the exponential coordinates of rotation as a (3, 3)
        numpy array.

    Returns:
        np.ndarray: Matrix exponential of the exponential coordinates as a numpy array.

    Raises:
        ValueError: If the input matrix is not a (3,3) numpy array.
        ValueError: If the input matrix is not a skew-symmetric matrix.

    Examples:
        >>> skew_sym = np.array([[0, -3, 2], [3, 0, -1], [-2, 1, 0]])
        >>> exp_mat = matrix_exp_3(skew_sym)
        >>> print(exp_mat)
        [[-0.69492056  0.71352099  0.08929286]
         [-0.19200697 -0.30378504  0.93319235]
         [ 0.69297817  0.63134970   0.34810748]]
    """
    if not isinstance(mat, np.ndarray) and not isinstance(mat, list):
        raise ValueError("Type Error: The matrix must be a numpy array or a list.")

    mat = np.array(mat) if isinstance(mat, list) else mat

    if mat.ndim != 2:
        raise ValueError("Shape Error: The matrix must be a 2-dimensional numpy array.")
    if mat.shape != (3, 3):
        raise ValueError("Shape Error: The matrix must be a (3, 3) numpy array.")

    rotation = so3_to_vec(mat)

    # Check if the rotation is zero to avoid singularity
    if np.abs(np.linalg.norm(rotation)) < 1.00e-06:
        return np.eye(3, dtype=np.float64)

    else:
        theta = axis_ang3(rotation)[1]
        unit_mat = mat / theta
        return np.eye(3, dtype=np.float64) + np.sin(theta) * unit_mat + (1 - np.cos(theta)) * (unit_mat @ unit_mat)


def matrix_log3(R: np.ndarray) -> np.ndarray:
    """
    Computes the matrix logarithm of a rotation matrix.

    Args:
        R (np.ndarray): Rotation matrix as a (3, 3) numpy array.

    Returns:
        np.ndarray: Matrix logarithm of the rotation matrix as a numpy array.

    Raises:
        ValueError: If the input matrix is not a (3,3) numpy array.
        ValueError: If the input matrix is not a rotation matrix.

    Examples:
        >>> rot_mat = np.array([[0.93629336, -0.27509585, 0.21835066],
                                [0.28962948, 0.95642509, -0.03695701],
                                [-0.19866933, 0.0978434, 0.97517033]])
        >>> log_mat = matrix_log_3(rot_mat)
        >>> print(log_mat)
        [[ 0.        , -0.28874894,  0.21322592],
         [ 0.28874894,  0.        , -0.06892461],
         [-0.21322592,  0.06892461,  0.        ]]
    """
    if not isinstance(R, np.ndarray) and not isinstance(R, list):
        raise ValueError("Type Error: The matrix must be a numpy array or a list.")

    R = np.array(R) if isinstance(R, list) else R

    if R.ndim != 2:
        raise ValueError("Shape Error: The matrix must be a 2-dimensional numpy array.")
    if R.shape != (3, 3):
        raise ValueError("Shape Error: The matrix must be a (3, 3) numpy array.")

    acos_arg = (np.trace(R) - 1) * 0.5

    # acos is undefined
    if acos_arg >= 1:
        return np.zeros((3, 3))

    # if theta = pi, three possible solutions.
    elif acos_arg <= -1:
        if not (np.abs(1 + R[2, 2]) < 1.00e-06):
            omg = (1.0 / np.sqrt(2 * (1 + R[2, 2]))) * (R[:, 2] + np.array([0.0, 0.0, 1.0]))
        elif not (np.abs(1 + R[1, 1]) < 1.00e-06):
            omg = (1.0 / np.sqrt(2 * (1 + R[1, 1]))) * (R[:, 1] + np.array([0.0, 1.0, 0.0]))
        else:
            omg = (1.0 / np.sqrt(2 * (1 + R[0, 0]))) * (R[:, 0] + np.array([1.0, 0.0, 0.0]))
        return vec_to_so3(np.pi * omg)

    else:
        theta = np.arccos(acos_arg)
        return theta * (1 / (2.0 * np.sin(theta))) * (R - R.T)


def project_to_SO3(mat: np.ndarray) -> np.ndarray:
    """
    Projects a matrix to the closest matrix in SO(3) using singular value
    decomposition.
    Source: http://hades.mech.northwestern.edu/index.php/Modern_Robotics_Linear_Algebra_Review

    Args:
        mat (np.ndarray): The matrix as a (3,3) numpy array.

    Returns:
        np.ndarray: The closest matrix in SO(3) as a numpy array.

    Raises:
        ValueError: If the input matrix is not a (3,3) numpy array.

    Examples:
        >>> mat = np.array([[ 0.675,  0.150,  0.720],
                            [ 0.370,  0.771, -0.511],
                            [-0.630,  0.619,  0.472]])
        >>> proj_mat = projectToSO3(mat)
        >>> print(proj_mat)
        [[ 0.67901136,  0.14894516,  0.71885945],
         [ 0.37320708,  0.77319584, -0.51272279],
         [-0.63218672,  0.61642804,  0.46942137]]
    """
    if not isinstance(mat, np.ndarray) and not isinstance(mat, list):
        raise ValueError("Type Error: The matrix must be a numpy array.")

    mat = np.array(mat) if isinstance(mat, list) else mat

    if mat.ndim != 2:
        raise ValueError("Shape Error: The matrix must be a 2-dimensional numpy array.")
    if mat.shape != (3, 3):
        raise ValueError("Shape Error: The matrix must be a (3, 3) numpy array.")

    U, s, V = np.linalg.svd(mat)
    R = U @ V
    if np.linalg.det(R) < 0:
        # Results may be far from mat.
        R[:, s[2, 2]] = -R[:, s[2, 2]]
    return R


def distance_to_SO3(mat: np.ndarray) -> float:
    """
    Compute the frobenius norm to describe the distance of a matrix from
    the SO(3) manifold.

    Args:
        mat (np.ndarray): The matrix as a (3,3) numpy array.

    Returns:
        float: The frobenius norm as a float.

    Raises:
        ValueError: If the input matrix is not a (3,3) numpy array.

    Examples:
        >>> mat = np.array([[ 0.675,  0.150,  0.720],
                            [ 0.370,  0.771, -0.511],
                            [-0.630,  0.619,  0.472]])
        >>> dist = distanceToSO3(mat)
        >>> print(dist)
        0.0663128178912833
    """
    if not isinstance(mat, np.ndarray) and not isinstance(mat, list):
        raise ValueError("Type Error: The matrix must be a numpy array.")

    mat = np.array(mat) if isinstance(mat, list) else mat

    if mat.ndim != 2:
        raise ValueError("Shape Error: The matrix must be a 2-dimensional numpy array.")
    if mat.shape != (3, 3):
        raise ValueError("Shape Error: The matrix must be a (3, 3) numpy array.")

    if np.linalg.det(mat) > 0:
        return np.linalg.norm(mat.T @ mat - np.eye(3, dtype=np.float64))
    else:
        return 1.00e10


def wrap360(angles: Union[Iterable[float], np.ndarray]) -> np.ndarray:
    """
    Wraps an input of angles between 0 and 360 degrees.

    Args:
        angles (Union[Iterable[float], np.ndarray]): The angles in degrees as a numpy array or a list.

    Returns:
        np.ndarray: The wrapped angles in degrees between 0 and 360 as a numpy array.

    Raises:
        ValueError: If the input angles are not a numpy array or a list.

    Examples:
        >>> angles = np.array([-30, 0, 90, 400])
        >>> wrapped_angles = wrap360(angles)
        >>> print(wrapped_angles)
        [330.   0.  90.  40.]
    """
    if not isinstance(angles, np.ndarray) and not isinstance(angles, list):
        raise ValueError("Type Error: The angles must be a numpy array or a list.")

    angles = np.array(angles) if isinstance(angles, list) else angles

    return np.mod(angles, 360.0)


def wrap180(angles: Union[Iterable[float], np.ndarray]) -> np.ndarray:
    """
    Wraps an input of angles between -180 and 180 degrees.

    Args:
        angles (Union[Iterable[float], np.ndarray]): The angles in degrees as a numpy array or a list.

    Returns:
        np.ndarray: The wrapped angles in degrees between -180 and 180 as a numpy array.

    Raises:
        ValueError: If the input angles are not a numpy array or a list.

    Examples:
        >>> angles = np.array([[-360, -270, -180, -90, 0, 90, 180, 270, 360])
        >>> wrapped_angles = wrap180(angles)
        >>> print(wrapped_angles)
        [  0. 90. -180. -90.   0.  90. -180. -90.   0.]
    """
    if not isinstance(angles, np.ndarray) and not isinstance(angles, list):
        raise ValueError("Type Error: The angles must be a numpy array or a list.")

    angles = np.array(angles) if isinstance(angles, list) else angles

    return np.mod(angles + 180.0, 360.0) - 180.0


def wrap2pi(angles: Union[Iterable[float], np.ndarray]) -> np.ndarray:
    """
    Wraps an input of angles between 0 and 2pi radians.

    Args:
        angles (Union[Iterable[float], np.ndarray]): The angles in radians as a numpy array or a list.

    Returns:
        np.ndarray: The wrapped angles in radians between 0 and 2pi as a numpy array.

    Raises:
        ValueError: If the input angles are not a numpy array or a list

    Examples:
        >>> angles = np.array([-np.pi/6, 0, np.pi/2, 5*np.pi])
        >>> wrapped_angles = wrap2pi(angles)
        >>> print(wrapped_angles)
        [5.7595865 0.         1.57079633 3.1415927]
    """
    if not isinstance(angles, np.ndarray) and not isinstance(angles, list):
        raise ValueError("Type Error: The angles must be a numpy array or a list.")

    angles = np.array(angles) if isinstance(angles, list) else angles

    return np.mod(angles, 2 * np.pi)


def wrap1pi(angles: Union[Iterable[float], np.ndarray]) -> np.ndarray:
    """
    Wraps an input of angles between -pi and pi radians.

    Args:
        angles (Union[Iterable[float], np.ndarray]): The angles in radians as a numpy array or a list.

    Returns:
        np.ndarray: The wrapped angles in radians between -pi and pi as a numpy array.

    Raises:
        ValueError: If the input angles are not a numpy array or a list.

    Examples:
        >>> angles = np.array([-2*np.pi, -3*np.pi/2, -np.pi, 0, np.pi, 3*np.pi/2, 2*np.pi])
        >>> wrapped_angles = wrap1pi(angles)
        >>> print(wrapped_angles)
        [  0.          1.57079633 3.14159265  0.         -3.14159265 -1.57079633  0.          ]
    """
    if not isinstance(angles, np.ndarray) and not isinstance(angles, list):
        raise ValueError("Type Error: The angles must be a numpy array or a list.")

    angles = np.array(angles) if isinstance(angles, list) else angles

    return np.mod(angles + np.pi, 2 * np.pi) - np.pi


def unwrap2pi(angles: Union[Iterable[float], np.ndarray]) -> np.ndarray:
    """
    Unwraps an input of angles wrapped between 0 and 2pi radians.

    Args:
        angles (Union[Iterable[float], np.ndarray]): The angles in radians as a numpy array or a list.

    Returns:
        np.ndarray: The unwrapped angles in radians as a numpy array.

    Raises:
        ValueError: If the input angles are not a numpy array or a list.

    Examples:
        >>> angles = np.array([np.pi/2, np.pi, 3*np.pi/2, 2*np.pi, np.pi/2, np.pi, 3*np.pi/2, 2*np.pi])
        >>> unwrapped_angles = unwrap2pi(angles)
        >>> print(unwrapped_angles)
        [1.57079633 3.14159265 4.71238898 6.28318531 7.85398163 9.42477796 10.99557429 12.56637061]

    Notes:
        The unwrapping is done using the numpy unwrap function.
        By default, if the discontinuity between angles is greater than pi radians, no unwrapping is done.
    """
    if not isinstance(angles, np.ndarray) and not isinstance(angles, list):
        raise ValueError("Type Error: The angles must be a numpy array or a list.")

    angles = np.array(angles) if isinstance(angles, list) else angles

    # Unwrap each column in the array
    unwrapped = np.unwrap(angles, axis=0) if angles.ndim > 1 else np.unwrap(angles)

    return unwrapped


def unwrap1pi(angles: Union[Iterable[float], np.ndarray]) -> np.ndarray:
    """
    Unwraps an input of angles wrapped between -pi and pi radians.

    Args:
        angles (Union[Iterable[float], np.ndarray]): The angles in radians as a numpy array or a list.

    Returns:
        np.ndarray: The unwrapped angles in radians as a numpy array.

    Raises:
        ValueError: If the input angles are not a numpy array or a list.

    Examples:
        >>> angles = np.array([-3*np.pi/2, -np.pi, -np.pi/2, 0, np.pi/2, np.pi, 3*np.pi/2, 2*np.pi])
        >>> unwrapped_angles = unwrap1pi(angles)
        >>> print(unwrapped_angles)
        [-4.71238898 -3.14159265 -1.57079633  0.          1.57079633  3.14159265  4.71238898  6.28318531]

    Notes:
        The unwrapping is done using the numpy unwrap function.
        By default, if the discontinuity between angles is greater than pi/2 radians, no unwrapping is done.
    """
    if not isinstance(angles, np.ndarray) and not isinstance(angles, list):
        raise ValueError("Type Error: The angles must be a numpy array or a list.")

    angles = np.array(angles) if isinstance(angles, list) else angles

    # Unwrap each column in the array
    unwrapped = np.unwrap(angles, axis=0, period=np.pi) if angles.ndim > 1 else np.unwrap(angles, period=np.pi)

    return unwrapped


def unwrap360(angles: Union[Iterable[float], np.ndarray]) -> np.ndarray:
    """
    Unwraps an input of angles wrapped between 0 and 360 degrees.

    Args:
        angles (Union[Iterable[float], np.ndarray]): The angles in degrees as a numpy array or a list.

    Returns:
        np.ndarray: The unwrapped angles in degrees as a numpy array.

    Raises:
        ValueError: If the input angles are not a numpy array or a list.

    Examples:
        >>> angles = np.array([30, 90, 180, 270, 360, 30, 90, 180, 270, 360])
        >>> unwrapped_angles = unwrap360(angles)
        >>> print(unwrapped_angles)
        [ 30.  90. 180. 270. 360. 390. 450. 540. 630. 720.]

    Notes:
        The unwrapping is done using the numpy unwrap function.
        By default, if the discontinuity between angles is greater than 180 degrees, no unwrapping is done.
    """
    if not isinstance(angles, np.ndarray) and not isinstance(angles, list):
        raise ValueError("Type Error: The angles must be a numpy array or a list.")

    angles = np.array(angles) if isinstance(angles, list) else angles

    # Unwrap each column in the array
    unwrapped = np.unwrap(angles, axis=0, period=360.0) if angles.ndim > 1 else np.unwrap(angles, period=360.0)

    return unwrapped


def unwrap180(angles: Union[Iterable[float], np.ndarray]) -> np.ndarray:
    """
    Unwraps an input of angles wrapped between -180 and 180 degrees.

    Args:
        angles (Union[Iterable[float], np.ndarray]): The angles in degrees as a numpy array or a list.

    Returns:
        np.ndarray: The unwrapped angles in degrees as a numpy array.

    Raises:
        ValueError: If the input angles are not a numpy array or a list.

    Examples:
        >>> angles = np.array([45, 90, 135, 180, 45, 90, 135, 180])
        >>> unwrapped_angles = unwrap180(angles)
        >>> print(unwrapped_angles)
        [ 45.  90. 135. 180. 225. 270. 315. 360.]

    Notes:
        The unwrapping is done using the numpy unwrap function.
        By default, if the discontinuity between angles is greater than 90 degrees, no unwrapping is done.
    """
    if not isinstance(angles, np.ndarray) and not isinstance(angles, list):
        raise ValueError("Type Error: The angles must be a numpy array or a list.")

    angles = np.array(angles) if isinstance(angles, list) else angles

    # Unwrap each column in the array
    unwrapped = np.unwrap(angles, axis=0, period=180.0) if angles.ndim > 1 else np.unwrap(angles, period=180.0)

    return unwrapped


def wrapunwrap(angles: Union[Iterable[float], np.ndarray]) -> np.ndarray:
    """
    Wraps an input of angle between -pi and pi radians and then unwraps it.

    Args:
        angles (Union[Iterable[float], np.ndarray]): The angles in radians as a numpy array or a list.

    Returns:
        np.ndarray: The wrapped and unwrapped angles in radians as a numpy array.

    Raises:
        ValueError: If the input angles are not a numpy array or a list.

    Examples:
        >>> angles = np.array([-3*np.pi/2, -np.pi, -np.pi/2, 0, np.pi/2, np.pi, 3*np.pi/2, 2*np.pi])
        >>> wrapped_unwrapped_angles = wrapunwrap(angles)
        >>> print(wrapped_unwrapped_angles)
        [-4.71238898 -3.14159265 -1.57079633  0.          1.57079633  3.14159265  4.71238898  6.28318531]
    """
    if not isinstance(angles, np.ndarray) and not isinstance(angles, list):
        raise ValueError("Type Error: The angles must be a numpy array or a list.")

    angles = np.array(angles) if isinstance(angles, list) else angles

    return unwrap1pi(wrap1pi(angles))


def wrapunwrap360(angles: Union[Iterable[float], np.ndarray]) -> np.ndarray:
    """
    Wraps an input of angle between 0 and 360 degrees and then unwraps it.

    Args:
        angles (Union[Iterable[float], np.ndarray]): The angles in degrees as a numpy array or a list.

    Returns:
        np.ndarray: The wrapped and unwrapped angles in degrees as a numpy array.

    Raises:
        ValueError: If the input angles are not a numpy array or a list.

    Examples:
        >>> angles = np.array([30, 90, 180, 270, 360, 390, 450, 540, 630, 720])
        >>> wrapped_unwrapped_angles = wrapunwrap360(angles)
        >>> print(wrapped_unwrapped_angles)
        [ 30.  90. 180. 270. 360. 390. 450. 540. 630. 720.]
    """
    if not isinstance(angles, np.ndarray) and not isinstance(angles, list):
        raise ValueError("Type Error: The angles must be a numpy array or a list.")

    angles = np.array(angles) if isinstance(angles, list) else angles

    return unwrap360(wrap360(angles))
