"""Door robot arm: can a bigger arm hold the doors at their centre? (2026-10-18; plain Python, no Blender)

Uses dra_fit.py (the scaled design, the rig's own inverse kinematics) and adds rough clearances: the lower arm as a box
from J2 to J3 (half width 0.46 S across the arm's plane, half depth 0.28 S in it), the forearm as a box from J3 to the
wrist centre (0.24 S, 0.22 S). Against the door's inner face (its real section, door_centre.json), the pocket's end
wall (a plane) and the joint limits. Checked on the old design first: at S 1 with the old attach point it gives the
end wall 0.247 m where rig_check measured 0.2436 m, and the door 1.66 m clear.

For each scale S and attach point (x along the door from its middle, arc offset up the curved section from its
middle), a dynamic programme looks for a carriage schedule along the door's slide (continuous, either direction, at most
1.5x the door's speed) that keeps every clearance; the score is the worst of (door gap - 0.05 m, end wall gap -
0.10 m, (joint margin - 3 deg) / 50): >= 0 means a collision-free fit exists.

    python build/dra_fit_map.py map        -> build/fit_map.json and the table
    python build/dra_fit_map.py section    -> previews/scale_up/fit_section.png (the door's section, mid-slide)
"""
import os
import sys
import json
import math
import copy

HERE = os.path.dirname(os.path.abspath(__file__))
ROOT = os.path.dirname(HERE)
sys.path.insert(0, HERE)
import dra_fit as F       # noqa: E402

DC0 = copy.deepcopy(F.DC)
LIM = F.LIMITS
MARG = dict(door=0.05, wall=0.10, joint=3.0)


def point_at(key, s_off):
    """the inner face's point s_off up the arc from its middle (toward the top edge) and its normal (local y, z)"""
    c = DC0[key]["inner_curve_yz"]
    L = [0.0]
    for i in range(1, len(c)):
        L.append(L[-1] + math.dist(c[i], c[i - 1]))
    target = L[-1] / 2 - s_off
    for i in range(1, len(c)):
        if L[i] >= target:
            f = (target - L[i - 1]) / (L[i] - L[i - 1])
            p = (c[i - 1][0] + (c[i][0] - c[i - 1][0]) * f, c[i - 1][1] + (c[i][1] - c[i - 1][1]) * f)
            t = (c[i][0] - c[i - 1][0], c[i][1] - c[i - 1][1])
            tl = math.hypot(*t)
            n = (-t[1] / tl, t[0] / tl)
            n0 = DC0[key]["normal_local"]
            if n[0] * n0[1] + n[1] * n0[2] < 0:
                n = (-n[0], -n[1])
            return p, n


def set_attach(x_off, s_off):
    """x_off: along the door from its middle toward the pocket's end wall (AFT local -x, FORE local +x)"""
    for key, sg in (("AFT", -1.0), ("FORE", 1.0)):
        p, n = point_at(key, s_off)
        F.DC[key]["attach_local"] = [DC0[key]["x_mid"] + sg * x_off, p[0], p[1]]
        F.DC[key]["normal_local"] = [0.0, n[0], n[1]]


def solve_c(D, key, ver, loc, c):
    K = D["kinematics"]
    dr = D["doors"][key]
    wc = dr["wc_local"][ver]
    yb, zb = D["carriage"]["centre_y"], D["carriage"]["underside_z"]
    dx, dy, dz = loc
    X, Y, H = dx + wc[0] - c, (yb - wc[1]) - dy, (zb - wc[2]) - dz
    q1 = math.atan2(Y, X)
    R, Hh = math.hypot(X, Y) + K["j2_r"], H - K["j2_h"]
    L2, L3, beta, phi_l0 = K["L2"], K["L3"], K["beta_rad"], K["phi_l0_rad"]
    Dd = math.hypot(R, Hh)
    cc = (L2 * L2 - L3 * L3 + Dd * Dd) / (2 * L2 * Dd)
    if abs(cc) >= 0.999:
        return None
    phi_l = math.atan2(Hh, R) + math.acos(cc)
    phi_f = math.atan2(Hh - L2 * math.sin(phi_l), R - L2 * math.cos(phi_l)) - beta
    xt, yt, zt = dr["tool_axes_C"]["x_t"], dr["tool_axes_C"]["y_t"], dr["tool_axes_C"]["z_t"]
    P = lambda v: math.cos(q1) * v[0] + math.sin(q1) * v[1]
    M11 = math.cos(phi_f) * P(xt) + math.sin(phi_f) * xt[2]
    M21 = -math.sin(q1) * xt[0] + math.cos(q1) * xt[1]
    M31 = math.cos(phi_f) * xt[2] - math.sin(phi_f) * P(xt)
    M12 = math.cos(phi_f) * P(yt) + math.sin(phi_f) * yt[2]
    M13 = math.cos(phi_f) * P(zt) + math.sin(phi_f) * zt[2]
    q = dict(J2=math.degrees(phi_l - phi_l0), J3=math.degrees(phi_f - phi_l + phi_l0),
             J4=math.degrees(math.atan2(-M21, M31)), J5=math.degrees(-math.acos(max(-1, min(1, M11)))),
             J6=math.degrees(math.atan2(-M12, -M13)))
    W = lambda r, h: (c + r * math.cos(q1), yb - r * math.sin(q1), zb - h)
    j2 = (-K["j2_r"], K["j2_h"])
    j3 = (j2[0] + L2 * math.cos(phi_l), j2[1] + L2 * math.sin(phi_l))
    w = (j2[0] + R, j2[1] + Hh)
    return q, dict(J2=W(*j2), J3=W(*j3), WC=W(*w)), q1


