"""Door robot arm: the fit's proof for one design before building (2026-10-18, scale-up and door-centre attach).

Every half frame of both doors' animation (1-120), both versions: the rig's inverse kinematics with the design's
linear carriage schedule, the arm's full surface points against the ship's surroundings (obstacle grid, instances
included) and both doors; joint values against their limits; the J3 cylinder's window; the carriage inside its travel.
Writes build/fit3d_proof.json.

    python build/dra_fit3d_proof.py '<design json>' c_closed c_open
"""
import os
import sys
import json
import math
HERE = os.path.dirname(os.path.abspath(__file__))
sys.path.insert(0, HERE)
os.environ["DRA_FIT_SUB"] = "0"
import dra_fit3d as F3     # noqa: E402

LIM1 = (-170.0, 170.0)


def proof(d, c_closed, c_open, step=2):
    Dz = F3.make_design(**d)
    out = dict(design=d, c_closed=c_closed, c_open=c_open, d_t=Dz["d_t"], root_z=Dz["root_z"], per={})
    for key in ("AFT", "FORE"):
        dr = F3.BASE["doors"][key]
        cc = c_closed if key == "AFT" else 2 * 3.05 - c_closed
        co = c_open if key == "AFT" else 2 * 3.05 - c_open
        B = (cc - co) / (dr["closed_x"] - dr["open_x"])
        A = cc - B * dr["closed_x"]
        for ver in ("A", "B"):
            r = dict(A=A, B=B, frames=0, lev_min=[99, "", "", None], door_min=[9.0, "", None],
                     joints={}, margin_min=[99.0, "", None], q1=[999, -999], L_over_S=[9, -9])
            for f, x, y, z in F3.DP[key][::step]:
                c = A + B * x
                q = F3.ik(Dz, key, ver, (x, y, z), c)
                r["frames"] += 1
                if q is None:
                    r["margin_min"] = [-99.0, "out of reach", f]
                    continue
                for j in ("q2", "q3", "q4", "q5", "q6"):
                    v = math.degrees(q[j])
                    lo, hi = (Dz["stroke"] if j == "q3" else F3.LIM[j])
                    a = r["joints"].setdefault(j, [v, v])
                    a[0], a[1] = min(a[0], v), max(a[1], v)
                    m = min(v - lo, hi - v)
                    if m < r["margin_min"][0]:
                        r["margin_min"] = [round(m, 2), j, f]
                q1 = math.degrees(q["q1"])
                r["q1"] = [min(r["q1"][0], q1), max(r["q1"][1], q1)]
                m1 = min(q1 - Dz["j1"][0], Dz["j1"][1] - q1)
                if m1 < r["margin_min"][0]:
                    r["margin_min"] = [round(m1, 2), "q1", f]
                r["L_over_S"] = [min(r["L_over_S"][0], q["L"] / d["S"]), max(r["L_over_S"][1], q["L"] / d["S"])]
                pts, _ = F3.pose_points(Dz, q, ver)
                for nm, P in pts.items():
                    if nm in ("carriage", "carriage pinion"):
                        continue
                    lev, who = F3.lookup(P)
                    i = int(lev.argmin())
                    if lev[i] < r["lev_min"][0]:
                        r["lev_min"] = [int(lev[i]), nm, str(F3.grid()["names"][who[i]]) if who[i] >= 0 else "", f]
                    if nm in ("tool B", "tool A hub") or nm.startswith("finger"):
                        continue
                    for dk in ("AFT", "FORE"):
                        g = F3.door_gap(Dz, dk, F3.door_location(dk, (x, y, z), key), P)
                        m = float(g.min())
                        if m < r["door_min"][0]:
                            r["door_min"] = [round(m, 3), nm + " / " + dk, f]
            r["joints"] = {j: [round(a, 1), round(b, 1)] for j, (a, b) in r["joints"].items()}
            r["q1"] = [round(v, 1) for v in r["q1"]]
            r["L_over_S"] = [round(v, 4) for v in r["L_over_S"]]
            out["per"]["%s %s" % (key, ver)] = r
            print(key, ver, json.dumps({k: v for k, v in r.items() if k != "frames"}), flush=True)
    return out


if __name__ == "__main__":
    d = json.loads(sys.argv[1])
    rep = proof(d, float(sys.argv[2]), float(sys.argv[3]))
    with open(os.path.join(HERE, "fit3d_proof.json"), "w") as fh:
        json.dump(rep, fh, indent=1)
    print("written fit3d_proof.json")