# 6축 산업용 로봇팔 (210kg급, 도달거리 약 2.7m) — 공장 바닥에 볼트로 고정된 다관절 로봇.
# 관절(A1~A6) 위치를 순운동학으로 계산하고, 각 링크를 관절 좌표계 안에서 만든 뒤 행렬로 배치했다.
# 링크 몸체는 두세 개의 원을 감싸는 볼록 껍질(convex hull) 측면 프로필을 돌출해 주물 같은 둥근 끝을 만들고,
# 베벨 모디파이어로 모서리를 굴렸다. 모터·감속기·밸런서는 원기둥, 케이블 호스는 스플라인 관으로 만들었다.
# 볼트·모터 핀 같은 반복 부품은 메시를 공유하는 링크 복제로 배치했다.
import math
from atelier import *

SHOT = {"azimuth": -58, "elevation": 12, "lens": 55, "light": "studio", "background": "#2a323d"}

# ── 재질 ─────────────────────────────────────────────
M_ORANGE = mat('Robot_Orange_Paint', '#e66f12', rough=0.36, metal=0.0, coat=0.35)
M_MOTOR = mat('Servo_Motor_Black', '#26282d', rough=0.42, metal=0.35)
M_CAST = mat('Gearbox_Cast_Grey', '#61666d', rough=0.45, metal=0.6)
M_STEEL = mat('Machined_Steel', '#b9bdc2', rough=0.22, metal=1.0)
M_CHROME = mat('Chrome_Rod', '#dde0e4', rough=0.07, metal=1.0)
M_PLATE = mat('Floor_Plate_Steel', '#454a51', rough=0.62, metal=0.55)
M_HOSE = mat('Cable_Hose_Black', '#1c1d20', rough=0.75)
M_BOLT = mat('Bolt_Zinc', '#8c9198', rough=0.3, metal=1.0)
M_LABEL = mat('Warning_Label_Yellow', '#f0c419', rough=0.5)
M_RUBBER = mat('Gripper_Pad_Rubber', '#3a3e44', rough=0.85)

# ── 자세(관절각) ─────────────────────────────────────
A1 = math.radians(0)       # 회전대
TH2 = math.radians(14)     # 하부암: 수직에서 앞으로 기운 각
PH3 = math.radians(6)      # 상부암: 수평에서 위로 든 각
BETA5 = math.radians(72)   # 손목 A5 굽힘(아래로)
L2 = 1.15                  # A2→A3
L3 = 1.22                  # A3→A5
A2X, A2Z = 0.35, 0.72      # A2 축 위치(회전대 기준)


def shade(ob, deg=32):
    """부드러운 셰이딩 + 각도 기준 날카로운 모서리 표시."""
    if ob.type != 'MESH':
        return ob
    bm = bmesh.new()
    bm.from_mesh(ob.data)
    lim = math.radians(deg)
    for f in bm.faces:
        f.smooth = True
    for e in bm.edges:
        if len(e.link_faces) == 2:
            e.smooth = e.calc_face_angle(0) < lim
        else:
            e.smooth = False
    bm.to_mesh(ob.data)
    bm.free()
    return ob


def rbevel(ob, w=0.015, seg=3, angle=40):
    m = ob.modifiers.new('Bevel', 'BEVEL')
    m.width = w
    m.segments = seg
    m.limit_method = 'ANGLE'
    m.angle_limit = math.radians(angle)
    m.harden_normals = True
    m.miter_outer = 'MITER_ARC'
    return ob


def hull2d(pts):
    pts = sorted(set((round(x, 6), round(y, 6)) for x, y in pts))
    def cross(o, a, b):
        return (a[0] - o[0]) * (b[1] - o[1]) - (a[1] - o[1]) * (b[0] - o[0])
    lo, hi = [], []
    for p in pts:
        while len(lo) >= 2 and cross(lo[-2], lo[-1], p) <= 0:
            lo.pop()
        lo.append(p)
    for p in reversed(pts):
        while len(hi) >= 2 and cross(hi[-2], hi[-1], p) <= 0:
            hi.pop()
        hi.append(p)
    return lo[:-1] + hi[:-1]


def circ(cx, cz, r, n=40):
    return [(cx + r * math.cos(2 * math.pi * i / n), cz + r * math.sin(2 * math.pi * i / n)) for i in range(n)]


