"""
RoboDK Curve Follow(AddMachiningProject) 실제 동작 테스트
★ 핵심: AddMachiningProject가 실제 경로를 뽑는지 확인
★ 주의: RoboDK GUI가 열린 상태에서 실행 (headless 금지)

수정 이력:
- [4] AddCurve 6xN 형식(xyz+법선) 사용 + setMachiningParameters로 곡선 연결
- [5] SolveIK 반환=Mat → .list() 변환 후 round
"""
import sys

print("=" * 55)
print("RoboDK Curve Follow 테스트")
print("=" * 55)

# ── 1. RoboDK 연결 ───────────────────────────────────────────
print("\n[1] RoboDK 연결 시도...")
try:
    from robodk.robolink import Robolink, ITEM_TYPE_ROBOT, ITEM_TYPE_PROGRAM
    from robodk.robomath import transl, roty, eye, pi, Mat
    RDK = Robolink()
    ver = RDK.Version()
    print(f"    ✅ 연결됨. RoboDK 버전: {ver}")
except Exception as e:
    print(f"    ❌ 연결 실패: {e}")
    print("    → RoboDK GUI가 열려있는지 확인하세요.")
    sys.exit(1)

# ── 2. AR2010 로봇 로드 ──────────────────────────────────────
print("\n[2] AR2010 로봇 로드 + TCP 525.2mm...")
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"    로봇: {robot.Name()}")

    tcp_pose = transl(0, 0, 525.2)
    tool = robot.AddTool(tcp_pose, '용접토치_525')
    robot.setTool(tool)
    print(f"    ✅ TCP 525.2mm 설정")
except Exception as e:
    print(f"    ❌ 로봇 로드 실패: {e}")
    sys.exit(1)

# ── 3. 테스트 용접선 정의 ────────────────────────────────────
print("\n[3] 테스트용 직선 용접선 (6xN: xyz + 법선 ijk)...")
# RoboDK AddCurve는 6xN(위치+법선)을 요구하거나 projection 필요
# 법선 방향: [0,0,-1] = 토치가 아래쪽(-Z)을 향함
curve_points_6xN = [
    [500, -200, 800,  0, 0, -1],
    [500,    0, 800,  0, 0, -1],
    [500,  200, 800,  0, 0, -1],
]
print(f"    점 {len(curve_points_6xN)}개")
print(f"    시작: {curve_points_6xN[0][:3]}, 법선: {curve_points_6xN[0][3:]}")
print(f"    끝:   {curve_points_6xN[-1][:3]}")

# ── 4. ★핵심 테스트: AddMachiningProject (Curve Follow) ──────
print("\n[4] ★AddMachiningProject + Curve Follow...")
try:
    # 기준 프레임 생성
    ref = RDK.AddFrame('테스트기준', robot)
    ref.setPose(eye(4))
    robot.setPoseFrame(ref)

    # ★ 수정: 6xN 형식으로 곡선 오브젝트 생성
    # AddCurve(points, reference, add_to_ref, projection_type)
    # projection_type=0 → 법선 그대로 사용 (6xN 필요)
    curve_obj = RDK.AddCurve(curve_points_6xN, ref, True, 0)
    if not curve_obj.Valid():
        raise RuntimeError("곡선 오브젝트 생성 실패 — 형식 확인 필요")
    print(f"    ✅ 곡선 오브젝트 생성: {curve_obj.Name()}")
    print(f"    타입 확인: {type(curve_obj)}")

    # ★ 수정: AddMachiningProject 생성 후 setMachiningParameters로 곡선 연결
    machining, status = RDK.AddMachiningProject('테스트_CurveFollow', robot)
    print(f"    AddMachiningProject 생성 상태: {status}")
    print(f"    machining Valid: {machining.Valid()}")

    if not machining.Valid():
        print("    ❌ AddMachiningProject 유효하지 않음 — 라이선스 잠김 가능성")
    else:
        print(f"    ✅ 프로젝트 생성: {machining.Name()}")

        # ★ 핵심 누락 수정: 곡선을 프로젝트에 연결
        machining.setMachiningParameters(ncfile='', part=curve_obj, params='')
        print(f"    ✅ 곡선 연결 완료")

        # 경로 계산
        update_result = machining.Update()
        print(f"    Update 반환값: {update_result}")
        print(f"    타입: {type(update_result)}")

        # Update 결과 해석
        if isinstance(update_result, (list, tuple)) and len(update_result) >= 2:
            instructions, time_s, distance_mm, valid_pct, status_msg = update_result
            print(f"    경로점 수: {instructions}")
            print(f"    소요시간 예상: {time_s:.1f}초")
            print(f"    경로 거리: {distance_mm:.1f}mm")
            print(f"    유효율: {valid_pct:.1%}")
            print(f"    상태: {status_msg}")
        else:
            print(f"    Update 원시값: {update_result}")

except Exception as e:
    print(f"    ❌ 오류: {type(e).__name__}: {e}")
    import traceback
    traceback.print_exc()

# ── 5. 기본 IK/FK 추출 테스트 ───────────────────────────────
print("\n[5] 기본 IK/FK 추출 (항상 동작해야 함)...")
try:
    test_pose = transl(500, 0, 800) * roty(pi)

    # ★ 수정: SolveIK 반환=Mat → .list() 후 처리
    joints_mat = robot.SolveIK(test_pose)
    print(f"    SolveIK 반환 타입: {type(joints_mat)}")

    joints_list = joints_mat.list()
    print(f"    joints_list 길이: {len(joints_list)}")
    print(f"    joints_list 내용: {joints_list}")

    if len(joints_list) == 0:
        print("    ⚠️  IK 해 없음 (작업범위 밖)")
    else:
        joints_6 = joints_list[:6]
        joints_rounded = [round(float(j), 2) for j in joints_6]
        print(f"    ✅ 관절각(도): {joints_rounded}")

        # FK 검증
        fk_pose = robot.SolveFK(joints_list)
        pos = fk_pose.Pos()
        err = ((pos[0]-500)**2 + (pos[1]-0)**2 + (pos[2]-800)**2) ** 0.5
        print(f"    FK 위치: [{round(pos[0],2)}, {round(pos[1],2)}, {round(pos[2],2)}]")
        print(f"    FK→목표 오차: {round(err, 4)} mm  {'✅' if err < 1.0 else '⚠️'}")

except Exception as e:
    print(f"    ❌ 오류: {type(e).__name__}: {e}")
    import traceback
    traceback.print_exc()

print("\n" + "=" * 55)
print("★ 판단 기준:")
print("  [4] '✅ 곡선 연결' + Update 경로점 수 > 0 → Curve Follow 정상")
print("  [4] '❌ 라이선스 잠김' → test_robodk_simple_joints.py 사용")
print("  [5] 관절각 출력 → Klampt 비교 가능")
print("=" * 55)