"""
weld_ik.py — 블럭: 토치끝→플랜지 변환 + IK (ik_ar2010 Jacobian DLS)
(welding_pipeline.py에서 2026-07-04 분할 — 로직 원문 그대로, 수정 없음)
⚠️ 동결값 연계: tcp_offset·기구학은 ar2010_kinematics.json에서 읽기만 한다.
"""
import sys
from pathlib import Path
import numpy as np

# ── ik_ar2010 경로 등록 (2번 영역: 야스카와티칭보정/src/jbi_viewer) ─────────
_IK_DIR = Path(__file__).resolve().parents[4] / '야스카와티칭보정' / 'src' / 'jbi_viewer'
if str(_IK_DIR) not in sys.path:
    sys.path.insert(0, str(_IK_DIR))

from ik_ar2010 import ik as _ik_solve
from fk_ar2010 import load_kinematics as _load_kine

# ── 토치 방향 플레이스홀더 (1번이 방향 미제공 시 사용) ──────────────────────
# RoboDK 기준 RPY [0, 90, -180] — 홈 자세 방향
# 1번이 실제 방향을 제공하면 이 상수는 무시됨
import numpy as _np
_r, _p, _y = _np.radians([0.0, 90.0, -180.0])
_cr, _sr = _np.cos(_r), _np.sin(_r)
_cp, _sp = _np.cos(_p), _np.sin(_p)
_cy, _sy = _np.cos(_y), _np.sin(_y)
PLACEHOLDER_ROT = (
    _np.array([[1,0,0],[0,_cr,-_sr],[0,_sr,_cr]]) @
    _np.array([[_cp,0,_sp],[0,1,0],[-_sp,0,_cp]]) @
    _np.array([[_cy,-_sy,0],[_sy,_cy,0],[0,0,1]])
)
del _r,_p,_y,_cr,_sr,_cp,_sp,_cy,_sy,_np


def torch_to_flange(torch_pos_mm, torch_rot, tcp_offset_mm=None, kine=None):
    """
    토치 끝 위치+방향 → 플랜지 위치+방향 변환.

    입력 좌표계: 로봇 BASE (S관절 원점)
    가정: TCP 회전성분 없음 (R_tcp = I) — TOOL.CND 번역값 기준

    Parameters
    ----------
    torch_pos_mm   : [X, Y, Z] mm (토치 끝, S관절 원점 기준)
    torch_rot      : 3×3 회전행렬 (토치 프레임 = 플랜지 프레임, R_tcp=I)
    tcp_offset_mm  : [X, Y, Z] mm (플랜지→토치 끝, 플랜지 프레임). None → JSON에서 로드
    kine           : ar2010_kinematics.json dict. None → 자동 로드

    Returns
    -------
    (flange_pos_mm, flange_rot) — ik_ar2010.ik()에 바로 전달 가능
    """
    if kine is None:
        kine = _load_kine()
    if tcp_offset_mm is None:
        t = kine['tcp_offset_mm']
        tcp_offset_mm = [t['X'], t['Y'], t['Z']]

    p_torch = np.array(torch_pos_mm, dtype=float)
    R_torch = np.array(torch_rot,    dtype=float)
    p_tcp   = np.array(tcp_offset_mm, dtype=float)

    # p_flange = p_torch - R_torch @ p_tcp  (R_tcp = I 가정)
    p_flange = p_torch - R_torch @ p_tcp
    return p_flange.tolist(), R_torch


def solve_ik_torch(torch_pos_mm, torch_rot, q0_deg=None, kine=None,
                   tcp_offset_mm=None):
    """
    토치 끝 목표 → 플랜지 변환 → IK 풀기.

    Parameters
    ----------
    torch_pos_mm  : [X, Y, Z] mm (토치 끝, S관절 원점 기준)
    torch_rot     : 3×3 회전행렬. None → PLACEHOLDER_ROT 사용
    q0_deg        : IK 초기 관절각 (도). None → [0]*6

    Returns
    -------
    list[float](6) : 수렴한 관절각 (도, SLURBT 순)
    None            : 미수렴
    """
    if kine is None:
        kine = _load_kine()
    if torch_rot is None:
        torch_rot = PLACEHOLDER_ROT

    p_flange, R_flange = torch_to_flange(torch_pos_mm, torch_rot, tcp_offset_mm, kine)
    return _ik_solve(p_flange, R_flange, q0_deg=q0_deg, kine=kine,
                     max_iter=500, tol_pos_mm=0.5, tol_ang_deg=0.5)