# geometry_helpers.py
# Author: Nosa Edoimioya
# Description: Geometry helper functions to generate DH parameters based on robot urdf file.
# Version: 0.1
# Date: 03-27-2024
# Based on https://github.com/mcevoyandy/urdf_to_dh
import numpy as np

EPSILON = 1.0e-5


def are_parallel(vec1: np.ndarray, vec2: np.ndarray) -> bool:
    """Determine whether two vectors are parallel.

    Args:
        vec1: First vector.
        vec2: Second vector.

    Returns:
        `bool` indicating whether the vectors are parallel.

    Side Effects:
        None.

    Raises:
        None.

    Preconditions:
        `vec1` and `vec2` are non-zero vectors.
    """
    vec1_unit = vec1 / np.linalg.norm(vec1)
    vec2_unit = vec2 / np.linalg.norm(vec2)

    return bool(np.all(abs(np.cross(vec1_unit, vec2_unit)) < EPSILON))


def are_collinear(
    point1: np.ndarray,
    vec1: np.ndarray,
    point2: np.ndarray,
    vec2: np.ndarray,
) -> bool:
    """Determine whether two vectors are collinear.

    Args:
        point1: A point on the first line.
        vec1: Direction vector of the first line.
        point2: A point on the second line.
        vec2: Direction vector of the second line.

    Returns:
        `bool` indicating whether the lines are collinear.

    Side Effects:
        None.

    Raises:
        None.

    Preconditions:
        `vec1` and `vec2` are non-zero vectors.
    """

    # To be collinear, vectors must be parallel
    if not are_parallel(vec1, vec2):
        return False

    # If parallel and point1 is coincident with point2, vectors are collinear
    if all(np.isclose(point1, point2)):
        return True

    # If vectors are parallel, point2 can be defined as p2 = p1 + t * v1
    t = np.zeros(3)
    for idx in range(0, 3):
        if vec1[idx] != 0:
            t[idx] = (point2[idx] - point1[idx]) / vec1[idx]
    p2 = point1 + t * vec1

    return np.allclose(p2, point2)


def lines_intersect(
    point1: np.ndarray,
    vec1: np.ndarray,
    point2: np.ndarray,
    vec2: np.ndarray,
) -> tuple[bool, np.ndarray]:
    """Determine whether two lines intersect.

    Args:
        point1: A point on the first line.
        vec1: Direction vector of the first line.
        point2: A point on the second line.
        vec2: Direction vector of the second line.

    Returns:
        `tuple[bool, np.ndarray]` with a flag indicating intersection and the
        solved coefficients.

    Side Effects:
        None.

    Raises:
        None.

    Preconditions:
        `vec1` and `vec2` are non-zero vectors.
    """
    epsilon = 1e-6
    x = np.zeros(2)

    if are_collinear(point1, vec1, point2, vec2):
        return False, x

    # If lines are parallel, lines don't intersect
    if are_parallel(vec1, vec2):
        return False, x

    # Test if lines intersect. Need to find non-singular pair to solve for coefficients
    for idx in range(0, 3):
        i = idx
        j = (idx + 1) % 3
        A = np.array([[vec1[i], -vec2[i]], [vec1[j], -vec2[j]]])
        b = np.array([[point2[i] - point1[i]], [point2[j] - point1[j]]])

        # If singular matrix, go to next set
        if np.isclose(np.linalg.det(A), 0):
            continue
        else:
            x = np.linalg.solve(A, b)

            # Test if solution generates a point of intersection
            p1 = point1 + x[0] * vec1
            p2 = point2 + x[1] * vec2

            if all(np.less(np.abs(p1 - p2), epsilon * np.ones(3))):
                return True, x

    return False, x
