import numpy as np
from numbers import Number


def _auxillary_func_one(z: Number) -> Number:
    return (1 - 0.5 * z * _cot(0.5 * z)) / (z ** 2)


def _auxillary_func_two(z: Number, rho: Number) -> Number:
    return (
        0.25 * rho * ((z * _csc(0.5 * z)) ** 2 + 2 * z * _cot(0.5 * z) - 8) / (z ** 4)
    )


def _cot(z):
    return 1 / np.tan(z)


def _csc(z):
    return 1 / np.sin(z)


def dexpinv_se3(x: np.ndarray, y: np.ndarray) -> np.ndarray:
    """Calculate $\mathop{dexpinv}_{x}^{-1} (y)$ and return the result.
    Both x and y must be elements of the Lie algebra se(3) represented as
    one-dimensional arrays of length 6. If this does not suit your implementation,
    feel free to modify the source code accordingly.

    Note that this method does not have any safe-guard against singularities that
    occur when the norm of the first three components of the x-argument becomes
    too small. To remedy this, it may be desirable to first check this value
    and if necessary, use a truncated series instead.

    Parameters
    ----------
    x : np.ndarray
        numpy array of length 6, element of se(3).
    y : np.ndarray
        numpy array of length 6, element of se(3).

    Returns
    -------
    np.ndarray
        The value of dexpinv_x y represented as an array of length 6.

    Raises
    ------
    TypeError
        Occurs when either x or y is not a numpy array.
    ValueError
        Occurs when either x or y is not of the appropriate length.
    """

    if not isinstance(x, np.ndarray) or not isinstance(y, np.ndarray):
        raise TypeError("x and y must be numpy arrays")

    if x.shape != (6,) or y.shape != (6,):
        raise ValueError("x and y must be one-dimensional arrays of length 6")

    A, a = np.split(x, 2)
    B, b = np.split(y, 2)
    alpha = np.linalg.norm(A)
    rho = np.inner(A, a)
    c1 = (
        B
        - 0.5 * np.cross(A, B)
        + _auxillary_func_one(alpha) * np.cross(A, np.cross(A, B))
    )
    c2 = (
        b
        - 0.5 * (np.cross(a, B) + np.cross(A, b))
        + _auxillary_func_two(alpha, rho) * np.cross(A, np.cross(A, B))
        + _auxillary_func_one(alpha)
        * (
            np.cross(a, np.cross(A, B))
            + np.cross(A, np.cross(a, B))
            + np.cross(A, np.cross(A, b))
        )
    )
    return np.hstack((c1, c2))