"""
trace_planner.py

One module for steps 2-4: line extraction, obstacle detection, and path planning.

    frame (1280x720) --> top-down view of the paper in the calibrated sheet frame (mm)
                     --> ink mask + obstacle mask
                     --> ordered centerline of the sharpie line
                     --> detour around each obstacle, rejoining the line
                     --> resampled waypoints (mm) with a per-point kind: ink / bridge / detour

Main functions:
    plan = plan_trace(frame, cam)                      # full plan from one frame (arm out of view)
    plan = replan(plan, current_xy, frame, cam, ...)   # mid-task: re-detect obstacles, reroute the rest
    vis  = draw_plan(frame, cam, plan)                 # overlay on the camera frame

All coordinates are in the sheet frame from camera_calib.py: mm, x to the right, y away from camera.

CLI (run from the repo root):
    python3 src/perception/trace_planner.py              # live frame from the camera
    python3 src/perception/trace_planner.py photo.png    # or a saved 1280x720 frame
Saves plan.json, plan_overlay.png (camera view) and plan_topdown.png (rectified
view) at the repo root.
"""
import json
import os
import sys
from collections import deque
from dataclasses import dataclass, field, asdict
from itertools import permutations, product

import cv2
import numpy as np

sys.path.insert(0, os.path.join(os.path.dirname(__file__), "..", "planning"))
from grid_astar_replanner import find_detour_path  # noqa: E402

# camera index is resolved dynamically in _grab() (by device name) -- see camera_device.py
CAMERA_CALIB_PATH = "calib_camera.json"

PAPER_W, PAPER_L = 215.9, 279.4
INK, BRIDGE, DETOUR, OVER = 0, 1, 2, 3
KIND_NAMES = {INK: "ink", BRIDGE: "bridge", DETOUR: "detour", OVER: "over"}

# Obstacle height isn't measured live (no ToF yet) -- this is the clearance plan_to_arm.py adds
# on top of the pen-contact height for an OVER segment. Deliberately generous for a small
# challenge obstacle; replace with a real ToF/height reading if one becomes available.
# TODO: measure/tune against your actual obstacle before the run.
OVER_CLEARANCE_MM = 40.0


# ============================================================ config
@dataclass
class PlanConfig:
    px_per_mm: float = 4.0          # resolution of the top-down working image
    edge_margin_mm: float = 3.0     # ignore this much of the paper border (edge shadow)

    # ink (black sharpie): darker than the local paper, and not colorful
    ink_strong: float = 0.35        # darkness (0..1 relative to local paper) that is surely ink
    ink_weak: float = 0.20          # fainter pixels kept only if connected to surely-ink pixels
    ink_max_chroma: float = 20.0
    gap_close_mm: float = 1.5       # bridge tiny breaks in the drawn line
    min_segment_mm: float = 8.0     # drop ink fragments shorter than this

    # obstacle: anything colorful, or anything dark that is much thicker than a sharpie line
    obst_chroma: float = 25.0
    obst_min_width_mm: float = 6.0  # dark blobs thicker than this are obstacles, not ink
    obst_min_area_mm2: float = 40.0
    obst_pad_mm: float = 2.0        # grow the detected obstacle a little (soft edges, shadow)

    # start/end dots: two big filled circles marking the line's endpoints, distinct from the
    # thin ink stroke and from obstacles (round, and much bigger across than the line is wide)
    dot_min_diameter_mm: float = 10.0
    dot_max_diameter_mm: float = 30.0
    dot_min_circularity: float = 0.7    # 4*pi*area/perimeter^2, 1.0 = perfect circle

    # planning
    clearance_mm: float = 15.0      # min distance from the obstacle outline to the pen path
    max_bridge_mm: float = 80.0     # warn if the line has a hidden stretch longer than this
    smooth_mm: float = 5.0          # moving-average window on the ink path
    step_mm: float = 2.0            # spacing of the output waypoints
    start: str = "far"              # "far": start at the end farthest from the camera, "near": opposite
    exclude_pad_mm: float = 10.0    # grow boxes passed in exclude_boxes_px (arm, marker)


@dataclass
class Plan:
    path_mm: np.ndarray             # (N, 2) waypoints to trace, in order
    kind: np.ndarray                # (N,) INK / BRIDGE / DETOUR per waypoint
    line_mm: np.ndarray             # (M, 2) the line itself, 1 mm spacing, before detours
    line_kind: np.ndarray           # (M,) INK / BRIDGE
    obstacles_mm: list              # outlines of detected obstacles
    keepouts_mm: list               # obstacle outlines grown by clearance_mm
    paper_mm: np.ndarray            # (4, 2) paper corners
    warnings: list = field(default_factory=list)
    topdown_vis: np.ndarray = None
    dots_mm: np.ndarray = None      # (2, 2) [start, end] dot centers, or None if not detected

    def to_json(self):
        return {
            "path_mm": np.round(self.path_mm, 2).tolist(),
            "kind": [KIND_NAMES[int(k)] for k in self.kind],
            "obstacles_mm": [np.round(o, 1).tolist() for o in self.obstacles_mm],
            "keepouts_mm": [np.round(o, 1).tolist() for o in self.keepouts_mm],
            "paper_mm": np.round(self.paper_mm, 1).tolist(),
            "dots_mm": np.round(self.dots_mm, 1).tolist() if self.dots_mm is not None else None,
            "length_mm": float(path_length(self.path_mm)),
            "warnings": self.warnings,
        }


