"""
RoboDK 용접 경로 생성 방식 완전 분석
- Curve Follow Project 구조 파악
- 3D 오브젝트 → 경로 변환 과정 추적
- 점 생성 알고리즘 역추적
"""
from robodk.robolink import Robolink, ITEM_TYPE_ROBOT, ITEM_TYPE_OBJECT, ITEM_TYPE_PROGRAM, ITEM_TYPE_FRAME, ITEM_TYPE_TOOL
from robodk.robomath import *
import json

RDK = Robolink()
print("=" * 60)
print("RoboDK 용접 경로 생성 방식 완전 분석")
print("=" * 60)

# ── 1. 전체 아이템 구조 파악 ──────────────────────────────
print("\n[1] 전체 스테이션 구조")
for t in range(1, 20):
    try:
        items = RDK.ItemList(t)
        if items:
            print(f"  타입 {t:2d}: {[i.Name() for i in items]}")
    except: pass

# ── 2. 3D 오브젝트(메시) 분석 ────────────────────────────
print("\n[2] 3D 오브젝트 분석 (타입 5)")
objects = RDK.ItemList(5)  # ITEM_TYPE_OBJECT
for obj in objects:
    print(f"\n  오브젝트: {obj.Name()}")
    try:
        pose = obj.Pose()
        pos = pose.Pos()
        print(f"    위치: x={pos[0]:.1f} y={pos[1]:.1f} z={pos[2]:.1f}")
    except Exception as e:
        print(f"    위치 ERR: {e}")

    # 오브젝트의 곡선(커브) 데이터 추출 시도
    try:
        curves = obj.GetPoints(0)  # 0=커브, 1=포인트
        print(f"    GetPoints(0) 반환: {type(curves)} len={len(curves) if curves else 0}")
        if curves and len(curves) > 0:
            print(f"    첫 번째 항목: {curves[0]}")
    except Exception as e:
        print(f"    GetPoints ERR: {e}")

    try:
        pts = obj.GetPoints(1)
        print(f"    GetPoints(1): {type(pts)} len={len(pts) if pts else 0}")
    except Exception as e:
        print(f"    GetPoints(1) ERR: {e}")

# ── 3. Weld 프로그램 → ArcStart~ArcEnd 구간 추출 ─────────
print("\n[3] Weld 프로그램 구조 (ArcStart~ArcEnd 구간)")
prog = RDK.Item('Weld')
weld_segments = []
current_seg = None

for i in range(prog.InstructionCount()):
    name, instype, movetype, isjointtarget, pose, joints = prog.Instruction(i)

    if 'ArcStart' in name:
        current_seg = {'start_idx': i, 'pts': [], 'name': name}
    elif 'ArcEnd' in name and current_seg is not None:
        current_seg['end_idx'] = i
        weld_segments.append(current_seg)
        current_seg = None
    elif current_seg is not None and movetype == 2 and pose is not None:
        pos = pose.Pos()
        rot = pose.Rot()
        current_seg['pts'].append({
            'name': name,
            'x': round(pos[0], 2),
            'y': round(pos[1], 2),
            'z': round(pos[2], 2),
            'pose': pose.tolist() if hasattr(pose, 'tolist') else str(pose)
        })

