# coding=utf-8
"""Utility functions for computing intersections between geometry in 2D space.

Taken mostly from the euclid package available at
https://pypi.org/project/euclid/
"""
from __future__ import division
import math

from .geometry2d.pointvector import Point2D, Vector2D


def _isclose(a, b, rel_tol=1e-09, abs_tol=1e-09):
    """Implementation of the math.isclose method from Python 3.5 onward."""
    return abs(a - b) <= max(rel_tol * max(abs(a), abs(b)), abs_tol)


def intersect_line2d(line_ray_a, line_ray_b):
    """Get the intersection between any Ray2D or LineSegment2D objects as a Point2D.

    This function calculates scaling parameters for ua and ub where:
        A.p + ua * A.v = B.p + ub * B.v
    Which represents the intersection point between line A and line B.

    The derivation of ua is achieved by crossing both sides of the above equation
    with the direction vector of B, and rearranging the formula:

    .. code-block:: python

        A.p + ua * A.v = B.p + ub * B.v
        (A.p + ua * A.v) x B.v = (B.p + ub * B.v) x B.v # Cross both sides with B.v
        (A.p x B.v) + (ua * A.v x B.v) = (B.p x B.v) + (ub * B.v x B.v) # B.v x B.v = 0
        ua = (B.p - A.p) x B.v / (A.v x B.v)

    Args:
        line_ray_a: A LineSegment2D or Ray2D object.
        line_ray_b: Another LineSegment2D or Ray2D to intersect.

    Returns:
        Point2D of intersection if it exists. None if no intersection exists.
    """
    # d is the determinant between lines, if 0 lines are collinear
    d = line_ray_b.v.y * line_ray_a.v.x - line_ray_b.v.x * line_ray_a.v.y
    if d == 0:
        return None

    # (dx, dy) = A.p - B.p
    dy = line_ray_a.p.y - line_ray_b.p.y
    dx = line_ray_a.p.x - line_ray_b.p.x

    # Find parameters ua and ub for intersection between two lines

    # Calculate scaling parameter for line_ray_b
    ua = (line_ray_b.v.x * dy - line_ray_b.v.y * dx) / d
    # Checks the bounds of ua to ensure it obeys ray/line behavior
    if not line_ray_a._u_in(ua):
        return None

    # Calculate scaling parameter for line_ray_b
    ub = (line_ray_a.v.x * dy - line_ray_a.v.y * dx) / d
    # Checks the bounds of ub to ensure it obeys ray/line behavior
    if not line_ray_b._u_in(ub):
        return None

    return Point2D(line_ray_a.p.x + ua * line_ray_a.v.x,
                   line_ray_a.p.y + ua * line_ray_a.v.y)


def intersect_line_segment2d(line_a, line_b):
    """Get the intersection between two LineSegment2D objects as a Point2D.

    This function is identical to intersect_line2d but has some extra checks to
    avoid certain cases of floating point tolerance issues. It is only intended
    to work with LineSegment2D and not Ray2D.

    Args:
        line_a: A LineSegment2D object.
        line_b: Another LineSegment2D intersect.

    Returns:
        Point2D of intersection if it exists. None if no intersection exists.
    """
    # d is the determinant between lines, if 0 lines are collinear
    d = line_b.v.y * line_a.v.x - line_b.v.x * line_a.v.y
    if d == 0:
        return None

    # (dx, dy) = A.p - B.p
    dy = line_a.p.y - line_b.p.y
    dx = line_a.p.x - line_b.p.x

    # Find parameters ua and ub for intersection between two lines

    # Calculate scaling parameter for line_b
    ua = (line_b.v.x * dy - line_b.v.y * dx) / d
    # Checks the bounds of ua to ensure it obeys ray/line behavior
    if not line_a._u_in(ua):
        return None

    # Calculate scaling parameter for line_b
    ub = (line_a.v.x * dy - line_a.v.y * dx) / d
    # Checks the bounds of ub to ensure it obeys ray/line behavior
    if not line_b._u_in(ub):
        return None

    # compute the intersection point
    int_pta = Point2D(line_a.p.x + ua * line_a.v.x, line_a.p.y + ua * line_a.v.y)
    int_ptb = Point2D(line_b.p.x + ub * line_b.v.x, line_b.p.y + ub * line_b.v.y)

    # if the two points are unequal, there's a floating point tolerance issue
    if _isclose(int_pta.x, int_ptb.x) and _isclose(int_pta.y, int_ptb.y):
        return int_pta
    return None


def intersect_line2d_infinite(line_ray_a, line_ray_b):
    """Get intersection between a Ray2D/LineSegment2D and another extended infinitely.

    Args:
        line_ray_a: A LineSegment2D or Ray2D object.
        line_ray_b: A LineSegment2D or Ray2D that will be extended infinitely
            for intersection.

    Returns:
        Point2D of intersection if it exists. None if no intersection exists.
    """
    d = line_ray_b.v.y * line_ray_a.v.x - line_ray_b.v.x * line_ray_a.v.y
    if d == 0:
        return None
    dy = line_ray_a.p.y - line_ray_b.p.y
    dx = line_ray_a.p.x - line_ray_b.p.x
    ua = (line_ray_b.v.x * dy - line_ray_b.v.y * dx) / d
    if not line_ray_a._u_in(ua):
        return None
    return Point2D(line_ray_a.p.x + ua * line_ray_a.v.x,
                   line_ray_a.p.y + ua * line_ray_a.v.y)