# ============================================================ geometry helpers
def to_sheet(cam, pts_px):
    return cv2.perspectiveTransform(np.float32(pts_px).reshape(1, -1, 2), cam["H"])[0].astype(float)


def to_pixel(cam, pts_mm):
    return cv2.perspectiveTransform(np.float32(pts_mm).reshape(1, -1, 2), np.linalg.inv(cam["H"]))[0].astype(float)


def path_length(p):
    return float(np.sum(np.linalg.norm(np.diff(p, axis=0), axis=1))) if len(p) > 1 else 0.0


def resample(p, step, kind=None):
    p = np.asarray(p, float)
    if len(p) < 2:
        return (p, kind) if kind is not None else p
    seg = np.linalg.norm(np.diff(p, axis=0), axis=1)
    keep = np.r_[True, seg > 1e-9]
    p = p[keep]
    kind = None if kind is None else np.asarray(kind)[keep]
    s = np.r_[0, np.cumsum(np.linalg.norm(np.diff(p, axis=0), axis=1))]
    n = max(2, int(round(s[-1] / step)) + 1)
    t = np.linspace(0, s[-1], n)
    out = np.c_[np.interp(t, s, p[:, 0]), np.interp(t, s, p[:, 1])]
    if kind is None:
        return out
    k = kind[np.clip(np.searchsorted(s, t, side="right") - 1, 0, len(kind) - 1)]
    return out, k


def smooth(p, window_pts):
    if window_pts < 3 or len(p) < window_pts:
        return p
    w = np.ones(window_pts) / window_pts
    pad = window_pts // 2
    q = np.pad(p, ((pad, pad), (0, 0)), mode="edge")
    out = np.c_[np.convolve(q[:, 0], w, "valid"), np.convolve(q[:, 1], w, "valid")]
    out[0], out[-1] = p[0], p[-1]                    # keep the true endpoints
    return out


def inside(poly, pt):
    return cv2.pointPolygonTest(np.float32(poly).reshape(-1, 1, 2), (float(pt[0]), float(pt[1])), False) >= 0


# ============================================================ top-down working image
class TopDown:
    """Rectified view of the paper area. Pixel (r, c) <-> sheet mm (x, y), far side at the top."""

    def __init__(self, frame, cam, paper_mm, cfg):
        self.s = cfg.px_per_mm
        lo, hi = paper_mm.min(axis=0) - 5, paper_mm.max(axis=0) + 5
        self.xmin, self.ymax = lo[0], hi[1]
        self.w, self.h = int((hi[0] - lo[0]) * self.s), int((hi[1] - lo[1]) * self.s)
        A = np.array([[self.s, 0, -self.s * self.xmin], [0, -self.s, self.s * self.ymax], [0, 0, 1]])
        self.img = cv2.warpPerspective(frame, A @ cam["H"], (self.w, self.h), flags=cv2.INTER_LINEAR)
        self.paper = self.poly_mask(paper_mm)
        m = int(cfg.edge_margin_mm * self.s)
        self.paper_inner = cv2.erode(self.paper, np.ones((2 * m + 1, 2 * m + 1), np.uint8))

    def mm_to_rc(self, p):
        p = np.atleast_2d(p)
        return np.c_[(self.ymax - p[:, 1]) * self.s, (p[:, 0] - self.xmin) * self.s]

    def rc_to_mm(self, rc):
        rc = np.atleast_2d(rc).astype(float)
        return np.c_[self.xmin + rc[:, 1] / self.s, self.ymax - rc[:, 0] / self.s]

    def poly_mask(self, poly_mm):
        m = np.zeros((self.h, self.w), np.uint8)
        rc = self.mm_to_rc(poly_mm)
        cv2.fillPoly(m, [np.int32(np.round(rc[:, ::-1]))], 255)
        return m

    def contour_mm(self, contour):
        c = contour.reshape(-1, 2).astype(float)          # (x=col, y=row)
        return self.rc_to_mm(c[:, ::-1])


