"""[Step2 v2] CAD 토치 굽힘 보정: 강체(마운트)+블렌드(구즈넥 smoothstep)+강체(노즐).
목표: [장착면 법선]과 [장착면중심→노즐팁 현] 사이각 = TOOL0 15.12°(atan(137.07/507)). φ 이분법. 단면 왜곡 없음.
그다음 균일스케일로 장착면중심→노즐팁=525.2 복원. 그룹·와이어 보존."""
import numpy as np, math
SRC=r'E:\도진팩토리\3D스캔및티칭시스템\assets\welding_torch_CAD_v1.obj'
DST=r'E:\도진팩토리\3D스캔및티칭시스템\assets\welding_torch_CAD_v2.obj'

lines=open(SRC,encoding='utf-8',errors='ignore').read().split('\n')
V=[]; vidx=[]
for i,l in enumerate(lines):
    if l.startswith('v '):
        p=l.split(); V.append([float(p[1]),float(p[2]),float(p[3])]); vidx.append(i)
V=np.array(V); z=V[:,2]

# 장착면 평면 (헤더 중심 근처 r45 실측 피팅 — 검증과 동일)
Ch=np.array([-194.5,11.6,-502.7])
d=np.linalg.norm(V-Ch,axis=1); Fm=V[d<45]
C_m=Fm.mean(0); M=Fm-C_m; _,_,vt=np.linalg.svd(M,full_matrices=False); N_m=vt[2]
if N_m[2]<0: N_m=-N_m
N_m=N_m/np.linalg.norm(N_m)
noz=np.array([0.0,0.0,-15.0])   # 노즐팁(헤더)
def ang(a,b): return math.degrees(math.acos(max(-1,min(1,np.dot(a,b)/(np.linalg.norm(a)*np.linalg.norm(b))))))
print('장착면 중심 C_m',C_m.round(2),'법선 N_m',N_m.round(4))
print('현재 [법선]↔[중심→노즐현] 각 =',round(ang(N_m, noz-C_m),3),'도  (목표 15.12)')

# 굽힘축 A = Z × (장착면중심 수평방향)
h=np.array([C_m[0],C_m[1],0.0])
A=np.cross([0,0,1.0], h); A=A/np.linalg.norm(A)
# 피벗 P = z≈-400 밴드 중심(센터라인)
Pb=V[(z>=-410)&(z<=-390)]; P=Pb.mean(0)
print('굽힘축 A',A.round(4),'| 피벗 P',P.round(1))

Z_RIG=-400.0; Z_DIST=-120.0   # 강체(<-400) / 블렌드(-400~-120) / 노즐강체(>-120)
def smooth(t): t=max(0,min(1,t)); return t*t*(3-2*t)
def wof(zz):
    if zz<=Z_RIG: return 0.0
    if zz>=Z_DIST: return 1.0
    return smooth((zz-Z_RIG)/(Z_DIST-Z_RIG))
def rot(p, theta, axis, piv):
    if theta==0: return p.copy()
    k=axis; v=p-piv; c=math.cos(theta); s=math.sin(theta)
    return piv + v*c + np.cross(k,v)*s + k*np.dot(k,v)*(1-c)

def noz_after(phi):
    return rot(noz, wof(noz[2])*phi, A, P)   # 노즐 z=-15 > -120 → w=1 → 전체 φ

# 이분법: [법선]↔[중심→노즐현] = 15.12
target=math.atan(137.07/507.0)*180/math.pi
lo,hi=math.radians(-30),math.radians(30)
def f(phi):
    n2=noz_after(phi); return ang(N_m, n2-C_m)-target
# 부호 방향 확인
if f(lo)*f(hi)>0:
    # 넓혀서
    lo,hi=math.radians(-60),math.radians(60)
for _ in range(60):
    mid=(lo+hi)/2
    if f(lo)*f(mid)<=0: hi=mid
    else: lo=mid
phi=(lo+hi)/2
print('★ φ =',round(math.degrees(phi),4),'도  → 보정후 각=',round(ang(N_m, noz_after(phi)-C_m),4),'(목표',round(target,4),')')

# 전 정점 굽힘 적용
Vb=np.array([rot(v, wof(v[2])*phi, A, P) for v in V])
# 균일스케일: 장착면중심→노즐팁 = 525.2 (팁 기준 스케일)
C_b=Vb[np.argmin(np.linalg.norm(V-C_m,axis=1))]  # 굽힘후 C_m에 해당하던 정점? 대신 C_m을 강체이므로 그대로
# C_m은 z=-502.7<-400 → 강체(w=0) → 굽힘 불변. noz는 굽힘됨.
noz_b=noz_after(phi)
L=np.linalg.norm(noz_b-C_m); scale=525.2/L
tip=noz_b
Vs=tip + (Vb-tip)*scale
# 검산
C_ms=tip + (C_m-tip)*scale
print('스케일',round(scale,5),'| 장착면중심→노즐팁 =',round(np.linalg.norm(noz_b*scale-C_ms if False else (tip-C_ms)),2) if False else round(np.linalg.norm(tip-C_ms),2))

# 재출력 (v만 교체, 그룹·와이어·면 보존)
out=lines[:]
for j,i in enumerate(vidx):
    x,y,zz=Vs[j]; out[i]=f'v {x:.4f} {y:.4f} {zz:.4f}'
hdr=[f'# DOZIKWORKS torch CAD v2 — bend-corrected in code (from v1)',
     f'# bend phi={math.degrees(phi):.4f}deg about axis=({A[0]:.4f},{A[1]:.4f},{A[2]:.4f}) pivot=({P[0]:.1f},{P[1]:.1f},{P[2]:.1f})',
     f'# rigid z<{Z_RIG}, blend {Z_RIG}~{Z_DIST} smoothstep, distal z>{Z_DIST}. uniform scale {scale:.5f} about nozzle tip',
     f'# [face normal]-[face center->nozzle tip] angle -> TOOL0 15.12deg. groups/wire preserved. origin=wire tip, nozzle=(0,0,-15)']
open(DST,'w',encoding='utf-8').write('\n'.join(hdr+out))
print('저장:',DST,'| 정점',len(V))