def does_intersection_exist_line2d(line_ray_a, line_ray_b):
    """Boolean denoting whether an intersection exists between Ray2D or LineSegment2D.

    This is slightly faster than actually computing the intersection but should only be
    used in cases where the actual point of intersection is not needed.

    Args:
        line_ray_a: A LineSegment2D or Ray2D object.
        line_ray_b: Another LineSegment2D or Ray2D to intersect.

    Returns:
        True if an intersection exists. False if no intersection exists.
    """
    d = line_ray_b.v.y * line_ray_a.v.x - line_ray_b.v.x * line_ray_a.v.y
    if d == 0:
        return False
    dy = line_ray_a.p.y - line_ray_b.p.y
    dx = line_ray_a.p.x - line_ray_b.p.x
    ua = (line_ray_b.v.x * dy - line_ray_b.v.y * dx) / d
    if not line_ray_a._u_in(ua):
        return False
    ub = (line_ray_a.v.x * dy - line_ray_a.v.y * dx) / d
    if not line_ray_b._u_in(ub):
        return False
    return True


def intersect_line2d_arc2d(line_ray, arc):
    """Get the intersection between any Ray2D/LineSegment2D and an Arc2D.

    Args:
        line_ray: A LineSegment2D or Ray2D object.
        arc: An Arc2D object along which the closest point will be determined.

    Returns:
        A list of 2 Point2D objects if a full intersection exists.
        A list with a single Point2D object if the line is tangent or intersects
        only once. None if no intersection exists.
    """
    a = line_ray.v.magnitude_squared
    b = 2 * (line_ray.v.x * (line_ray.p.x - arc.c.x) +
             line_ray.v.y * (line_ray.p.y - arc.c.y))
    c = arc.c.magnitude_squared + line_ray.p.magnitude_squared - \
        2 * arc.c.dot(line_ray.p) - arc.r ** 2
    det = b ** 2 - 4 * a * c
    if det < 0:
        return None
    sq = math.sqrt(det)
    u1 = (-b + sq) / (2 * a)
    u2 = (-b - sq) / (2 * a)
    pt1 = Point2D(line_ray.p.x + u1 * line_ray.v.x,
                  line_ray.p.y + u1 * line_ray.v.y) if line_ray._u_in(u1) else None
    pt2 = Point2D(line_ray.p.x + u2 * line_ray.v.x,
                  line_ray.p.y + u2 * line_ray.v.y) if line_ray._u_in(u2) else None

    if u1 == u2:  # Tangent
        pt = Point2D(line_ray.p.x + u1 * line_ray.v.x,
                     line_ray.p.y + u1 * line_ray.v.y)
        return pt if arc._pt_in(pt) else None

    pts = [p for p in (pt1, pt2) if p is not None and arc._pt_in(p)]
    return pts if len(pts) != 0 else None


def intersect_line2d_infinite_arc2d(line_ray, arc):
    """Get intersection between an Arc2D and a Ray2D/LineSegment2D extended infinitely.

    Args:
        line_ray: A LineSegment2D or Ray2D that will be extended infinitely
            for intersection.
        arc: An Arc2D object along which the closest point will be determined.

    Returns:
        A list of 2 Point2D objects if a full intersection exists.
        A list with a single Point2D object if the line is tangent or intersects
        only once. None if no intersection exists.
    """
    a = line_ray.v.magnitude_squared
    b = 2 * (line_ray.v.x * (line_ray.p.x - arc.c.x) +
             line_ray.v.y * (line_ray.p.y - arc.c.y))
    c = arc.c.magnitude_squared + line_ray.p.magnitude_squared - \
        2 * arc.c.dot(line_ray.p) - arc.r ** 2
    det = b ** 2 - 4 * a * c
    if det < 0:
        return None
    sq = math.sqrt(det)
    u1 = (-b + sq) / (2 * a)
    u2 = (-b - sq) / (2 * a)

    if u1 == u2:  # Tangent
        pt = Point2D(line_ray.p.x + u1 * line_ray.v.x, line_ray.p.y + u1 * line_ray.v.y)
        return pt if arc._pt_in(pt) else None

    pt1 = Point2D(line_ray.p.x + u1 * line_ray.v.x, line_ray.p.y + u1 * line_ray.v.y)
    pt2 = Point2D(line_ray.p.x + u2 * line_ray.v.x, line_ray.p.y + u2 * line_ray.v.y)
    pts = [p for p in (pt1, pt2) if arc._pt_in(p)]
    return pts if len(pts) != 0 else None