print(f"  총 용접 구간: {len(weld_segments)}개")
for si, seg in enumerate(weld_segments):
    pts = seg['pts']
    print(f"\n  [구간 {si+1}] {len(pts)}점")
    if pts:
        p0, pN = pts[0], pts[-1]
        dx = pN['x']-p0['x']
        dy = pN['y']-p0['y']
        dz = pN['z']-p0['z']
        dist = (dx**2+dy**2+dz**2)**0.5
        print(f"    시작: ({p0['x']}, {p0['y']}, {p0['z']})")
        print(f"    끝:   ({pN['x']}, {pN['y']}, {pN['z']})")
        print(f"    거리: {dist:.1f}mm")
        # 직선 여부 판단
        if len(pts) == 2:
            print(f"    형태: 직선 (MoveL 2점)")
        else:
            # 중간점이 직선 위에 있는지 확인
            max_dev = 0
            for p in pts[1:-1]:
                # 직선 위 거리 계산 (간단히)
                t = ((p['x']-p0['x'])*dx + (p['y']-p0['y'])*dy + (p['z']-p0['z'])*dz) / (dist**2+1e-9)
                px_on = p0['x']+t*dx
                py_on = p0['y']+t*dy
                pz_on = p0['z']+t*dz
                dev = ((p['x']-px_on)**2+(p['y']-py_on)**2+(p['z']-pz_on)**2)**0.5
                max_dev = max(max_dev, dev)
            print(f"    형태: {'직선' if max_dev<1 else '곡선'} (최대편차 {max_dev:.2f}mm, {len(pts)}점)")

        # 모든 점 출력 (곡선 구간)
        if len(pts) > 2:
            print(f"    점 목록:")
            for p in pts:
                print(f"      ({p['x']:8.2f}, {p['y']:8.2f}, {p['z']:8.2f})")

# ── 4. 툴 자세(토치 방향) 분석 ───────────────────────────
print("\n[4] 용접 토치 자세 분석 (첫 번째 구간)")
prog2 = RDK.Item('Weld')
in_arc = False
for i in range(prog2.InstructionCount()):
    name, instype, movetype, isjointtarget, pose, joints = prog2.Instruction(i)
    if 'ArcStart' in name: in_arc=True
    if 'ArcEnd' in name: in_arc=False; break
    if in_arc and movetype==2 and pose is not None:
        print(f"  {name}:")
        # 4x4 포즈 행렬 출력
        try:
            print(f"    포즈행렬(위치+방향): {[round(v,3) for v in pose.Pos()]}")
            # 회전 행렬 (방향 벡터)
            # pose는 4x4 행렬: [[r00,r01,r02,tx],[r10,r11,r12,ty],[r20,r21,r22,tz],[0,0,0,1]]
            arr = pose.rows if hasattr(pose,'rows') else None
            if arr:
                print(f"    X방향: {[round(arr[j][0],3) for j in range(3)]}")
                print(f"    Y방향: {[round(arr[j][1],3) for j in range(3)]}")
                print(f"    Z방향(토치): {[round(arr[j][2],3) for j in range(3)]}")
        except Exception as e:
            print(f"    포즈 ERR: {e}")

# ── 5. Weld 1 오브젝트 (커브 데이터) ─────────────────────
print("\n[5] 'Weld 1' 오브젝트 상세 분석")
weld1 = RDK.Item('Weld 1')
if weld1.Valid():
    print(f"  유효: True")
    print(f"  타입: {weld1.type()}")
    try:
        # RoboDK에서 커브 오브젝트는 GetPoints로 경로점 추출 가능
        for mode in range(5):
            try:
                pts, feat = weld1.GetPoints(mode)
                print(f"  GetPoints(mode={mode}): pts={len(pts) if pts else 0} feat={feat}")
                if pts and len(pts) > 0 and len(pts) < 20:
                    for p in pts[:5]:
                        print(f"    {p}")
            except Exception as e2:
                print(f"  GetPoints(mode={mode}) ERR: {e2}")
    except Exception as e:
        print(f"  ERR: {e}")

# ── 결과 저장 ──────────────────────────────────────────────
result = {
    "weld_segments": weld_segments,
    "summary": {
        "total_segments": len(weld_segments),
        "segment_types": [
            "직선" if len(s['pts'])<=2 else "곡선"
            for s in weld_segments
        ]
    }
}
with open(r"E:\도진팩토리\3D스캔및티칭시스템\robodk_deep.json", "w", encoding="utf-8") as f:
    json.dump(result, f, ensure_ascii=False, indent=2)
print("\n✅ 분석 완료 → robodk_deep.json")