# From Geometric Tools Engine
# https://www.geometrictools.com/GTEngine/Include/Mathematics/GteDistSegmentSegment.h

import numpy as np

def SegmentsClosestPoints(P0, P1, Q0, Q1):
    P0 = np.asarray(P0)
    P1 = np.asarray(P1)
    Q0 = np.asarray(Q0)
    Q1 = np.asarray(Q1)

    # The code allows degenerate line segments; that is, P0 and P1 can be
    # the same point or Q0 and Q1 can be the same point.  The quadratic
    # function for squared distance between the segment is
    #   R(s,t) = a*s^2 - 2*b*s*t + c*t^2 + 2*d*s - 2*e*t + f
    # for (s,t) in [0,1]^2 where
    #   a = Dot(P1-P0,P1-P0), b = Dot(P1-P0,Q1-Q0), c = Dot(Q1-Q0,Q1-Q0),
    #   d = Dot(P1-P0,P0-Q0), e = Dot(Q1-Q0,P0-Q0), f = Dot(P0-Q0,P0-Q0)
    P1mP0 = P1 - P0
    Q1mQ0 = Q1 - Q0
    P0mQ0 = P0 - Q0
    mA = P1mP0 @ P1mP0
    mB = P1mP0 @ Q1mQ0
    mC = Q1mQ0 @ Q1mQ0
    mD = P1mP0 @ P0mQ0
    mE = Q1mQ0 @ P0mQ0

    mF00 = mD
    mF10 = mF00 + mA
    mF01 = mF00 - mB
    mF11 = mF10 - mB

    mG00 = -mE
    mG10 = mG00 - mB
    mG01 = mG00 + mC
    mG11 = mG10 + mC

    parameter = [0., 0.]
    if mA > 0. and mC > 0.:
        # Compute the solutions to dR/ds(s0,0) = 0 and dR/ds(s1,1) = 0.  The
        # location of sI on the s-axis is stored in classifyI (I = 0 or 1).  If
        # sI <= 0, classifyI is -1.  If sI >= 1, classifyI is 1.  If 0 < sI < 1,
        # classifyI is 0.  This information helps determine where to search for
        # the minimum point (s,t).  The fij values are dR/ds(i,j) for i and j in
        # {0,1}.

        sValue = [0., 0.]
        sValue[0] = _GetClampedRoot(mA, mF00, mF10)
        sValue[1] = _GetClampedRoot(mA, mF01, mF11)

        classify = [0, 0]
        for i in range(2):
            if sValue[i] <= 0.:
                classify[i] = -1
            elif sValue[i] >= 1.:
                classify[i] = +1
            else:
                classify[i] = 0

        if classify[0] == -1 and classify[1] == -1:
            # The minimum must occur on s = 0 for 0 <= t <= 1.
            parameter[0] = 0.
            parameter[1] = _GetClampedRoot(mC, mG00, mG01)
        elif classify[0] == +1 and classify[1] == +1:
            # The minimum must occur on s = 1 for 0 <= t <= 1.
            parameter[0] = 1.
            parameter[1] = _GetClampedRoot(mC, mG10, mG11)
        else:
            # The line dR/ds = 0 intersects the domain [0,1]^2 in a
            # nondegenerate segment.  Compute the endpoints of that segment,
            # end[0] and end[1].  The edge[i] flag tells you on which domain
            # edge end[i] lives: 0 (s=0), 1 (s=1), 2 (t=0), 3 (t=1).
            edge = [0, 0]
            end = [[0., 0.], [0., 0.]]
            _ComputeIntersection(sValue, classify, edge, end,
                                 mB, mF00, mF10)

            # The directional derivative of R along the segment of
            # intersection is
            #   H(z) = (end[1][1]-end[1][0])*dR/dt((1-z)*end[0] + z*end[1])
            # for z in [0,1].  The formula uses the fact that dR/ds = 0 on
            # the segment.  Compute the minimum of H on [0,1].
            _ComputeMinimumParameters(edge, end, parameter,
                                      mB, mC, mE, mG00, mG01, mG10, mG11)
    else:
        if mA > 0.:
            # The Q-segment is degenerate (Q0 and Q1 are the same point) and
            # the quadratic is R(s,0) = a*s^2 + 2*d*s + f and has (half)
            # first derivative F(t) = a*s + d.  The closest P-point is
            # interior to the P-segment when F(0) < 0 and F(1) > 0.
            parameter[0] = _GetClampedRoot(mA, mF00, mF10)
            parameter[1] = 0.
        elif mC > 0.:
            # The P-segment is degenerate (P0 and P1 are the same point) and
            # the quadratic is R(0,t) = c*t^2 - 2*e*t + f and has (half)
            # first derivative G(t) = c*t - e.  The closest Q-point is
            # interior to the Q-segment when G(0) < 0 and G(1) > 0.
            parameter[0] = 0.
            parameter[1] = _GetClampedRoot(mC, mG00, mG01)
        else:
            # P-segment and Q-segment are degenerate.
            parameter[0] = 0.
            parameter[1] = 0.


    return (1. - parameter[0]) * P0 + parameter[0] * P1, (1. - parameter[1]) * Q0 + parameter[1] * Q1


