# Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
# Copyright (C) 2013-2022 German Aerospace Center (DLR) and others.
# This program and the accompanying materials are made available under the
# terms of the Eclipse Public License 2.0 which is available at
# https://www.eclipse.org/legal/epl-2.0/
# This Source Code may also be made available under the following Secondary
# Licenses when the conditions for such availability set forth in the Eclipse
# Public License 2.0 are satisfied: GNU General Public License, version 2
# or later which is available at
# https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
# SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later

# @file    geomhelper.py
# @author  Daniel Krajzewicz
# @author  Jakob Erdmann
# @author  Michael Behrisch
# @author  Matthias Schwamborn
# @date    2013-02-25

from __future__ import absolute_import
import math
import sys

INVALID_DISTANCE = -1

# back-ported from python 3 for backward compatibility
# https://www.python.org/dev/peps/pep-0485/#proposed-implementation


def isclose(a, b, rel_tol=1e-09, abs_tol=0.0):
    return abs(a-b) <= max(rel_tol * max(abs(a), abs(b)), abs_tol)


def distance(p1, p2):
    dx = p1[0] - p2[0]
    dy = p1[1] - p2[1]
    return math.sqrt(dx * dx + dy * dy)


def polyLength(polygon):
    return sum([distance(a, b) for a, b in zip(polygon[:-1], polygon[1:])])


def addToBoundingBox(coordList, bbox=None):
    if bbox is None:
        minX = 1e400
        minY = 1e400
        maxX = -1e400
        maxY = -1e400
    else:
        minX, minY, maxX, maxY = bbox
    for x, y in coordList:
        minX = min(x, minX)
        minY = min(y, minY)
        maxX = max(x, maxX)
        maxY = max(y, maxY)
    return minX, minY, maxX, maxY


def lineOffsetWithMinimumDistanceToPoint(point, line_start, line_end, perpendicular=False):
    """Return the offset from line (line_start, line_end) where the distance to
    point is minimal"""
    p = point
    p1 = line_start
    p2 = line_end
    d = distance(p1, p2)
    u = ((p[0] - p1[0]) * (p2[0] - p1[0])) + ((p[1] - p1[1]) * (p2[1] - p1[1]))
    if d == 0. or u < 0. or u > d * d:
        if perpendicular:
            return INVALID_DISTANCE
        if u < 0.:
            return 0.
        return d
    return u / d


def polygonOffsetAndDistanceToPoint(point, polygon, perpendicular=False):
    """Return the offset and the distance from the polygon start where the distance to the point is minimal"""
    p = point
    s = polygon
    seen = 0
    minDist = 1e400
    minOffset = INVALID_DISTANCE
    for i in range(len(s) - 1):
        pos = lineOffsetWithMinimumDistanceToPoint(
            p, s[i], s[i + 1], perpendicular)
        dist = minDist if pos == INVALID_DISTANCE else distance(
            p, positionAtOffset(s[i], s[i + 1], pos))
        if dist < minDist:
            minDist = dist
            minOffset = pos + seen
        if perpendicular and i != 0 and pos == INVALID_DISTANCE:
            # even if perpendicular is set we still need to check the distance
            # to the inner points
            cornerDist = distance(p, s[i])
            if cornerDist < minDist:
                pos1 = lineOffsetWithMinimumDistanceToPoint(
                    p, s[i - 1], s[i], False)
                pos2 = lineOffsetWithMinimumDistanceToPoint(
                    p, s[i], s[i + 1], False)
                if pos1 == distance(s[i - 1], s[i]) and pos2 == 0.:
                    minOffset = seen
                    minDist = cornerDist
        seen += distance(s[i], s[i + 1])
    return minOffset, minDist


def polygonOffsetWithMinimumDistanceToPoint(point, polygon, perpendicular=False):
    """Return the offset from the polygon start where the distance to the point is minimal"""
    return polygonOffsetAndDistanceToPoint(point, polygon, perpendicular)[0]


def distancePointToLine(point, line_start, line_end, perpendicular=False):
    """Return the minimum distance between point and the line (line_start, line_end)"""
    p1 = line_start
    p2 = line_end
    offset = lineOffsetWithMinimumDistanceToPoint(
        point, line_start, line_end, perpendicular)
    if offset == INVALID_DISTANCE:
        return INVALID_DISTANCE
    if offset == 0:
        return distance(point, p1)
    u = offset / distance(line_start, line_end)
    intersection = (p1[0] + u * (p2[0] - p1[0]), p1[1] + u * (p2[1] - p1[1]))
    return distance(point, intersection)


