from GeomBase import Matrix3D, Point3D, Vector3D, epsilon
from Ray import Ray
from Segment import Segment
from Line import Line
from Polyline import Polyline
from Plane import Plane
from Triangle import Triangle
import math


def nearZero(x):
    if math.fabs(x) < epsilon:
        return True
    else:
        return False


def distance(obj1, obj2):
    if isinstance(obj1, Point3D) and isinstance(obj2, Line):
        P, Q, V = obj2.P, obj1, obj2.V
        t = P.pointTo(Q).dotProduct(V)
        R = P + V.amplified(t)
        return Q.distance(R)
    elif isinstance(obj1, Point3D) and isinstance(obj2, Ray):
        P, Q, V = obj2.P, obj1, obj2.V
        t = P.pointTo(Q).dotProduct(V)
        if t > 0:
            R = P + V.amplified(t)
            return Q.distance(R)
        return Q.distance(P)
    elif isinstance(obj1, Point3D) and isinstance(obj2, Segment):
        Q, P, P1, V = obj1, obj2.A, obj2.B, obj2.direction().normalized()
        L = obj2.length()
        t = P.pointTo(Q).dotProduct(V)
        if t <= 0:
            return Q.distance(P)
        elif t >= L:
            return Q.distance(P1)
        else:
            R = P + V.amplified(t)
            return Q.distance(R)
    elif isinstance(obj1, Point3D) and isinstance(obj2, Plane):
        P, Q, N = obj2.P, obj1, obj2.N
        angle = N.getAngle(P.pointTo(Q))
        return P.distance(Q) * math.cos(angle)
    elif isinstance(obj1, Line) and isinstance(obj2, Line):
        P1, V1, P2, V2 = obj1.P, obj1.V, obj2.P, obj2.V
        N = V1.crossProduct(V2)
        if N.isZeroVector():
            return distance(P1, obj2)
        return distance(P1, Plane(P2, N))
    elif isinstance(obj1, Line) and isinstance(obj2, Plane):
        if obj1.V.crossProduct(obj2.N).isZeroVector():
            return distance(obj1.P, obj2)
        else:
            return 0
    elif isinstance(obj1, Ray) and isinstance(obj2, Plane):
        pass
    elif isinstance(obj1, Segment) and isinstance(obj2, Plane):
        pass


def intersectLineLine(line1: Line, line2: Line):
    P1, V1, P2, V2 = line1.P, line1.V, line2.P, line2.V
    P1P2 = P1.pointTo(P2)
    deno = V1.dy * V2.dx - V1.dx * V2.dy
    if deno != 0:
        t1 = -(-P1P2.dy * V2.dx + P1P2.dx * V2.dy) / deno
        t2 = -(-P1P2.dy * V1.dx + P1P2.dx * V1.dy) / deno
        return P1 + V1.amplified(t1), t1, t2
    else:
        deno = V1.dz * V2.dy - V1.dy * V2.dz
        if deno != 0:
            t1 = -(-P1P2.dz * V2.dy + P1P2.dy * V2.dz) / deno
            t2 = -(-P1P2.dz * V1.dy + P1P2.dy * V1.dz) / deno
            return P1 + V1.amplified(t1), t1, t2
        return None, 0, 0


def intersectSegmentPlane(seg: Segment, plane: Plane):
    A, B, P, N = seg.A, seg.B, plane.P, plane.N
    V = A.pointTo(B)
    PA = P.pointTo(A)
    if V.dotProduct(N) == 0:
        return None
    else:
        t = -(PA.dotProduct(N)) / V.dotProduct(N)
        if t >= 0 and t <= 1:
            return A + (V.amplified(t))
    return None