# ============================================================ detection
def detect_masks(td, cfg, exclude=None):
    """Returns (ink, obstacle) boolean masks in the top-down image."""
    lab = cv2.cvtColor(td.img, cv2.COLOR_BGR2LAB).astype(np.float32)
    L = lab[..., 0]
    paper_px = td.paper_inner > 0
    if paper_px.sum() < 1000:
        return np.zeros_like(paper_px), np.zeros_like(paper_px)

    # local paper brightness: brightest level within ~40 mm, smoothed, so gradual lighting changes
    # don't read as ink. Floored at 85% of the overall paper level so a big dark object can't
    # pull its own background down and hide itself.
    p90 = float(np.percentile(L[paper_px], 90))
    Lp = np.where(paper_px, L, p90).astype(np.float32)
    kd, kb = int(40 * td.s) | 1, int(15 * td.s) | 1
    bg = cv2.GaussianBlur(cv2.dilate(Lp, np.ones((kd, kd), np.uint8)), (kb, kb), 0)
    bg = np.maximum(bg, 0.85 * p90)
    dark = np.clip(1.0 - L / np.maximum(bg, 1.0), 0, 1)
    a0, b0 = np.median(lab[..., 1][paper_px]), np.median(lab[..., 2][paper_px])
    chroma = np.hypot(lab[..., 1] - a0, lab[..., 2] - b0)

    valid = paper_px.copy()
    if exclude is not None:
        valid &= exclude == 0

    # obstacles: colorful blobs, or dark blobs too thick to be a sharpie stroke
    colorful = (chroma > cfg.obst_chroma) & valid
    darkish = (dark > cfg.ink_strong) & valid
    d = int(cfg.obst_min_width_mm * td.s) | 1
    thick = cv2.morphologyEx(darkish.astype(np.uint8), cv2.MORPH_OPEN,
                             cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (d, d))) > 0
    obst = (colorful | thick).astype(np.uint8)
    obst = cv2.morphologyEx(obst, cv2.MORPH_CLOSE, np.ones((5, 5), np.uint8))
    n, lbl, st, _ = cv2.connectedComponentsWithStats(obst)
    min_area = cfg.obst_min_area_mm2 * td.s ** 2
    obst = np.isin(lbl, [i for i in range(1, n) if st[i, cv2.CC_STAT_AREA] >= min_area])
    pad = int(cfg.obst_pad_mm * td.s)
    if pad > 0 and obst.any():
        obst = cv2.dilate(obst.astype(np.uint8), cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (2 * pad + 1,) * 2)) > 0

    # ink: dark, grey, not obstacle. Hysteresis: weak pixels survive only if touching strong ones
    grey = chroma < cfg.ink_max_chroma
    strong = (dark > cfg.ink_strong) & grey & valid & ~obst
    weak = (dark > cfg.ink_weak) & grey & valid & ~obst
    n, lbl = cv2.connectedComponents(weak.astype(np.uint8), connectivity=8)
    keep = np.unique(lbl[strong])
    ink = np.isin(lbl, keep[keep > 0])
    g = int(cfg.gap_close_mm * td.s) | 1
    ink = cv2.morphologyEx(ink.astype(np.uint8), cv2.MORPH_CLOSE,
                           cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (g, g))) > 0
    ink &= ~obst
    return ink, obst


def thin(mask):
    """Zhang-Suen thinning, vectorised. mask: bool. Returns a 1-px skeleton."""
    img = mask.astype(np.uint8).copy()
    ys, xs = np.where(img)
    if len(ys) == 0:
        return img.astype(bool)
    r0, r1, c0, c1 = max(ys.min() - 2, 0), ys.max() + 3, max(xs.min() - 2, 0), xs.max() + 3
    sub = img[r0:r1, c0:c1]
    while True:
        changed = False
        for step in (0, 1):
            P = np.pad(sub, 1)
            p2, p3, p4, p5 = P[:-2, 1:-1], P[:-2, 2:], P[1:-1, 2:], P[2:, 2:]
            p6, p7, p8, p9 = P[2:, 1:-1], P[2:, :-2], P[1:-1, :-2], P[:-2, :-2]
            B = p2 + p3 + p4 + p5 + p6 + p7 + p8 + p9
            seq = [p2, p3, p4, p5, p6, p7, p8, p9, p2]
            A = sum(((seq[i] == 0) & (seq[i + 1] == 1)).astype(np.uint8) for i in range(8))
            if step == 0:
                c_a, c_b = p2 * p4 * p6, p4 * p6 * p8
            else:
                c_a, c_b = p2 * p4 * p8, p2 * p6 * p8
            m = (sub == 1) & (B >= 2) & (B <= 6) & (A == 1) & (c_a == 0) & (c_b == 0)
            if m.any():
                sub[m] = 0
                changed = True
        if not changed:
            break
    img[r0:r1, c0:c1] = sub
    return img.astype(bool)


def longest_path(pixels):
    """pixels: (N, 2) rows/cols of one skeleton component. Longest geodesic path (drops spurs)."""
    idx = {(int(r), int(c)): i for i, (r, c) in enumerate(pixels)}
    nbr = [[] for _ in range(len(pixels))]
    for i, (r, c) in enumerate(pixels):
        for dr in (-1, 0, 1):
            for dc in (-1, 0, 1):
                if (dr or dc) and (r + dr, c + dc) in idx:
                    nbr[i].append(idx[(r + dr, c + dc)])

    def bfs(s):
        dist = np.full(len(pixels), -1)
        par = np.full(len(pixels), -1)
        dist[s] = 0
        q = deque([s])
        while q:
            u = q.popleft()
            for v in nbr[u]:
                if dist[v] < 0:
                    dist[v], par[v] = dist[u] + 1, u
                    q.append(v)
        return dist, par

    d0, _ = bfs(0)
    a = int(np.argmax(d0))
    da, par = bfs(a)
    b = int(np.argmax(da))
    out = [b]
    while out[-1] != a:
        out.append(int(par[out[-1]]))
    return pixels[out[::-1]]