def box(a, b, u, lat, dep):
    ax = [b[i] - a[i] for i in range(3)]
    L = math.sqrt(sum(v * v for v in ax))
    ax = [v / L for v in ax]
    l = (u[1], -u[0], 0.0)
    p = (l[1] * ax[2] - l[2] * ax[1], l[2] * ax[0] - l[0] * ax[2], l[0] * ax[1] - l[1] * ax[0])
    return [tuple(e[i] + sl * lat * l[i] + sd * dep * p[i] for i in range(3)) for e in (a, b) for sl in (-1, 1) for sd in (-1, 1)]


def metrics(D, S, key, loc, c, vers=("A", "B")):
    ew = D["scene"]["end_wall_faces_x"][key]
    sgx = 1.0 if key == "AFT" else -1.0
    curve = F.door_curve(key, loc)
    lx0, lx1 = DC0[key]["length_x"]
    n, a = F.DC[key]["normal_local"], F.DC[key]["attach_local"]
    side = 1.0 if F.side_dist((0, loc[1] + a[1] + n[1] * 0.3, loc[2] + a[2] + n[2] * 0.3), curve) > 0 else -1.0
    worst = [9.0, 9.0, 99.0]
    for ver in vers:
        r = solve_c(D, key, ver, loc, c)
        if r is None:
            return [-9.0, -9.0, -99.0]
        q, Pt, q1 = r
        jm = min(min(q[j] - LIM[j][0], LIM[j][1] - q[j]) for j in q)
        u = (math.cos(q1), -math.sin(q1))
        cs = box(Pt["J2"], Pt["J3"], u, 0.46 * S, 0.28 * S) + box(Pt["J3"], Pt["WC"], u, 0.24 * S, 0.22 * S)[:4]
        dg = min([side * F.side_dist(p, curve) for p in cs if loc[0] + lx0 <= p[0] <= loc[0] + lx1] or [9.0])
        wl = min([sgx * (p[0] - ew) for p in cs] + [sgx * (c - ew) - F.HALF0 * S])
        worst = [min(worst[0], dg), min(worst[1], wl), min(worst[2], jm)]
    return worst


def score(m):
    return min(m[0] - MARG["door"], m[1] - MARG["wall"], (m[2] - MARG["joint"]) / 50.0)


def schedule(S, x_off, s_off, n_c=31, bmax=1.5):
    """the best continuous carriage schedule along the AFT door's path (the FORE side is its mirror)"""
    set_attach(x_off, s_off)
    D, _ = F.scaled(S, -7.0)
    rail = D["scene"]["rail"]["usable_AFT"]
    cmax = rail[1] - F.GAP_INNER - F.HALF0 * S
    cmin = D["scene"]["end_wall_faces_x"]["AFT"] + 0.05 + F.HALF0 * S
    cs = [cmin + (cmax - cmin) * k / (n_c - 1) for k in range(n_c)]
    dz = -7.503
    xs = [0.2216 - k * 0.25 for k in range(25)] + [-5.9179]
    ph1 = [min(score(metrics(D, S, "AFT", (0.2216, y, dz), c)) for y in (-6.1, -5.8, -5.5, -5.2, -4.88)) for c in cs]
    stage = [[score(metrics(D, S, "AFT", (x, -4.88, dz), c)) for c in cs] for x in xs]
    stage[0] = [min(a, b) for a, b in zip(stage[0], ph1)]
    best = [(stage[0][j], [j]) for j in range(n_c)]
    for k in range(1, len(xs)):
        ddx = xs[k - 1] - xs[k]
        nb = []
        for j in range(n_c):
            cand = None
            for i in range(n_c):
                if abs(cs[i] - cs[j]) > bmax * ddx + 1e-9:
                    continue
                v = min(best[i][0], stage[k][j])
                if cand is None or v > cand[0]:
                    cand = (v, best[i][1] + [j])
            nb.append(cand or (-99.0, [j]))
        best = nb
    v, path = max(best, key=lambda b: b[0])
    worst = min(((metrics(D, S, "AFT", (x, -4.88, dz), cs[j]), x) for x, j in zip(xs, path)), key=lambda t: score(t[0]))
    return dict(score=round(v, 3), worst_door_m=round(worst[0][0], 3), worst_wall_m=round(worst[0][1], 3),
                worst_joint_margin_deg=round(worst[0][2], 1), at_door_x=round(worst[1], 3),
                carriage=[[round(x, 3), round(cs[j], 3)] for x, j in zip(xs, path)])