def _GetClampedRoot(slope, h0, h1):
    # Theoretically, r is in (0,1).  However, when the slope is nearly zero,
    # then so are h0 and h1.  Significant numerical rounding problems can
    # occur when using floating-point arithmetic.  If the rounding causes r
    # to be outside the interval, clamp it.  It is possible that r is in
    # (0,1) and has rounding errors, but because h0 and h1 are both nearly
    # zero, the quadratic is nearly constant on (0,1).  Any choice of p
    # should not cause undesirable accuracy problems for the final distance
    # computation.
    #
    # NOTE:  You can use bisection to recompute the root or even use
    # bisection to compute the root and skip the division.  This is generally
    # slower, which might be a problem for high-performance applications.

    r = 0.
    if h0 < 0.:
        if h1 > 0.:
            r = -h0 / slope
            if r > 1.:
                r = 0.5
            # The slope is positive and -h0 is positive, so there is no
            # need to test for a negative value and clamp it.
        else:
            r = 1.
    else:
        r = 0.
    return r


def _ComputeIntersection(sValue, classify, edge, end,
                         mB, mF00, mF10):
    # The divisions are theoretically numbers in [0,1].  Numerical rounding
    # errors might cause the result to be outside the interval.  When this
    # happens, it must be that both numerator and denominator are nearly
    # zero.  The denominator is nearly zero when the segments are nearly
    # perpendicular.  The numerator is nearly zero when the P-segment is
    # nearly degenerate (mF00 = a is small).  The choice of 0.5 should not
    # cause significant accuracy problems.
    #
    # NOTE:  You can use bisection to recompute the root or even use
    # bisection to compute the root and skip the division.  This is generally
    # slower, which might be a problem for high-performance applications.

    if classify[0] < 0:
        edge[0] = 0
        end[0][0] = 0.
        end[0][1] = mF00 / mB
        if end[0][1] < 0 or end[0][1] > 1.:
            end[0][1] = 0.5

        if classify[1] == 0:
            edge[1] = 3
            end[1][0] = sValue[1]
            end[1][1] = 1
        else:  # classify[1] > 0
            edge[1] = 1
            end[1][0] = 1.
            end[1][1] = mF10 / mB
            if end[1][1] < 0. or end[1][1] > 1.:
                end[1][1] = 0.5
    elif classify[0] == 0:
        edge[0] = 2
        end[0][0] = sValue[0]
        end[0][1] = 0.

        if classify[1] < 0:
            edge[1] = 0
            end[1][0] = 0.
            end[1][1] = mF00 / mB
            if end[1][1] < 0. or end[1][1] > 1.:
                end[1][1] = 0.5
        elif classify[1] == 0:
            edge[1] = 3
            end[1][0] = sValue[1]
            end[1][1] = 1.
        else:
            edge[1] = 1
            end[1][0] = 1.
            end[1][1] = mF10 / mB
            if end[1][1] < 0. or end[1][1] > 1.:
                end[1][1] = 0.5
    else:  # classify[0] > 0
        edge[0] = 1
        end[0][0] = 1.
        end[0][1] = mF10 / mB
        if end[0][1] < 0. or end[0][1] > 1.:
            end[0][1] = 0.5

        if classify[1] == 0:
            edge[1] = 3
            end[1][0] = sValue[1]
            end[1][1] = 1.
        else:
            edge[1] = 0
            end[1][0] = 0.
            end[1][1] = mF00 / mB
            if end[1][1] < 0 or end[1][1] > 1.:
                end[1][1] = 0.5


def _ComputeMinimumParameters(edge, end, parameter,
                              mB, mC, mE, mG00, mG01, mG10, mG11):
    delta = end[1][1] - end[0][1]
    h0 = delta * (-mB * end[0][0] + mC * end[0][1] - mE)
    if h0 >= 0.:
        if edge[0] == 0:
            parameter[0] = 0.
            parameter[1] = _GetClampedRoot(mC, mG00, mG01)
        elif edge[0] == 1:
            parameter[0] = 1.
            parameter[1] = _GetClampedRoot(mC, mG10, mG11)
        else:
            parameter[0] = end[0][0]
            parameter[1] = end[0][1]
    else:
        h1 = delta * (-mB * end[1][0] + mC * end[1][1] - mE)
        if h1 <= 0.:
            if edge[1] == 0:
                parameter[0] = 0.
                parameter[1] = _GetClampedRoot(mC, mG00, mG01)
            elif edge[1] == 1:
                parameter[0] = 1.
                parameter[1] = _GetClampedRoot(mC, mG10, mG11)
            else:
                parameter[0] = end[1][0]
                parameter[1] = end[1][1]
        else:  # h0 < 0 and h1 > 0
            z = min(max(h0 / (h0 - h1), 0.), 1.)
            omz = 1. - z
            parameter[0] = omz * end[0][0] + z * end[1][0]
            parameter[1] = omz * end[0][1] + z * end[1][1]