def distancePointToPolygon(point, polygon, perpendicular=False):
    """Return the minimum distance between point and polygon"""
    p = point
    s = polygon
    minDist = None
    for i in range(0, len(s) - 1):
        dist = distancePointToLine(p, s[i], s[i + 1], perpendicular)
        if dist == INVALID_DISTANCE and perpendicular and i != 0:
            # distance to inner corner
            dist = distance(point, s[i])
        if dist != INVALID_DISTANCE:
            if minDist is None or dist < minDist:
                minDist = dist
    if minDist is not None:
        return minDist
    else:
        return INVALID_DISTANCE


def positionAtOffset(p1, p2, offset):
    if isclose(offset, 0.):  # for pathological cases with dist == 0 and offset == 0
        return p1

    dist = distance(p1, p2)

    if isclose(dist, offset):
        return p2

    if offset > dist:
        return None

    return (p1[0] + (p2[0] - p1[0]) * (offset / dist), p1[1] + (p2[1] - p1[1]) * (offset / dist))


def positionAtShapeOffset(shape, offset):
    seenLength = 0
    curr = shape[0]
    for next in shape[1:]:
        nextLength = distance(curr, next)
        if seenLength + nextLength > offset:
            return positionAtOffset(curr, next, offset - seenLength)
        seenLength += nextLength
        curr = next
    return shape[-1]


def angle2D(p1, p2):
    theta1 = math.atan2(p1[1], p1[0])
    theta2 = math.atan2(p2[1], p2[0])
    dtheta = theta2 - theta1
    while dtheta > math.pi:
        dtheta -= 2.0 * math.pi
    while dtheta < -math.pi:
        dtheta += 2.0 * math.pi
    return dtheta


def naviDegree(rad):
    return normalizeAngle(math.degrees(math.pi / 2. - rad), 0, 360, 360)


def fromNaviDegree(degrees):
    return math.pi / 2. - math.radians(degrees)


def normalizeAngle(a, lower, upper, circle):
    while a < lower:
        a = a + circle
    while a > upper:
        a = a - circle
    return a


def minAngleDegreeDiff(d1, d2):
    return min(normalizeAngle(d1 - d2, 0, 360, 360),
               normalizeAngle(d2 - d1, 0, 360, 360))


def isWithin(pos, shape):
    angle = 0.
    for i in range(0, len(shape) - 1):
        p1 = ((shape[i][0] - pos[0]), (shape[i][1] - pos[1]))
        p2 = ((shape[i + 1][0] - pos[0]), (shape[i + 1][1] - pos[1]))
        angle = angle + angle2D(p1, p2)
    i = len(shape) - 1
    p1 = ((shape[i][0] - pos[0]), (shape[i][1] - pos[1]))
    p2 = ((shape[0][0] - pos[0]), (shape[0][1] - pos[1]))
    angle = angle + angle2D(p1, p2)
    return math.fabs(angle) >= math.pi


def sideOffset(fromPos, toPos, amount):
    scale = amount / distance(fromPos, toPos)
    return (scale * (fromPos[1] - toPos[1]),
            scale * (toPos[0] - fromPos[0]))


def sub(a, b):
    return (a[0] - b[0], a[1] - b[1])


def add(a, b):
    return (a[0] + b[0], a[1] + b[1])


def mul(a, x):
    return (a[0] * x, a[1] * x)


def dotProduct(a, b):
    return a[0] * b[0] + a[1] * b[1]


def orthoIntersection(a, b):
    c = add(a, b)
    quot = dotProduct(c, a)
    if quot != 0:
        return mul(mul(c, dotProduct(a, a)), 1 / quot)
    else:
        return None


def length(a):
    return math.sqrt(dotProduct(a, a))


def norm(a):
    return mul(a, 1 / length(a))


def narrow(fromPos, pos, toPos, amount):
    """detect narrow turns which cannot be shifted regularly"""
    a = sub(toPos, pos)
    b = sub(pos, fromPos)
    c = add(a, b)
    dPac = dotProduct(a, c)
    if dPac == 0:
        return True
    x = dotProduct(a, a) * length(c) / dPac
    return x < amount


def move2side(shape, amount):
    shape = [s for i, s in enumerate(shape) if i == 0 or shape[i-1] != s]
    if len(shape) < 2:
        return shape
    if polyLength(shape) == 0:
        return shape
    result = []
    for i, pos in enumerate(shape):
        if i == 0:
            fromPos = pos
            toPos = shape[i + 1]
            if fromPos != toPos:
                result.append(sub(fromPos, sideOffset(fromPos, toPos, amount)))
        elif i == len(shape) - 1:
            fromPos = shape[i - 1]
            toPos = pos
            if fromPos != toPos:
                result.append(sub(toPos, sideOffset(fromPos, toPos, amount)))
        else:
            fromPos = shape[i - 1]
            toPos = shape[i + 1]
            # check for narrow turns
            if narrow(fromPos, pos, toPos, amount):
                # print("narrow at i=%s pos=%s" % (i, pos))
                pass
            else:
                a = sideOffset(fromPos, pos, -amount)
                b = sideOffset(pos, toPos, -amount)
                c = orthoIntersection(a, b)
                if orthoIntersection is not None:
                    pos2 = add(pos, c)
                else:
                    extend = norm(sub(pos, fromPos))
                    pos2 = add(pos, mul(extend, amount))
                result.append(pos2)
    # print("move2side", amount)
    # print(shape)
    # print(result)
    # print()
    return result


