# _translate_test.py — 번역기(dzw_translate.py) 검증
#   ① 자세 수학: 회전행렬 직교성, KUKA 오일러각·ABB 쿼터니언 → 행렬 복원 왕복 일치
#   ② 규약: 공구 Z+가 -tool_dir 방향인가, 진행각이 실제로 기울이는가
#   ③ 산출물: KUKA .src / ABB .mod / CSV 문법·점수 일치, base-shift 반영
import json, math, os, subprocess, sys, tempfile

HERE = os.path.dirname(os.path.abspath(__file__))
sys.path.insert(0, os.path.dirname(HERE))
import dzw_translate as T

def mat_from_kuka(A, B, C):
    a, b, c = map(math.radians, (A, B, C))
    Rz = [[math.cos(a), -math.sin(a), 0], [math.sin(a), math.cos(a), 0], [0, 0, 1]]
    Ry = [[math.cos(b), 0, math.sin(b)], [0, 1, 0], [-math.sin(b), 0, math.cos(b)]]
    Rx = [[1, 0, 0], [0, math.cos(c), -math.sin(c)], [0, math.sin(c), math.cos(c)]]
    def mm(X, Y): return [[sum(X[i][k]*Y[k][j] for k in range(3)) for j in range(3)] for i in range(3)]
    return mm(mm(Rz, Ry), Rx)

def mat_from_quat(q):
    w, x, y, z = q
    return [[1-2*(y*y+z*z), 2*(x*y-w*z), 2*(x*z+w*y)],
            [2*(x*y+w*z), 1-2*(x*x+z*z), 2*(y*z-w*x)],
            [2*(x*z-w*y), 2*(y*z+w*x), 1-2*(x*x+y*y)]]

def mat_diff(P, Q):
    return max(abs(P[i][j]-Q[i][j]) for i in range(3) for j in range(3))

fails = []
def check(name, ok, detail=""):
    print(("  ✅ " if ok else "  ❌ ") + name + (" — "+detail if detail and not ok else ""))
    if not ok: fails.append(name)

print("=== 1. 자세 수학 (여러 방향 무작위 재현시드) ===")
import random; random.seed(42)
worst_o = worst_k = worst_q = 0
for _ in range(200):
    pts = [[random.uniform(-1000, 1000) for _ in range(3)] for _ in range(3)]
    td = T.norm([random.uniform(-1, 1) for _ in range(3)])
    R = T.frame_at(pts, 1, td, random.uniform(0, 15))
    # 직교성: RᵀR=I
    I = [[sum(R[k][i]*R[k][j] for k in range(3)) for j in range(3)] for i in range(3)]
    worst_o = max(worst_o, max(abs(I[i][j]-(1 if i == j else 0)) for i in range(3) for j in range(3)))
    worst_k = max(worst_k, mat_diff(R, mat_from_kuka(*T.mat_to_kuka_abc(R))))
    worst_q = max(worst_q, mat_diff(R, mat_from_quat(T.mat_to_quat(R))))
check("회전행렬 직교성 (200회 최악 %.2e)" % worst_o, worst_o < 1e-9)
check("KUKA 오일러각 왕복 (최악 %.2e)" % worst_k, worst_k < 1e-9)
check("ABB 쿼터니언 왕복 (최악 %.2e)" % worst_q, worst_q < 1e-9)

print("=== 2. 규약 ===")
R0 = T.frame_at([[0, 0, 0], [100, 0, 0]], 0, [0, 0, 1], 0)   # 토치가 +Z에서 내려봄, 진행 +X
check("공구Z+ = -tool_dir", abs(R0[2][2]-(-1)) < 1e-9 and abs(R0[0][2]) < 1e-9)
check("공구X+ = 진행방향", abs(R0[0][0]-1) < 1e-9)
R10 = T.frame_at([[0, 0, 0], [100, 0, 0]], 0, [0, 0, 1], 10)  # 진행각 10° → Z가 X쪽으로 기움
ang = math.degrees(math.acos(max(-1, min(1, -R10[2][2]))))
check("진행각 10° 반영 (실측 %.2f°)" % ang, abs(ang-10) < 0.01)

print("=== 3. 산출물 ===")
ws = {"format": "dozikworks-workspec", "version": "1.0",
      "product": {"name": "test12500", "frame": "product", "table_z_mm": -577.4},
      "seams": [{"no": 1, "pts_mm": [[100, 200, -500], [200, 200, -500], [300, 220, -500]],
                 "tool_dir": [0, -0.707, 0.707], "travel_angle_deg": 10,
                 "move_pts": [{"i": 0, "type": "L"}], "tag": "리브"},
                {"no": 2, "pts_mm": [[400, 300, -450], [450, 300, -450]],
                 "tool_dir": [0, 0, 1], "travel_angle_deg": 10, "move_pts": [], "tag": None}],
      "weld_params": {"speed_line_cmmin": 45, "speed_curve_cmmin": 42.5},
      "provenance": {"created": "test", "source": "unit", "note": ""}}
tmp = tempfile.mkdtemp()
src = os.path.join(tmp, "t.workspec.json")
json.dump(ws, open(src, "w", encoding="utf-8"), ensure_ascii=False)
py = sys.executable
for robot, ext, needle in [("kuka", ".src", "LIN {X"), ("abb", ".mod", "MoveL [["), ("csv", ".csv", "seg,x_mm")]:
    r = subprocess.run([py, os.path.join(os.path.dirname(HERE), "dzw_translate.py"), src,
                        "--robot", robot, "--base-shift", "1000,0,577.4"],
                       capture_output=True, timeout=60)
    out = os.path.join(tmp, "t_" + robot + ext)
    ok = r.returncode == 0 and os.path.exists(out)
    body = open(out, encoding="utf-8").read() if ok else ""
    lines = [l for l in body.splitlines() if needle.split(",")[0] in l or (robot != "csv" and needle in l)]
    npts = body.count("LIN {") if robot == "kuka" else body.count("MoveL") if robot == "abb" else len(body.splitlines())-1
    check(robot.upper()+" 생성+점수 5", ok and npts == 5, (r.stderr.decode("utf-8", "replace")[:100] if not ok else "점수 %d" % npts))
    if robot == "csv" and ok:
        first = body.splitlines()[1].split(",")
        check("base-shift 반영 (x=1100)", abs(float(first[1])-1100.0) < 0.01)
        check("태그 전달 (리브)", first[9] == "리브")


print("=== 4. 산업용 백엔드 (RoboDK 포스트) ===")
if os.path.isdir("C:/RoboDK/Posts"):
    r = subprocess.run([py, os.path.join(os.path.dirname(HERE), "dzw_translate.py"), src,
                        "--robot", "post:Hyundai"], capture_output=True, timeout=90)
    job = os.path.join(tmp, "12500.JOB")
    okp = r.returncode == 0 and os.path.exists(job)
    body = open(job, encoding="utf-8", errors="replace").read() if okp else ""
    check("Hyundai .JOB 생성+이동 5", okp and body.count("MOVE L") == 5,
          r.stderr.decode("utf-8", "replace")[-120:] if not okp else "")
else:
    print("  [건너뜀] C:/RoboDK/Posts 없음")

print()
print("=== 결과:", "PASS" if not fails else ("FAIL: " + ", ".join(fails)))
sys.exit(0 if not fails else 1)