"""二维凸体的支持映射、距离 GJK 与 EPA。长度采用同一单位。"""
from dataclasses import dataclass
from math import cos, sin, hypot


def add(a, b): return tuple(x+y for x, y in zip(a, b))
def sub(a, b): return tuple(x-y for x, y in zip(a, b))
def scale(a, k): return tuple(x*k for x in a)
def dot(a, b): return sum(x*y for x, y in zip(a, b))
def cross(a, b): return a[0]*b[1]-a[1]*b[0]
def norm(a): return hypot(*a)


@dataclass(frozen=True)
class Polygon:
    vertices: tuple

    def support(self, direction):
        if not self.vertices:
            raise ValueError("凸多边形需要顶点")
        return max(self.vertices, key=lambda x: dot(x, direction))


@dataclass(frozen=True)
class Circle:
    center: tuple
    radius: float

    def support(self, direction):
        if self.radius < 0:
            raise ValueError("半径非负")
        length = norm(direction)
        direction = scale(direction, 1/length) if length else (1.0, 0.0)
        return add(self.center, scale(direction, self.radius))


def box(center, half, angle=0.0):
    """由半边长和逆时针弧度生成四个顶点。"""
    c, s = cos(angle), sin(angle)
    return Polygon(tuple(add(center, (c*x-s*y, s*x+c*y))
                         for x, y in [(-half[0], -half[1]), (half[0], -half[1]),
                                      (half[0], half[1]), (-half[0], half[1])]))


@dataclass(frozen=True)
class Support:
    w: tuple
    a: tuple
    b: tuple


def support(a, b, direction):
    pa, pb = a.support(direction), b.support(scale(direction, -1))
    return Support(sub(pa, pb), pa, pb)


def segment_weights(a, b):
    edge = sub(b, a)
    length2 = dot(edge, edge)
    t = max(0.0, min(1.0, -dot(a, edge)/length2)) if length2 else 0.0
    return [1-t, t]


def closest(simplex):
    """最近点、保留的支持点、非负重心权；退化三角形回到边。"""
    candidates = []
    if len(simplex) == 3:
        a, b, c = [x.w for x in simplex]
        e, f = sub(b, a), sub(c, a)
        area = cross(e, f)
        if abs(area) > 1e-14*max(norm(e)*norm(f), 1e-300):
            u, v = cross(scale(a, -1), f)/area, cross(e, scale(a, -1))/area
            weights = [1-u-v, u, v]
            if min(weights) >= 0:
                return (0.0, 0.0), list(simplex), weights
    for i, vertex in enumerate(simplex):
        candidates.append((dot(vertex.w, vertex.w), [vertex], [1.0]))
        for j in range(i):
            weights = segment_weights(simplex[j].w, vertex.w)
            point = add(scale(simplex[j].w, weights[0]), scale(vertex.w, weights[1]))
            candidates.append((dot(point, point), [simplex[j], vertex], weights))
    _, vertices, weights = min(candidates, key=lambda item: item[0])
    active = [(p, w) for p, w in zip(vertices, weights) if w > 0]
    point = tuple(sum(p.w[j]*w for p, w in active) for j in range(2))
    return point, [p for p, _ in active], [w for _, w in active]


def witnesses(simplex, weights):
    return tuple(tuple(sum(getattr(p, name)[j]*w for p, w in zip(simplex, weights))
                       for j in range(2)) for name in ("a", "b"))


def gjk(a, b, tolerance=1e-9, max_iterations=64):
    """真实距离位于 lower/upper；终止状态区分收敛、近原点、停滞与迭代上限。"""
    simplex = [support(a, b, (1.0, 0.0))]
    history, lower = [], 0.0
    for iteration in range(max_iterations):
        point, simplex, weights = closest(simplex)
        upper = norm(point)
        pa, pb = witnesses(simplex, weights)
        if upper <= tolerance:
            status = "near_origin"
            history.append({"simplex": simplex[:], "point": point, "lower": 0., "upper": upper})
            break
        new = support(a, b, scale(point, -1/upper))
        lower = max(lower, 0.0, dot(point, new.w)/upper)
        # 舍入下界至当前可行上界，保留几何长度的容差单位。
        lower = min(lower, upper)
        history.append({"simplex": simplex[:], "point": point, "support": new,
                        "lower": lower, "upper": upper})
        if upper-lower <= tolerance:
            status = "converged"
            break
        if any(norm(sub(new.w, p.w)) <= tolerance*1e-3 for p in simplex):
            status = "stalled"
            break
        simplex.append(new)
    else:
        point, simplex, weights = closest(simplex)
        upper = norm(point)
        pa, pb = witnesses(simplex, weights)
        status = "iteration_limit"
    return {"status": status, "lower": lower, "upper": upper,
            "point": point, "simplex": simplex, "weights": weights,
            "a": pa, "b": pb, "normal": scale(point, -1/upper) if upper else (1., 0.),
            "history": history}