def isClosedPolygon(polygon):
    return (len(polygon) >= 2) and (polygon[0] == polygon[-1])


def splitPolygonAtLengths2D(polygon, lengths):
    """
    Returns the polygon segments split at the given 2D-lengths.
    """
    if (len(polygon) <= 1 or len(lengths) == 0):
        return [polygon]
    offsets = [offset for offset in sorted(lengths) if offset > 0.0 and offset < polyLength(polygon)]
    ret = []
    seenLength = 0
    curr = polygon[0]
    polygonIndex = 0
    for offset in offsets:
        currSlice = [curr]
        while polygonIndex < len(polygon) - 1:
            next = polygon[polygonIndex + 1]
            if offset < seenLength + distance(curr, next):
                splitPos = positionAtOffset(curr, next, offset - seenLength)
                if not isclose(distance(currSlice[-1], splitPos), 0):
                    currSlice.append(splitPos)
                seenLength += distance(curr, splitPos)
                curr = splitPos
                break
            else:
                if not isclose(distance(currSlice[-1], next), 0):
                    currSlice.append(next)
                seenLength += distance(curr, next)
                curr = next
                polygonIndex += 1
        ret.append(currSlice)
    if polygonIndex < len(polygon) - 1:
        finalSlice = [curr] + polygon[polygonIndex + 1:]
        ret.append(finalSlice)
    return ret


def intersectsAtLengths2D(polygon1, polygon2):
    """
    Returns the 2D-length from polygon1's start to all intersections between polygon1 and polygon2.
    """
    ret = []
    if (len(polygon1) == 0 or len(polygon2) == 0):
        return ret
    polygon1Length = polyLength(polygon1)
    for j in range(len(polygon2) - 1):
        p21 = polygon2[j]
        p22 = polygon2[j + 1]
        pos = 0.0
        for i in range(len(polygon1) - 1):
            p11 = polygon1[i]
            p12 = polygon1[i + 1]
            pIntersection = [0.0, 0.0]
            if (intersectsLineSegment(p11, p12, p21, p22, 0.0, pIntersection, True)):
                for k in range(0, len(pIntersection), 2):
                    length = pos + distance(p11, (pIntersection[k], pIntersection[k + 1]))
                    # special case for closed polygons
                    if isClosedPolygon(polygon1) and isclose(length, polygon1Length):
                        length = 0.0
                    # check for duplicate
                    isDuplicate = False
                    for result in ret:
                        if isclose(length, result):
                            isDuplicate = True
                            break
                    if not isDuplicate:
                        ret.append(length)
            pos += distance(p11, p12)
    return ret


def intersectsPolygon(polygon1, polygon2):
    """
    Returns whether the polygons intersect on at least one of their segments.
    """
    if (len(polygon1) < 2 or len(polygon2) < 2):
        return False
    for i in range(len(polygon1) - 1):
        p11 = polygon1[i]
        p12 = polygon1[i + 1]
        for j in range(len(polygon2) - 1):
            p21 = polygon2[j]
            p22 = polygon2[j + 1]
            if (intersectsLineSegment(p11, p12, p21, p22)):
                return True
    return False


