"""PROOF of the driver plan in Blender itself (headless, nothing saved): builds the rig's object chain as empties with the
exact simple-expression drivers from design.json, plays the doors' animation (frames 1-120) and measures:
- every driver is a simple expression and valid (the user's Blender runs no Python in files)
- the tool's contact point and axes follow the door target (position and orientation), evaluated by Blender's depsgraph
  after ONE frame_set (no lag between the chained solver empties)
- the J3 piston's eye stays on the truss knob (the cylinder and piston drivers)
- the energy chain links: consecutive spacing = pitch (no stretching), the first link at the carriage, all inside the tray
- the version switch: hide_viewport / hide_render drivers
Run: blender -b --factory-startup source/ship_context_2026-10-16.blend --python build/design_driver_proto.py -- AFT build/proto_AFT.json
"""
import bpy, json, math, sys, os
from mathutils import Matrix, Vector

HERE = os.path.dirname(os.path.abspath(__file__))
ROOT = os.path.dirname(HERE)
sys.path.insert(0, HERE)
import design_kin as K

KEY, OUT = sys.argv[sys.argv.index("--") + 1:][:2]
D = json.load(open(os.path.join(ROOT, "design.json")))
DR = D["doors"][KEY]
KIN = D["kinematics"]
DRV = D["drivers"][KEY]
P = "DoorArm_%s" % KEY
scn = bpy.context.scene
for o in bpy.data.objects:
    if not o.users_scene:
        scn.collection.objects.link(o)
scn.frame_start, scn.frame_end = 1, 120


def empty(name, parent=None, loc=(0, 0, 0), rot=(0, 0, 0)):
    o = bpy.data.objects.new(name, None)
    scn.collection.objects.link(o)
    if parent:
        o.parent = parent
        o.matrix_parent_inverse = Matrix.Identity(4)
    o.location = loc
    o.rotation_euler = rot
    return o


V = lambda a: Vector(a)
J = KIN["joints"]
root = empty(P, None, (0, D["carriage"]["centre_y"], D["carriage"]["underside_z"]), (math.pi, 0, 0))
car = empty(P + " | carriage", root)
tur = empty(P + " | turret", car)
low = empty(P + " | lower arm", tur, V(J["J2"]["origin"]))
upp = empty(P + " | upper arm", low, V(J["J3"]["origin"]) - V(J["J2"]["origin"]))
fore = empty(P + " | forearm", upp, V(J["J4"]["origin"]) - V(J["J3"]["origin"]))
wh = empty(P + " | wrist head", fore, V(J["J5"]["origin"]) - V(J["J4"]["origin"]))
fl = empty(P + " | flange", wh)
tip = empty(P + " | tool contact (test)", fl, (D["tools"]["A"]["d_t"], 0, 0))
cyl = empty(P + " | J3 cylinder", low, V(KIN["rod"]["trunnion_in_lower"]))
pis = empty(P + " | J3 piston", cyl, V(DRV["piston_rest_location"]))
knob = empty(P + " | knob (test)", upp, V(KIN["rod"]["knob_in_upper"]))
for s in DRV["solver_empties"]:
    o = empty(s)
    for d in DRV["joint_and_solver_drivers"] + DRV["chain_link_drivers"]:
        if d["target"] == s:
            o[d["path"][2:-2]] = 0.0
tray = empty(P + " | chain tray")
links = []
for i in range(D["energy_chain"][KEY]["links"]):
    links.append(empty(P + " | chain link %02d" % i, tray, (0, 0, D["energy_chain"][KEY]["z_bottom"] + 0.05)))
ctrl = empty("DoorArm | CONTROLS")
ctrl["version"] = 0
partA = empty(P + " | tool A (test)", fl)
partB = empty(P + " | tool B (test)", fl)

made = []


def add_driver(d):
    ob = bpy.data.objects[d["target"]]
    if d["index"] is None:
        fc = ob.driver_add(d["path"])
    else:
        fc = ob.driver_add(d["path"], d["index"])
    dv = fc.driver
    dv.type = 'SCRIPTED'
    for nm, src, path in d["vars"]:
        v = dv.variables.new()
        v.name = nm
        v.type = 'SINGLE_PROP'
        v.targets[0].id_type = 'OBJECT'
        v.targets[0].id = bpy.data.objects[src]
        v.targets[0].data_path = path
    dv.expression = d["expr"]
    made.append((ob.name, d["path"], d["index"], dv))


for d in DRV["joint_and_solver_drivers"] + DRV["chain_link_drivers"]:
    add_driver(d)
for ob, expr in ((partA, "v > 0.5"), (partB, "v < 0.5")):
    for path in ("hide_viewport", "hide_render"):
        add_driver(dict(target=ob.name, path=path, index=None, expr=expr, vars=[("v", "DoorArm | CONTROLS", '["version"]')]))
bpy.context.view_layer.update()
scn.frame_set(1)
bad = [(n, p, i, dv.expression) for n, p, i, dv in made if not (dv.is_simple_expression and dv.is_valid)]

