# -*- coding: utf-8 -*-
import logging

logger = logging.getLogger(__name__)

# from the book ISBN: 1-55860-732-3

EPSILON = 1e-9


# // Clamp n to lie within the range [min, max]
# float Clamp(float n, float min, float max) {
# if (n < min) return min;
# if (n > max) return max;
# return n;
# }
def clamp(n, min_value, max_value):
    if n < min_value:
        return min_value
    if n > max_value:
        return max_value
    return n


def closest_point_segment_segment(p1, q1, p2, q2):
    """
    # // Computes closest points C1 and C2 of S1(s)=P1+s*(Q1-P1) and
    # // S2(t)=P2+t*(Q2-P2), returning s and t. Function result is squared
    # // distance between S1(s) and S2(t)
    """
    # float ClosestPtSegmentSegment(Point p1, Point q1, Point p2, Point q2, float &s, float &t, Point &c1, Point &c2)
    # {
    # Vector d1 = q1 - p1; // Direction vector of segment S1
    d1 = q1 - p1
    # Vector d2 = q2 - p2; // Direction vector of segment S2
    d2 = q2 - p2
    # Vector r = p1 - p2;
    r = p1 - p2
    # float a = Dot(d1, d1); // Squared length of segment S1, always nonnegative
    a = d1.dot(d1)
    # float e = Dot(d2, d2); // Squared length of segment S2, always nonnegative
    e = d2.dot(d2)
    # float f = Dot(d2, r); // Check if either or both segments degenerate into points
    f = d2.dot(r)
    # if (a <= EPSILON && e <= EPSILON) {
    if a <= EPSILON and e <= EPSILON:
        # // Both segments degenerate into points
        # s = t = 0.0f;
        s = 0.0
        t = 0.0
        # c1 = p1;
        # c2 = p2;
        c1 = p1
        c2 = p2
        # return Dot(c1 - c2, c1 - c2);
        return (c1 - c2).dot(c1 - c2), s, t, c1, c2
    # }
    # if (a <= EPSILON) {
    if a <= EPSILON:
        # // First segment degenerates into a point
        # s = 0.0f;
        s = 0.0
        # t = f / e; // s = 0 => t = (b*s + f) / e = f / e
        t = f / e
        # t = Clamp(t, 0.0f, 1.0f);
        t = clamp(t, 0.0, 1.0)
    # } else {
    else:
        # float c = Dot(d1, r);
        c = d1.dot(r)
        # if (e <= EPSILON) {
        if e <= EPSILON:
            # // Second segment degenerates into a point
            # t = 0.0f;
            t = 0.0
            # s = Clamp(-c / a, 0.0f, 1.0f); // t = 0 => s = (b*t - c) / a = -c / a
            s = clamp(-c / a, 0.0, 1.0)
        # } else {
        else:
            # // The general nondegenerate case starts here
            # float b = Dot(d1, d2);
            b = d1.dot(d2)
            # float denom = a*e-b*b; // Always nonnegative
            denom = a * e - b * b
            # // If segments not parallel, compute the closest point on L1 to L2 and
            # // clamp to segment S1. Else pick arbitrary s (here 0)
            # if (denom != 0.0f) {
            if denom != 0.0:
                # s = Clamp((b*f - c*e) / denom, 0.0f, 1.0f);
                s = clamp((b * f - c * e) / denom, 0.0, 1.0)
            # } else s = 0.0f;
            else:
                s = 0.0
            # // Compute point on L2 closest to S1(s) using
            # // t = Dot((P1 + D1*s) - P2,D2) / Dot(D2,D2) = (b*s + f) / e
            # t = (b*s + f) / e;
            t = (b * s + f) / e
            # // If t in [0,1] done. Else clamp t, recompute s for the new value
            # // of t using s = Dot((P2 + D2*t) - P1,D1) / Dot(D1,D1)= (t*b - c) / a
            # // and clamp s to [0, 1]
            # if (t < 0.0f) {
            if t < 0.0:
                # t = 0.0f;
                t = 0.0
                # s = Clamp(-c / a, 0.0f, 1.0f);
                s = clamp(-c / a, 0.0, 1.0)
            # } else if (t > 1.0f) {
            elif t > 1.0:
                # t = 1.0f;
                t = 1.0
                # s = Clamp((b - c) / a, 0.0f, 1.0f);
                s = clamp((b - c) / a, 0.0, 1.0)
            # }
        # }
    # }
    # c1 = p1 + d1 * s;
    c1 = p1 + d1 * s
    # c2 = p2 + d2 * t;
    c2 = p2 + d2 * t
    # return Dot(c1 - c2, c1 - c2);
    # }
    return (c1 - c2).dot(c1 - c2), s, t, c1, c2


if __name__ == '__main__':
    from pygame import Vector3 as Vec

    p1 = Vec(0, 0, 0)
    p2 = Vec(1, 1, 1)
    q1 = Vec(1, 0, 0)
    q2 = Vec(0, 1, 1)
    dist, s, t, c1, c2 = closest_point_segment_segment(p1, p2, q1, q2)
    print(dist, s, t, c1, c2)

    p1 = Vec(0, 0, 0)
    p2 = Vec(1, 0, 1)
    q1 = Vec(0, 1, 0)
    q2 = Vec(0, 1, 1)
    dist, s, t, c1, c2 = closest_point_segment_segment(p1, p2, q1, q2)
    print(dist, s, t, c1, c2)
