"""
작업2A: Klampt FK 게이트 — URDF 로드 후 RoboDK와 FK 일치 확인
★ 비교 의미의 전제: 같은 관절각 → RoboDK FK ≈ Klampt FK (mm/deg)
★ URDF 취득: pip install xacro 후 ros-industrial xacro 변환

실행: python test_klampt_fk_gate.py (RoboDK 없이 단독 실행 가능)
"""
import sys, json, os, math

print("=" * 60)
print("작업2A: Klampt FK 게이트 (URDF 로드 + FK 일치 확인)")
print("=" * 60)

# ── 1. Klampt 임포트 ─────────────────────────────────────────
print("\n[1] Klampt 임포트...")
try:
    import klampt
    from klampt import WorldModel
    from klampt.math import so3, se3
    print(f"    ✅ klampt {klampt.__version__}")
except ImportError as e:
    print(f"    ❌ {e}\n    → pip install klampt")
    sys.exit(1)

# ── 2. URDF 취득 시도 ─────────────────────────────────────────
print("\n[2] AR2010 URDF 취득 시도...")

URDF_CANDIDATES = [
    # ★ Klampt는 한국어 경로 처리 불가 → 영문 경로 우선
    r'C:\Users\user\AppData\Local\klampt_robots\ar2010_kinematic.urdf',
    r'E:\도진팩토리\VAULT\robots\YASKAWA\ar2010_kinematics.urdf',
]

urdf_path = None
for p in URDF_CANDIDATES:
    if os.path.exists(p):
        urdf_path = p
        print(f"    ✅ URDF 발견: {p}")
        break

if urdf_path is None:
    # xacro 변환 시도
    print("    ⚠️  URDF 없음 — xacro 변환 시도...")
    XACRO_CANDIDATES = [
        r'E:\도진팩토리\VAULT\robots\YASKAWA\ar2010.xacro',
        r'E:\도진팩토리\VAULT\robots\YASKAWA\ar2010_macro.xacro',
        r'C:\Users\user\Desktop\ROBOT\ar2010.xacro',
    ]
    xacro_src = None
    for p in XACRO_CANDIDATES:
        if os.path.exists(p):
            xacro_src = p
            break

    if xacro_src:
        try:
            import xacro
            doc = xacro.process_file(xacro_src)
            out_urdf = r'E:\도진팩토리\VAULT\robots\YASKAWA\ar2010_kinematic.urdf'
            with open(out_urdf, 'w') as f:
                f.write(doc.toxml())
            urdf_path = out_urdf
            print(f"    ✅ xacro 변환 완료: {out_urdf}")
        except Exception as e:
            print(f"    ❌ xacro 변환 실패: {e}")
    else:
        print("    ❌ xacro 파일도 없음.")
        print("\n    ★ 수동 준비 방법 (한 번만):")
        print("    1) pip install xacro requests")
        print("    2) 아래 xacro 파일 다운로드:")
        print("       https://github.com/ros-industrial/motoman/tree/melodic-devel/motoman_ar2010_support/urdf")
        print("       → ar2010.urdf.xacro, ar2010_macro.xacro, common_files.xacro")
        print(f"       → 저장 위치: E:\\도진팩토리\\VAULT\\robots\\YASKAWA\\")
        print("    3) python -m xacro ar2010.urdf.xacro -o ar2010.urdf")
        print("    4) 이 스크립트 재실행")
        sys.exit(0)

# ── 3. Klampt URDF 로드 ──────────────────────────────────────
print("\n[3] Klampt WorldModel 로드...")
world = WorldModel()
try:
    world.loadRobot(urdf_path)
    robot = world.robot(0)
    n_dof = robot.numDrivers()
    n_link = robot.numLinks()
    print(f"    ✅ 로드됨. DOF: {n_dof}, 링크 수: {n_link}")
    print(f"    링크 이름: {[robot.link(i).getName() for i in range(min(n_link,10))]}")
except Exception as e:
    print(f"    ❌ 로드 실패: {e}")
    print("    → URDF에서 <visual>/<collision> mesh 제거 후 재시도 필요")
    sys.exit(1)

# ── 4. RoboDK 정답지 로드 ────────────────────────────────────
print("\n[4] RoboDK 정답지 로드 (robodk_ref_joints.json)...")
ref_path = r'E:\도진팩토리\3D스캔및티칭시스템\robodk_ref_joints.json'
if not os.path.exists(ref_path):
    print("    ❌ robodk_ref_joints.json 없음")
    print("    → test_robodk_simple_joints.py 먼저 실행하세요")
    sys.exit(1)

with open(ref_path, 'r', encoding='utf-8') as f:
    ref_data = json.load(f)
ok_poses = [r for r in ref_data['results'] if r.get('ok')]
print(f"    ✅ {len(ok_poses)}개 검증된 자세 로드")

# ── 5. FK 게이트: 같은 관절각 → Klampt FK vs RoboDK FK ──────
print(f"\n[5] FK 게이트 (관절각 → 양쪽 FK 비교)...")
print(f"    {'자세':10} {'RoboDK FK xyz':28} {'Klampt FK xyz':28} {'위치차(mm)':10}")
print("    " + "-"*85)

ee_link = robot.numLinks() - 1  # 끝단 링크

gate_results = []
for r in ok_poses[:5]:  # 최대 5개
    name = r['name']
    joints_deg = r['joints_deg']
    robodk_fk = r['fk_flange_xyz']  # mm (플랜지 기준, TCP 오프셋 제거)

    try:
        # 도 → 라디안, Klampt 설정
        joints_rad = [math.radians(j) for j in joints_deg]

        # 드라이버 인덱스 → 정확한 링크 인덱스에 설정
        q = robot.getConfig()
        for i in range(min(6, n_dof)):
            link_idx = robot.driver(i).getAffectedLink()
            q[link_idx] = joints_rad[i]
        robot.setConfig(q)

        # FK
        T = robot.link(ee_link).getTransform()
        R_k, t_k = T
        klampt_fk_m = list(t_k)
        klampt_fk_mm = [v * 1000.0 for v in klampt_fk_m]  # m → mm

        # 위치 차이
        err = sum((a-b)**2 for a,b in zip(robodk_fk, klampt_fk_mm)) ** 0.5

        rdk_str = f"[{robodk_fk[0]:.1f},{robodk_fk[1]:.1f},{robodk_fk[2]:.1f}]"
        klp_str = f"[{klampt_fk_mm[0]:.1f},{klampt_fk_mm[1]:.1f},{klampt_fk_mm[2]:.1f}]"
        status = "✅" if err < 5.0 else "⚠️ 모델 불일치"

        print(f"    {name:10} {rdk_str:28} {klp_str:28} {err:8.2f}mm {status}")
        gate_results.append({'name': name, 'err_mm': round(err, 2), 'ok': err < 5.0})

    except Exception as e:
        print(f"    {name:10} ❌ {e}")
        gate_results.append({'name': name, 'ok': False, 'reason': str(e)})

ok_gate = [g for g in gate_results if g.get('ok')]
print(f"\n★ FK 게이트: {len(ok_gate)}/{len(gate_results)} 일치 (기준: 5mm)")
if len(ok_gate) == len(gate_results):
    print("  → 모델 일치. IK 비교 의미 있음 — 작업2B(IK) 진행 가능")
elif len(ok_gate) == 0:
    print("  → 모델 불일치. URDF 원점/링크 순서 확인 필요 (링크오프셋/기저좌표계 차이)")
else:
    print("  → 일부 불일치. 링크 매핑 검토 필요")
print("=" * 60)