CBT_project/visualization/res_plot_3d.py

import torch
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.colors import to_rgba
from matplotlib.lines import Line2D
from mpl_toolkits.mplot3d.art3d import Poly3DCollection
import os
import errno
import time
from datetime import datetime
import csv

from core.cylinder import generate_cylinder_n_torch, generate_cylinder_o_torch, snap_to_discrete_values
from core.intersection import center_line_intersections_torch
from core.scoring import cl_score_torch, compute_overlap_ratio_from_cylinder_mask, cl_score_torch_xfr
from imaging.orientation import (azimuth_rotation, analyze_vertebral_tilt_contour,
                                  best_symmetry_plane, best_upper_endplate_plane,
                                  segment_spinous_process, segment_vertebral_body)
from utils.helpers import save_with_unique_name

# Volume absorption 渲染（Beer-Lambert）：每 voxel 不透明度 = 1 - exp(-mu * voxel_width)
# 骨頭核心厚度達 70-90 voxel，沿視線堆疊會使任何 per-voxel alpha 累積成不透明。
# 因此以「抽稀 (SUBSAMPLE) 降低堆疊數量」+「低 mu 控制每點吸收」兩項共同調出淡薄 X-ray 陰影，
# 同時保留皮質 / 鬆質的吸收入射差異（mu 比值）。
BONE_MU_CORTICAL = 0.02    # 1/mm  → 每 voxel = 1-exp(-0.02*0.5) ~ 0.010
BONE_MU_TRABECULAR = 0.005 # 1/mm  → 每 voxel = 1-exp(-0.005*0.5) ~ 0.0025
BONE_MARKER_SIZE = 3.0     # 骨骼散點點面積 (pt^2)；略大以補償抽稀後的顆粒感
BONE_SUBSAMPLE = 1         # 每 10 個骨 voxel 畫 1 個，降低堆疊不透明度（1=全畫）

def _retry_robust(fn, *args, retries=20, delay=0.5, **kwargs):
    """對 ENOENT/EEXIST 重試：NFS 上輸出樹被外部刪除（或多 worker 併發建同一
    output 目錄）會有短暫的 ENOENT 窗口，重試可恢復；其他錯誤直接丟出。"""
    for i in range(retries):
        try:
            return fn(*args, **kwargs)
        except OSError as e:
            if e.errno not in (errno.ENOENT, errno.EEXIST) or i == retries - 1:
                raise
            time.sleep(delay)

def set_axes_equal_3d(ax):
    """
    Make axes of 3D plot have equal scale so that spheres appear as spheres,
    cubes as cubes, etc.
    """
    x_limits = ax.get_xlim3d()
    y_limits = ax.get_ylim3d()
    z_limits = ax.get_zlim3d()

    x_range = abs(x_limits[1] - x_limits[0])
    x_middle = np.mean(x_limits)
    y_range = abs(y_limits[1] - y_limits[0])
    y_middle = np.mean(y_limits)
    z_range = abs(z_limits[1] - z_limits[0])
    z_middle = np.mean(z_limits)

    plot_radius = 0.5*max([x_range, y_range, z_range])

    ax.set_xlim3d([x_middle - plot_radius, x_middle + plot_radius])
    ax.set_ylim3d([y_middle - plot_radius, y_middle + plot_radius])
    ax.set_zlim3d([z_middle - plot_radius, z_middle + plot_radius])
    
    try:
        ax.set_box_aspect([1, 1, 1])
    except AttributeError:
        pass

