"""
welding_pipeline.py 스모크 테스트 (합성 입력)

STL 없이 하드코딩된 토치 끝 위치 3개로 파이프라인 하위단계 검증:
  solve_ik_torch() → generate_jbi() → 파일 포맷 확인

검증 기준:
  1. IK 수렴 (None 아닌 결과)
  2. JBI 파일 포맷 정확 (POSTYPE PULSE, //INST, ///GROUP1 RB1, NOP)
  3. 왕복 위치오차 < 2mm (토치 기준, TCP 역변환 포함)

실로봇 주의: CEO 승인 없이 투입 금지.
좌표계: 로봇 BASE (S관절 원점). 바닥 기준 아님.
"""
import sys
import tempfile
from pathlib import Path
import numpy as np

# welding_pipeline 경로 등록
sys.path.insert(0, str(Path(__file__).parent))
from welding_pipeline import (
    solve_ik_torch, generate_jbi, torch_to_flange, PLACEHOLDER_ROT
)

# ── ik_ar2010 직접 임포트 (왕복검증용 FK) ──────────────────────────────────
_IK_DIR = Path(__file__).resolve().parents[4] / '야스카와티칭보정' / 'src' / 'jbi_viewer'
sys.path.insert(0, str(_IK_DIR))
from fk_ar2010 import load_kinematics, fk_matrix, deg_to_pulse, AXIS_ORDER

kine = load_kinematics()
tcp_mm = np.array([kine['tcp_offset_mm']['X'],
                   kine['tcp_offset_mm']['Y'],
                   kine['tcp_offset_mm']['Z']])

# ── 합성 토치 끝 목표 (FK 역산으로 도달 가능성 보장) ──────────────────────
# 방법: 관절각 → FK(플랜지) → 토치 끝 = 플랜지 + R_플랜지 @ tcp
# 이렇게 만들어야 ik_ar2010이 반드시 수렴 가능한 목표임을 보장 (컨닝 금지: 기존 데이터 무관 관절각)
_fk_configs_deg = [
    [ 15.0, -10.0,  50.0,  40.0, -20.0,  30.0],
    [-20.0,   5.0,  60.0, -50.0,  15.0, -40.0],
    [ 35.0,  20.0,  45.0,  25.0, -35.0,  60.0],
]

from fk_ar2010 import fk_matrix, deg_to_pulse

def _make_torch_target(q_deg, kine, tcp_mm):
    T = fk_matrix(deg_to_pulse(q_deg, kine), kine)
    p_flange = T[:3, 3]
    R_flange = T[:3, :3]
    p_torch = p_flange + R_flange @ tcp_mm
    R_torch = R_flange
    return p_torch.tolist(), R_torch

torch_targets_with_rot = [_make_torch_target(q, kine, tcp_mm) for q in _fk_configs_deg]
torch_targets = [p for p, _ in torch_targets_with_rot]
torch_rots    = [r for _, r in torch_targets_with_rot]

print("=" * 60)
print("welding_pipeline.py 스모크 테스트")
print("입력: 합성 토치 끝 좌표 (S관절 원점 기준)")
print("=" * 60)

# ── IK 풀기 ─────────────────────────────────────────────────────────────────
poses_with_joints = []
prev_q = None
print(f"\n{'#':>2}  {'토치 위치 (mm)':>28}  {'IK수렴':>6}  {'플랜지 위치':>24}")
print("-" * 70)

SEED_OFFSET = np.array([20.0, -15.0, 15.0, -20.0, 10.0, -15.0])
limits = kine['robodk_joint_limits_deg']
lim_min = np.array([limits[ax]['min'] for ax in AXIS_ORDER])
lim_max = np.array([limits[ax]['max'] for ax in AXIS_ORDER])

for i, (torch_pos, torch_rot) in enumerate(zip(torch_targets, torch_rots)):
    p_flange, R_flange = torch_to_flange(torch_pos, torch_rot, kine=kine)
    seed = np.clip(np.array(_fk_configs_deg[i]) + SEED_OFFSET, lim_min, lim_max).tolist()
    q_deg = solve_ik_torch(torch_pos, torch_rot, q0_deg=seed, kine=kine)

    if q_deg is not None:
        prev_q = q_deg
        flange_str = f"[{p_flange[0]:.1f}, {p_flange[1]:.1f}, {p_flange[2]:.1f}]"
    else:
        flange_str = "—"

    torch_str = f"[{torch_pos[0]:.0f}, {torch_pos[1]:.0f}, {torch_pos[2]:.0f}]"
    print(f"{i+1:>2}  {torch_str:>28}  {'✓' if q_deg is not None else '✗':>6}  {flange_str:>24}")
    poses_with_joints.append((torch_pos, torch_rot, q_deg))