def closest_point2d_on_line2d(point, line_ray):
    """Get the closest Point2D on a LineSegment2D or Ray2D to the input point.

    Args:
        point: A Point2D object.
        line_ray: A LineSegment2D or Ray2D object along which the closest point
            will be determined.

    Returns:
        Point2D for the closest point on the line_ray to point.
    """
    d = line_ray.v.magnitude_squared
    if d == 0:  # zero-length segment; just return the end point
        return line_ray.p
    u = ((point.x - line_ray.p.x) * line_ray.v.x +
         (point.y - line_ray.p.y) * line_ray.v.y) / d
    if not line_ray._u_in(u):
        u = max(min(u, 1.0), 0.0)
    return Point2D(line_ray.p.x + u * line_ray.v.x, line_ray.p.y + u * line_ray.v.y)


def closest_point2d_on_line2d_infinite(point, line_ray):
    """Get the closest Point2D on a Ray2D/LineSegment2D extended infinitely.

    Args:
        point: A Point2D object.
        line_ray: A LineSegment2D or Ray2D object that will be extended infinitely
            to determine where the closest point lies.

    Returns:
        Point2D for the closest point on the line_ray to point.
    """
    d = line_ray.v.magnitude_squared
    if d == 0:  # zero-length segment; just return the end point
        return line_ray.p
    u = ((point.x - line_ray.p.x) * line_ray.v.x +
         (point.y - line_ray.p.y) * line_ray.v.y) / d
    return Point2D(line_ray.p.x + u * line_ray.v.x, line_ray.p.y + u * line_ray.v.y)


def closest_point2d_between_line2d(line_ray_a, line_ray_b):
    """Get the two closest Point2D between two LineSegment2D objects.

    When the closest point on one of the segments lies in the middle of the
    segment, this will be accounted for. Also note that the line segments
    should not intersect for the result to be valid.

    Args:
        line_ray_a: A LineSegment2D object.
        line_ray_b: Another LineSegment2D to which closest points will
            be determined.

    Returns:
        A tuple with two elements

        - dists[0]: The distance between the two LineSegment2D objects.
        - pts[0]: A tuple of two Point2D objects representing:

        1) The point on line_ray_a that is closest to line_ray_b
        2) The point on line_ray_b that is closest to line_ray_a
    """
    # one of the 4 endpoints must be a closest point
    pt_1 = closest_point2d_on_line2d(line_ray_a.p, line_ray_b)
    dist_1 = pt_1.distance_to_point(line_ray_a.p)
    a_p2 = line_ray_a.p2
    pt_2 = closest_point2d_on_line2d(a_p2, line_ray_b)
    dist_2 = pt_2.distance_to_point(a_p2)
    pt_3 = closest_point2d_on_line2d(line_ray_b.p, line_ray_a)
    dist_3 = pt_3.distance_to_point(line_ray_b.p)
    b_p2 = line_ray_b.p2
    pt_4 = closest_point2d_on_line2d(b_p2, line_ray_a)
    dist_4 = pt_4.distance_to_point(b_p2)

    # sort the closest points based on their distance
    dists = [dist_1, dist_2, dist_3, dist_4]
    pts = [(line_ray_a.p, pt_1), (a_p2, pt_2), (pt_3, line_ray_b.p), (pt_4, b_p2)]
    dists, i = zip(*sorted(zip(dists, range(len(pts)))))
    return dists[0], pts[i[0]]


def closest_end_point2d_between_line2d(line_a, line_b):
    """Get the two closest end Point2Ds between two LineSegment2D objects.

    The result will always be composed of endpoints of the segments and will not
    account for cases where the closest point lies in the middle of a segment.
    For cases where such middle points are important, the
    closest_point2d_between_line2d method should be used.

    Args:
        line_a: A LineSegment2D object.
        line_b: Another LineSegment2D to which closest points will
            be determined.

    Returns:
        A tuple with two elements

        - dist: The distance between the two endpoints objects.
        - pts: A tuple of two Point2D objects representing:

        1) The end point on line_a that is closest to line_b
        2) The end point on line_b that is closest to line_a
    """
    # one of the 4 endpoints must be a closest point
    pts = [
        (line_a.p1, line_b.p1), (line_a.p1, line_b.p2),
        (line_a.p2, line_b.p1), (line_a.p2, line_b.p2)
    ]
    dists = [p1.distance_to_point(p2) for p1, p2 in pts]
    # sort the closest points based on their distance
    dists, i = zip(*sorted(zip(dists, range(len(pts)))))
    return dists[0], pts[i[0]]


def closest_point2d_on_arc2d(point, arc):
    """Get the closest Point2D on a Arc2D to the input point.

    Args:
        point: A Point2D object.
        arc: An Arc2D object along which the closest point will be determined.

    Returns:
        Point2D for the closest point on arc to point.
    """
    v = point - arc.c
    v = v.normalize() * arc.r
    if arc.is_circle:
        return Point2D(arc.c.x + v.x, arc.c.y + v.y)
    else:
        a = Vector2D(1, 0).angle_counterclockwise(v)
        if (not arc.is_inverted and arc.a1 < a < arc.a2) or \
                (arc.is_inverted and arc.a1 > a > arc.a2):
            return Point2D(arc.c.x + v.x, arc.c.y + v.y)
        else:
            if arc.p1.distance_to_point(point) <= arc.p2.distance_to_point(point):
                return arc.p1
    return arc.p2