def extract_segments(td, ink, cfg):
    skel = thin(ink)
    n, lbl = cv2.connectedComponents(skel.astype(np.uint8), connectivity=8)
    segs = []
    for i in range(1, n):
        px = np.argwhere(lbl == i)
        if len(px) < 3:
            continue
        seg = td.rc_to_mm(longest_path(px))
        if path_length(seg) >= cfg.min_segment_mm:
            segs.append(seg)
    return segs


def chain_segments(segs):
    """Order and orient segments to minimise the total hidden (bridged) length."""
    if len(segs) <= 1:
        return segs
    if len(segs) <= 6:
        best, best_cost = None, np.inf
        for order in permutations(range(len(segs))):
            if order[0] > order[-1]:                    # reversed order is the same chain
                continue
            for flips in product((False, True), repeat=len(segs)):
                chain = [segs[i][::-1] if f else segs[i] for i, f in zip(order, flips)]
                cost = sum(np.linalg.norm(chain[j][-1] - chain[j + 1][0]) for j in range(len(chain) - 1))
                if cost < best_cost:
                    best, best_cost = chain, cost
        return best
    rest = sorted(segs, key=path_length, reverse=True)       # greedy for many fragments
    chain = [rest.pop(0)]
    while rest:
        end = chain[-1][-1]
        j, flip = min(((j, f) for j in range(len(rest)) for f in (False, True)),
                      key=lambda jf: np.linalg.norm(end - (rest[jf[0]][-1] if jf[1] else rest[jf[0]][0])))
        s = rest.pop(j)
        chain.append(s[::-1] if flip else s)
    return chain


def build_line(segs, cfg, warnings):
    """Chain segments into one 1 mm-spaced line with ink/bridge labels."""
    chain = chain_segments(segs)
    pts, kind = [], []
    for j, s in enumerate(chain):
        s = smooth(resample(s, 1.0), max(3, int(cfg.smooth_mm) | 1))
        if j > 0:
            gap = np.linalg.norm(s[0] - pts[-1])
            if gap > cfg.max_bridge_mm:
                warnings.append(f"hidden stretch of {gap:.0f} mm between line pieces; check the overlay")
            b = resample(np.vstack([pts[-1], s[0]]), 1.0)[1:-1]
            pts.extend(b)
            kind.extend([BRIDGE] * len(b))
        pts.extend(s)
        kind.extend([INK] * len(s))
    return np.array(pts), np.array(kind)


def detect_endpoint_dots(td, cfg):
    """
    Finds the two big filled dots marking the line's start and end -- distinct from the thin
    ink stroke (much wider) and from obstacles (round; an obstacle isn't assumed to be).
    Simple absolute darkness against the paper is enough here (unlike detect_masks()'s local-
    brightness ink threshold) since these dots are meant to be unambiguous, solid marks.

    Returns (dots_mm, mask): dots_mm is a (2, 2) array of the two dot centers in sheet mm, in
    no particular start/end order yet -- plan_trace() decides that from cfg.start. mask is
    their combined region (dilated a couple mm), to exclude them from ink/obstacle detection.
    Returns (None, empty_mask) if it can't find exactly two plausible dots.
    """
    lab = cv2.cvtColor(td.img, cv2.COLOR_BGR2LAB).astype(np.float32)
    L = lab[..., 0]
    paper_px = td.paper_inner > 0
    empty = np.zeros_like(paper_px, np.uint8)
    if paper_px.sum() < 1000:
        return None, empty

    p90 = float(np.percentile(L[paper_px], 90))
    dark = ((L < 0.6 * p90) & paper_px).astype(np.uint8)

    # The ink stroke usually touches (or overlaps) the dot it starts/ends at, so on the raw dark
    # mask the dot and its line are one connected, non-circular blob. Morphological opening with
    # a kernel narrower than the dot but wider than the line strips the thin stroke off first,
    # leaving just the dot's own disc to measure -- the same "thick vs. thin" trick detect_masks()
    # uses to tell an obstacle from ink, just tuned to the dot's own size here.
    open_d = max(3, int(0.5 * cfg.dot_min_diameter_mm * td.s) | 1)
    opened = cv2.morphologyEx(dark, cv2.MORPH_OPEN, cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (open_d, open_d)))
    n, lbl, st, cent = cv2.connectedComponentsWithStats(opened)
    lo_area = np.pi * (cfg.dot_min_diameter_mm * td.s / 2) ** 2
    hi_area = np.pi * (cfg.dot_max_diameter_mm * td.s / 2) ** 2

    candidates = []
    for i in range(1, n):
        area = st[i, cv2.CC_STAT_AREA]
        if not (lo_area <= area <= hi_area):
            continue
        m = (lbl == i).astype(np.uint8)
        c, _ = cv2.findContours(m, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
        if not c:
            continue
        cnt = max(c, key=cv2.contourArea)
        perim = cv2.arcLength(cnt, True)
        if perim <= 0:
            continue
        circularity = 4 * np.pi * cv2.contourArea(cnt) / (perim ** 2)
        if circularity < cfg.dot_min_circularity:
            continue
        candidates.append((i, circularity))
    if len(candidates) < 2:
        return None, empty

    candidates.sort(key=lambda t: -t[1])          # most circular first
    keep = [i for i, _ in candidates[:2]]
    dots_mm = np.array([td.rc_to_mm([cent[i][1], cent[i][0]])[0] for i in keep])
    mask = np.isin(lbl, keep).astype(np.uint8)
    pad = int(2 * td.s)
    mask = cv2.dilate(mask, cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (2 * pad + 1,) * 2))
    return dots_mm, mask