def intersect(obj1, obj2):
    if isinstance(obj1, Line) and isinstance(obj2, Line):
        P, t1, t2 = intersectLineLine(obj1, obj2)
        return P
    elif isinstance(obj1, Segment) and isinstance(obj2, Segment):
        line1, line2 = Line(obj1.A, obj1.direction()), Line(obj2.A, obj2.direction())
        P, t1, t2 = intersectLineLine(line1, line2)
        if P is not None:
            if t1 >= 0 and t1 <= obj1.length() and t2 >= 0 and t2 <= obj2.length():
                return P
        return None
    elif isinstance(obj1, Line) and isinstance(obj2, Segment):
        line1, line2 = obj1, Line(obj2.A, obj2.direction())
        P, t1, t2 = intersectLineLine(line1, line2)
        if P:
            if t2 >= 0 and t2 <= obj2.length():
                return P
            else:
                return None
    elif isinstance(obj1, Line) and isinstance(obj2, Ray):
        pass
    elif isinstance(obj1, Ray) and isinstance(obj2, Segment):
        pass
    elif isinstance(obj1, Ray) and isinstance(obj2, Ray):
        pass
    elif isinstance(obj1, Line) and isinstance(obj2, Plane):
        P1, V, P2, N = obj1.P, obj1.V, obj2.P, obj2.N
        dotPro = V.dotProduct(N)
        if dotPro != 0:
            t = P1.pointTo(P2).dotProduct(N) / dotPro
            return P1 + V.amplified(t)
        return None
    elif isinstance(obj1, Ray) and isinstance(obj2, Plane):
        pass
    elif isinstance(obj1, Segment) and isinstance(obj2, Plane):
        return intersectSegmentPlane(obj1, obj2)
    pass


def pointOnRay(p: Point3D, ray: Ray):
    v = ray.P.pointTo(p)
    return (
        True
        if v.dotProduct(ray.V) >= 0 and v.crossProduct(ray.V).isZeroVector()
        else False
    )


def pointInPolygon(p: Point3D, polygon: Polyline):
    passCount = 0
    ray = Ray(p, Vector3D(1, 0, 0))
    segments = []
    for i in range(polygon.count() - 1):
        seg = Segment(polygon.point(i), polygon.point(i + 1))
        segments.append(seg)

    for seg in segments:
        line1, line2 = Line(ray.P, ray.V), Line(seg.A, seg.direction())
        P, t1, t2 = intersectLineLine(line1, line2)
        if P is not None:
            if nearZero(t1):
                return -1
            elif (
                seg.A.y != p.y
                and seg.B.y != p.y
                and t1 > 0
                and t2 > 0
                and t2 < seg.length()
            ):
                passCount += 1
    upSegments, downSegments = [], []
    for seg in segments:
        if seg.A.isIdentical(ray.P) or seg.B.isIdentical(ray.P):
            return -1
        elif pointOnRay(seg.A, ray) ^ pointOnRay(seg.B, ray):
            if seg.A.y >= p.y and seg.B.y >= p.y:
                upSegments.append(seg)
            elif seg.A.y <= p.y and seg.B.y <= p.y:
                downSegments.append(seg)
    passCount += min(len(upSegments), len(downSegments))
    if passCount % 2 == 1:
        return 1
    return 0


def intersectTrianglePlane(triangle, plane):
    AB = Segment(triangle.A, triangle.B)
    AC = Segment(triangle.A, triangle.C)
    BC = Segment(triangle.B, triangle.C)
    c1 = intersectSegmentPlane(AB, plane)
    c2 = intersectSegmentPlane(AC, plane)
    c3 = intersectSegmentPlane(BC, plane)
    if c1 is None:
        if c2 is not None and c3 is not None:
            if c2.distance(c3) != 0:
                return Segment(c2, c3)
    elif c2 is None:
        if c1 is not None and c3 is not None:
            if c1.distance(c3) != 0:
                return Segment(c1, c3)
    elif c3 is None:
        if c2 is not None and c1 is not None:
            if c1.distance(c2) != 0:
                return Segment(c1, c2)

    if c1 and c2 and c3:
        if c1.distance(c2) != 0:
            return Segment(c1, c2)
        else:
            return Segment(c1, c3)
    return None


def intersectTriangleZPlane(triangle: Triangle, z):
    if triangle.zMinPnt() > z:
        return None
    if triangle.zMaxPnt() < z:
        return None
    return intersectTrianglePlane(triangle, Plane.zPlane(z))


def rotatePolygons(polygons, angle, center=None):
    dx = 0 if not center else center.x
    dy = 0 if not center else center.y
    mt = Matrix3D.createTranlsateMatrix(-dx, -dy, 0)
    mr = Matrix3D.createRotateMatrix("Z", angle)
    mb = Matrix3D.createTranlsateMatrix(dx, dy, 0)
    m = mt * mr * mb
    newpolys = []
    for poly in polygons:
        newpolys.append(poly.multiplied(m))
    return newpolys