def res_plt_2_torch(
    spine_tensor: torch.Tensor,
    cortical_tensor: torch.Tensor,
    image_shape: tuple[int, int, int],
    image2_path: str,
    base_folder: str,
    label_str: str,
    diameter_l: float,
    length_l: float,
    diameter_r: float,
    length_r: float,
    best_position_l: list[float],
    best_position_r: list[float],
    swarm_size: int,
    max_iter: int,
    total_time: float,
    spacing: list[float],
    CBT: bool,
    device: torch.device,
    grid=None
) -> None:
    """
    Same plotting function as before, but it uses torch-based generation 
    and then moves data to CPU for matplotlib 3D scatter.
    """
    cyl_l = generate_cylinder_n_torch(
        diameter_l,
        length_l,
        best_position_l[0],
        best_position_l[1],
        best_position_l[2],
        best_position_l[3],
        best_position_l[4],
        image_shape,
        spacing,
        device,
        grid
    )

    cyl_lo = generate_cylinder_o_torch(
        diameter_l,
        length_l,
        best_position_l[0],
        best_position_l[1],
        best_position_l[2],
        best_position_l[3],
        best_position_l[4],
        image_shape,
        spacing,
        device,
        grid
    )
    cyl_r = generate_cylinder_n_torch(
        diameter_r,
        length_r,
        best_position_r[0],
        best_position_r[1],
        best_position_r[2],
        best_position_r[3],
        best_position_r[4],
        image_shape,
        spacing,
        device,
        grid
    )
    cyl_ro = generate_cylinder_o_torch(
        diameter_r,
        length_r,
        best_position_r[0],
        best_position_r[1],
        best_position_r[2],
        best_position_r[3],
        best_position_r[4],
        image_shape,
        spacing,
        device,
        grid
    )

    intersections_l, line_mask_l = center_line_intersections_torch(
        best_position_l[0],
        best_position_l[1],
        best_position_l[2],
        best_position_l[3],
        best_position_l[4],
        int(length_l),
        spine_tensor,
        spacing,
        device
    )
    loss_l = cl_score_torch(cortical_tensor, spine_tensor, cyl_l, cyl_lo, intersections_l)

    intersections_r, line_mask_r = center_line_intersections_torch(
        best_position_r[0],
        best_position_r[1],
        best_position_r[2],
        best_position_r[3],
        best_position_r[4],
        int(length_r),
        spine_tensor,
        spacing,
        device
    )
    # loss_r = cl_score_torch(cortical_tensor, spine_tensor, cyl_r, cyl_ro, intersections_r)
    # loss_r 放在下方 VBODY mask 計算之後：計入與 PSO 目標函數相同的 VBODY voxel 獎勵

    if CBT:
        azi = float('nan')
        alt = float('nan')
    else:
        azi = azimuth_rotation(image2_path)
        res = analyze_vertebral_tilt_contour(image2_path, edge_type='superior', show_plot=False, debug=False)
        alt = res['superior']['tilt_angle_deg']

    # Move data to CPU for plotting
    line_mask_l_cpu = line_mask_l.cpu().numpy()
    line_mask_r_cpu = line_mask_r.cpu().numpy()
    cyl_l_cpu = cyl_l.cpu().numpy()
    cyl_lo_cpu = cyl_lo.cpu().numpy()
    cyl_r_cpu = cyl_r.cpu().numpy()
    cyl_ro_cpu = cyl_ro.cpu().numpy()
    spine_cpu = spine_tensor.cpu().numpy()

    z_lin1, y_lin1, x_lin1 = np.where(line_mask_l_cpu == 1)
    z_lin2, y_lin2, x_lin2 = np.where(line_mask_r_cpu == 1)

    z_cyl_l1, y_cyl_l1, x_cyl_l1 = np.where(cyl_l_cpu == 1)
    z_cyl_l2, y_cyl_l2, x_cyl_l2 = np.where(cyl_lo_cpu == 1)
    z_cyl_r1, y_cyl_r1, x_cyl_r1 = np.where(cyl_r_cpu == 1)
    z_cyl_r2, y_cyl_r2, x_cyl_r2 = np.where(cyl_ro_cpu == 1)

    # 骨頭 voxel 依「體積吸收」分成兩組：皮質（高不透明度）與鬆質（低不透明度）
    cortical_cpu = cortical_tensor.cpu().numpy()
    voxel_mm = float(spacing[0])
    alpha_cortical = 1.0 - np.exp(-BONE_MU_CORTICAL * voxel_mm)
    alpha_trabecular = 1.0 - np.exp(-BONE_MU_TRABECULAR * voxel_mm)
    z_corti, y_corti, x_corti = np.where((spine_cpu == 1) & (cortical_cpu == 1))
    z_trab, y_trab, x_trab = np.where((spine_cpu == 1) & (cortical_cpu == 0))

    # 中矢狀面：骨頭的最佳鏡稱面，一般平面 a·x + b·y + c·z = d（法線方向任意）
    sym = best_symmetry_plane(spine_cpu)

    # 棘突：鏡稱面中線帶（|s|<=w）且在 AP 谷底之後側的骨 voxel，換不同顏色標示
    # 棘突缺如（先前 laminectomy / 棘突切除）時 sp_mask=None，不標示。
    sp_mask, sp_th, sp_info = segment_spinous_process(spine_cpu, sym)
    if sp_info['mode'] == 'no_spinous':
        top_off = f"{sp_info['top_off']:.1f}" if sp_info.get('top_off') is not None else 'n/a'
        print(f"[NO-SP] 中線後側缺如（先前 laminectomy / 棘突切除）: "
              f"deficit={sp_info['deficit']:.1f} voxel ({sp_info['deficit'] * 0.5:.1f} mm), "
              f"rear3={sp_info['rear3']} voxel, top_off={top_off} voxel "
              f"-> 不標示棘突；椎體用放寬後側谷底切分")
        sp_corti = sp_trab = None
    elif sp_mask is not None and sp_mask.any():
        sp_corti = sp_mask[z_corti, y_corti, x_corti]
        sp_trab = sp_mask[z_trab, y_trab, x_trab]
        sp_n_bone = max(int(spine_cpu.sum()), 1)
        print(f"[SPINOUS] n={sp_info['n_sp']} "
              f"({100.0 * sp_info['n_sp'] / sp_n_bone:.1f}% of bone) "
              f"band=+/-{sp_info['band_w']:.1f} voxel  AP>={sp_info['ap_thresh']:.1f} "
              f"mode={sp_info['mode']}")
    else:
        sp_corti = sp_trab = None

    # 上終板平面：RANSAC 擬合骨頭頂面（前側）的最佳 a·x + b·y + c·z = d
    symp = best_upper_endplate_plane(spine_cpu)

    # 椎體：上終板之下（排除跨終板的後側構造）且中線 AP 谷底之前側的骨 voxel，
    # 換不同顏色標示（見 segment_vertebral_body；谷底優先取中線帶，
    # 中線搜尋 fallback 時退回終板下整體 AP 分佈谷底）
    vb_mask, vb_th, vb_info = segment_vertebral_body(spine_cpu, sym, symp,
                                                     sp_th, sp_info['mode'])
    if vb_mask is not None and vb_mask.any():
        vb_corti = vb_mask[z_corti, y_corti, x_corti]
        vb_trab = vb_mask[z_trab, y_trab, x_trab]
        print(f"[VBODY] n={vb_info['n_vb']} "
              f"({100.0 * vb_info['n_vb'] / max(int(spine_cpu.sum()), 1):.1f}% of bone) "
              f"AP<{vb_info['ap_thresh']:.1f} mode={vb_info['mode']}")
        if vb_info['mode'] == 'quantile':
            print(f"[VBODY] WARNING: 未找到體/弓後側谷底，閾值退回 55 百分位 "
                  f"（可能切進椎體內），建議人工核對該 level 的椎體邊界")
    else:
        vb_corti = vb_trab = None
        print(f"[VBODY] skipped: {vb_info['mode']}")

    # loss_r 使用與 PSO 目標函數相同的 VBODY 獎勵（回報分數與優化一致）
    vbody_tensor = None
    if vb_mask is not None and vb_mask.any():
        vbody_tensor = torch.from_numpy(vb_mask.astype(np.uint8)).to(device=device)
    loss_r = cl_score_torch_xfr(cortical_tensor, spine_tensor, cyl_r, cyl_ro, intersections_r,
                                vbody_tensor=vbody_tensor)

    # X-ray 外觀：骨頭合成一個半透明體積吸收點雲（下方）；
    # 螺絲（中心線 + 圓柱 + 入口軌跡延长）合成一個全不透明點雲，永遠畫在骨頭之上
    def _rgba_block(n, color, a):
        arr = np.empty((n, 4))
        arr[:] = to_rgba(color)
        arr[:, 3] = a
        return arr

    x_bone = np.concatenate([x_corti, x_trab])
    y_bone = np.concatenate([y_corti, y_trab])
    z_bone = np.concatenate([z_corti, z_trab])
    bone_rgba = np.concatenate([
        _rgba_block(len(x_corti), 'lightblue', float(alpha_cortical)),
        _rgba_block(len(x_trab), 'lightblue', float(alpha_trabecular)),
    ])
    bone_size = np.full(len(x_bone), BONE_MARKER_SIZE)

    # 抽稀：降低堆疊不透明度以呈現淡薄 X-ray 陰影
    if BONE_SUBSAMPLE > 1:
        x_bone = x_bone[::BONE_SUBSAMPLE]
        y_bone = y_bone[::BONE_SUBSAMPLE]
        z_bone = z_bone[::BONE_SUBSAMPLE]
        bone_rgba = bone_rgba[::BONE_SUBSAMPLE]
        bone_size = bone_size[::BONE_SUBSAMPLE]
    # 平面 patch 的範圍用「完整骨頭」（含 VBODY / SP）算：
    # VBODY 前側是整顆骨最前緣，剔除後綠色終板 patch 會縮小
    x_bone_all, y_bone_all, z_bone_all = x_bone, y_bone, z_bone
    # VBODY / 棘突拆成獨立上層（在 _fill_ax 內畫）：
    # 繪製順序 基底骨(5) < VBODY gold(6) < SP purple(6.5) < 終板(7) < 鏡稱面(8) < 螺絲(10)
    # axial 視角（ax3）相機在 +y 前側，椎體 physically 擋在棘突與相機之間，
    # 棘突最後畫 -> 紫色不被 gold 遮住
    vb_flag = np.zeros(x_bone.shape, dtype=bool)
    sp_flag = np.zeros(x_bone.shape, dtype=bool)
    if vb_corti is not None:
        f = np.concatenate([vb_corti, vb_trab]).astype(bool)
        if BONE_SUBSAMPLE > 1:
            f = f[::BONE_SUBSAMPLE]
        vb_flag |= f
    if sp_corti is not None:
        f = np.concatenate([sp_corti, sp_trab]).astype(bool)
        if BONE_SUBSAMPLE > 1:
            f = f[::BONE_SUBSAMPLE]
        sp_flag |= f

    vbody_pts = None
    vbody_mask = vb_flag & ~sp_flag
    if vbody_mask.any():
        vbody_pts = (x_bone[vbody_mask], y_bone[vbody_mask], z_bone[vbody_mask])
    sp_pts = None
    if sp_flag.any():
        sp_pts = (x_bone[sp_flag], y_bone[sp_flag], z_bone[sp_flag])

    # 基底骨層去掉 VBODY / SP voxel（由上面的專屬圖層畫）
    base_mask = ~(vb_flag | sp_flag)
    x_bone = x_bone[base_mask]
    y_bone = y_bone[base_mask]
    z_bone = z_bone[base_mask]
    bone_rgba = bone_rgba[base_mask]
    bone_size = bone_size[base_mask]

    x_screw = np.concatenate([x_lin1, x_lin2, x_cyl_l1, x_cyl_l2, x_cyl_r1, x_cyl_r2])
    y_screw = np.concatenate([y_lin1, y_lin2, y_cyl_l1, y_cyl_l2, y_cyl_r1, y_cyl_r2])
    z_screw = np.concatenate([z_lin1, z_lin2, z_cyl_l1, z_cyl_l2, z_cyl_r1, z_cyl_r2])

    _a, _b, _c, _d = sym['plane']
    _n = np.array([_a, _b, _c])
    _u = np.array(sym['u'])
    _v = np.array(sym['v'])
    _p0 = _d * _n                                # 平面上最接近原點的點
    xyz_bone = np.stack([x_bone_all - _p0[0], y_bone_all - _p0[1], z_bone_all - _p0[2]], axis=1)
    _pu = xyz_bone @ _u
    _pv = xyz_bone @ _v
    _U_, _V_ = np.meshgrid(np.linspace(_pu.min(), _pu.max(), 8),
                           np.linspace(_pv.min(), _pv.max(), 8))
    _Xp = _p0[0] + _U_ * _u[0] + _V_ * _v[0]
    _Yp = _p0[1] + _U_ * _u[1] + _V_ * _v[1]
    _Zp = _p0[2] + _U_ * _u[2] + _V_ * _v[2]

    _EX = _EY = _EZ = None
    if symp is not None:
        _ea, _eb, _ec, _ed = symp['plane']
        _en = np.array([_ea, _eb, _ec])
        _eu = np.array(symp['u'])
        _ev = np.array(symp['v'])
        _ep0 = _ed * _en
        xz_ep = np.stack([x_bone_all - _ep0[0], y_bone_all - _ep0[1], z_bone_all - _ep0[2]], axis=1)
        _pu_ep = xz_ep @ _eu
        _pv_ep = xz_ep @ _ev
        _EU, _EV = np.meshgrid(np.linspace(_pu_ep.min(), _pu_ep.max(), 8),
                               np.linspace(_pv_ep.min(), _pv_ep.max(), 8))
        _EX = _ep0[0] + _EU * _eu[0] + _EV * _ev[0]
        _EY = _ep0[1] + _EU * _eu[1] + _EV * _ev[1]
        _EZ = _ep0[2] + _EU * _eu[2] + _EV * _ev[2]
    screw_rgba = np.concatenate([
        _rgba_block(len(x_lin1), 'r', 1.0),
        _rgba_block(len(x_lin2), 'r', 1.0),
        _rgba_block(len(x_cyl_l1), 'darkcyan', 1.0),
        _rgba_block(len(x_cyl_l2), 'pink', 1.0),
        _rgba_block(len(x_cyl_r1), 'blue', 1.0),
        _rgba_block(len(x_cyl_r2), 'pink', 1.0),
    ])
    screw_size = np.concatenate([
        np.full(len(x_lin1), 3), np.full(len(x_lin2), 3),
        np.full(len(x_cyl_l1), 36), np.full(len(x_cyl_l2), 36),
        np.full(len(x_cyl_r1), 36), np.full(len(x_cyl_r2), 36),
    ])

    fig = plt.figure(figsize=(12, 12))

    legend_handles = [
        Line2D([], [], marker='o', ls='', ms=6, color='darkcyan', label='Cylinder(L)'),
        Line2D([], [], marker='o', ls='', ms=6, color='blue', label='Cylinder(R)'),
    ]
    if vb_corti is not None:
        legend_handles.append(
            Line2D([], [], marker='o', ls='', ms=6, color='gold', label='VertebralBody'))
    if sp_corti is not None:
        legend_handles.append(
            Line2D([], [], marker='o', ls='', ms=6, color='purple', label='SpinousProcess'))

    def _fill_ax(ax):
        # X-ray 外觀：關閉 mplot3d 依深度自動排序 zorder（否則半透明骨頭會被重繪到
        # 螺絲上方）；改為固定分層：
        # 基底骨 zorder=5 < VBODY 6 < SP 6.5 < 終板 7 < 鏡稱面 8 < 螺絲 10
        ax.computed_zorder = False
        sc_bone = ax.scatter(x_bone, y_bone, z_bone, c=bone_rgba, s=bone_size, marker='o')
        sc_bone.set_zorder(5)
        if vbody_pts is not None:
            sc_vb = ax.scatter(vbody_pts[0], vbody_pts[1], vbody_pts[2],
                               c=to_rgba('gold', 0.95), s=BONE_MARKER_SIZE, marker='o')
            sc_vb.set_zorder(6)
        if sp_pts is not None:
            sc_sp = ax.scatter(sp_pts[0], sp_pts[1], sp_pts[2],
                               c=to_rgba('purple', 0.95), s=BONE_MARKER_SIZE, marker='o')
            sc_sp.set_zorder(6.5)
        sc_screw = ax.scatter(x_screw, y_screw, z_screw, c=screw_rgba, s=screw_size, marker='o')
        sc_screw.set_zorder(10)
        # 中矢狀面（理論左右對稱切分面）：半透明橘色平面 x = x_mid
        # 平面邊緣畫橘色線，讓 axial / 正視（側看時）也能清楚看到切分線
        plane = ax.plot_surface(_Xp, _Yp, _Zp, color='orange', alpha=0.30,
                                linewidth=1.0, edgecolor='orange', rstride=1, cstride=1)
        plane.set_zorder(8)
        # 上終板平面：半透明綠色平面（邊緣綠線）
        if _EX is not None:
            ep = ax.plot_surface(_EX, _EY, _EZ, color='green', alpha=0.35,
                                 linewidth=1.0, edgecolor='green', rstride=1, cstride=1)
            ep.set_zorder(7)

    ax1 = fig.add_subplot(221, projection='3d')
    _fill_ax(ax1)
    ax1.set_xlabel('X-axis'); ax1.set_ylabel('Y-axis'); ax1.set_zlabel('Z-axis')
    set_axes_equal_3d(ax1)

    ax2 = fig.add_subplot(222, projection='3d')
    ax2.view_init(elev=90, azim=-90, roll=0)
    _fill_ax(ax2)
    ax2.set_xlabel('X-axis'); ax2.set_ylabel('Y-axis'); ax2.set_zlabel('Z-axis')
    set_axes_equal_3d(ax2)
    ax2.legend(handles=legend_handles)

    ax3 = fig.add_subplot(223, projection='3d')
    ax3.view_init(elev=0, azim=90, roll=0)
    _fill_ax(ax3)
    ax3.set_xlabel('X-axis'); ax3.set_ylabel('Y-axis'); ax3.set_zlabel('Z-axis')
    set_axes_equal_3d(ax3)

    ax4 = fig.add_subplot(224, projection='3d')
    ax4.view_init(elev=0, azim=0, roll=0)
    _fill_ax(ax4)
    ax4.set_xlabel('X-axis'); ax4.set_ylabel('Y-axis'); ax4.set_zlabel('Z-axis')
    set_axes_equal_3d(ax4)
    
    cyl_points_l = torch.sum(cyl_l).item()
    cyl_points_r = torch.sum(cyl_r).item()

    overlap_l = ((cortical_tensor == 1) & (cyl_l == 1)).sum().item()
    overlap_r = ((cortical_tensor == 1) & (cyl_r == 1)).sum().item()
    overlap_b_l = ((spine_tensor == 1) & (cyl_l == 1)).sum().item()
    overlap_b_r = ((spine_tensor == 1) & (cyl_r == 1)).sum().item()

    overlap_cortical_l = (overlap_l / cyl_points_l) * 100 if cyl_points_l else 0.0
    overlap_cortical_r = (overlap_r / cyl_points_r) * 100 if cyl_points_r else 0.0
    overlap_vertebral_l = (overlap_b_l / cyl_points_l) * 100 if cyl_points_l else 0.0
    overlap_vertebral_r = (overlap_b_r / cyl_points_r) * 100 if cyl_points_r else 0.0
    cb_ratio_l = overlap_cortical_l/overlap_vertebral_l if overlap_vertebral_l else 0.0
    cb_ratio_r = overlap_cortical_r/overlap_vertebral_r if overlap_vertebral_r else 0.0
    user_altitude_l = 90 - best_position_l[4] - alt
    user_altitude_r = 90 - best_position_r[4] - alt
    user_azimuth_l = 90 - best_position_l[3] - azi
    user_azimuth_r = 90 - best_position_r[3] - azi

    # 螺絲方向向量（與 generate_cylinder_n_torch 同慣例）：
    # d = (cos(az)·sin(alt), sin(az)·sin(alt), cos(alt))，alt = 相对 +z 的極角
    # Azimuth 相对鏡稱面（法線 s，theta_v = atan2(sy, sx)）：
    #   Azimuth_Lateral = az - theta_v - 90 （面內 AP 軸起的帶號發散角，+ = L 側往外，− = R 側）
    # Altitude 相對上終板面（法線 e，朝上）：
    #   Altitude_Cephalad_Endplate = 90 - (d 與 e 的夾角)
    sym_n = np.asarray(sym['normal'], dtype=float)
    theta_v = float(np.degrees(np.arctan2(sym_n[1], sym_n[0])))
    if symp is not None:
        e_n = np.asarray(symp['normal'], dtype=float)
        e_n = e_n / np.linalg.norm(e_n)
        tau_y = float(np.degrees(np.arctan2(e_n[1], e_n[2])))
        tau_x = float(np.degrees(np.arctan2(e_n[0], e_n[2])))
    else:
        e_n = None
        tau_y = float('nan')
        tau_x = float('nan')

    def _rel_angles(az_deg, alt_deg):
        az_r = np.radians(az_deg)
        alt_r = np.radians(alt_deg)
        d_v = np.array([np.cos(az_r) * np.sin(alt_r),
                        np.sin(az_r) * np.sin(alt_r),
                        np.cos(alt_r)])
        az_lateral = az_deg - theta_v - 90.0
        if e_n is not None:
            ang_norm = float(np.degrees(np.arccos(np.clip(d_v @ e_n, -1.0, 1.0))))
            alt_cep = 90.0 - ang_norm
        else:
            alt_cep = float('nan')
        return az_lateral, alt_cep

    azlat_l, acep_l = _rel_angles(best_position_l[3], best_position_l[4])
    azlat_r, acep_r = _rel_angles(best_position_r[3], best_position_r[4])

    date_str = datetime.now().strftime("%Y%m%d")
    # 旋轉後影像存在 <volume_id>/rotated/ 下：parent 是 'rotated'，
    # 再上一層才是 volume id（未旋轉路徑不受影響）
    img_parent = os.path.dirname(image2_path)
    patient_id = os.path.basename(os.path.dirname(img_parent)) \
        if os.path.basename(img_parent) == 'rotated' else os.path.basename(img_parent)
    output_folder = os.path.join(base_folder, date_str, patient_id)
    _retry_robust(os.makedirs, output_folder, exist_ok=True)
    csv_path = os.path.join(output_folder, 'output.csv')
    
    # 欄位標題 (Header)。CBT 模式下恆為 nan 的 2D 參考欄
    # (Azimuth_Diff / Altitude_Diff / User_Azimuth / User_Altitude) 不寫入 CSV。
    headers = [
        'Label', 'Side', 'Diameter', 'Length', 'Swarm_Size', 'Max_Iter',
        'Position_XYZ', 'Raw_Azimuth', 'Raw_Altitude',
        'Intersections', 'Best_Loss', 'cyl_points', 'Overlap_Cortical', 'Overlap_Bone',
        'Cortical_Bone_Ratio',
        'Sym_Theta_v_deg', 'Endplate_Tau_y_deg', 'Endplate_Tau_x_deg',
        'Azimuth_Lateral_deg',
        'Altitude_Cephalad_Endplate_deg',
        'Total_Time'
    ]

    def _fmt(v):
        return '' if not np.isfinite(v) else f"{float(v):.2f}"

    # 檢查檔案是否存在 (決定是否寫入標題)；舊 schema 的檔案按欄位名稱重映射後
    # 改以新 Header 重寫（舊檔多出的欄位捨去、缺的欄位補空白），避免 append 欄位錯位
    file_exists = os.path.isfile(csv_path)
    if file_exists:
        with _retry_robust(open, csv_path, newline='') as f:
            old_rows = [row for row in csv.reader(f) if any(c.strip() for c in row)]
        if not old_rows or old_rows[0] != headers:
            old_h = old_rows[0] if old_rows else None
            with _retry_robust(open, csv_path, 'w', newline='') as f:
                w = csv.writer(f)
                w.writerow(headers)
                for r in (old_rows[1:] if old_rows else []):
                    if old_h:
                        d = dict(zip(old_h, r))
                        w.writerow([d.get(h, '') for h in headers])
                    else:
                        w.writerow(r + [''] * max(0, len(headers) - len(r)))

    try:
        with _retry_robust(open, csv_path, 'a', newline='') as csvfile:
            writer = csv.writer(csvfile)

            # 新檔案寫入 Header
            if not file_exists:
                writer.writerow(headers)

            # 寫入 Left 數據
            writer.writerow([
                label_str,
                'L',
                diameter_l,
                length_l,
                swarm_size,
                max_iter,
                # f"({best_position_l[0]:.2f}, {best_position_l[1]:.2f}, {best_position_l[2]:.2f})",
                f"({best_position_l[2]:.2f}, {best_position_l[1]:.2f}, {best_position_l[0]:.2f})",
                f"{best_position_l[3]:.2f}",
                f"{best_position_l[4]:.2f}",
                intersections_l,
                f"{loss_l:.2f}",
                cyl_points_l,
                f"{overlap_cortical_l:.2f}",
                f"{overlap_vertebral_l:.2f}",
                f"{(overlap_cortical_l/overlap_vertebral_l if overlap_vertebral_l!=0 else 0):.2f}",
                _fmt(theta_v),
                _fmt(tau_y),
                _fmt(tau_x),
                _fmt(azlat_l),
                _fmt(acep_l),
                f"{total_time:.2f}"
            ])
            
            # 寫入 Right 數據
            writer.writerow([
                label_str,
                'R',
                diameter_r,
                length_r,
                swarm_size,
                max_iter,
                # f"({best_position_r[0]:.2f}, {best_position_r[1]:.2f}, {best_position_r[2]:.2f})",
                f"({best_position_r[2]:.2f}, {best_position_r[1]:.2f}, {best_position_r[0]:.2f})",
                f"{best_position_r[3]:.2f}",
                f"{best_position_r[4]:.2f}",
                intersections_r,
                f"{loss_r:.2f}",
                cyl_points_r,
                f"{overlap_cortical_r:.2f}",
                f"{overlap_vertebral_r:.2f}",
                f"{(overlap_cortical_r/overlap_vertebral_r if overlap_vertebral_r!=0 else 0):.2f}",
                _fmt(theta_v),
                _fmt(tau_y),
                _fmt(tau_x),
                _fmt(azlat_r),
                _fmt(acep_r),
                f"{total_time:.2f}"
            ])
        print(f"[CSV Saved] {csv_path}")

    except Exception as e:
        print(f"[Error] Failed to write CSV: {e}")

    fig.text(0.5, 0.98, f'{label_str} Best Position', ha='center', fontsize=15)
    fig.text(
        0.5, 0.44,
        f'L: Diameter = {diameter_l} mm, {length_l} mm, '
        f'R: Diameter = {diameter_r} mm, {length_r} mm, '
        f'Swarm size = {swarm_size}, Iteration = {max_iter}, Total time = {total_time:.2f} s',
        ha='center', fontsize=12
    )

    # 角度註記：CBT 沒有 2D 參考面（user az/alt 為 nan），Azimuth/Altitude 直接顯示
    # 最佳化出的原始角（＝CSV 的 Raw_Azimuth / Raw_Altitude）；TPS 沿用 2D 參考之相對角。
    # 另補上相對骨骼的角度（與 CSV 同參數）：
    #   Azimuth_Lateral  = 螺絲在鏡稱面內相對 AP 軸的發散角（+ = L 側往外，− = R 側）
    #   Altitude_Endplate = 螺絲相對上終板面的仰角
    # 終板面擬合失敗（nan）時該段自動略過。
    def _fig_angle_segs(az, alt, azlat, acep):
        segs = [f'Azimuth = {az:.2f}', f'Altitude = {alt:.2f}']
        if np.isfinite(azlat):
            segs.append(f'Azimuth_Lateral = {azlat:.2f}')
        if np.isfinite(acep):
            segs.append(f'Altitude_Endplate = {acep:.2f}')
        return ', '.join(segs)

    if CBT:
        _ang_l = _fig_angle_segs(float(best_position_l[3]), float(best_position_l[4]), azlat_l, acep_l)
        _ang_r = _fig_angle_segs(float(best_position_r[3]), float(best_position_r[4]), azlat_r, acep_r)
    else:
        _ang_l = _fig_angle_segs(user_azimuth_l, user_altitude_l, azlat_l, acep_l)
        _ang_r = _fig_angle_segs(user_azimuth_r, user_altitude_r, azlat_r, acep_r)

    fig.text(
        0.5, 0.03,
        f'Left : Position = ({best_position_l[2]:.2f}, {best_position_l[1]:.2f}, {best_position_l[0]:.2f}), '
        f'{_ang_l}, '
        f'Intersection = {intersections_l}, Score = {overlap_cortical_l:.2f} / {overlap_vertebral_l:.2f} / {cb_ratio_l:.2f}',
        ha='center', fontsize=8
    )
    fig.text(
        0.5, 0.01,
        f'Right : Position = ({best_position_r[2]:.2f}, {best_position_r[1]:.2f}, {best_position_r[0]:.2f}), '
        f'{_ang_r}, '
        f'Intersection = {intersections_r}, Score = {overlap_cortical_r:.2f} / {overlap_vertebral_r:.2f} / {cb_ratio_r:.2f}',
        ha='center', fontsize=8
    )

    fig.tight_layout()

    date_str = datetime.now().strftime("%Y%m%d")
    file_name = os.path.basename(image2_path) 
    level = file_name.split('_')[0]
    output_folder = os.path.join(base_folder, date_str, patient_id)

    if CBT == True:
        way = 'CBT'
    
    else:
        way = 'TPS'

    # 建目錄 + 存檔一起重試：輸出樹被外部刪除（NFS 刪除競態）時，重試會重建目錄
    path = None
    def _save_fig_once():
        nonlocal path
        _retry_robust(os.makedirs, output_folder, exist_ok=True)
        # 檔名只用 level（volume id 已在資料夾名裡，不重複）
        path = save_with_unique_name(output_folder, level, way,
                                     diameter_l, length_l, diameter_r, length_r,
                                     swarm_size, max_iter)
        fig.savefig(path, dpi=200, bbox_inches="tight")

    _retry_robust(_save_fig_once)
    print("[Saved figure]", path)
    plt.close(fig)


def eval_overlap_from_position(
    pos,
    optimize_size: bool,
    spine_tensor: torch.Tensor,
    image_shape,
    spacing,
    device: torch.device,
    grid=None,
    fixed_diameter: float | None = None,
    fixed_length: float | None = None,
):
    """
    根據 position 生成 cylinder mask，再算 overlap ratio
    """

    if optimize_size:
        d, L = snap_to_discrete_values(pos[5], pos[6])
        params_5 = pos[:5]
    else:
        if fixed_diameter is None or fixed_length is None:
            raise ValueError("fixed_diameter and fixed_length must be provided when optimize_size=False")
        d, L = fixed_diameter, fixed_length
        params_5 = pos

    z, y, x, az, alt = params_5

    cyl_mask = generate_cylinder_n_torch(
        d, L,
        z, y, x,
        az, alt,
        image_shape, spacing,
        device=device,
        grid=grid
    )

    overlap = compute_overlap_ratio_from_cylinder_mask(cyl_mask, spine_tensor)
    return overlap, d, L