def obstacle_polys(td, obst, cfg):
    obstacles, keepouts = [], []
    n, lbl = cv2.connectedComponents(obst.astype(np.uint8))
    r = int(cfg.clearance_mm * td.s)
    ker = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (2 * r + 1, 2 * r + 1))
    for i in range(1, n):
        m = (lbl == i).astype(np.uint8)
        c, _ = cv2.findContours(m, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
        obstacles.append(td.contour_mm(max(c, key=cv2.contourArea)))
        grown = cv2.dilate(m, ker)
        c, _ = cv2.findContours(grown, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
        keepouts.append(resample(td.contour_mm(max(c, key=cv2.contourArea)), 1.0))
    return obstacles, keepouts


# ============================================================ planning
def _push_clear(p, ko, margin_mm):
    """
    Nudges p directly away from the keepout polygon's centroid by
    margin_mm, so it starts with real clearance from the boundary rather
    than sitting right on top of it (a point that's technically outside a
    polygon by a fraction of a mm is numerically indistinguishable from
    "on the boundary" once it hits a discretized grid). Safe for a
    roughly convex, blob-shaped obstacle -- which is what this is for.
    """
    center = ko.mean(axis=0)
    d = np.asarray(p, float) - center
    n = np.linalg.norm(d)
    if n < 1e-6:
        return np.asarray(p, float)
    return np.asarray(p, float) + (d / n) * margin_mm


def _push_until_clear(p, ko, step_mm=0.5, margin_mm=1.0, max_steps=400):
    """
    If p is inside ko, walks it directly away from the centroid until
    it's outside, plus a small margin. Final correctness guarantee for
    _astar_arc(): the grid search's blocking test is only an
    approximation of the true polygon (a discrete ring of circles, plus
    the classic grid-search failure mode where an 8-connected diagonal
    move can "corner-cut" through the gap between two diagonally-blocked
    cells without ever landing on a blocked one) -- so a returned arc can
    still clip the exact polygon in rare spots even though the search
    "thought" it was clear. This repairs that directly against the same
    inside() test used everywhere else, rather than trusting the
    approximation.
    """
    center = ko.mean(axis=0)
    p = np.asarray(p, float)
    if not inside(ko, p):
        return p
    d = p - center
    n = np.linalg.norm(d)
    direction = d / n if n > 1e-6 else np.array([1.0, 0.0])
    for _ in range(max_steps):
        p = p + direction * step_mm
        if not inside(ko, p):
            return p + direction * margin_mm
    return p  # obstacle bigger than expected; best effort


def _astar_arc(a, b, ko, paper_mm, grid_step_cm=0.15, boundary_spacing_mm=4.0,
                push_margin_mm=2.0):
    """
    Weighted-A* detour from a to b around keepout polygon ko, bounded by
    the paper -- via grid_astar_replanner.find_detour_path(). Units: this
    module works in mm, grid_astar_replanner works in cm, so everything
    is converted at the boundary.

    The keepout (already the obstacle's outline grown by the clearance
    margin) is passed to A* as a ring of point obstacles sampled evenly
    around its actual boundary at roughly grid resolution, rather than
    approximated as one circle -- a circle either has to be big enough to
    cover the polygon's farthest point (which can then wrongly trap a
    start/goal that's legitimately just outside the real polygon nearby)
    or risks cutting across the polygon's true shape wherever it bulges
    past the circle. a and b are also nudged a small margin clear of the
    boundary first: a point that's technically outside the polygon by a
    fraction of a mm is otherwise indistinguishable from being on top of
    it once everything is snapped to the grid. The returned arc still
    starts/ends at the original a/b, not the nudged points.
    """
    a, b = np.asarray(a, float), np.asarray(b, float)
    a_clear = _push_clear(a, ko, push_margin_mm)
    b_clear = _push_clear(b, ko, push_margin_mm)

    boundary_mm = resample(ko, boundary_spacing_mm)
    point_radius_cm = 0.65 * boundary_spacing_mm / 10.0  # overlap between neighbors
    lo, hi = paper_mm.min(axis=0), paper_mm.max(axis=0)

    to_cm = lambda p: (p[0] / 10.0, p[1] / 10.0)  # noqa: E731
    result_cm = find_detour_path(
        start=to_cm(a_clear),
        goal=to_cm(b_clear),
        obstacles=[to_cm(p) for p in boundary_mm],
        workspace_bounds=(lo[0] / 10.0, lo[1] / 10.0, hi[0] / 10.0, hi[1] / 10.0),
        safety_radius_cm=point_radius_cm,
        grid_step_cm=grid_step_cm,
    )
    if result_cm is None:
        return None
    arc_mm = np.array(result_cm) * 10.0  # cm -> mm

    # A*'s shortcut smoothing can leave a few widely-spaced waypoints whose
    # endpoints are clear but whose straight chord between them clips the
    # true polygon (the point-ring model is an approximation, and 8-
    # connected grid search can also "corner-cut" diagonally). Densify
    # first so every few mm of the actual path gets checked, not just the
    # sparse waypoints, then repair anything that's still inside.
    full = np.vstack([a, arc_mm, b])
    dense = resample(full, 1.0)
    dense = np.array([_push_until_clear(p, ko) for p in dense])
    return dense


def _nearest_idx(pts, p):
    return int(np.argmin(np.linalg.norm(pts - np.asarray(p, float), axis=1)))


def _boundary_arc(a, b, ko, push_margin_mm=2.0):
    """
    Bug-algorithm-style detour: walk the keepout polygon's own boundary from
    the point nearest a to the point nearest b, taking whichever direction
    (clockwise/counterclockwise) is shorter. No search -- O(len(ko)) -- and
    reliable for a single blob-shaped obstacle, which is the case this
    challenge actually has. This is the PRIMARY detour method; _astar_arc()
    is only the fallback for cases a simple boundary walk can't handle
    (obstacle shape means the walk would leave the paper).
    """
    a, b = np.asarray(a, float), np.asarray(b, float)
    a_c, b_c = _push_clear(a, ko, push_margin_mm), _push_clear(b, ko, push_margin_mm)
    n = len(ko)
    ia, ib = _nearest_idx(ko, a_c), _nearest_idx(ko, b_c)
    fwd = ko[[(ia + k) % n for k in range((ib - ia) % n + 1)]]
    bwd = ko[[(ia - k) % n for k in range((ia - ib) % n + 1)]]
    arc = fwd if path_length(fwd) <= path_length(bwd) else bwd
    full = np.vstack([a[None], a_c[None], arc, b_c[None], b[None]])
    dense = resample(full, 1.0)
    return np.array([_push_until_clear(p, ko) for p in dense])


def _leaves_paper(arc, paper_mm, margin_mm=2.0):
    hull = cv2.convexHull(np.float32(paper_mm)).reshape(-1, 1, 2)
    return any(cv2.pointPolygonTest(hull, (float(x), float(y)), True) < -margin_mm for x, y in arc)


def detour(line, kind, keepouts, paper_mm, warnings):
    """
    Replace every stretch of the line inside a keep-out with an in-plane
    route around it. Priority order, cheapest/most-reliable first:
      1. boundary walk (_boundary_arc) -- fast, robust for one blob obstacle
      2. weighted A* (_astar_arc) -- fallback if (1) would leave the paper
      3. OVER -- last resort if neither finds an in-plane route: lift the
         pen and travel straight over the obstacle instead of through it.
         The ink line gets a gap there; that's physically unavoidable once
         no route around the obstacle exists on the paper.
    Never returns a path that goes straight through the obstacle.
    """
    path, pk = line.copy(), kind.copy()
    for ko in keepouts:
        ko = np.asarray(ko)
        ins = np.array([inside(ko, p) for p in path])
        if not ins.any():
            continue
        runs, i = [], 0
        while i < len(ins):
            if ins[i]:
                j = i
                while j + 1 < len(ins) and ins[j + 1]:
                    j += 1
                runs.append((i, j))
                i = j + 1
            else:
                i += 1
        new_p, new_k, last = [], [], 0
        for (i, j) in runs:
            if i == 0 and j == len(path) - 1:
                # the entire remaining path is inside this keepout -- trimming both ends would
                # leave nothing to trace at all. Go OVER the whole stretch instead of vanishing
                # it (same last-resort logic as a normal mid-path detour failure, just applied
                # to the full remaining path rather than one segment).
                warnings.append("an obstacle's clearance zone covers the entire remaining path; "
                                 "going OVER all of it instead of trimming everything away")
                new_p.append(path); new_k.append(np.full(len(path), OVER))
                last = len(path)
                continue
            if i == 0 or j == len(path) - 1:
                warnings.append("the line starts or ends inside an obstacle's clearance zone; that end is trimmed")
                if i == 0:
                    last = j + 1
                    continue
                new_p.append(path[last:i]); new_k.append(pk[last:i]); last = len(path)
                continue
            a, b = path[i - 1], path[j + 1]
            arc, arc_kind, method = None, DETOUR, "boundary walk"
            candidate = _boundary_arc(a, b, ko)
            if not _leaves_paper(candidate, paper_mm):
                arc = candidate
            else:
                method = "weighted A*"
                candidate = _astar_arc(a, b, ko, paper_mm)
                if candidate is not None and not _leaves_paper(candidate, paper_mm):
                    arc = candidate
            if arc is None:
                warnings.append("no in-plane route around an obstacle fit on the paper "
                                 "(boundary walk and weighted A* both failed); "
                                 "going OVER it instead -- the line will have a gap there")
                arc, arc_kind, method = resample(np.vstack([a, b]), 1.0), OVER, "over"
            new_p += [path[last:i], arc]
            new_k += [pk[last:i], np.full(len(arc), arc_kind)]
            last = j + 1
        new_p.append(path[last:]); new_k.append(pk[last:])
        path = np.vstack([q for q in new_p if len(q)])
        pk = np.concatenate([q for q in new_k if len(q)])
    return path, pk


def find_paper(frame, cam):
    """
    Finds the paper's actual corners this frame (camera_calib.find_paper_quad() -- plain Otsu
    threshold + largest contour + a 4-corner fit, the same technique check() already uses to
    sanity-check a calibration), converted to sheet mm through the already-calibrated homography
    -- not a separately-derived one. Falls back to the calibration sheet's assumed position only
    if a clean 4-corner quad isn't found this frame (e.g. the paper's blob is touching another
    bright object in view -- that needs a physical fix, not a threshold tweak).
    """
    import camera_calib as cc
    quad = cc.find_paper_quad(frame)
    if quad is not None:
        return to_sheet(cam, quad), True
    return np.array([[0, 0], [PAPER_W, 0], [PAPER_W, PAPER_L], [0, PAPER_L]], float), False


def _exclusion(td, cam, boxes_px, cfg):
    if not boxes_px:
        return None
    m = np.zeros((td.h, td.w), np.uint8)
    for x1, y1, x2, y2 in boxes_px:
        m |= td.poly_mask(to_sheet(cam, [[x1, y1], [x2, y1], [x2, y2], [x1, y2]]))
    p = int(cfg.exclude_pad_mm * td.s)
    return cv2.dilate(m, np.ones((2 * p + 1, 2 * p + 1), np.uint8))


def _finish(line, lk, obstacles, keepouts, paper, cfg, warnings, td, ink=None, obst=None, dots_mm=None):
    path, pk = detour(line, lk, keepouts, paper, warnings)
    path, pk = resample(path, cfg.step_mm, pk)
    # detour()'s own correctness pass (_push_until_clear) runs on the 1mm-densified arc BEFORE
    # this resample -- resampling to cfg.step_mm afterward can reintroduce a sub-mm clip near a
    # keepout corner (confirmed directly: up to ~0.45mm on a real replan() case) since it isn't
    # re-verified after. Cheap final repair, same test used everywhere else (inside()).
    for ko in keepouts:
        ko = np.asarray(ko)
        path = np.array([_push_until_clear(p, ko) if inside(ko, p) else p for p in path])
    plan = Plan(path, pk, line, lk, obstacles, keepouts, paper, warnings, dots_mm=dots_mm)
    plan.topdown_vis = draw_topdown(td, plan, ink, obst)
    return plan


def plan_trace(frame, cam, cfg=None, exclude_boxes_px=None):
    """Full plan from one frame. Take the frame with the arm parked out of view if possible."""
    cfg = cfg or PlanConfig()
    warnings = []
    paper, found = find_paper(frame, cam)
    if not found:
        warnings.append("paper outline not detected; using the calibration sheet's position")
    td = TopDown(frame, cam, paper, cfg)
    dots_mm, dot_mask = detect_endpoint_dots(td, cfg)
    excl = _exclusion(td, cam, exclude_boxes_px, cfg)
    excl = dot_mask if excl is None else (excl | dot_mask)
    ink, obst = detect_masks(td, cfg, excl)
    segs = extract_segments(td, ink, cfg)
    if not segs:
        debug = td.img.copy()
        debug[ink] = (0.4 * debug[ink] + 0.6 * np.array([255, 200, 0])).astype(np.uint8)
        debug[obst] = (0.4 * debug[obst] + 0.6 * np.array([0, 0, 255])).astype(np.uint8)
        cv2.imwrite("plan_topdown.png", debug)
        raise RuntimeError("no line found on the paper. Saved plan_topdown.png (orange = detected "
                            "ink pixels, red = detected obstacle/dot pixels) -- check whether the "
                            "line shows up there at all, and if so, why it's not orange.")
    line, lk = build_line(segs, cfg, warnings)
    if dots_mm is not None:
        # cfg.start still decides WHICH physical dot is the start (far/near the camera) --
        # dots just make that point exact instead of guessed from the ink's own endpoint.
        far_dot_first = dots_mm[0][1] > dots_mm[1][1]
        if (cfg.start == "far") != far_dot_first:
            dots_mm = dots_mm[::-1]
        if np.linalg.norm(line[-1] - dots_mm[0]) < np.linalg.norm(line[0] - dots_mm[0]):
            line, lk = line[::-1], lk[::-1]
        line = np.vstack([dots_mm[0], line, dots_mm[1]])
        lk = np.r_[INK, lk, INK]
    else:
        warnings.append("start/end dots not detected; used the near/far heuristic instead")
        far_first = line[0, 1] < line[-1, 1]
        if (cfg.start == "far") == far_first:
            line, lk = line[::-1], lk[::-1]
    obstacles, keepouts = obstacle_polys(td, obst, cfg)
    if not obstacles:
        warnings.append("no obstacle detected")
    return _finish(line, lk, obstacles, keepouts, paper, cfg, warnings, td, ink, obst, dots_mm)


def replan(plan, current_xy, frame, cam, cfg=None, exclude_boxes_px=None):
    """
    Mid-task: keep the already-known line, re-detect obstacles in a fresh frame and reroute the part
    not yet traced. Pass the arm's box (and the held marker's) in exclude_boxes_px so the arm itself
    isn't taken for an obstacle. Returns a new Plan starting at current_xy.
    """
    cfg = cfg or PlanConfig()
    warnings = []
    td = TopDown(frame, cam, plan.paper_mm, cfg)
    _, obst = detect_masks(td, cfg, _exclusion(td, cam, exclude_boxes_px, cfg))
    k = int(np.argmin(np.linalg.norm(plan.line_mm - np.asarray(current_xy), axis=1)))
    line = np.vstack([np.asarray(current_xy, float), plan.line_mm[k + 1:]])
    lk = np.r_[INK, plan.line_kind[k + 1:]]
    obstacles, keepouts = obstacle_polys(td, obst, cfg)
    return _finish(line, lk, obstacles, keepouts, plan.paper_mm, cfg, warnings, td, None, obst, plan.dots_mm)


def path_blocked(plan, frame, cam, cfg=None, exclude_boxes_px=None, from_index=0):
    """Cheap check during tracing: does any obstacle in this frame hit the remaining path?"""
    cfg = cfg or PlanConfig()
    td = TopDown(frame, cam, plan.paper_mm, cfg)
    _, obst = detect_masks(td, cfg, _exclusion(td, cam, exclude_boxes_px, cfg))
    if not obst.any():
        return False
    _, keepouts = obstacle_polys(td, obst, cfg)
    rest = plan.path_mm[from_index:]
    return any(inside(ko, p) for ko in keepouts for p in rest[::2])


# ============================================================ drawing
COL = {INK: (0, 200, 0), BRIDGE: (0, 200, 255), DETOUR: (255, 0, 255), OVER: (0, 128, 255)}


def draw_topdown(td, plan, ink=None, obst=None):
    vis = td.img.copy()
    if ink is not None:
        vis[ink] = (0.4 * vis[ink] + 0.6 * np.array([255, 200, 0])).astype(np.uint8)
    if obst is not None:
        vis[obst] = (0.4 * vis[obst] + 0.6 * np.array([0, 0, 255])).astype(np.uint8)
    for ko in plan.keepouts_mm:
        cv2.polylines(vis, [np.int32(td.mm_to_rc(ko)[:, ::-1])], True, (0, 0, 255), 1)
    rc = np.int32(td.mm_to_rc(plan.path_mm)[:, ::-1])
    for i in range(len(rc) - 1):
        cv2.line(vis, tuple(rc[i]), tuple(rc[i + 1]), COL[int(plan.kind[i])], 2)
    cv2.circle(vis, tuple(rc[0]), 8, (0, 255, 0), -1)
    cv2.circle(vis, tuple(rc[-1]), 8, (0, 0, 255), -1)
    return vis


def draw_plan(frame, cam, plan):
    vis = frame.copy()
    for ko in plan.keepouts_mm:
        cv2.polylines(vis, [np.int32(to_pixel(cam, ko))], True, (0, 0, 255), 1)
    px = np.int32(np.round(to_pixel(cam, plan.path_mm)))
    for i in range(len(px) - 1):
        cv2.line(vis, tuple(px[i]), tuple(px[i + 1]), COL[int(plan.kind[i])], 2)
    cv2.circle(vis, tuple(px[0]), 7, (0, 255, 0), -1)
    cv2.circle(vis, tuple(px[-1]), 7, (0, 0, 255), -1)
    y = 30
    for text, col in [("green: on the line", COL[INK]), ("orange: hidden stretch, bridged", COL[BRIDGE]),
                      ("magenta: detour (around)", COL[DETOUR]), ("blue: over (pen lifted, gap in line)", COL[OVER]),
                      ("red outline: clearance zone", (0, 0, 255))]:
        cv2.putText(vis, text, (15, y), cv2.FONT_HERSHEY_SIMPLEX, 0.55, col, 2)
        y += 24
    return vis


# ============================================================ CLI
def _grab():
    import camera_device as cd
    backend = cv2.CAP_DSHOW if sys.platform.startswith("win") else cv2.CAP_ANY
    cap = cv2.VideoCapture(cd.resolve_index(), backend)
    cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280)
    cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720)
    for _ in range(10):
        cap.read()
    frames = [cap.read()[1] for _ in range(15)]
    cap.release()
    return np.median(np.stack(frames), axis=0).astype(np.uint8)


