"""
Klampt AR2010 IK 자체검증 테스트
★ FK(IK(P)) = P' 일치 확인 — RoboDK 없이 단독 검증
★ 설치: pip install klampt
★ URDF: ros-industrial/motoman AR2010 (없으면 generic 6DOF로 대체)
"""
import sys
import math

print("="*50)
print("Klampt IK 자체검증 테스트")
print("="*50)

# ── 1. Klampt 임포트 ─────────────────────────────────────────
print("\n[1] Klampt 임포트...")
try:
    import klampt
    from klampt import WorldModel, RobotModel
    from klampt.model import ik
    print(f"    ✅ klampt 로드됨")
except ImportError as e:
    print(f"    ❌ klampt 임포트 실패: {e}")
    print("    → pip install klampt")
    sys.exit(1)

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

URDF_PATHS = [
    r'E:\도진팩토리\VAULT\robots\YASKAWA\ar2010.urdf',
    r'C:\Users\user\Desktop\ROBOT\ar2010.urdf',
    r'E:\도진팩토리\3D스캔및티칭시스템\ar2010.urdf',
]

world = WorldModel()
robot = None

for path in URDF_PATHS:
    try:
        import os
        if os.path.exists(path):
            res = world.loadRobot(path)
            if res >= 0:
                robot = world.robot(0)
                print(f"    ✅ URDF 로드: {path}")
                print(f"    링크 수: {robot.numLinks()}, DOF: {robot.numDrivers()}")
                break
    except Exception as e:
        continue

if robot is None:
    print("    ⚠️  AR2010 URDF 없음 — 내장 간단모델로 대체 시도...")
    # klampt 내장 예제 로봇으로 IK 동작 확인만
    try:
        import klampt.math.vectorops as vo
        print("    klampt 수학 모듈 동작 확인:")
        v = [1.0, 2.0, 3.0]
        n = vo.norm(v)
        print(f"    norm([1,2,3]) = {n:.4f} (기대: 3.7417)")
        print("    ✅ Klampt 수학 라이브러리 정상 동작")
        print("\n    ★ URDF 없이 IK 테스트 불가 — 아래 준비 필요:")
        print("    1) ros-industrial/motoman 저장소에서 AR2010 URDF 다운로드")
        print("    2) E:\\도진팩토리\\VAULT\\robots\\YASKAWA\\ar2010.urdf 에 저장")
        print("    3) 이 스크립트 재실행")
        print("\n    다운로드 명령:")
        print("    pip install requests")
        print("    (또는 브라우저에서 직접 다운로드)")
        sys.exit(0)
    except Exception as e:
        print(f"    ❌ Klampt 기본 모듈 오류: {e}")
        sys.exit(1)

# ── 3. FK(IK(P)) = P' 자체검증 ──────────────────────────────
print("\n[3] FK(IK(P)) = P' 자체검증 (3개 자세)...")

# AR2010 링크 수에 따라 끝단 링크 자동 설정
ee_link = robot.numLinks() - 1  # 마지막 링크 = 끝단

test_poses_mm = [
    ([500.0, -200.0, 900.0],  [0.0, 0.0, -1.0]),  # 정면 왼쪽
    ([600.0,    0.0, 800.0],  [0.0, 0.0, -1.0]),  # 정면
    ([500.0,  200.0, 700.0],  [0.0, 0.0, -1.0]),  # 정면 오른쪽
]

results = []
for i, (pos_mm, direction) in enumerate(test_poses_mm):
    print(f"\n    [자세 {i+1}] 목표: {pos_mm}")
    try:
        # mm → m 변환 (Klampt는 m 단위)
        pos_m = [p/1000.0 for p in pos_mm]

        # IK 설정
        goal = ik.objective(robot.link(ee_link),
                           local=[0,0,0],
                           world=pos_m)
        res = ik.solve(goal, iters=100, tol=1e-4)

        if res:
            # FK로 되돌리기
            T = robot.link(ee_link).getTransform()
            R, t = T
            pos_fk_m = list(t)
            pos_fk_mm = [p*1000.0 for p in pos_fk_m]

            err = math.sqrt(sum((a-b)**2 for a,b in zip(pos_mm, pos_fk_mm)))
            print(f"    IK 해: {[round(math.degrees(robot.getConfig()[j]),1) for j in range(min(6,robot.numDrivers()))]}°")
            print(f"    FK 되돌림: [{round(pos_fk_mm[0],2)}, {round(pos_fk_mm[1],2)}, {round(pos_fk_mm[2],2)}]")
            print(f"    위치오차: {round(err,4)} mm  {'✅' if err < 1.0 else '⚠️ 큰 오차'}")
            results.append({'pose': i+1, 'err_mm': round(err,4), 'ok': err < 1.0})
        else:
            print(f"    ⚠️  IK 해 없음 (작업범위 밖 가능성)")
            results.append({'pose': i+1, 'err_mm': None, 'ok': False})

    except Exception as e:
        print(f"    ❌ 오류: {e}")
        results.append({'pose': i+1, 'err_mm': None, 'ok': False})

# ── 4. 결과 요약 ─────────────────────────────────────────────
print("\n" + "="*50)
print("★ Klampt IK 자체검증 결과:")
ok_count = sum(1 for r in results if r['ok'])
for r in results:
    status = "✅" if r['ok'] else "❌"
    err = f"{r['err_mm']}mm" if r['err_mm'] is not None else "해 없음"
    print(f"  자세{r['pose']}: {status} 오차 {err}")

print(f"\n  {ok_count}/{len(results)} 성공")
if ok_count == len(results):
    print("  → Klampt IK 신뢰 가능. RoboDK 비교 단계로 진행 가능.")
elif ok_count == 0:
    print("  → URDF 모델 또는 작업범위 확인 필요.")
else:
    print("  → 일부 자세 범위 밖 — 자세 위치 조정 필요.")
print("="*50)