"""
weld_jbi.py — 블럭: JBI 파일 생성 (Motoman 포맷, POSTYPE PULSE)
(welding_pipeline.py에서 2026-07-04 분할 — 로직 원문 그대로, 수정 없음)
⚠️ 동결 로직: _deg_to_pulse_inline은 ik_ar2010.py의 deg_to_pulse와 동일 — 수정 절대 금지(철칙 7조).
"""
import numpy as np
from weld_ik import _load_kine


def pose_to_motoman_xyzwpr(pos_mm, rot):
    """
    pos_mm: (3,) mm
    rot: scipy Rotation
    반환: (x, y, z, Rx, Ry, Rz) — mm, degrees (Motoman 오일러 ZYX)
    """
    x, y, z = pos_mm
    euler = rot.as_euler('ZYX', degrees=True)
    Rz, Ry, Rx = euler
    return x, y, z, Rx, Ry, Rz


JBI_HEADER = """/JOB
//NAME WELDING_AUTO
//POS
///NPOS {npos},0,0,0,0,0
///TOOL 0
///POSTYPE PULSE
///PULSE
"""

JBI_FOOTER = """/END
"""


def _deg_to_pulse_inline(joint_deg_6: list, kine: dict) -> list[int]:
    """
    관절각(도, SLURBT 순, 6개) → 펄스 정수 변환.
    ar2010_kinematics.json 기준. (ik_ar2010.py의 deg_to_pulse와 동일 로직)
    """
    ppd  = kine['pulses_per_deg']
    cal  = kine['calibration_offset']
    sign = kine['axis_sign']
    axes = ['S', 'L', 'U', 'R', 'B', 'T']
    return [
        int(round(cal.get(ax, 0) + sign.get(ax, 1) * joint_deg_6[i] * ppd[ax]))
        for i, ax in enumerate(axes)
    ]


def generate_jbi(poses_with_joints, output_path, weld_speed_v=50.0, approach_dist=100.0,
                 kine=None):
    """
    poses_with_joints: list of (pos_mm, rot, joint_deg_list)
      joint_deg_list: 6개 관절각(도, SLURBT 순) — ik_ar2010.ik() 반환값
    output_path: 저장할 .JBI 파일 경로
    kine: ar2010_kinematics.json dict. None이면 자동 로드.

    수정 이력:
      - [BUG FIX] XYZ 좌표 기록 → 펄스 변환으로 교체 (POSTYPE PULSE 선언과 일치)
      - [BUG FIX] /INST → //INST (실제 JBI 포맷 기준)
      - [BUG FIX] ///GROUP1 RB1 추가
      - IK 미수렴(None) 점은 쓰지 않고 경고만 출력
    """
    if kine is None:
        kine = _load_kine()

    # IK 수렴 포인트만 추출
    valid = []
    for i, (pos, rot, joints) in enumerate(poses_with_joints):
        if joints is None:
            print(f"[JBI WARN] point {i}: IK 미수렴 — 건너뜀 (잘못된 펄스 금지)")
            continue
        joints_6 = list(joints)[:6]
        pulses = _deg_to_pulse_inline(joints_6, kine)
        valid.append((i, pulses, pos, rot))

    npos = len(valid)
    lines = []
    lines.append(JBI_HEADER.format(npos=npos))

    for ci, (orig_i, pulses, pos, rot) in enumerate(valid):
        lines.append("C{:05d}={}".format(ci, ','.join(str(p) for p in pulses)))

    lines.append('//INST')
    lines.append(f'///DATE {__import__("datetime").datetime.now().strftime("%Y/%m/%d %H:%M")}')
    lines.append('///ATTR SC,RW')
    lines.append('///GROUP1 RB1')
    lines.append('NOP')
    lines.append('MOVJ C00000 VJ=25.00')
    lines.append('ARCON AS=10 AF=0.0 AC=0 AVP=0 T=0.00')

    for ci in range(npos):
        lines.append("MOVL C{:05d} V={:.1f}".format(ci, weld_speed_v))

    lines.append('ARCOF')
    lines.append('MOVJ C00000 VJ=25.00')
    lines.append('END')

    with open(output_path, "w", encoding="utf-8") as f:
        f.write("\n".join(lines))

    print(f"[JBI] 저장완료: {output_path} | 유효 포인트: {npos}/{len(poses_with_joints)}")