def slab(name, circles, y0, y1, material=M_ORANGE, extra=(), bw=0.03):
    """XZ 평면 원들의 볼록 껍질을 Y 방향 y0~y1 로 돌출한 주물형 몸체."""
    pts = list(extra)
    for c in circles:
        pts += circ(*c)
    prof = hull2d(pts)
    ob = extrude_profile(name, prof, y1 - y0, loc=(0, (y0 + y1) / 2, 0), material=material)
    shade(ob, 30)
    rbevel(ob, bw, 4, 30)
    return ob


def ycyl(name, r, y0, y1, x=0.0, z=0.0, material=M_CAST, seg=48, r2=None):
    """Y 축 방향 원기둥 (y0→y1)."""
    ob = cylinder(name, r=r, h=abs(y1 - y0), r2=r2, seg=seg, base=True, material=material,
                  loc=(x, y0, z), rot=(-math.pi / 2 if y1 > y0 else math.pi / 2, 0, 0))
    return shade(ob)


def xcyl(name, r, x0, x1, y=0.0, z=0.0, material=M_CAST, seg=48, r2=None):
    ob = cylinder(name, r=r, h=abs(x1 - x0), r2=r2, seg=seg, base=True, material=material,
                  loc=(x0, y, z), rot=(0, math.pi / 2 if x1 > x0 else -math.pi / 2, 0))
    return shade(ob)


def zcyl(name, r, z0, z1, x=0.0, y=0.0, material=M_CAST, seg=48, r2=None):
    ob = cylinder(name, r=r, h=z1 - z0, r2=r2, seg=seg, base=True, material=material, loc=(x, y, z0))
    return shade(ob)


def put(objs, M):
    for o in objs:
        o.matrix_basis = M @ o.matrix_basis
    return objs


def frame(origin, ry=0.0):
    """회전대 좌표계 → 월드: A1 회전 · 이동 · Y축 회전."""
    return (Matrix.Rotation(A1, 4, 'Z') @ Matrix.Translation(Vector(origin))
            @ Matrix.Rotation(ry, 4, 'Y'))


def tube_between(name, p, q, r, material, seg=32):
    p, q = Vector(p), Vector(q)
    d = q - p
    ob = cylinder(name, r=r, h=d.length, seg=seg, base=True, material=material)
    ob.matrix_world = Matrix.Translation(p) @ d.to_track_quat('Z', 'Y').to_matrix().to_4x4()
    return shade(ob)


def catmull(points, n=8):
    P = [Vector(p) for p in points]
    P = [P[0]] + P + [P[-1]]
    out = []
    for i in range(1, len(P) - 2):
        p0, p1, p2, p3 = P[i - 1], P[i], P[i + 1], P[i + 2]
        for k in range(n):
            t = k / n
            out.append(0.5 * ((2 * p1) + (-p0 + p2) * t + (2 * p0 - 5 * p1 + 4 * p2 - p3) * t * t
                              + (-p0 + 3 * p1 - 3 * p2 + p3) * t ** 3))
    out.append(P[-2])
    return [tuple(v) for v in out]


# ── 관절 위치 (회전대 평면 XZ) ────────────────────────
d2 = Vector((math.sin(TH2), 0, math.cos(TH2)))
A2 = Vector((A2X, 0, A2Z))
A3 = A2 + L2 * d2
d3 = Vector((math.cos(PH3), 0, math.sin(PH3)))
A5 = A3 + L3 * d3
PH5 = PH3 - BETA5


# ═══ 1. 바닥 고정판과 베이스 프레임 (고정) ═══════════════
plate = box('Floor_Mounting_Plate', (1.10, 1.10, 0.035), loc=(0.0, 0, 0), base=True, material=M_PLATE)
shade(plate); rbevel(plate, 0.006, 2)
bolt = cylinder('Anchor_Bolt', r=0.026, h=0.03, seg=6, loc=(0.48, 0.48, 0.035), material=M_BOLT)
rbevel(bolt, 0.004, 1)
for i, (sx, sy) in enumerate([(1, 1), (-1, 1), (-1, -1), (1, -1), (0, 1), (0, -1), (1, 0), (-1, 0)]):
    b = bolt if i == 0 else duplicate(bolt)
    b.location = (0.48 * sx, 0.48 * sy, 0.035)

