"""
AR2010 용접 자동경로 파이프라인
STL → 심검출 → 토치자세 → IK → JBI

[2026-07-04 블럭화] 로직은 3개 블럭으로 분할됨 (원문 그대로 이동, 수정 없음):
  weld_seam.py : STL→포인트클라우드 + 심검출 + 경로 생성
  weld_ik.py   : 토치끝→플랜지 변환 + IK (PLACEHOLDER_ROT 포함)
  weld_jbi.py  : JBI 생성 (펄스 변환 — 동결 로직)
이 파일은 호환 파사드 — 기존 import 경로(from welding_pipeline import ...)는 전부 그대로 작동한다.

출처:
- 심검출: romi-lab/robotic-welding-demo (demo_all.py)
- IK: ik_ar2010.py (Jacobian DLS, 6DOF 위치+방향)
- JBI: 실제 JBI 파일(000000POINT.JBI) 포맷 직독

──────────────────────────────────────────────────────────
입력 인터페이스 스펙 (1번 ↔ 2번 연결 기준)
──────────────────────────────────────────────────────────
좌표계: 로봇 BASE (S관절 원점 = RoboDK 기준)
  ※ 물리적 바닥 기준이 아님. S관절 판면이 Z=0.
  ※ 물리 바닥 ↔ S관절 높이 차: 505mm (상수, ar2010_kinematics.json)

입력 대상: 토치 끝(torch tip) 위치 + 토치 방향
  - pos_mm  : [X, Y, Z] mm  (S관절 원점 기준)
  - rot_3x3 : 3×3 회전행렬  (토치 프레임 방향 = 플랜지 방향, R_tcp=I 가정)
  - 토치 방향 미확정 시: 고정 플레이스홀더 사용 (weld_ik.PLACEHOLDER_ROT)

내부 변환: 토치 끝 → 플랜지
  p_flange = p_torch - R_torch @ tcp_offset_mm   (R_tcp=I 가정)
  R_flange = R_torch
  (tcp_offset_mm: ar2010_kinematics.json TOOL 0, [-133.246, -10.813, 504.281] mm)

실로봇 주의: CEO 승인 없이 투입 금지.
──────────────────────────────────────────────────────────
"""

import sys
from pathlib import Path
import numpy as np
from scipy.interpolate import splprep, splev
from scipy.spatial.transform import Rotation as R
from scipy.spatial import KDTree
import open3d as o3d

# ── 블럭 재수출 (기존 API 100% 호환) ──────────────────────────────────────
from weld_seam import (
    stl_to_pointcloud, find_feature_value, cluster_groove, thin_line,
    sort_points, generate_trajectory, find_normal, find_orientation,
)
from weld_ik import (
    _IK_DIR, _ik_solve, _load_kine, PLACEHOLDER_ROT,
    torch_to_flange, solve_ik_torch,
)
from weld_jbi import (
    pose_to_motoman_xyzwpr, JBI_HEADER, JBI_FOOTER,
    _deg_to_pulse_inline, generate_jbi,
)


# ─────────────────────────────────────────────
# 5. 전체 파이프라인
# ─────────────────────────────────────────────

def run_pipeline(stl_path, output_jbi_path, voxel_size=0.003):
    """
    STL → 심검출 → 토치자세 → IK → JBI 전체 파이프라인
    """
    print("[1/5] STL 로드 + 포인트 클라우드 변환:", stl_path)
    pcd = stl_to_pointcloud(stl_path)
    pcd = pcd.voxel_down_sample(voxel_size)
    pcd.estimate_normals(
        search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size * 5, max_nn=30)
    )

    print("[2/5] 심검출 (asymmetry feature)")
    feature = find_feature_value("asymmetry", pcd, voxel_size)
    groove_pcd = cluster_groove(pcd, feature)
    groove_pts = np.asarray(groove_pcd.points)
    print("  검출된 홈 포인트:", len(groove_pts))

    print("[3/5] 중심선 추출 + 경로 생성")
    projected, _ = thin_line(groove_pts)
    sorted_pts = sort_points(projected)
    trajectory = generate_trajectory(sorted_pts, num_output=50)

    # mm 변환 (STL이 mm 단위이면 그대로, m이면 *1000)
    # 여기서는 m 기준 STL이라고 가정 → mm 변환
    trajectory_mm = trajectory * 1000.0

    surface_normal = find_normal(pcd)
    poses = find_orientation(trajectory, surface_normal)  # m 단위 경로

    print("[4/5] IK 계산 (AR2010 — ik_ar2010 Jacobian DLS)")
    kine = _load_kine()
    poses_with_joints = []
    prev_q = None

    for i, (pos_m, rot) in enumerate(poses):
        torch_pos_mm = (pos_m * 1000.0).tolist()
        torch_rot    = rot.as_matrix()
        joint_deg = solve_ik_torch(torch_pos_mm, torch_rot, q0_deg=prev_q, kine=kine)
        if joint_deg is not None:
            prev_q = joint_deg
        else:
            print(f"  [WARN] 포인트 {i+1}: IK 미수렴 — 건너뜀")
        poses_with_joints.append((torch_pos_mm, rot, joint_deg))
        if i % 10 == 0:
            print("  포인트 {}/{} IK완료 (수렴: {})".format(
                i + 1, len(poses), joint_deg is not None))

    print("[5/5] JBI 파일 생성:", output_jbi_path)
    generate_jbi(poses_with_joints, output_jbi_path)

    print("=== 파이프라인 완료 ===")
    return poses_with_joints


# ─────────────────────────────────────────────
# 실행 예시
# ─────────────────────────────────────────────

if __name__ == "__main__":
    import sys

    if len(sys.argv) < 3:
        print("사용법: python welding_pipeline.py <STL파일> <출력JBI파일>")
        print("예시: python welding_pipeline.py workpiece.stl WELDING_AUTO.JBI")
        sys.exit(1)

    stl_path = sys.argv[1]
    jbi_path = sys.argv[2]
    run_pipeline(stl_path, jbi_path)