"""独立几何参考核对 GJK/EPA：SAT 穿透深度、穷举点到边距离与解析边界。"""
import math
import random
from collision2d import (Polygon, Circle, Support, box, closest, dot, sub, norm,
                         gjk, epa, query, plane_circle)


def close(a, b, tolerance=2e-8):
    assert abs(a-b) <= tolerance, (a, b)


def point_segment_distance(p, a, b):
    # 单独按两个端点投影判据编写，作为算法之外的几何参考。
    edge = (b[0]-a[0], b[1]-a[1])
    projection = (p[0]-a[0])*edge[0]+(p[1]-a[1])*edge[1]
    length2 = edge[0]**2+edge[1]**2
    if projection <= 0:
        return math.dist(p, a)
    if projection >= length2:
        return math.dist(p, b)
    area = edge[0]*(p[1]-a[1])-edge[1]*(p[0]-a[0])
    return abs(area)/math.sqrt(length2)


def reference(a, b):
    penetration = math.inf
    separated = False
    for vertices in (a.vertices, b.vertices):
        for i, p in enumerate(vertices):
            q = vertices[(i+1) % len(vertices)]
            dx, dy = q[0]-p[0], q[1]-p[1]
            n = (-dy/math.hypot(dx, dy), dx/math.hypot(dx, dy))
            pa = [dot(x, n) for x in a.vertices]
            pb = [dot(x, n) for x in b.vertices]
            depth = min(max(pa)-min(pb), max(pb)-min(pa))
            separated |= depth < 0
            penetration = min(penetration, depth)
    if not separated:
        return -penetration
    distances = []
    for first, second in ((a, b), (b, a)):
        for p in first.vertices:
            for i, q in enumerate(second.vertices):
                distances.append(point_segment_distance(p, q, second.vertices[(i+1) % len(second.vertices)]))
    return min(distances)


for points, expected in [([(2, 1), (-1, 1)], (0, 1)),
                         ([(-1, -1), (2, -1), (-1, 2)], (0, 0)),
                         ([(1, 1), (3, 1), (1, 3)], (1, 1)),
                         ([(-2, 1), (2, 1), (0, 3)], (0, 1)),
                         ([(-1, 0), (2, 0), (3, 0)], (0, 0)),
                         ([(1, 2), (1, 2), (1, 2)], (1, 2))]:
    simplex = [Support(p, p, (0, 0)) for p in points]
    point, active, weights = closest(simplex)
    close(math.dist(point, expected), 0)
    close(sum(weights), 1)
    assert min(weights) >= 0
print("PASS 单纯形内部、顶点、边、共线与重复点")

fixtures = [(box((0, 0), (1, 1)), box((3, 0), (1, 1)), 1),
            (box((0, 0), (1, 1)), box((3, 3), (1, 1)), math.sqrt(2)),
            (box((0, 0), (1, 1)), box((1.5, 0), (1, 1)), -.5),
            (box((0, 0), (2, 2)), box((.2, .1), (.5, .5)), -2.3),
            (box((0, 0), (1, 1)), box((2, 0), (1, 1)), 0)]
for a, b, expected in fixtures:
    result = query(a, b)
    close(result["signed_distance"], expected)
    residual = sub(sub(result["b"], result["a"]),
                   tuple(result["signed_distance"]*x for x in result["normal"]))
    close(norm(residual), 0)
print("PASS 分离、斜向距离、穿透、包含与恰好相切")

rng = random.Random(1010)
maximum = 0
for _ in range(500):
    a = box((rng.uniform(-1, 1), rng.uniform(-1, 1)),
            (rng.uniform(.2, 1.2), rng.uniform(.2, 1.2)), rng.uniform(-3, 3))
    b = box((rng.uniform(-2, 2), rng.uniform(-2, 2)),
            (rng.uniform(.2, 1.2), rng.uniform(.2, 1.2)), rng.uniform(-3, 3))
    exact, result = reference(a, b), query(a, b)
    error = abs(exact-result["signed_distance"])
    maximum = max(maximum, error)
    close(exact, result["signed_distance"])
    assert result["status"] == "converged", result["status"]
    assert result["lower"]-1e-8 <= abs(exact) <= result["upper"]+1e-8
    close(norm(sub(sub(result["b"], result["a"]),
                   tuple(result["signed_distance"]*x for x in result["normal"]))), 0)
    for step in result["history"]:
        assert step["lower"]-1e-8 <= abs(exact) <= step["upper"]+1e-8
print("PASS 500 对旋转盒与独立 SAT/距离参考，最大误差", maximum)

a, b = fixtures[2][:2]
original = query(a, b)
swapped = query(b, a)
close(swapped["signed_distance"], original["signed_distance"])
close(norm(tuple(x+y for x, y in zip(swapped["normal"], original["normal"]))), 0)
translation = (13., -7.)
moved = [Polygon(tuple((x+translation[0], y+translation[1]) for x, y in shape.vertices)) for shape in (a, b)]
close(query(*moved)["signed_distance"], original["signed_distance"])
for factor in (.001, 1000):
    scaled = [Polygon(tuple((factor*x, factor*y) for x, y in shape.vertices)) for shape in (a, b)]
    close(query(*scaled)["signed_distance"], factor*original["signed_distance"])
print("PASS 几何交换、平移与尺度一致性")

c = Circle((.3, -.2), .7)
for angle in [0, .4, 1.7, 3.1]:
    direction = (math.cos(angle), math.sin(angle))
    point = c.support(direction)
    close(math.dist(point, c.center), c.radius)
    close(dot(point, direction), dot(c.center, direction)+c.radius)
limited = epa(Circle((0, 0), 1), Circle((0, 0), 1),
              gjk(Circle((0, 0), 1), Circle((0, 0), 1))["simplex"], max_iterations=1)
assert limited["status"] == "iteration_limit"
assert limited["lower"]-1e-12 <= 2 <= limited["upper"]+1e-12
print("PASS 圆支持点解析值与未收敛 EPA 的有效区间")

for gap, candidate, active in [(.026, False, False), (.025, True, False),
                              (.020, True, False), (.019, True, True), (-.01, True, True)]:
    # 半径为零可精确命中十进制阈值，避免相加再相减的舍入。
    result = plane_circle((0, gap), 0, .02, .005)
    assert result["candidate"] == candidate
    assert result["active"] == active
    close(result["residual"], gap-.02)
print("PASS margin + gap 候选范围、margin 激活范围与边界")