"""Physical-space clearance checks for capsule-shaped laser marks."""

from __future__ import annotations

import math


def point_segment_distance_sq(point, start, end):
    dx, dy = end[0] - start[0], end[1] - start[1]
    length_sq = dx * dx + dy * dy
    if length_sq == 0:
        return (point[0] - start[0]) ** 2 + (point[1] - start[1]) ** 2
    t = max(0.0, min(1.0, ((point[0] - start[0]) * dx + (point[1] - start[1]) * dy) / length_sq))
    return (point[0] - start[0] - t * dx) ** 2 + (point[1] - start[1] - t * dy) ** 2


def segments_distance_sq(a0, a1, b0, b1):
    """Minimum squared distance, including a proper crossing at zero."""
    def cross(p, q, r):
        return (q[0] - p[0]) * (r[1] - p[1]) - (q[1] - p[1]) * (r[0] - p[0])

    ab0, ab1 = cross(a0, a1, b0), cross(a0, a1, b1)
    ba0, ba1 = cross(b0, b1, a0), cross(b0, b1, a1)
    if ab0 * ab1 < 0 and ba0 * ba1 < 0:
        return 0.0
    return min(
        point_segment_distance_sq(a0, b0, b1),
        point_segment_distance_sq(a1, b0, b1),
        point_segment_distance_sq(b0, a0, a1),
        point_segment_distance_sq(b1, a0, a1),
    )


def capsule_segment(cx, cy, half_length, angle):
    dx, dy = half_length * math.cos(angle), half_length * math.sin(angle)
    return (cx - dx, cy - dy), (cx + dx, cy + dy)


def has_clearance(candidate, accepted, clearance, skip_index=None):
    """Capsules are (start, end, radius), all in the same physical units."""
    start, end, radius = candidate
    for index, (other_start, other_end, other_radius) in enumerate(accepted):
        if index == skip_index:
            continue
        limit = radius + other_radius + clearance
        if (max(start[0], end[0]) + limit < min(other_start[0], other_end[0])
                or max(other_start[0], other_end[0]) + limit < min(start[0], end[0])
                or max(start[1], end[1]) + limit < min(other_start[1], other_end[1])
                or max(other_start[1], other_end[1]) + limit < min(start[1], end[1])):
            continue
        if segments_distance_sq(start, end, other_start, other_end) < limit * limit:
            return False
    return True