if __name__ == "__main__":
    import camera_calib as cc
    cam = cc.load(CAMERA_CALIB_PATH)
    frame = cv2.imread(sys.argv[1]) if len(sys.argv) > 1 else _grab()
    if frame is None or frame.shape[:2] != (720, 1280):
        raise SystemExit("need a 1280x720 frame")
    plan = plan_trace(frame, cam)
    json.dump(plan.to_json(), open("plan.json", "w"), indent=1)
    cv2.imwrite("plan_overlay.png", draw_plan(frame, cam, plan))
    cv2.imwrite("plan_topdown.png", plan.topdown_vis)
    k = plan.kind
    print(f"path: {len(plan.path_mm)} waypoints, {path_length(plan.path_mm):.0f} mm "
          f"(ink {np.mean(k == INK):.0%}, bridged {np.mean(k == BRIDGE):.0%}, "
          f"detour {np.mean(k == DETOUR):.0%}, over {np.mean(k == OVER):.0%})")
    print(f"start {np.round(plan.path_mm[0], 1)} -> end {np.round(plan.path_mm[-1], 1)} mm, "
          f"{len(plan.obstacles_mm)} obstacle(s)")
    for w in plan.warnings:
        print("warning:", w)
    print("saved plan.json, plan_overlay.png, plan_topdown.png")