# @see src/utils/geom/PositionVector::intersects(p11, p12, p21, p22 ...)
def intersectsLineSegment(p11, p12, p21, p22, withinDist=0.0, pIntersection=None, storeEndPointsIfCoincident=False):
    """
    Returns whether the line segments defined by Line p11,p12 and Line p21,p22 intersect.
    If not set to 'None', 'pIntersection' serves as a storage for the intersection point(s).
    Parameter 'storeEndPointsIfCoincident' is an option for storing the endpoints of the
    line segment defined by the intersecting set of line1 and line2 if applicable.
    """
    eps = sys.float_info.epsilon
    # dy2 * dx1 - dx2 * dy1
    denominator = (p22[1] - p21[1]) * (p12[0] - p11[0]) - (p22[0] - p21[0]) * (p12[1] - p11[1])
    # dx2 * (p11.y - p21.y) - dy2 * (p11.x - p21.x)
    numera = (p22[0] - p21[0]) * (p11[1] - p21[1]) - (p22[1] - p21[1]) * (p11[0] - p21[0])
    # dx1 * (p11.y - p21.y) - dy1 * (p11.x - p21.x)
    numerb = (p12[0] - p11[0]) * (p11[1] - p21[1]) - (p12[1] - p11[1]) * (p11[0] - p21[0])
    # Are the lines coincident?
    if (math.fabs(numera) < eps and math.fabs(numerb) < eps and math.fabs(denominator) < eps):
        a1 = 0.0
        a2 = 0.0
        a3 = 0.0
        a4 = 0.0
        a = -1e12
        isVertical = (p11[0] == p12[0])
        if (not isVertical):
            # line1 and line2 are not vertical
            a1 = p11[0] if p11[0] < p12[0] else p12[0]
            a2 = p12[0] if p11[0] < p12[0] else p11[0]
            a3 = p21[0] if p21[0] < p22[0] else p22[0]
            a4 = p22[0] if p21[0] < p22[0] else p21[0]
        else:
            # line1 and line2 are vertical
            a1 = p11[1] if p11[1] < p12[1] else p12[1]
            a2 = p12[1] if p11[1] < p12[1] else p11[1]
            a3 = p21[1] if p21[1] < p22[1] else p22[1]
            a4 = p22[1] if p21[1] < p22[1] else p21[1]
        if (a1 <= a3 and a3 <= a2):
            # one endpoint of line2 lies on line1
            if (a4 <= a2):
                # line2 is a subset of line1
                a = (a3 + a4) / 2.0
                if (storeEndPointsIfCoincident and pIntersection is not None):
                    pIntersection[0] = p21[0]
                    pIntersection[1] = p21[1]
                    pIntersection.append(p22[0])
                    pIntersection.append(p22[1])
                    return True
            else:
                # the other endpoint of line2 lies beyond line1
                a = (a3 + a2) / 2.0
                if (storeEndPointsIfCoincident and pIntersection is not None):
                    if (not isVertical):
                        pIntersection[0] = a3
                        pIntersection[1] = p21[1] if p21[0] < p22[0] else p22[1]
                        pIntersection.append(a2)
                        pIntersection.append(p12[1] if p11[0] < p12[0] else p11[1])
                    else:
                        pIntersection[0] = p21[0] if p21[1] < p22[1] else p22[0]
                        pIntersection[1] = a3
                        pIntersection.append(p12[0] if p11[1] < p12[1] else p11[0])
                        pIntersection.append(a2)
                    return True
        if (a3 <= a1 and a1 <= a4):
            # one endpoint of line1 lies on line2
            if (a2 <= a4):
                # line1 is a subset of line2
                a = (a1 + a2) / 2.0
                if (storeEndPointsIfCoincident and pIntersection is not None):
                    pIntersection[0] = p11[0]
                    pIntersection[1] = p11[1]
                    pIntersection.append(p12[0])
                    pIntersection.append(p12[1])
                    return True
            else:
                # the other endpoint of line1 lies beyond line2
                a = (a1 + a4) / 2.0
                if (storeEndPointsIfCoincident and pIntersection is not None):
                    if (not isVertical):
                        pIntersection[0] = a1
                        pIntersection[1] = p11[1] if p11[0] < p12[0] else p12[1]
                        pIntersection.append(a4)
                        pIntersection.append(p22[1] if p21[0] < p22[0] else p21[1])
                    else:
                        pIntersection[0] = p11[0] if p11[1] < p12[1] else p12[0]
                        pIntersection[1] = a1
                        pIntersection.append(p22[0] if p21[1] < p22[1] else p21[0])
                        pIntersection.append(a4)
                    return True
        if (a != -1e12):
            if (pIntersection is not None):
                if (not isVertical):
                    # line1 and line2 are not vertical
                    mu = (a - p11[0]) / (p12[0] - p11[0])
                    x = a
                    y = p11[1] + mu * (p12[1] - p11[1])
                else:
                    # line1 and line2 are vertical
                    x = p11[0]
                    y = a
                    if (p12[1] == p11[1]):
                        mu = 0
                    else:
                        mu = (a - p11[1]) / (p12[1] - p11[1])
                pIntersection[0] = x
                pIntersection[1] = y
            return True
        return False
    # Are the lines parallel?
    if (math.fabs(denominator) < eps):
        return False
    # Is the intersection along the segments?
    mua = numera / denominator
    # Reduce rounding errors for lines ending in the same point
    if (math.fabs(p12[0] - p22[0]) < eps and math.fabs(p12[1] - p22[1]) < eps):
        mua = 1.0
    else:
        offseta = withinDist / distance(p11, p12)
        offsetb = withinDist / distance(p21, p22)
        mub = numerb / denominator
        if (mua < -offseta or mua > 1 + offseta or mub < -offsetb or mub > 1 + offsetb):
            return False
    if (pIntersection is not None):
        x = p11[0] + mua * (p12[0] - p11[0])
        y = p11[1] + mua * (p12[1] - p11[1])
        mu = mua
        pIntersection[0] = x
        pIntersection[1] = y
    return True