c = 0.11
s = 0.40
base_pts = [(s - c, -s), (s, -s + c), (s, s - c), (s - c, s), (-s + c, s), (-s, s - c), (-s, -s + c), (-s + c, -s)]
base_frame = prism('Base_Frame', base_pts, 0.20, loc=(0, 0, 0.035), material=M_ORANGE)
shade(base_frame); rbevel(base_frame, 0.025, 4)
base_neck = zcyl('Base_Frame_Neck', 0.37, 0.225, 0.29, material=M_ORANGE, seg=64)
rbevel(base_neck, 0.012, 2)
bearing = zcyl('A1_Slewing_Bearing', 0.385, 0.29, 0.315, material=M_CAST, seg=64)
# 베이스 뒤쪽 전원·신호 커넥터 판
cp = box('Base_Connector_Plate', (0.05, 0.34, 0.15), loc=(-0.415, 0, 0.06), base=True, material=M_CAST)
shade(cp); rbevel(cp, 0.006, 2)
for j, yy in enumerate((-0.08, 0.08)):
    xcyl(f'Base_Power_Plug_{j + 1}', 0.04, -0.44, -0.52, y=yy, z=0.135, material=M_MOTOR)
    xcyl(f'Base_Plug_Nut_{j + 1}', 0.048, -0.44, -0.465, y=yy, z=0.135, material=M_BOLT, seg=6)
# 베이스 프레임 볼트
fb = cylinder('Base_Bolt', r=0.018, h=0.022, seg=6, loc=(0, 0, 0.235), material=M_BOLT)
for i in range(8):
    a = 2 * math.pi * (i + 0.5) / 8
    o = fb if i == 0 else duplicate(fb)
    o.location = (0.335 * math.cos(a) * 1.18, 0.335 * math.sin(a) * 1.18, 0.235)


# ═══ 2. 회전대 (A1 로 회전) ══════════════════════════
car = []
car.append(zcyl('Carousel_Drum', 0.40, 0.315, 0.43, material=M_ORANGE, seg=64))
rbevel(car[-1], 0.02, 3)
car.append(slab('Carousel_Body', [(A2X, A2Z, 0.30), (-0.27, 0.68, 0.15), (0.05, 0.52, 0.30)], -0.20, 0.26,
                extra=[(-0.40, 0.42), (0.62, 0.42)], bw=0.035))
car.append(ycyl('A2_Gearbox', 0.24, -0.20, -0.24, x=A2X, z=A2Z, material=M_CAST))
car.append(ycyl('A2_Gearbox_Flange', 0.27, 0.26, 0.30, x=A2X, z=A2Z, material=M_ORANGE))
car.append(ycyl('A2_Motor', 0.10, 0.30, 0.56, x=A2X, z=A2Z, material=M_MOTOR))
car.append(ycyl('A2_Motor_Cap', 0.085, 0.56, 0.59, x=A2X, z=A2Z, material=M_CAST))
car.append(ycyl('A2_Motor_Plug', 0.035, 0.0, 0.07, x=A2X, z=A2Z + 0.10, material=M_MOTOR))
car[-1].location.y = 0.40
car.append(zcyl('A1_Motor', 0.085, 0.26, 0.62, x=-0.20, y=0.30, material=M_MOTOR))
car.append(zcyl('A1_Motor_Cap', 0.07, 0.62, 0.65, x=-0.20, y=0.30, material=M_CAST))
# 밸런서 브래킷(회전대 뒤쪽)
BAL_Y = -0.56
P1 = Vector((-0.27, 0, 0.68))
car.append(ycyl('Balancer_Bracket_Pin', 0.05, -0.20, BAL_Y - 0.08, x=P1.x, z=P1.z, material=M_STEEL))
car.append(ycyl('Balancer_Bracket_Boss', 0.11, -0.20, -0.25, x=P1.x, z=P1.z, material=M_ORANGE))
put(car, frame((0, 0, 0)))


