# -*- coding: utf-8 -*-
"""Humanoid arm station solver - single source of truth for human_arm.py.

Every joint module occupies the same axial envelope in its own frame:
    s = 0            rear datum (back face of the 63.6 square PCBA)
    s = 0..56        the motor itself (square tail, then the Ø60 housing)
    s = 56.6..64.4   the split clamp band, Ø62 with lugs out to R38
    s = 65.0         the driven hub's mating plane (half B's back face + 4.06 spigot)
    s = 65..74       the driven plate itself (PLATE_T thick, growing away from the motor)

so a module plus its joint costs  65 + PLATE_T  of axial room and R_MOT = 38 across.
That is what forces the proportions: the shoulder axis ends up ~460 mm over the
foot for a 15 mm ground clearance under the fingertips.  Everything below is derived
from those two facts, so changing Z_TIP or L1 re-flows the whole chain.
"""
import math

try:
    import sys
    sys.stdout.reconfigure(encoding="utf-8")
except Exception:
    pass

HUB = 65.0                 # rear datum -> driven hub mating plane  (validated: 65.000)
PLATE_T = 9.0              # driven plate thickness, grows away from the motor
R_MOT = 38.0               # clamp lug radius = the across-axis envelope
R_BARREL = 30.0
GAP = 4.0                  # design clearance between two module envelopes
GAP_MIN = 2.0

# ------------------------------------------------------------------ vertical chain
# Humanoid standard: J1 Roll(+X) -> J2 Pitch(+Y) -> J3 Upper Arm Roll(-Z) -> J4 Elbow Pitch(+Y) -> J5 Wrist Pitch(+Y) -> J6 Tool Roll(-Z)
FOOT_H = 20.0              # base foot height; the fingertips must stay above it
Z_RL = 610.0               # J1 shoulder roll axis
Z_SH = 490.0               # J2 shoulder pitch axis
Z_R3 = 445.0               # M3 upper arm roll rear datum (-Z axis)
Z_HUB3 = Z_R3 - HUB        # 380.0 J3 hub plane
Z_EL = 332.0               # J4 elbow pitch axis (+Y axis) - compact direct knuckle
Z_EI = Z_EL                # backward compatibility alias
FOREARM = 110.0            # shortened forearm length (was 135.0, shortened 25mm per user)
Z_WR = Z_EL - FOREARM      # 222.0 J5 wrist pitch axis (+Y axis)
Z_R6 = Z_WR - 46.0         # 176.0 M6 tool roll rear datum (-Z axis)
Z_HUB6 = Z_R6 - HUB        # 111.0 J6 hub plane
Z_TIP = Z_HUB6 - 18.0      # 93.0 P09 tool flange bottom face (73mm clearance above foot)
L1 = Z_SH - Z_EL           # 158.0 humerus length (was 180.0, empty cylinder eliminated)

SWEEP_R = 92.0             # farthest point of the humerus from the shoulder pitch axis
POD_R = 47.0               # saddle drum outer radius about the roll axis

X_J1_HUB = 14.0            # J1 hub mating plane, forward of the humerus plate's ±44
X_BAY_EXT = 3.0            # M1 侧插槽加长量（2026-10-01 用户裁定：插槽开口 5.06+3=8.06，
                           # 比电机前端面多 3mm 斜插余量；P03 托架前端 8.5+3=11.5）
X_FORK_GAP = 1.0           # P04 肩叉前移量：叉板背面(10.5+1.0=11.5)与 P03 托架端面(11.5)
                           # 0.0 硬贴平（2026-10-02 用户两次要求"搞成平的"，0.5mm 缝仍被判台阶）。
                           # 注意：J1 相对转动时 M2 预紧会经此面产生摩擦阻力，若转动发涩可微松 M2。
X_ARM = X_J1_HUB + PLATE_T / 2.0 + X_FORK_GAP   # 19.5 the limb's own plane in x
Z_R1 = X_J1_HUB - HUB      # -51  J1 rear datum, measured in x: M1 lies behind the shoulder
Z_CHEST1 = Z_RL - 140.0    # 470 torso mast top
Z_CHEST0 = FOOT_H
Z_SH_TOP = Z_RL + R_MOT + GAP              # 652 shoulder pod top
Z_R4 = Z_EL                # compatibility
Z_CAP = Z_HUB3             # compatibility
Z_M6 = Z_R6                # compatibility

# ------------------------------------------------------------------ lateral chain (Y, left arm outboard +Y)
# Aligned Architecture: all motors centered laterally on Y_AXIS = 128.0 mm
# J2, J4, J5 face the SAME direction (+Y) with driven hubs on Y_HUB = 165.0 mm!
Y_AXIS = 128.0              # Shared lateral centerline of the entire limb
Y_HUB = 165.0               # Shared driven hub mating plane for J2, J4, J5
Y_REAR = Y_HUB - HUB        # 100.0 (Rear datum for M2, M4, M5)
Y_COVER = Y_REAR + 56.0     # 156.0 (Front cover face for M2, M4, M5)