def run_map():
    out = {}
    print("score >= 0: a collision-free carriage schedule exists (door >= %.2f m, end wall >= %.2f m, joints >= %.0f deg)"
          % (MARG["door"], MARG["wall"], MARG["joint"]))
    for S in (1.0, 1.1, 1.2, 1.3, 1.4, 1.5):
        for x_off in (0.0, 0.7, 1.5, 2.0):
            row = []
            for s_off in (0.0, 1.6, 2.4, 3.2):
                r = schedule(S, x_off, s_off)
                out["S%.2f x%.1f s%.1f" % (S, x_off, s_off)] = r
                row.append("%+.2f" % r["score"])
            print("S %.2f  plate %.1f m from the door's middle along x | arc 0.0 / 1.6 / 2.4 / 3.2 m up: %s"
                  % (S, x_off, "  ".join(row)), flush=True)
    with open(os.path.join(HERE, "fit_map.json"), "w") as fh:
        json.dump(out, fh, indent=1)


def run_section():
    """a picture: the door's section at mid-slide (frame 30) with the arm, as now and scaled 1.5x to the centre"""
    from PIL import Image, ImageDraw
    W_, H_ = 1400, 900
    im = Image.new("RGB", (W_, H_), (250, 250, 248))
    dr = ImageDraw.Draw(im)
    loc = None
    for f, x, y, z in F.DP["AFT"]:
        if abs(f - 30.0) < 1e-6:
            loc = (x, y, z)
    panels = [("as now: 1.0x, the old attach point (high on the door)", 1.0, None, 0),
              ("1.5x, the plate at the door's centre", 1.5, (0.0, 0.0), 700)]
    for title, S, att, ox in panels:
        if att is None:
            for key in ("AFT", "FORE"):
                F.DC[key]["attach_local"] = F.BASE["doors"][key]["attach_local"]
                F.DC[key]["normal_local"] = F.BASE["doors"][key]["normal_local"]
            D, _ = F.scaled(S, F.BASE["doors"]["AFT"]["carriage"]["c_open"])
            c = F.BASE["doors"]["AFT"]["carriage"]["A"] + F.BASE["doors"]["AFT"]["carriage"]["B"] * loc[0]
        else:
            set_attach(*att)
            D, _ = F.scaled(S, -7.0)
            sch = schedule(S, *att)["carriage"]
            c = min(sch, key=lambda t: abs(t[0] - loc[0]))[1]
        # y-z view seen from the moon pool (+x): y to the left = inboard on the right
        sc_ = 95.0
        T = lambda y, z: (ox + 350 + (y + 5.8) * -sc_, 60 + (-3.6 - z) * sc_)
        dr.text((ox + 20, 20), title, fill=(20, 20, 20))
        deck = D["scene"]["deck_underside_z"]
        dr.line([T(-9.5, deck), T(-2.0, deck)], fill=(90, 90, 90), width=3)
        dr.text(T(-2.4, deck - 0.05), "deck", fill=(90, 90, 90))
        curve = [T(y, z) for y, z in F.door_curve("AFT", loc)]
        dr.line(curve, fill=(40, 90, 200), width=5)
        dr.text(curve[len(curve) // 3], "  door's inner face (door pulled in, mid-slide)", fill=(40, 90, 200))
        r = solve_c(D, "AFT", "B", loc, c)
        if r is None:
            continue
        q, Pt, q1 = r
        u = (math.cos(q1), -math.sin(q1))
        for (a, b, lat, dep, col) in ((Pt["J2"], Pt["J3"], 0.46 * S, 0.28 * S, (200, 60, 50)),
                                      (Pt["J3"], Pt["WC"], 0.24 * S, 0.22 * S, (230, 140, 40))):
            cs = box(a, b, u, lat, dep)
            ys = [p[1] for p in cs]
            zs = [p[2] for p in cs]
            dr.rectangle([T(max(ys), max(zs)), T(min(ys), min(zs))], outline=col, width=3)
        dr.line([T(D["carriage"]["centre_y"], D["carriage"]["underside_z"]), T(Pt["J2"][1], Pt["J2"][2]),
                 T(Pt["J3"][1], Pt["J3"][2]), T(Pt["WC"][1], Pt["WC"][2])], fill=(20, 20, 20), width=3)
        m = metrics(D, S, "AFT", loc, c, vers=("B",))
        dr.text((ox + 20, 40), "door gap %.2f m (negative = inside the door), end wall gap %.2f m, joints %.0f deg from limits"
                % (m[0], m[1], m[2]), fill=(150, 30, 30) if m[0] < 0 else (30, 120, 30))
        dr.text((ox + 20, H_ - 30), "boxes: the lower arm (red) and the forearm (orange) seen along the rail", fill=(60, 60, 60))
    out = os.path.join(ROOT, "previews", "scale_up")
    os.makedirs(out, exist_ok=True)
    im.save(os.path.join(out, "fit_section.png"))
    print("written", os.path.join(out, "fit_section.png"))


if __name__ == "__main__":
    if len(sys.argv) > 1 and sys.argv[1] == "section":
        run_section()
    else:
        run_map()