# ── JBI 생성 ─────────────────────────────────────────────────────────────────
print("\n" + "=" * 60)
print("JBI 파일 생성 + 포맷 확인")
print("=" * 60)

with tempfile.NamedTemporaryFile(suffix='.JBI', mode='w', delete=False,
                                  encoding='utf-8') as f:
    jbi_path = Path(f.name)

from scipy.spatial.transform import Rotation as ScipyR
# generate_jbi expects (pos_mm, scipy_Rotation, joint_deg_list)
poses_for_jbi = [
    (p, ScipyR.from_matrix(r), q)
    for (p, r, q) in poses_with_joints
]
generate_jbi(poses_for_jbi, jbi_path, kine=kine)

content = jbi_path.read_text(encoding='utf-8')
lines = content.splitlines()
print("--- JBI 앞 22줄 ---")
for ln in lines[:22]:
    print(f"  {ln}")
print(f"  ... (총 {len(lines)}줄)")

# 포맷 체크
checks = {
    '///POSTYPE PULSE': any('///POSTYPE PULSE' in ln for ln in lines),
    '//INST (이중슬래시)': any(ln.startswith('//INST') for ln in lines),
    '///GROUP1 RB1': any('///GROUP1 RB1' in ln for ln in lines),
    'NOP': any(ln.strip() == 'NOP' for ln in lines),
    'C=정수 (+ 없음)': any(ln.startswith('C') and '=' in ln for ln in lines),
}
print("\n포맷 체크:")
for k, v in checks.items():
    print(f"  {k:28s}: {'✓' if v else '✗'}")

# ── 왕복 검증 (토치 끝 기준) ──────────────────────────────────────────────────
print("\n" + "=" * 60)
print("왕복 검증: 토치 끝 목표 → IK → FK → 토치 끝 복원 오차")
print("(TCP 역변환 포함 — 이게 실제 용접 오차)")
print("=" * 60)
print(f"{'#':>2}  {'토치위치오차mm':>14}  {'판정':>6}")
print("-" * 30)

all_ok = True
for i, (torch_pos, torch_rot_i, q_deg) in enumerate(poses_with_joints):
    if q_deg is None:
        print(f"{i+1:>2}  {'IK 미수렴':>14}  {'불합격':>6}")
        all_ok = False
        continue

    # FK → 플랜지 위치
    T = fk_matrix(deg_to_pulse(q_deg, kine), kine)
    p_flange_fk = T[:3, 3]
    R_flange_fk = T[:3, :3]

    # 플랜지 → 토치 끝 복원
    p_torch_recovered = p_flange_fk + R_flange_fk @ tcp_mm

    err = np.linalg.norm(p_torch_recovered - np.array(torch_pos))
    ok = err < 2.0
    all_ok = all_ok and ok
    print(f"{i+1:>2}  {err:>14.4f}  {'합격' if ok else '불합격':>6}")

print("-" * 30)
print(f"전체: {'합격 ✓' if all_ok else '불합격 ✗'}")

jbi_path.unlink()

print("\n" + "=" * 60)
print("【보고】")
print("=" * 60)
converged = sum(1 for _, _, q in poses_with_joints if q is not None)
print(f"| IK 수렴          | {converged}/{len(torch_targets)}개")
print(f"| JBI 포맷         | {'✓' if all(checks.values()) else '일부 ✗'}")
print(f"| 토치 끝 왕복     | {'✓ <2mm' if all_ok else '✗ >2mm'}")
print(f"| 좌표계           | S관절 원점 (RoboDK 기준)")
print(f"| TCP 역변환       | p_flange = p_torch - R_torch @ tcp (R_tcp=I 가정)")
print(f"| 방향 미확정 시   | PLACEHOLDER_ROT 사용 (RPY [0,90,-180])")
print(f"| 실로봇 주의      | CEO 승인 없이 투입 금지")