Y_H = Y_AXIS                # backward compatibility: limb centre plane
Y_R_IN = Y_REAR
Y_R_OUT = Y_COVER
PL_IN = (Y_COVER, Y_HUB)
PL_OUT = (Y_COVER, Y_HUB)
Y_M6 = Y_AXIS
Y_CHEST = 62.0              # mast half width
X_CHEST = 6.0               # chest plate half thickness

# ------------------------------------------------------------------ 台架（非常规构件）
# P01 立柱 / P02 底盘 是【固定测试台架】，不是机器人本体的一部分（用户 2026-10-02
# 确认）；它在扫描中代表机械臂与**躯干/机身连接界面**的那块实体。因此：
#
#   - 臂撞立柱 = 臂撞躯干，是真约束，不能忽略。sweep_v2 报出的组合卡死（J3 ±162°、
#     J4 ±18°、J5 ±36° 等，全部是工具法兰 P09 撞立柱 P01_A）就是这类真约束。
#   - 但台架的**位置与截面是一个假设**。躯干侧工作空间只和这个假设一样准：真机躯干
#     挪后一点或外形不同，这些限位就变。所以台架本身不要为了放宽行程去改（那是改测试
#     工装，不是改设计）；要改的是"真机躯干占位"这个输入。
#   - `--no-stand` 仅用于诊断：给出不含躯干约束的自由空间行程，用来分辨某个限位到底是
#     臂自身折叠产生的、还是躯干挡的。默认不排除。
FIXTURE_PREFIX = ("P01_", "P02_")


def is_fixture(name):
    """该零件是否属于固定测试台架（代表躯干/安装界面，非机器人本体）。"""
    return name.startswith(FIXTURE_PREFIX)


# ------------------------------------------------------------------ 关节表
# tag, cn, rear, d, xdir, holder of the motor, driven part
#   J1 roll  +X  fore-aft, in the shoulder base -> drives P04 肩叉
#   J2 pitch +Y  outboard in the fork          -> drives P05 大臂旋转座
#   J3 roll  -Z  in the shoulder roll base     -> drives P06 大臂骨架
#   J4 pitch +Y  in the humerus bone           -> drives P07 小臂骨架
#   J5 pitch +Y  in the forearm bone           -> drives P08 腕偏转座
#   J6 roll  -Z  in the wrist bracket          -> drives P09 工具法兰
# J1 roll 电机沿 +X 轴旋转 5.35°，使尾端 4xM3 螺纹槽对齐 45°/135°/225°/315° 规整四角
J1_DTHETA = math.radians(5.35)
J1_XDIR = (0, math.cos(J1_DTHETA), math.sin(J1_DTHETA))

JOINTS = [
    ("J1", "肩抬举", (Z_R1, Y_AXIS, Z_RL), (1, 0, 0), J1_XDIR, "P03_肩吊臂_螺旋体35%_墙5底5_竖打", "P04_肩叉_螺旋体30%_墙5底5_竖打"),
    ("J2", "肩俯仰", (X_ARM, Y_REAR, Z_SH), (0, 1, 0), (1, 0, 0), "P04_肩叉_螺旋体30%_墙5底5_竖打", "P05_大臂旋转座_螺旋体30%_墙4底5_竖打"),
    ("J3", "大臂自转", (X_ARM, Y_AXIS, Z_R3), (0, 0, -1), (1, 0, 0), "P05_大臂旋转座_螺旋体30%_墙4底5_竖打", "P06_大臂骨架_螺旋体25%_墙4底5_竖打"),
    ("J4", "肘部俯仰", (X_ARM, Y_REAR, Z_EL), (0, 1, 0), (1, 0, 0), "P06_大臂骨架_螺旋体25%_墙4底5_竖打", "P07_小臂骨架_螺旋体30%_墙5底5_竖打"),
    ("J5", "腕部俯仰", (X_ARM, Y_REAR, Z_WR), (0, 1, 0), (1, 0, 0), "P07_小臂骨架_螺旋体30%_墙5底5_竖打", "P08_腕偏转座_螺旋体25%_墙4底5_竖打"),
    ("J6", "腕自转", (X_ARM, Y_AXIS, Z_R6), (0, 0, -1), (1, 0, 0), "P08_腕偏转座_螺旋体25%_墙4底5_竖打", "P09_工具法兰_螺旋体30%_墙4底5_竖打"),
]
AXIS = {t[0]: dict(rear=t[2], d=t[3], xdir=t[4]) for t in JOINTS}


def hub_plane(tag):
    j = AXIS[tag]
    return tuple(j["rear"][i] + j["d"][i] * HUB for i in range(3))


