"""Door robot arm: the closed-form inverse kinematics in plain Python (the same maths as the rig's drivers,
design_drivers.py and DESIGN.md section 5). Used by the sweep (dra_sweep.py) to know which joint angles each version
really uses along the doors' animation. No Blender needed for solve(); door_path() reads the doors in a scene.
"""
import math

import json as _json
import os as _os
with open(_os.path.join(_os.path.dirname(_os.path.dirname(_os.path.abspath(__file__))), "design.json")) as _f:
    _T = _json.load(_f)["tools"]
D_T = {"A": _T["A"]["d_t"], "B": _T["B"]["d_t"]}     # wrist centre to the door face along the face normal (design.json)


def solve(D, key, ver, door_loc):
    """joint values (radians; L, c in metres) for door 'AFT'/'FORE', version 'A'/'B', door location (x, y, z)"""
    K = D["kinematics"]
    dr = D["doors"][key]
    att, nrm = dr["attach_local"], dr["normal_local"]
    wc = [att[i] + D_T[ver] * nrm[i] for i in range(3)]
    yb, zb = D["carriage"]["centre_y"], D["carriage"]["underside_z"]
    A, B = dr["carriage"]["A"], dr["carriage"]["B"]
    dx, dy, dz = door_loc
    c = A + B * dx
    X = (1 - B) * dx + (wc[0] - A)
    Y = (yb - wc[1]) - dy
    H = (zb - wc[2]) - dz
    q1 = math.atan2(Y, X)
    R = math.hypot(X, Y) + K["j2_r"]
    Hh = 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 = max(-1.0, min(1.0, (L2 * L2 - L3 * L3 + Dd * Dd) / (2 * L2 * Dd)))
    phi_l = math.atan2(Hh, R) + math.acos(cc)
    q2 = phi_l - phi_l0
    phi_f = math.atan2(Hh - L2 * math.sin(phi_l), R - L2 * math.cos(phi_l)) - beta
    q3 = phi_f - phi_l + phi_l0
    ax = dr.get("tool_axes_C_" + ver, dr["tool_axes_C"])
    xt, yt, zt = ax["x_t"], ax["y_t"], ax["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]
    q5 = -math.acos(max(-1.0, min(1.0, M11)))
    q4 = math.atan2(-M21, M31)
    q6 = math.atan2(-M12, -M13)
    rod = K["rod"]
    tl = rod["trunnion_in_lower"]
    j3l = [K["joints"]["J3"]["origin"][i] - K["joints"]["J2"]["origin"][i] for i in range(3)]
    ka, kb = rod["knob_in_upper"][0], rod["knob_in_upper"][2]
    kx = (j3l[0] - tl[0]) + ka * math.cos(q3) - kb * math.sin(q3)
    kz = (j3l[2] - tl[2]) + ka * math.sin(q3) + kb * math.cos(q3)
    d0x = rod["knob_rest_A"][0] - rod["trunnion_rest_A"][0]
    d0z = rod["knob_rest_A"][2] - rod["trunnion_rest_A"][2]
    L = math.hypot(kx, kz)
    cyl = math.atan2(kz, kx) - math.atan2(d0z, d0x)
    return dict(J1=q1, J2=q2, J3=q3, J4=q4, J5=q5, J6=q6, L=L, cyl=cyl, c=c)


def door_path(scene, door_ob, frames=range(1, 121)):
    """the door's location at each frame of its own animation -> {frame: (x, y, z)}"""
    out = {}
    for f in frames:
        scene.frame_set(f)
        t = door_ob.matrix_world.translation
        out[f] = (t.x, t.y, t.z)
    return out


def path(D, ver, door_paths):
    """[(door, frame, {joint: radians})] for both doors"""
    out = []
    for key, dp in door_paths.items():
        for f, loc in sorted(dp.items()):
            out.append((key, f, solve(D, key, ver, loc)))
    return out


def ranges_deg(p):
    """{joint: [min, max]} in degrees over a path"""
    r = {}
    for _, _, q in p:
        for j in ("J1", "J2", "J3", "J4", "J5", "J6"):
            v = math.degrees(q[j])
            lo_hi = r.setdefault(j, [v, v])
            lo_hi[0] = min(lo_hi[0], v)
            lo_hi[1] = max(lo_hi[1], v)
    return r