"""의존 라이브러리 설치 확인 + AR2010 체인 로드 테스트"""
import sys

def check(name):
    try:
        __import__(name)
        print("  OK:", name)
        return True
    except ImportError:
        print("  MISSING:", name, " → pip install", name)
        return False

print("=== 의존성 확인 ===")
ok = all([
    check("numpy"),
    check("scipy"),
    check("open3d"),
    check("ikpy"),
    check("trimesh"),
])

if not ok:
    print("\n누락된 패키지를 설치하세요:")
    print("  pip install open3d ikpy trimesh scipy numpy")
    sys.exit(1)

print("\n=== AR2010 체인 로드 테스트 ===")
import os, numpy as np
import ikpy.chain

urdf = os.path.join(os.path.dirname(__file__), "ar2010_urdf.urdf")
if not os.path.exists(urdf):
    print("  MISSING URDF:", urdf)
    sys.exit(1)

chain = ikpy.chain.Chain.from_urdf_file(
    urdf,
    active_links_mask=[False, True, True, True, True, True, True, False]
)
print("  체인 링크 수:", len(chain.links))

# FK 테스트 (영점)
zero_joints = [0.0] * 8
fk = chain.forward_kinematics(zero_joints)
tcp_pos = fk[:3, 3] * 1000  # mm
print("  영점 TCP 위치(mm):", np.round(tcp_pos, 1))

# IK 테스트 — 현재 TCP 위치에서 다시 IK
ik = chain.inverse_kinematics(
    target_position=fk[:3, 3],
    target_orientation=fk[:3, :3],
    orientation_mode="all",
    initial_position=zero_joints
)
import math
deg = [round(math.degrees(j), 2) for j in ik[1:7]]
print("  IK 결과 (도):", deg)

print("\n=== 모든 테스트 통과 ===")