"""Finger kinematics of version A (pure math): open (ref pose) and closed (gripping the door's grab knob).
Units ref1 px in the tool frame: X' along the tool axis from the wrist centre, r radial outward. Prints and returns data."""
import math
S = 0.0022
K = (107.0, 79.0)                 # knuckle (ref1: top finger's knuckle cap at x_img 458, y_img 534)
L1, L2 = 82.0, 70.0               # proximal (knuckle -> distal pin), distal (pin -> hook base)
A1_OPEN, A2_OPEN = 24.7, 4.9      # absolute angles (deg, outward positive) measured on ref1
HOOK_LEN, HOOK_REL = 18.0, -84.2  # hook: 18 px, bent 84.2 deg inward from the distal link
HEAD_C, HEAD_R, HEAD_T, STEM_R = 200.0, 29.5, 23.0, 13.6     # grab knob: head centre X', radius, thickness; stem radius (px)
E_CLOSED = (232.0, 39.0)          # hook base when closed


def fk(a1, a2):
    pin = (K[0] + L1 * math.cos(math.radians(a1)), K[1] + L1 * math.sin(math.radians(a1)))
    e = (pin[0] + L2 * math.cos(math.radians(a2)), pin[1] + L2 * math.sin(math.radians(a2)))
    h = math.radians(a2 + HOOK_REL)
    tip = (e[0] + HOOK_LEN * math.cos(h), e[1] + HOOK_LEN * math.sin(h))
    return pin, e, tip


def solve_closed():
    dx, dy = E_CLOSED[0] - K[0], E_CLOSED[1] - K[1]
    d = math.hypot(dx, dy)
    g = math.acos((L1 * L1 + d * d - L2 * L2) / (2 * L1 * d))
    a1 = math.degrees(math.atan2(dy, dx) + g)                     # knuckle bends outward (claw)
    pin = (K[0] + L1 * math.cos(math.radians(a1)), K[1] + L1 * math.sin(math.radians(a1)))
    a2 = math.degrees(math.atan2(E_CLOSED[1] - pin[1], E_CLOSED[0] - pin[0]))
    return a1, a2


def data():
    a1c, a2c = solve_closed()
    pin_o, e_o, tip_o = fk(A1_OPEN, A2_OPEN)
    pin_c, e_c, tip_c = fk(a1c, a2c)
    head_back = HEAD_C + HEAD_T / 2
    hook_half = 5.0
    out = dict(
        units="ref1 px in the tool frame (x 0.0022 = m); X' along the tool axis from the wrist centre, r radial",
        knuckle=list(K), proximal_len=L1, distal_len=L2, hook=dict(length=HOOK_LEN, bend_deg=HOOK_REL, thickness=10),
        open=dict(proximal_deg=A1_OPEN, distal_abs_deg=A2_OPEN, pin=[round(v, 1) for v in pin_o], hook_base=[round(v, 1) for v in e_o],
                  tip=[round(v, 1) for v in tip_o]),
        closed=dict(proximal_deg=round(a1c, 2), distal_abs_deg=round(a2c, 2), pin=[round(v, 1) for v in pin_c], hook_base=[round(v, 1) for v in e_c],
                    tip=[round(v, 1) for v in tip_c]),
        grip=dict(proximal_turn_deg=round(a1c - A1_OPEN, 2), distal_turn_deg=round((a2c - a1c) - (A2_OPEN - A1_OPEN), 2),
                  control="g in 0..1 (0 open as in ref1-3, 1 closed on the knob); proximal angle = g * proximal_turn, distal angle = g * distal_turn, "
                          "both turning the finger toward the tool axis"),
        knob=dict(head_centre=HEAD_C, head_radius=HEAD_R, head_thickness=HEAD_T, stem_radius=STEM_R),
        clearances_px=dict(hook_edge_behind_head=round(tip_c[0] - hook_half - head_back, 2), hook_edge_above_stem=round(tip_c[1] - hook_half - STEM_R, 2),
                           distal_link_over_head_rim=round(min(pin_c[1] + (e_c[1] - pin_c[1]) * (x - pin_c[0]) / (e_c[0] - pin_c[0])
                                                              for x in (HEAD_C - HEAD_T / 2, HEAD_C + HEAD_T / 2)) - 9 - HEAD_R, 2),
                           tips_of_neighbour_fingers=round(math.sqrt(2) * tip_c[1] - 10, 2)),
    )
    out["clearances_m"] = {k: round(v * S, 4) for k, v in out["clearances_px"].items()}
    return out


if __name__ == "__main__":
    import json
    print(json.dumps(data(), indent=1))