"""VO collision check — scratch, mirrors the geometry in Kuwata 2014."""
import math

def in_collision_cone(p_rel, v_rel, r_safe, horizon_s=120.0):
    """p_rel, v_rel: (x,y) of contact relative to us. r_safe: combined radius."""
    d = math.hypot(*p_rel)
    if d <= r_safe:
        return True
    # half-angle of the cone to the safety disc
    theta = math.asin(min(r_safe / d, 1.0))
    # bearing to contact vs relative velocity direction
    brg = math.atan2(-p_rel[1], -p_rel[0])
    vdir = math.atan2(v_rel[1], v_rel[0])
    dang = abs((vdir - brg + math.pi) % (2 * math.pi) - math.pi)
    closing = -(p_rel[0]*v_rel[0] + p_rel[1]*v_rel[1]) / max(d, 1e-6)
    if closing <= 0:
        return False  # opening range
    tca = d / max(closing, 1e-6)
    return dang < theta and tca < horizon_s

# TODO: bias selection to starboard side of cone for crossing/headon