def hull(points):
    """支持点的逆时针凸包，保留各点的原形体见证信息。"""
    points = sorted({p.w: p for p in points}.values(), key=lambda p: p.w)
    if len(points) <= 1:
        return points
    def chain(sequence):
        result = []
        for p in sequence:
            while len(result) >= 2 and cross(sub(result[-1].w, result[-2].w),
                                             sub(p.w, result[-1].w)) <= 0:
                result.pop()
            result.append(p)
        return result
    return chain(points)[:-1]+chain(reversed(points))[:-1]


def epa(a, b, simplex, tolerance=1e-9, max_iterations=128):
    """由含原点的 GJK 单纯形建立内接凸包，求穿透深度的上下界。"""
    poly = list(simplex)
    directions = [(1., 0.), (-1., 0.), (0., 1.), (0., -1.)]
    for i, p in enumerate(simplex):
        for q in simplex[:i]:
            e = sub(p.w, q.w)
            if norm(e):
                directions += [(e[1], -e[0]), (-e[1], e[0])]
    point, active, weights = closest(simplex)
    interior = len(active) == 3 and norm(point) < tolerance and min(weights) > 1e-8
    poly = hull(active if interior else poly+[support(a, b, d) for d in directions])
    if len(poly) < 3:
        p, active, weights = closest(poly)
        pa, pb = witnesses(active, weights)
        return {"status": "degenerate", "lower": 0., "upper": 0., "depth": 0.,
                "normal": (1., 0.), "a": pa, "b": pb, "history": []}
    history = []
    for _ in range(max_iterations):
        edges = []
        for i, p in enumerate(poly):
            q = poly[(i+1) % len(poly)]
            e = sub(q.w, p.w)
            normal = scale((e[1], -e[0]), 1/norm(e))
            edges.append((dot(normal, p.w), i, normal))
        lower, i, normal = min(edges, key=lambda item: item[0])
        if lower < -tolerance:
            raise ValueError("EPA 初始凸包需要包含原点")
        lower = max(0.0, lower)
        new = support(a, b, normal)
        upper = max(lower, dot(normal, new.w))
        pair = [poly[i], poly[(i+1) % len(poly)]]
        weights = segment_weights(pair[0].w, pair[1].w)
        pa, pb = witnesses(pair, weights)
        history.append({"poly": poly[:], "edge": i, "normal": normal,
                        "support": new, "lower": lower, "upper": upper})
        if upper-lower <= tolerance:
            status = "converged"
            break
        if any(norm(sub(p.w, new.w)) <= tolerance*1e-3 for p in poly):
            status = "stalled"
            break
        # 重建凸包会同时移除可见的连续边链，二维中便于直接检查。
        poly = hull(poly+[new])
    else:
        status = "iteration_limit"
    return {"status": status, "lower": lower, "upper": upper, "depth": lower,
            "normal": normal, "a": pa, "b": pb, "history": history}


def query(a, b, tolerance=1e-9):
    """法线由形体 A 指向 B；见证点满足 b-a = signed_distance * normal。"""
    distance = gjk(a, b, tolerance)
    if distance["upper"] > tolerance:
        return {**distance, "signed_distance": distance["upper"], "algorithm": "GJK"}
    penetration = epa(a, b, distance["simplex"], tolerance)
    return {**penetration, "signed_distance": -penetration["depth"], "algorithm": "EPA",
            "gjk": distance}


def plane_circle(center, radius, margin=0., gap=0.):
    """固定水平地面为 A，圆为 B；阈值为本地 3.10.0 混合后的 pair 参数。"""
    distance = center[1]-radius
    return {"a": (center[0], 0.), "b": (center[0], distance), "normal": (0., 1.),
            "signed_distance": distance, "candidate": distance <= margin+gap,
            "active": distance < margin, "residual": distance-margin}