# ═══ 3. 하부암 (A2 기준, 로컬 +Z 가 팔 방향) ══════════
low = []
LY0, LY1 = -0.47, -0.24
low.append(slab('Lower_Arm', [(0, 0, 0.27), (0, L2, 0.21), (-0.02, 0.55, 0.20)], LY0, LY1, bw=0.035))
low.append(ycyl('A2_Hub_Cap', 0.16, LY0, LY0 - 0.025, material=M_CAST))
low.append(ycyl('A2_Hub_Cap_Center', 0.07, LY0 - 0.025, LY0 - 0.04, material=M_STEEL))
low.append(ycyl('A3_Hub_Cap', 0.13, LY0, LY0 - 0.025, z=L2, material=M_CAST))
low.append(ycyl('A2_Gearbox_Ring', 0.25, LY1, -0.24, material=M_CAST))
# 밸런서 연결점(하부암 뒤쪽)
P2l = Vector((-0.17, 0, 0.74))
low.append(ycyl('Balancer_Arm_Pin', 0.045, LY0, BAL_Y - 0.08, x=P2l.x, z=P2l.z, material=M_STEEL))
low.append(ycyl('Balancer_Arm_Boss', 0.09, LY0, LY0 - 0.02, x=P2l.x, z=P2l.z, material=M_ORANGE))
# 경고 라벨
low.append(slab('Lower_Arm_Cover_Panel', [(0.01, 0.36, 0.17), (0.0, 0.84, 0.145)], LY0 - 0.012, LY0 + 0.02, bw=0.008))
lab = box('Warning_Label', (0.11, 0.003, 0.095), loc=(0.0, LY0 - 0.0135, 0.62), material=M_LABEL)
low.append(lab)
tri = prism('Warning_Label_Symbol', [(-0.035, -0.03), (0.035, -0.03), (0.0, 0.032)], 0.002,
            loc=(0.0, LY0 - 0.015, 0.62), rot=(math.pi / 2, 0, 0), material=M_MOTOR)
low.append(tri)
low.append(ycyl('A3_Hub_Cap_Center', 0.055, LY0 - 0.025, LY0 - 0.04, z=L2, material=M_STEEL))
Mlow = frame(A2, TH2)
put(low, Mlow)

# ── 밸런서 실린더 ──
P1w = frame((0, 0, 0)) @ Vector((P1.x, BAL_Y, P1.z))
P2w = Mlow @ Vector((P2l.x, BAL_Y, P2l.z))
dv = (P2w - P1w)
eye1 = ycyl('Balancer_Eye_Rear', 0.075, BAL_Y + 0.05, BAL_Y - 0.05, material=M_CAST)
eye1.matrix_basis = Matrix.Translation(P1w - Vector((0, BAL_Y, 0))) @ eye1.matrix_basis
eye2 = ycyl('Balancer_Eye_Front', 0.06, BAL_Y + 0.045, BAL_Y - 0.045, material=M_CHROME)
eye2.matrix_basis = Matrix.Translation(P2w - Vector((0, BAL_Y, 0))) @ eye2.matrix_basis
u = dv.normalized()
bal_body = tube_between('Balancer_Cylinder', P1w + u * 0.05, P1w + u * (dv.length * 0.66), 0.088, M_MOTOR, 48)
rbevel(bal_body, 0.01, 2)
bal_end = tube_between('Balancer_Gland', P1w + u * (dv.length * 0.66), P1w + u * (dv.length * 0.70), 0.07, M_CAST)
bal_rod = tube_between('Balancer_Piston_Rod', P1w + u * 0.3, P2w - u * 0.04, 0.035, M_CHROME)


# ═══ 4. 상부암 (A3 기준, 로컬 +X 가 팔 방향) ══════════
up = []
UY0, UY1 = -0.24, 0.16
UYC = (UY0 + UY1) / 2
up.append(slab('Upper_Arm_Housing', [(0, 0, 0.23), (-0.40, 0.03, 0.17), (0.50, 0.0, 0.16)], UY0, UY1, bw=0.03))
up.append(ycyl('A3_Gearbox', 0.20, UY1, UY1 + 0.04, material=M_CAST))
up.append(ycyl('A3_Motor', 0.085, UY1, UY1 + 0.24, x=-0.24, z=0.05, material=M_MOTOR))
up.append(ycyl('A3_Motor_Cap', 0.07, UY1 + 0.24, UY1 + 0.27, x=-0.24, z=0.05, material=M_CAST))
up.append(slab('Upper_Arm_Cover_Panel', [(0.27, 0.0, 0.13), (0.47, 0.0, 0.11)], UY0 - 0.012, UY0 + 0.02, bw=0.008))
# 손목 모터 3개 (A4·A5·A6)
for i, (yy, zz) in enumerate([(UYC - 0.085, 0.07), (UYC + 0.085, 0.07), (UYC, -0.075)]):
    up.append(xcyl(f'A{4 + i}_Wrist_Motor', 0.062, -0.50, -0.80, y=yy, z=zz, material=M_MOTOR))
    up.append(xcyl(f'A{4 + i}_Wrist_Motor_Cap', 0.05, -0.80, -0.83, y=yy, z=zz, material=M_CAST))