door = bpy.data.objects[DR["door"]]
arm = K.Arm(D["scale"]["m_per_ref1_px"])
xt_w, yt_w, zt_w = (Vector(DR["tool_axes_world"][k]) for k in ("x_t", "y_t", "z_t"))
tool_c = (K.w2c(tuple(xt_w)), K.w2c(tuple(yt_w)), K.w2c(tuple(zt_w)))
ch = D["energy_chain"][KEY]
rep = dict(door=KEY, drivers=len(made), not_simple_or_invalid=bad, frames=0, tip_err_mm=0.0, axis_err_deg=0.0, piston_eye_err_mm=0.0,
           q_vs_python_deg=0.0, chain=dict(pitch_err_mm=0.0, first_link_from_carriage_mm=0.0, outside_tray_mm=0.0, loop_x=[99, -99]))
yb, zb = D["carriage"]["centre_y"], D["carriage"]["underside_z"]
for f in range(1, 121):
    scn.frame_set(f)                       # one frame_set: no extra updates
    dl = door.matrix_world.translation.copy()
    target = dl + Vector(DR["attach_local"])
    rep["tip_err_mm"] = max(rep["tip_err_mm"], (tip.matrix_world.translation - target).length * 1000)
    R = fl.matrix_world.to_3x3().normalized()
    for col, want in ((0, xt_w), (1, yt_w), (2, zt_w)):
        a = R.col[col].angle(want, 0.0)
        rep["axis_err_deg"] = max(rep["axis_err_deg"], math.degrees(a))
    rep["piston_eye_err_mm"] = max(rep["piston_eye_err_mm"], (pis.matrix_world.translation - knob.matrix_world.translation).length * 1000)
    # python IK for comparison
    c = DR["carriage"]["A"] + DR["carriage"]["B"] * dl.x
    WC = target - xt_w * D["tools"]["A"]["d_t"]
    q = K.solve(arm, K.w2c((WC.x - c, WC.y - yb, WC.z - zb)), tool_c, branch5=DR["wrist_branch"])
    got = dict(q1=tur.rotation_euler.z, q2=-low.rotation_euler.y, q3=-upp.rotation_euler.y, q4=fore.rotation_euler.x, q5=wh.rotation_euler.y, q6=fl.rotation_euler.x)
    for k in got:
        rep["q_vs_python_deg"] = max(rep["q_vs_python_deg"], abs(math.degrees(got[k] - q[k])))
    # chain
    cx = car.matrix_world.translation.x
    pts = [l.matrix_world.translation.copy() for l in links]
    for i in range(1, len(pts)):
        dd = (pts[i] - pts[i - 1]).length
        # on the bend the chord is a little shorter than the pitch
        rep["chain"]["pitch_err_mm"] = max(rep["chain"]["pitch_err_mm"], abs(dd - ch["pitch"]) * 1000)
    rep["chain"]["first_link_from_carriage_mm"] = max(rep["chain"]["first_link_from_carriage_mm"], abs(abs(pts[0].x - cx) - ch["pitch"] / 2) * 1000)
    tx0, tx1 = ch["tray"]["x"]
    ty0, ty1 = ch["tray"]["y"]
    sl = ch["tray"]["end_slant"]
    for p_ in pts:
        t_ = (p_.y - sl["at_y"][0]) / (sl["at_y"][1] - sl["at_y"][0])
        xe = sl["x"][0] + t_ * (sl["x"][1] - sl["x"][0])
        if KEY == "AFT":
            out = max(tx0 - (p_.x - 0.0375), 0, (p_.x + 0.0375) - xe, ty0 - (p_.y - 0.03), (p_.y + 0.03) - ty1)
        else:
            out = max(xe - (p_.x - 0.0375), 0, (p_.x + 0.0375) - tx1, ty0 - (p_.y - 0.03), (p_.y + 0.03) - ty1)
        rep["chain"]["outside_tray_mm"] = max(rep["chain"]["outside_tray_mm"], out * 1000)
    rep["chain"]["loop_x"] = [min(rep["chain"]["loop_x"][0], min(p_.x for p_ in pts)), max(rep["chain"]["loop_x"][1], max(p_.x for p_ in pts))]
    rep["frames"] += 1
# version switch
sw = {}
for v in (0, 1):
    ctrl["version"] = v
    ctrl.update_tag()
    bpy.context.view_layer.update()
    scn.frame_set(scn.frame_current)
    sw[str(v)] = dict(A_hidden=partA.hide_viewport, B_hidden=partB.hide_viewport, A_hide_render=partA.hide_render, B_hide_render=partB.hide_render)
rep["version_switch"] = sw
for k in ("tip_err_mm", "axis_err_deg", "piston_eye_err_mm", "q_vs_python_deg"):
    rep[k] = round(rep[k], 5)
for k in ("pitch_err_mm", "first_link_from_carriage_mm", "outside_tray_mm"):
    rep["chain"][k] = round(rep["chain"][k], 3)
rep["chain"]["loop_x"] = [round(v, 4) for v in rep["chain"]["loop_x"]]
json.dump(rep, open(OUT, "w"), indent=1)
print("PROTO", json.dumps(rep))