def seg_dist(p1, q1, p2, q2):
    """Minimum distance between two 3-D segments (parameters clamped to [0,1])."""
    d1 = [q1[i] - p1[i] for i in range(3)]
    d2 = [q2[i] - p2[i] for i in range(3)]
    r = [p1[i] - p2[i] for i in range(3)]
    a = sum(x * y for x, y in zip(d1, d1))
    e = sum(x * y for x, y in zip(d2, d2))
    f = sum(x * y for x, y in zip(d2, r))
    b = sum(x * y for x, y in zip(d1, d2))
    s = sum(x * y for x, y in zip(d1, r))
    if a < 1e-12:
        t0 = 0.0
        t1 = max(0.0, min(1.0, f / e)) if e > 1e-12 else 0.0
    elif e < 1e-12:
        t1 = 0.0
        t0 = max(0.0, min(1.0, -s / a))
    else:
        den = a * e - b * b
        if abs(den) < 1e-12:
            t0 = 0.0
            t1 = max(0.0, min(1.0, f / e))
        else:
            t0 = max(0.0, min(1.0, (b * f - e * s) / den))
            t1 = max(0.0, min(1.0, (e * f - b * s) / den))
    c = [p1[i] + d1[i] * t0 - (p2[i] + d2[i] * t1) for i in range(3)]
    return math.hypot(*c)


def columns():
    """Each module as a segment (rear datum -> hub plane) plus its across-axis radius."""
    out = {}
    for tag, cn, rear, d, xd, holder, driven in JOINTS:
        q = tuple(rear[i] + d[i] * HUB for i in range(3))
        out[tag] = (rear, q, R_MOT)
    return out


def relabel():
    """The designed 1-D relations that set the stations.  Each pair is separated along
    one global coordinate because the two columns are perpendicular or collinear there."""
    C = {}
    C["pod drum clear of the pitch sweep"] = (Z_RL - POD_R) - (Z_SH + SWEEP_R)
    C["bridge vs elbow sleeve"] = (Y_R_IN + 8.0) - 96.0
    C["cheek vs arm x envelope"] = 64.0 - (X_ARM + 42.0)
    C["fingertip above the foot"] = Z_TIP - FOOT_H
    C["roll motor rear below the elbow motor"] = (Z_EI - R_MOT) - Z_R4
    C["wrist-cap plate above the wrist motor"] = (Z_CAP - PLATE_T) - (Z_WR + R_MOT)
    C["fork plate above the humerus plate"] = (Z_RL - 42.0) - (Z_SH + 44.0)
    C["gripper motor below the wrist motor"] = (Z_WR - R_MOT) - Z_M6
    C["gripper clamp band vs palm plate"] = PL_OUT[0] - (Y_M6 + 31.0)
    C["elbow motor rear vs chest plate edge"] = Y_R_IN - Y_CHEST
    C["gripper tail vs chest plate edge"] = (Y_M6 - 36.0) - Y_CHEST
    C["fingertip to ground"] = Z_TIP
    return C


def report():
    print("vertical chain:  tip=%.0f  J6hub=%.0f  J5axis=%.0f  J4hub=%.0f  J4rear=%.0f  "
          "J3axis=%.0f  J2axis=%.0f  J1roll=%.0f  deck=%.0f  pod top=%.0f"
          % (Z_TIP, Z_M6 - HUB, Z_WR, Z_CAP, Z_R4, Z_EI, Z_SH, Z_RL, Z_CHEST1, Z_SH_TOP))
    print("lateral:  Y_R_IN=%.0f  Y_H=%.0f  Y_R_OUT=%.0f  Y_M6=%.0f  chest half-width=%.0f\n"
          % (Y_R_IN, Y_H, Y_R_OUT, Y_M6, Y_CHEST))
    for tag, cn, rear, d, xd, holder, driven in JOINTS:
        h = hub_plane(tag)
        print("  %-3s %-5s rear=(%6.1f,%6.1f,%6.1f) d=(%2d,%2d,%2d) hub=(%6.1f,%6.1f,%6.1f)  %s -> %s"
              % (tag, cn, rear[0], rear[1], rear[2], d[0], d[1], d[2], h[0], h[1], h[2], holder, driven))
    print("\n-- designed relations (each must be >= %.0f mm) --" % GAP_MIN)
    bad = 0
    for k, v in relabel().items():
        if v < GAP_MIN:
            bad += 1
        print("  %+-7.1f  %-46s %s" % (v, k, "OK" if v >= GAP_MIN else "*** CLASH ***"))
    print("\n  humerus L1=%.0f   forearm(elbow->wrist)=%.0f   hand(wrist->tip)=%.0f"
          % (L1, Z_EI - Z_WR, Z_WR - Z_TIP))
    print("  shoulder->tip = %.0f mm   height to pod top = %.0f mm   torso %.0f..%.0f"
          % (Z_SH - Z_TIP, Z_SH_TOP, Z_CHEST0, Z_CHEST1))
    return bad


if __name__ == "__main__":
    sys.exit(1 if report() else 0)