"""
RoboDK .robot → URDF 변환 시도
★ 방법1: RoboDK API Export (GUI 열린 상태)
★ 방법2: robot.getParam으로 DH 파라미터 확인

실행 전: RoboDK GUI 열린 상태
"""
import sys, os

print("=" * 55)
print("RoboDK → URDF 추출 시도")
print("=" * 55)

try:
    from robodk.robolink import Robolink, ITEM_TYPE_ROBOT
    from robodk.robomath import eye
    RDK = Robolink()
    print(f"[1] 연결됨: {RDK.Version()}")
except Exception as e:
    print(f"❌ {e}"); sys.exit(1)

try:
    robot = RDK.Item('', ITEM_TYPE_ROBOT)
    if not robot.Valid():
        robot = RDK.AddFile(
            r'E:\도진팩토리\VAULT\robots\YASKAWA\Yaskawa-Motoman-AR2010.robot'
        )
    print(f"[2] 로봇: {robot.Name()}")
except Exception as e:
    print(f"❌ {e}"); sys.exit(1)

# ── 방법1: Save as URDF ──────────────────────────────────────
out_urdf = r'E:\도진팩토리\VAULT\robots\YASKAWA\ar2010_from_robodk.urdf'
print(f"\n[3] 방법1: robot.Save('{out_urdf}') 시도...")
try:
    result = robot.Save(out_urdf)
    if os.path.exists(out_urdf):
        sz = os.path.getsize(out_urdf)
        print(f"    ✅ 저장됨: {out_urdf} ({sz:,} bytes)")
    else:
        print(f"    ⚠️  Save 반환={result}, 파일 없음")
except Exception as e:
    print(f"    ❌ {type(e).__name__}: {e}")

# ── 방법2: DH 파라미터 API ───────────────────────────────────
print("\n[4] 방법2: DH 파라미터 확인...")
dh_params = {}
for param in ['DH_a', 'DH_d', 'DH_alpha', 'DH_theta', 'nDOF']:
    try:
        val = robot.getParam(param)
        dh_params[param] = val
        print(f"    {param}: {val}")
    except Exception as e:
        print(f"    {param}: ❌ {e}")

# ── 방법3: 링크 정보 출력 ────────────────────────────────────
print("\n[5] 방법3: 링크 정보 (참고용)...")
try:
    joints = robot.Joints()
    print(f"    현재 관절각: {joints.list()}")
    limits = robot.JointLimits()
    print(f"    관절 제한(하한): {limits[0].list()}")
    print(f"    관절 제한(상한): {limits[1].list()}")
except Exception as e:
    print(f"    ❌ {e}")

# ── 방법4: RoboDK GUI File→Export 안내 ──────────────────────
print("\n" + "=" * 55)
print("★ 결과 요약:")
if os.path.exists(out_urdf):
    print(f"  ✅ 방법1 성공: {out_urdf}")
    print(f"  → test_klampt_fk_gate.py 에서 이 파일 사용 가능")
else:
    print("  ❌ 방법1/2 실패 시 수동 변환 필요:")
    print("  RoboDK GUI에서:")
    print("  1) AR2010 로봇 우클릭")
    print("  2) Save as ... → URDF 형식 선택")
    print(f"  3) 저장 위치: E:\\도진팩토리\\VAULT\\robots\\YASKAWA\\ar2010.urdf")
    print("\n  또는 ros-industrial/motoman 저장소 xacro 변환:")
    print("  pip install xacro")
    print("  → xacro 파일을 VAULT/robots/YASKAWA/ 에 두고 재실행")
print("=" * 55)