# A4 회전 링과 전완(forearm) 관
up.append(xcyl('A4_Bearing_Ring', 0.145, 0.62, 0.66, y=UYC, material=M_CAST))
up.append(xcyl('Forearm_Tube', 0.13, 0.66, 1.06, y=UYC, r2=0.105, material=M_ORANGE))
rbevel(up[-1], 0.01, 2)
# 손목 요크 (A5 지지 포크)
for side, nm in ((-1, 'L'), (1, 'R')):
    yc = UYC + side * 0.105
    up.append(slab(f'Wrist_Yoke_{nm}', [(1.03, 0.0, 0.10), (L3, 0.0, 0.095)], yc - 0.025, yc + 0.025, bw=0.012))
    up.append(ycyl(f'A5_Pivot_Cap_{nm}', 0.06, yc + side * 0.025, yc + side * 0.04, x=L3, material=M_CAST))
up.append(xcyl('Wrist_Yoke_Bridge', 0.105, 1.0, 1.08, y=UYC, material=M_ORANGE))
Mup = frame(A3, -PH3)
put(up, Mup)


# ═══ 5. 손목(A5)·플랜지(A6)·그리퍼 ══════════════════
wr = []
wr.append(ycyl('A5_Wrist_Body', 0.085, UYC - 0.079, UYC + 0.079, material=M_ORANGE))
rbevel(wr[-1], 0.012, 2)
wr.append(xcyl('A5_Wrist_Neck', 0.072, 0.0, 0.16, y=UYC, material=M_ORANGE))
wr.append(xcyl('A6_Tool_Flange', 0.08, 0.16, 0.19, y=UYC, material=M_STEEL))
fb6 = xcyl('A6_Flange_Bolt', 0.008, 0.19, 0.197, y=UYC + 0.055, material=M_BOLT, seg=8)
wr.append(fb6)
for i in range(1, 6):
    a = 2 * math.pi * i / 6
    wr.append(duplicate(fb6, loc=(0.19, UYC + 0.055 * math.cos(a), 0.055 * math.sin(a))))
# 공압 2지 그리퍼
wr.append(xcyl('Gripper_Adapter', 0.07, 0.19, 0.22, y=UYC, material=M_CAST))
gb = box('Gripper_Body', (0.12, 0.16, 0.24), loc=(0.28, UYC, 0), material=M_CAST)
shade(gb); rbevel(gb, 0.012, 2)
wr.append(gb)
for side, nm in ((-1, 'L'), (1, 'R')):
    f = box(f'Gripper_Finger_{nm}', (0.20, 0.09, 0.035), loc=(0.43, UYC, side * 0.06), material=M_STEEL)
    shade(f); rbevel(f, 0.006, 2)
    wr.append(f)
    pad = box(f'Gripper_Pad_{nm}', (0.08, 0.075, 0.012), loc=(0.48, UYC, side * 0.0365), material=M_RUBBER)
    wr.append(pad)
Mwr = frame(A5, -PH5)
put(wr, Mwr)


# ═══ 6. 케이블 드레스 팩 ═══════════════════════════
Mc = frame((0, 0, 0))
hose_pts = [Mc @ Vector(p) for p in [(-0.30, 0.08, 0.60), (-0.38, 0.0, 0.95)]]
hose_pts += [Mlow @ Vector(p) for p in [(-0.285, -0.31, 0.50), (-0.255, -0.30, 0.95), (-0.10, -0.20, 1.25)]]
hose_pts += [Mup @ Vector(p) for p in [(-0.30, UYC, 0.235), (0.05, UYC, 0.258), (0.45, UYC, 0.19), (0.85, UYC, 0.142), (1.03, UYC, 0.10)]]
HOSE = catmull(hose_pts, 12)
curve_tube('Cable_Dress_Hose', HOSE, radius=0.032, material=M_HOSE, resolution=2, bevel_res=4)

# 호스 클램프: 호스 접선 방향을 축으로 하는 고리 + 몸체로 내려가는 받침
clamp_ring = torus('Hose_Clamp', R=0.037, r=0.009, seg=24, rseg=8, material=M_CAST)
for k, idx in enumerate([30, 46, 62, 76]):
    p, q = Vector(HOSE[idx]), Vector(HOSE[idx + 1])
    t = (q - p).normalized()
    ob = clamp_ring if k == 0 else duplicate(clamp_ring)
    ob.matrix_basis = Matrix.Translation(p) @ t.to_track_quat('Z', 'Y').to_matrix().to_4x4()