CBT_project/core/optimizer.py

import time
from datetime import datetime
import SimpleITK as sitk
import torch
from imaging.orientation import (azimuth_rotation, analyze_vertebral_tilt_contour,
                                  best_symmetry_plane, best_upper_endplate_plane,
                                  segment_spinous_process)
from config.constant import ALLOWED_DIAMETERS, ALLOWED_LENGTHS
from core.objective import OptimizationContext, make_objective_function, make_objective_function_xfr
from pyswarm import pso
from core.cylinder import generate_cylinder_n_torch, generate_cylinder_o_torch, snap_to_discrete_values, create_coordinate_grid, snap_to_discrete_values_xfr
from core.intersection import center_line_intersections_torch
from core.scoring import compute_overlap_ratio_from_cylinder_mask, is_solution_ok, cl_score_torch_xfr
from config.constant import OVERLAP_THRESH
from visualization.res_plot_3d import res_plt_2_torch

LATERAL_REFINE_MIN_IN_BONE = 0.97


def refine_lateral_longer(
    z, x, azimuth, altitude, diameter_raw, length_raw,
    side, az_bounds, x_bounds, y_indices,
    image_shape, spacing, device, grid,
    cortical_tensor, spine_tensor,
):
    """
    Deterministic local refinement after PSO: try aiming more laterally and
    using a longer screw. Only accepts a candidate if it stays
    >= LATERAL_REFINE_MIN_IN_BONE inside bone AND improves the score.

    left  (x-lower half): more lateral = larger azimuth, entry shifted toward -x
    right (x-upper half): more lateral = smaller azimuth, entry shifted toward +x
    """
    def _score_candidate(cand):
        z_c, x_c, az_c, alt_c, d_c, L_c = cand
        y_c = y_indices[round(z_c), round(x_c)]
        if y_c < 0:
            return None
        cyl = generate_cylinder_n_torch(d_c, L_c, z_c, y_c, x_c, az_c, alt_c,
                                        image_shape, spacing, device, grid)
        cyl_o = generate_cylinder_o_torch(d_c, L_c, z_c, y_c, x_c, az_c, alt_c,
                                          image_shape, spacing, device, grid)
        if cyl.sum().item() == 0:
            return None
        inter, _ = center_line_intersections_torch(z_c, y_c, x_c, az_c, alt_c,
                                                   L_c, spine_tensor, spacing, device)
        loss = cl_score_torch_xfr(cortical_tensor, spine_tensor, cyl, cyl_o, inter)
        in_bone = ((spine_tensor == 1) & (cyl == 1)).sum().item() / cyl.sum().item()
        return {'pos': cand, 'loss': loss, 'in_bone': in_bone}

    d_snap, L_snap = snap_to_discrete_values_xfr(diameter_raw, length_raw)
    az_lo, az_hi = az_bounds
    x_lo, x_hi = x_bounds

    az_steps = [0.0, 5.0, 10.0, 15.0] if side == "L" else [0.0, -5.0, -10.0, -15.0]
    x_shifts = [0.0, -4.0, -8.0] if side == "L" else [0.0, 4.0, 8.0]

    best = _score_candidate((z, x, azimuth, altitude, d_snap, L_snap))
    if best is None:
        return z, x, azimuth, altitude, diameter_raw, length_raw, False, None

    for ds in az_steps:
        az_c = min(az_hi - 0.01, max(az_lo + 0.01, azimuth + ds))
        if abs(az_c - azimuth) < 0.5 and ds != 0:
            continue
        for dx in x_shifts:
            x_c = min(x_hi, max(x_lo, x + dx))
            if abs(x_c - x) < 0.5 and dx != 0:
                continue
            for L_c in sorted({L_snap} | {l for l in ALLOWED_LENGTHS if l > L_snap}):
                cand = _score_candidate((z, x_c, az_c, altitude, d_snap, L_c))
                if cand is None:
                    continue
                if cand['in_bone'] >= LATERAL_REFINE_MIN_IN_BONE and cand['loss'] < best['loss']:
                    best = cand

    z_r, x_r, az_r, alt_r, d_r, L_r = best['pos']
    adopted = not (abs(z_r - z) < 1e-6 and abs(x_r - x) < 1e-6
                   and abs(az_r - azimuth) < 1e-6 and abs(L_r - L_snap) < 1e-6)
    return z_r, x_r, az_r, alt_r, d_r, L_r, adopted, best


def _validate_bounds(lb, ub, name):
    for i, (lo, hi) in enumerate(zip(lb, ub)):
        if lo >= hi:
            raise ValueError(f'PSO bounds invalid for {name}: dim {i} lower {lo} >= upper {hi}')


def get_first_nonzero_y(arr):
    OUTSIDE_VALUE = -100

    # 1. Create a boolean mask where elements are non-zero
    mask = arr != 0
    
    # 2. Find the index of the first True value along the Y axis (axis=1)
    y_indices = np.argmax(mask, axis=1)
    
    # 3. Edge Case Handling: If a whole (x, z) column is zero, argmax returns 0.
    # We need to distinguish this from an actual non-zero value at index 0.
    has_nonzero = np.any(mask, axis=1)
    
    # 4. Replace indices where there were no non-zeros with a sentinel value (e.g., -1)
    y_indices = np.where(has_nonzero, y_indices, OUTSIDE_VALUE)
    
    y_indices = np.where(y_indices < arr.shape[1] * .1, OUTSIDE_VALUE, y_indices)
    y_indices = np.where(y_indices > arr.shape[1] * .4, OUTSIDE_VALUE, y_indices)
    
    return y_indices.astype(np.float32)

def constraint_y(x, y_indices):
    return y_indices[round(x[0]), round(x[1])]

def run_pso_torch_xfr(
    label_str: str,
    image1_path: str,
    image2_path: str,
    image3_path: str,
    folder: str,
    swarm_size: int,
    max_iter: int,
    spacing: list,
    CBT: bool,
    device: torch.device,
    optimize_size: bool = True,
    grid=None,
    debug=False,
    omega = 0.9,
):
    """
    Main function to run PSO. 
    如果 optimize_size=True，diameter 和 length 也會被最佳化
    如果 optimize_size=False，使用預設值（向後兼容）
    """
    start_time = time.time()

    # Use global references
    global image1_array, image2_array, image2_shape, image3_array
    global diameter, length  # 這些現在只用於非最佳化模式
    global spine_tensor, cortical_tensor, spine_roi_tensor

    # Load images
    image1 = sitk.ReadImage(image1_path)
    image2 = sitk.ReadImage(image2_path)
    image3 = sitk.ReadImage(image3_path)
    image1_array = sitk.GetArrayFromImage(image1)
    image2_array = sitk.GetArrayFromImage(image2)
    image3_array = sitk.GetArrayFromImage(image3)
    image2_shape = image2_array.shape
    image_shape = image2_shape

    # Move arrays to torch
    cortical_tensor = torch.from_numpy(image1_array).to(device=device, dtype=torch.uint8)
    spine_tensor = torch.from_numpy(image2_array).to(device=device, dtype=torch.uint8)
    spine_roi_tensor = torch.from_numpy(image3_array).to(device=device, dtype=torch.uint8)

    # 建立明確的優化上下文（取代舊的跨檔案 global 注入；狀態全部顯式傳入目標函數）
    ctx = OptimizationContext(
        cortical_tensor=cortical_tensor,
        spine_tensor=spine_tensor,
        spine_roi_tensor=spine_roi_tensor,
        image1_array=image1_array,
        image2_array=image2_array,
        image3_array=image3_array,
        image2_shape=image2_shape,
        spacing=spacing,
        device=device,
        grid=grid,
        diameter=diameter if not optimize_size else None,
        length=length if not optimize_size else None,
    )

    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']

    # ===== az/alt 搜尋範圍錨點：以椎體自身座標系取代 2D 近似 =====
    # 鏡稱對稱面（最佳 mirror plane）：法線 (s_nx, s_ny, s_nz)，s_nx>0 固定符號
    #   theta_v = atan2(s_ny, s_nx) 是椎體真實左右軸相對 +x 的旋轉角，
    #   取代舊 2D 輪廓前點角 azi（舊式 az 中心 90-azi == 新式 90+theta_v）。
    # 上終板面（normal 朝上, e_nz>0）：
    #   tau_y = atan2(e_ny, e_nz)：終板 AP 面傾斜；負 = 面向 +y（終板側）升高，
    #       軌跡需爬得更陡 → altitude 中心減小（舊式 65-alt 的 3D 版）。
    #   tau_x = atan2(e_nx, e_nz)：終板 LR 面傾斜；正 = 面向小 x（L 側）升高，
    #       L 側需更陡（-tau_x）、R 側較平（+tau_x）。
    sym_plane = best_symmetry_plane(image2_array)

    # 入口面 y_indices 取「每個 (z,x) 柱第一個 bone voxel（最後側）」，
    # 中線柱會落在棘突上（入口太靠內後）。先把棘突（鏡稱面中線後側，
    # 見 segment_spinous_process）從 image2_array 移除再取 surface，
    # 讓 y_indices 永不落在棘突上（loss 用 spine_tensor，不受影響）。
    sp_mask, sp_th, sp_info = segment_spinous_process(image2_array, sym_plane)
    if sp_mask is not None and sp_mask.any():
        n_sp = int(sp_mask.sum())
        image2_array[sp_mask] = 0
        print(f"[Y-INDEX] spinous process removed from entry surface: {n_sp} vox "
              f"(band=+/-{sp_info['band_w']:.1f} voxel, AP>={sp_info['ap_thresh']:.1f}, "
              f"mode={sp_info['mode']})")

    y_indices = get_first_nonzero_y(image2_array)
    objective_fn = make_objective_function_xfr(ctx, y_indices)

    s_nx, s_ny, s_nz = sym_plane['normal']
    theta_v = float(np.degrees(np.arctan2(s_ny, s_nx)))
    endplate_plane = best_upper_endplate_plane(image2_array)
    if endplate_plane is not None:
        e_nx, e_ny, e_nz = endplate_plane['normal']
        tau_y = float(np.degrees(np.arctan2(e_ny, e_nz)))
        tau_x = float(np.degrees(np.arctan2(e_nx, e_nz)))
    else:
        tau_y, tau_x = 0.0, 0.0
    print(f"[PLANE] mirror  : {sym_plane['plane'][0]:+.3f}x {sym_plane['plane'][1]:+.3f}y "
          f"{sym_plane['plane'][2]:+.3f}z = {sym_plane['plane'][3]:.1f}"
          f"  (theta_v={theta_v:+.2f} deg, mirror ratio={sym_plane['ratio']:.3f})")
    if endplate_plane is not None:
        p = endplate_plane['plane']
        print(f"[PLANE] endplate: {p[0]:+.3f}x {p[1]:+.3f}y {p[2]:+.3f}z = {p[3]:.1f}"
              f"  (tau_y={tau_y:+.2f} deg, tau_x={tau_x:+.2f} deg, "
              f"inlier={endplate_plane['inlier_ratio']:.2f})")
    else:
        print("[PLANE] endplate: 資料不足，altitude 錨點退回 tau_y=tau_x=0")

    # flat_min_index = np.argmin(y_indices)
    # z_border, x_border = np.unravel_index(flat_min_index, y_indices.shape)

    # 脊椎中線：整段 (全體積) 骨頭 x 範圍的中點。
    # 不取單一行的原因 (0005 L5)：椎體軸狀面旋轉時單行只罩到單側骨塊
    # (x 1..76 / W=224 → x_mid≈0.17W)，L/R 兩個 band 被壓到同一側。
    # 不用鏡稱對稱軸的原因 (0001 L4)：逐切面對稱軸會被肋、後側要素
    # 左右不對稱與椎體傾斜牽引 (69.0 vs 範圍中點 74.5)，把 R band 內緣
    # (x_mid+0.1W) 拉進中線棘突/椎板區，R 側入口落在棘突上 (太靠內後)。
    # Laminectomy 只移除中線後側要素，左右極端 x 位置不變，
    # 所以範圍中點同樣不受其影響，作為 L/R band 分割線比對稱軸穩定。
    x_with_nonzero = np.where(np.any(image2_array != 0, axis=(0, 1)))[0]
    x1 = x_with_nonzero[0]
    x2 = x_with_nonzero[-1]
    x_width = x2 - x1

    # print(x1,x2)
    # exit()

    # x_mid = (x1 + x2) / 2
    # x1 = x_mid-image_shape[2]*.1
    # x2 = x_mid+image_shape[2]*.1

    z_sum = np.sum(image2_array, axis=(1, 2))
    z_with_nonzero = np.where(z_sum > 0)[0]
    z1 = z_with_nonzero[0]
    z2 = z_with_nonzero[-1]   
    z_height = z2-z1

    # print(x1,x2)
    # exit()

    # print(x_border, z_border)
    # exit()
    # import sys
    # import numpy
    # numpy.set_printoptions(threshold=sys.maxsize)
    # print(y_indices)
    # exit()

    # 設定基本的 bounds
    if CBT == True:    
        # z_bounds = (0, image_shape[0]-1)
        # z_bounds = (z1, (z1+z2)/2)
        # z_bounds = (.1*image_shape[0], .8*image_shape[0])
        z_bounds = (z1+z_height*.1, z1+z_height*.9)

        # x_bounds_right = (image_shape[2]/2 + image_shape[2]/10, image_shape[2] - 1)
        # x_bounds_left  = (0, image_shape[2]/2 - image_shape[2]/10 - 1)
        # x_bounds_right = (x2, image_shape[2]*.9)
        # x_bounds_left  = (image_shape[2]*.1, x1)
        x_bounds_right = (x1+x_width*.6, +x_width*.9)
        x_bounds_left  = (x1+x_width*.1, +x_width*.4)

        # 脊椎若被體積邊界切到（真正偏心、骨頭貼著左/右邊緣），
        # 對應那側的 x band 下限會 >= 上限，PSO 會丟 "upper-bound must be greater"。
        # 出錯時 clamp 成同側最小寬度（5% 寬度）的合法 band。
        min_band = .05 * image_shape[2]
        if x_bounds_left[1] <= x_bounds_left[0]:
            x_bounds_left = (x_bounds_left[0], x_bounds_left[0] + min_band)
        if x_bounds_right[0] >= x_bounds_right[1]:
            x_bounds_right = (x_bounds_right[1] - min_band, x_bounds_right[1])

        # 舊 2D 版本（以輪廓角 azi / 矢狀面傾斜 alt 平移固定範圍），保留供對照：
        # azimuth_bounds_l = ((98-azi), (120-azi))
        # azimuth_bounds_r = ((60-azi), (82-azi))
        # altitude_bounds = ((60-alt), (70-alt))
        # 新：範圍以「椎體自身座標系」為中心 —— 椎體系中 az=90° 是 AP 直向、
        # L 帶 = AP 後退 8~30°（偏 -x）、R 帶 = AP 前進 8~30°（偏 +x）、
        # altitude 60~70。再換算回影像系：az 整體加 theta_v（鏡稱面），
        # altitude 加 tau_y（終板 AP 傾斜）並逐側加 -/+tau_x（終板 LR 傾斜）。
        azimuth_bounds_l = ((98+theta_v), (120+theta_v))
        azimuth_bounds_r = ((60+theta_v), (82+theta_v))
        altitude_bounds_l = ((60+tau_y-tau_x), (70+tau_y-tau_x))
        altitude_bounds_r = ((60+tau_y+tau_x), (70+tau_y+tau_x))

    else:      
        z_bounds = (0, image_shape[0] - 1)   
        y_bounds = (image_shape[1]/5, image_shape[1]/2 - 1)   
        x_bounds_left = (0, image_shape[2]/2 - image_shape[2]/10 - 1)   
        x_bounds_right = (image_shape[2]/2 + image_shape[2]/10, image_shape[2] - 1)
        azimuth_bounds_l = (60-azi, 90-azi) 
        azimuth_bounds_r = (90-azi, 120-azi) 
        altitude_bounds = (65-alt, 80-alt)
        # 非 CBT 分支維持單一式，兩側同用
        altitude_bounds_l = altitude_bounds
        altitude_bounds_r = altitude_bounds

    def eval_overlap_from_position(pos, side: str, optimize_size: bool,
                               spine_tensor: torch.Tensor,
                               image_shape, spacing):
        """
        根據 PSO 給的 position 生成 cylinder mask，再算 overlap ratio
        side: "L" or "R" 只是方便 debug
        """
        if optimize_size:
            # d, L = snap_to_discrete_values(pos[5], pos[6])
            d, L = snap_to_discrete_values_xfr(pos[5], pos[6])
            params_5 = pos[:5]
        else:
            d, L = diameter, length
            params_5 = pos

        cyl_mask = generate_cylinder_n_torch(
            d, L,
            params_5[0], params_5[1], params_5[2],
            params_5[3], params_5[4],
            image_shape, spacing, device, grid
        )

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

    if optimize_size:
        # 模式 1：優化 diameter 和 length
        print("=== 最佳化模式：最佳化位置、角度、直徑和長度 ===")
        
        # 設定 diameter 和 length 的 bounds（連續範圍）
        diameter_bounds = (min(ALLOWED_DIAMETERS), max(ALLOWED_DIAMETERS)*1.01)
        length_bounds = (min(ALLOWED_LENGTHS), max(ALLOWED_LENGTHS)*1.01)
        
        # bounds 現在有 7 個參數（altitude 分 L/R 兩側，由終板面 tau_x 決定）
        lb_l = [z_bounds[0], x_bounds_left[0], azimuth_bounds_l[0], 
                altitude_bounds_l[0], diameter_bounds[0], length_bounds[0]]
        ub_l = [z_bounds[1], x_bounds_left[1], azimuth_bounds_l[1], 
                altitude_bounds_l[1], diameter_bounds[1], length_bounds[1]]

        lb_r = [z_bounds[0], x_bounds_right[0], azimuth_bounds_r[0], 
                altitude_bounds_r[0], diameter_bounds[0], length_bounds[0]]
        ub_r = [z_bounds[1], x_bounds_right[1], azimuth_bounds_r[1], 
                altitude_bounds_r[1], diameter_bounds[1], length_bounds[1]]
        
    else:
        # 模式 2：固定 diameter 和 length（向後兼容）
        print("=== 固定尺寸模式：最佳化位置和角度 ===")
        # 使用預設值（需要在調用時提供）
        diameter = 4.5  # 或從參數傳入
        length = 45     # 或從參數傳入
        
        lb_l = [z_bounds[0], y_bounds[0], x_bounds_left[0], azimuth_bounds_l[0], altitude_bounds_l[0]]
        ub_l = [z_bounds[1], y_bounds[1], x_bounds_left[1], azimuth_bounds_l[1], altitude_bounds_l[1]]

        lb_r = [z_bounds[0], y_bounds[0], x_bounds_right[0], azimuth_bounds_r[0], altitude_bounds_r[0]]
        ub_r = [z_bounds[1], y_bounds[1], x_bounds_right[1], azimuth_bounds_r[1], altitude_bounds_r[1]]

    if True or debug:
        print(lb_l)
        print(ub_l)
        print(lb_r)
        print(ub_r)

    best_loss_l = float('inf')
    best_loss_r = float('inf')
    best_position_l = None
    best_position_r = None

    # Left side optimization
    print(f"\n=== {label_str} 左側 ===")
    _validate_bounds(lb_l, ub_l, f'{label_str} L')
    position_l, loss_l = pso(objective_fn, lb_l, ub_l,
        # ieqcons=[constraint_y],
        swarmsize=swarm_size,
        omega = omega,
        maxiter=max_iter, debug=debug)

    z, x, azimuth, altitude, diameter, length = position_l
    az_pso, x_pso, L_pso = azimuth, x, length
    y = y_indices[round(z), round(x)]

    z, x, azimuth, altitude, diameter, length, adopted_l, ref_l = refine_lateral_longer(
        z, x, azimuth, altitude, diameter, length,
        "L", azimuth_bounds_l, x_bounds_left, y_indices,
        image_shape, spacing, device, grid,
        cortical_tensor, spine_tensor,
    )
    y = y_indices[round(z), round(x)]
    if adopted_l:
        loss_l = ref_l['loss']
        print(f"[LEFT] lateral-refine: az {az_pso:.2f} -> {azimuth:.2f}, x {x_pso:.2f} -> {x:.2f}, "
              f"L {L_pso:.2f} -> {length:.2f}, in-bone {ref_l['in_bone']*100:.1f}%")
    else:
        print("[LEFT] lateral-refine: no improvement")

    position_l = z, y, x, azimuth, altitude, diameter, length

    overlap_l, diameter_l, length_l = eval_overlap_from_position(
        position_l, "L", optimize_size, spine_tensor, image_shape, spacing
    )
    print(f"[LEFT] overlap: {overlap_l*100:.1f}%")

    if optimize_size:
        print(f"[LEFT] Position: {position_l[:5]}")
        print(f"[LEFT] Diameter: {diameter_l} mm (raw: {position_l[5]:.2f})")
        print(f"[LEFT] Length: {length_l} mm (raw: {position_l[6]:.2f})")
        print(f"[LEFT] Loss: {loss_l}\n")
        best_position_l = list(position_l[:5]) + [diameter_l, length_l]
    else:
        print(f"[LEFT] Position: {position_l}")
        best_position_l = position_l

    best_loss_l = loss_l
    best_overlap_l = overlap_l   # 新增
    
    # max_retries = 0
    # retries = 0

    # 左側 retry：loss 要 <=0 且 overlap >= 0.5 才算過關
    # while (best_loss_l > 0 or best_overlap_l < OVERLAP_THRESH) and retries < max_retries:
        # position_l, loss_l = pso(objective_fn, lb_l, ub_l, swarmsize=swarm_size, maxiter=max_iter)
        # overlap_l, diameter_l, length_l = eval_overlap_from_position(
            # position_l, "L", optimize_size, spine_tensor, image_shape, spacing
        # )

    # 只要找到更好的 loss（或你想用 loss+overlap 綜合排序也行）就更新 best
    # 安全版本：優先選「合格解」；沒有合格解時才用 loss 最小的當備案
        # candidate_pos = (list(position_l[:5]) + [diameter_l, length_l]) if optimize_size else position_l

        # candidate_ok = is_solution_ok(loss_l, overlap_l, OVERLAP_THRESH)
        # best_ok = is_solution_ok(best_loss_l, best_overlap_l, OVERLAP_THRESH)

        # if candidate_ok and (not best_ok or loss_l < best_loss_l):
            # best_position_l = candidate_pos
            # best_loss_l = loss_l
            # best_overlap_l = overlap_l
            # print(f"[LEFT][retry {retries+1}] ✅ ok | loss={loss_l:.4f}, overlap={overlap_l*100:.1f}%")
        # elif (not best_ok) and (loss_l < best_loss_l):
            # best 還不合格時，先用更小 loss 的當暫存（至少越來越好）
            # best_position_l = candidate_pos
            # best_loss_l = loss_l
            # best_overlap_l = overlap_l
            # print(f"[LEFT][retry {retries+1}] ⚠️ not ok | loss improved={loss_l:.4f}, overlap={overlap_l*100:.1f}%")
        # else:
            # print(f"[LEFT][retry {retries+1}] ❌ no improve | loss={loss_l:.4f}, overlap={overlap_l*100:.1f}%")

        # retries += 1
    
    # Right side optimization
    print(f"\n=== {label_str} 右側 ===")
    _validate_bounds(lb_r, ub_r, f'{label_str} R')
    position_r, loss_r = pso(objective_fn, lb_r, ub_r,
        # ieqcons=[constraint_y],
        swarmsize=swarm_size,
        omega = omega,
        maxiter=max_iter, debug=debug)

    z, x, azimuth, altitude, diameter, length = position_r
    az_pso, x_pso, L_pso = azimuth, x, length
    y = y_indices[round(z), round(x)]

    z, x, azimuth, altitude, diameter, length, adopted_r, ref_r = refine_lateral_longer(
        z, x, azimuth, altitude, diameter, length,
        "R", azimuth_bounds_r, x_bounds_right, y_indices,
        image_shape, spacing, device, grid,
        cortical_tensor, spine_tensor,
    )
    y = y_indices[round(z), round(x)]
    if adopted_r:
        loss_r = ref_r['loss']
        print(f"[RIGHT] lateral-refine: az {az_pso:.2f} -> {azimuth:.2f}, x {x_pso:.2f} -> {x:.2f}, "
              f"L {L_pso:.2f} -> {length:.2f}, in-bone {ref_r['in_bone']*100:.1f}%")
    else:
        print("[RIGHT] lateral-refine: no improvement")

    position_r = z, y, x, azimuth, altitude, diameter, length

    overlap_r, diameter_r, length_r = eval_overlap_from_position(
        position_r, "R", optimize_size, spine_tensor, image_shape, spacing
    )
    print(f"[RIGHT] overlap: {overlap_r*100:.1f}%")

    if optimize_size:
        # diameter_r, length_r = snap_to_discrete_values(position_r[5], position_r[6])
        diameter_r, length_r = snap_to_discrete_values_xfr(position_r[5], position_r[6])
        print(f"[RIGHT] Position: {position_r[:5]}")
        print(f"[RIGHT] Diameter: {diameter_r} mm (raw: {position_r[5]:.2f})")
        print(f"[RIGHT] Length: {length_r} mm (raw: {position_r[6]:.2f})")
        print(f"[RIGHT] Loss: {loss_r}\n")
        
        best_position_r = list(position_r[:5]) + [diameter_r, length_r]
    else:
        print(f"[RIGHT] Position: {position_r}")
        print(f"[RIGHT] Loss: {loss_r}\n")
        best_position_r = position_r
    
    best_loss_r = loss_r
    best_overlap_r = overlap_r 
    
    # 如果需要 retry（loss > 0）
    # max_retries = 10
    # retries = 0

    # while (best_loss_r > 0 or best_overlap_r < OVERLAP_THRESH) and retries < max_retries:
        # position_r, loss_r = pso(objective_fn, lb_r, ub_r, swarmsize=swarm_size, maxiter=max_iter)
        # overlap_r, diameter_r, length_r = eval_overlap_from_position(
            # position_r, "R", optimize_size, spine_tensor, image_shape, spacing
        # )

        # 只要找到更好的 loss（或你想用 loss+overlap 綜合排序也行）就更新 best
        # 這裡給你一個更安全的版本：優先選「合格解」；沒有合格解時才用 loss 最小的當備案
        # candidate_pos = (list(position_r[:5]) + [diameter_r, length_r]) if optimize_size else position_r

        # candidate_ok = is_solution_ok(loss_r, overlap_r, OVERLAP_THRESH)
        # best_ok = is_solution_ok(best_loss_r, best_overlap_r, OVERLAP_THRESH)

        # if candidate_ok and (not best_ok or loss_r < best_loss_r):
            # best_position_r = candidate_pos
            # best_loss_r = loss_r
            # best_overlap_r = overlap_r
            # print(f"[RIGHT][retry {retries+1}] ✅ ok | loss={loss_r:.4f}, overlap={overlap_r*100:.1f}%")
        # elif (not best_ok) and (loss_r < best_loss_r):
            # best 還不合格時，先用更小 loss 的當暫存（至少越來越好）
            # best_position_r = candidate_pos
            # best_loss_r = loss_r
            # best_overlap_r = overlap_r
            # print(f"[RIGHT][retry {retries+1}] ⚠️ not ok | loss improved={loss_r:.4f}, overlap={overlap_r*100:.1f}%")
        # else:
            # print(f"[RIGHT][retry {retries+1}] ❌ no improve | loss={loss_r:.4f}, overlap={overlap_r*100:.1f}%")

        # retries += 1
    
    end_time = time.time()
    total_time = end_time - start_time

    # 提取最終的 diameter 和 length
    if optimize_size:
        final_diameter_l = best_position_l[5]
        final_length_l = best_position_l[6]
        final_diameter_r = best_position_r[5]
        final_length_r = best_position_r[6]
        
        print(f"\n=== {label_str} 最終結果 ===")
        print(f"Left - Diameter: {final_diameter_l} mm, Length: {final_length_l} mm")
        print(f"Right - Diameter: {final_diameter_r} mm, Length: {final_length_r} mm")
    else:
        final_diameter_l = diameter
        final_length_l = length
        final_diameter_r = diameter
        final_length_r = length

    res_plt_2_torch(
    spine_tensor,
    cortical_tensor,
    image_shape,
    image2_path,
    folder,
    label_str,
    final_diameter_l,
    final_length_l,
    final_diameter_r,
    final_length_r,
    best_position_l,
    best_position_r,
    swarm_size,
    max_iter,
    total_time,
    spacing,
    CBT,
    device,
    grid)
    
    return best_position_l, best_loss_l, best_position_r, best_loss_r, total_time


def run_pso_torch(
    label_str: str,
    image1_path: str,
    image2_path: str,
    image3_path: str,
    folder: str,
    swarm_size: int,
    max_iter: int,
    spacing: list,
    CBT: bool,
    device: torch.device,
    optimize_size: bool = True,
    grid=None,
    debug=True,
):
    """
    Main function to run PSO. 
    如果 optimize_size=True，diameter 和 length 也會被最佳化
    如果 optimize_size=False，使用預設值（向後兼容）
    """
    start_time = time.time()

    # Use global references
    global image1_array, image2_array, image2_shape, image3_array
    global diameter, length  # 這些現在只用於非最佳化模式
    global spine_tensor, cortical_tensor, spine_roi_tensor

    # Load images
    image1 = sitk.ReadImage(image1_path)
    image2 = sitk.ReadImage(image2_path)
    image3 = sitk.ReadImage(image3_path)
    image1_array = sitk.GetArrayFromImage(image1)
    image2_array = sitk.GetArrayFromImage(image2)
    image3_array = sitk.GetArrayFromImage(image3)
    image2_shape = image2_array.shape
    image_shape = image2_shape

    # Move arrays to torch
    cortical_tensor = torch.from_numpy(image1_array).to(device=device, dtype=torch.uint8)
    spine_tensor = torch.from_numpy(image2_array).to(device=device, dtype=torch.uint8)
    spine_roi_tensor = torch.from_numpy(image3_array).to(device=device, dtype=torch.uint8)

    # 建立明確的優化上下文（取代舊的跨檔案 global 注入；狀態全部顯式傳入目標函數）
    ctx = OptimizationContext(
        cortical_tensor=cortical_tensor,
        spine_tensor=spine_tensor,
        spine_roi_tensor=spine_roi_tensor,
        image1_array=image1_array,
        image2_array=image2_array,
        image3_array=image3_array,
        image2_shape=image2_shape,
        spacing=spacing,
        device=device,
        grid=grid,
        diameter=diameter if not optimize_size else None,
        length=length if not optimize_size else None,
    )
    objective_fn = make_objective_function(ctx)

    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']

    # 設定基本的 bounds
    if CBT == True:    
        z_bounds = (0, image_shape[0] - 1)
        y_bounds = (image_shape[1]/5, image_shape[1]/2 - 1)
        x_bounds_right = (image_shape[2]/2 + image_shape[2]/10, image_shape[2] - 1)
        x_bounds_left  = (0, image_shape[2]/2 - image_shape[2]/10 - 1)
        # CBT 參數依據 references/：Santoni 2009 (Spine J 9:366) 冠狀面 25-30° cranial (caudo-cephalad)、
        # 軸狀面自正中線向外 (medial→lateral) ≤30°；Delgado-Fernandez 2017 (ASJ 11:817)、Kim 2022 (SSRR 6:1)
        azimuth_bounds_l = ((98-azi), (120-azi))
        azimuth_bounds_r = ((60-azi), (82-azi))
        altitude_bounds = ((60-alt), (70-alt))

        # xfr
        # z_bounds = (0, image_shape[0] - 1)
        y_bounds = (0, image_shape[1]/2)
        # x_bounds_right = (image_shape[2]/2, image_shape[2] - 1)
        # x_bounds_left  = (0, image_shape[2]/2)
        # azimuth_bounds_l = (90, 135)
        # azimuth_bounds_r = (45, 90)
        # altitude_bounds = (0, 90)
    else:      
        z_bounds = (0, image_shape[0] - 1)   
        y_bounds = (image_shape[1]/5, image_shape[1]/2 - 1)   
        x_bounds_left = (0, image_shape[2]/2 - image_shape[2]/10 - 1)   
        x_bounds_right = (image_shape[2]/2 + image_shape[2]/10, image_shape[2] - 1)
        azimuth_bounds_l = (60-azi, 90-azi) 
        azimuth_bounds_r = (90-azi, 120-azi) 
        altitude_bounds = (65-alt, 80-alt)

    def eval_overlap_from_position(pos, side: str, optimize_size: bool,
                               spine_tensor: torch.Tensor,
                               image_shape, spacing):
        """
        根據 PSO 給的 position 生成 cylinder mask，再算 overlap ratio
        side: "L" or "R" 只是方便 debug
        """
        if optimize_size:
            # d, L = snap_to_discrete_values(pos[5], pos[6])
            d, L = snap_to_discrete_values_xfr(pos[5], pos[6])
            params_5 = pos[:5]
        else:
            d, L = diameter, length
            params_5 = pos

        cyl_mask = generate_cylinder_n_torch(
            d, L,
            params_5[0], params_5[1], params_5[2],
            params_5[3], params_5[4],
            image_shape, spacing, device, grid
        )

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

    if optimize_size:
        # 模式 1：優化 diameter 和 length
        print("=== 最佳化模式：最佳化位置、角度、直徑和長度 ===")
        
        # 設定 diameter 和 length 的 bounds（連續範圍）
        diameter_bounds = (min(ALLOWED_DIAMETERS), max(ALLOWED_DIAMETERS))
        length_bounds = (min(ALLOWED_LENGTHS), max(ALLOWED_LENGTHS))
        
        # bounds 現在有 7 個參數
        lb_l = [z_bounds[0], y_bounds[0], x_bounds_left[0], azimuth_bounds_l[0], 
                altitude_bounds[0], diameter_bounds[0], length_bounds[0]]
        ub_l = [z_bounds[1], y_bounds[1], x_bounds_left[1], azimuth_bounds_l[1], 
                altitude_bounds[1], diameter_bounds[1], length_bounds[1]]

        lb_r = [z_bounds[0], y_bounds[0], x_bounds_right[0], azimuth_bounds_r[0], 
                altitude_bounds[0], diameter_bounds[0], length_bounds[0]]
        ub_r = [z_bounds[1], y_bounds[1], x_bounds_right[1], azimuth_bounds_r[1], 
                altitude_bounds[1], diameter_bounds[1], length_bounds[1]]
        
    else:
        # 模式 2：固定 diameter 和 length（向後兼容）
        print("=== 固定尺寸模式：最佳化位置和角度 ===")
        # 使用預設值（需要在調用時提供）
        diameter = 4.5  # 或從參數傳入
        length = 45     # 或從參數傳入
        
        lb_l = [z_bounds[0], y_bounds[0], x_bounds_left[0], azimuth_bounds_l[0], altitude_bounds[0]]
        ub_l = [z_bounds[1], y_bounds[1], x_bounds_left[1], azimuth_bounds_l[1], altitude_bounds[1]]

        lb_r = [z_bounds[0], y_bounds[0], x_bounds_right[0], azimuth_bounds_r[0], altitude_bounds[0]]
        ub_r = [z_bounds[1], y_bounds[1], x_bounds_right[1], azimuth_bounds_r[1], altitude_bounds[1]]

    if debug:
        print(lb_l)
        print(ub_l)
        print(lb_r)
        print(ub_r)

    best_loss_l = float('inf')
    best_loss_r = float('inf')
    best_position_l = None
    best_position_r = None

    # Left side optimization
    print("\n=== 左側 ===")
    position_l, loss_l = pso(objective_fn, lb_l, ub_l, swarmsize=swarm_size, maxiter=max_iter, debug=debug)

    overlap_l, diameter_l, length_l = eval_overlap_from_position(
        position_l, "L", optimize_size, spine_tensor, image_shape, spacing
    )
    print(f"[LEFT] overlap: {overlap_l*100:.1f}%")

    if optimize_size:
        print(f"[LEFT] Position: {position_l[:5]}")
        print(f"[LEFT] Diameter: {diameter_l} mm (raw: {position_l[5]:.2f})")
        print(f"[LEFT] Length: {length_l} mm (raw: {position_l[6]:.2f})")
        best_position_l = list(position_l[:5]) + [diameter_l, length_l]
    else:
        print(f"[LEFT] Position: {position_l}")
        best_position_l = position_l

    best_loss_l = loss_l
    best_overlap_l = overlap_l   # 新增
    
    # max_retries = 0
    # retries = 0

    # 左側 retry：loss 要 <=0 且 overlap >= 0.5 才算過關
    # while (best_loss_l > 0 or best_overlap_l < OVERLAP_THRESH) and retries < max_retries:
        # position_l, loss_l = pso(objective_fn, lb_l, ub_l, swarmsize=swarm_size, maxiter=max_iter)
        # overlap_l, diameter_l, length_l = eval_overlap_from_position(
            # position_l, "L", optimize_size, spine_tensor, image_shape, spacing
        # )

    # 只要找到更好的 loss（或你想用 loss+overlap 綜合排序也行）就更新 best
    # 安全版本：優先選「合格解」；沒有合格解時才用 loss 最小的當備案
        # candidate_pos = (list(position_l[:5]) + [diameter_l, length_l]) if optimize_size else position_l

        # candidate_ok = is_solution_ok(loss_l, overlap_l, OVERLAP_THRESH)
        # best_ok = is_solution_ok(best_loss_l, best_overlap_l, OVERLAP_THRESH)

        # if candidate_ok and (not best_ok or loss_l < best_loss_l):
            # best_position_l = candidate_pos
            # best_loss_l = loss_l
            # best_overlap_l = overlap_l
            # print(f"[LEFT][retry {retries+1}] ✅ ok | loss={loss_l:.4f}, overlap={overlap_l*100:.1f}%")
        # elif (not best_ok) and (loss_l < best_loss_l):
            # best 還不合格時，先用更小 loss 的當暫存（至少越來越好）
            # best_position_l = candidate_pos
            # best_loss_l = loss_l
            # best_overlap_l = overlap_l
            # print(f"[LEFT][retry {retries+1}] ⚠️ not ok | loss improved={loss_l:.4f}, overlap={overlap_l*100:.1f}%")
        # else:
            # print(f"[LEFT][retry {retries+1}] ❌ no improve | loss={loss_l:.4f}, overlap={overlap_l*100:.1f}%")

        # retries += 1
    
    # Right side optimization
    print("\n=== 右側 ===")
    position_r, loss_r = pso(objective_fn, lb_r, ub_r, swarmsize=swarm_size, maxiter=max_iter, debug=debug)
    overlap_r, diameter_r, length_r = eval_overlap_from_position(
        position_r, "R", optimize_size, spine_tensor, image_shape, spacing
    )
    print(f"[RIGHT] overlap: {overlap_r*100:.1f}%")

    if optimize_size:
        # diameter_r, length_r = snap_to_discrete_values(position_r[5], position_r[6])
        diameter_r, length_r = snap_to_discrete_values_xfr(position_r[5], position_r[6])
        print(f"[RIGHT] Position: {position_r[:5]}")
        print(f"[RIGHT] Diameter: {diameter_r} mm (raw: {position_r[5]:.2f})")
        print(f"[RIGHT] Length: {length_r} mm (raw: {position_r[6]:.2f})")
        print(f"[RIGHT] Loss: {loss_r}\n")
        
        best_position_r = list(position_r[:5]) + [diameter_r, length_r]
    else:
        print(f"[RIGHT] Position: {position_r}")
        print(f"[RIGHT] Loss: {loss_r}\n")
        best_position_r = position_r
    
    best_loss_r = loss_r
    best_overlap_r = overlap_r 
    
    # 如果需要 retry（loss > 0）
    # max_retries = 10
    # retries = 0

    # while (best_loss_r > 0 or best_overlap_r < OVERLAP_THRESH) and retries < max_retries:
        # position_r, loss_r = pso(objective_fn, lb_r, ub_r, swarmsize=swarm_size, maxiter=max_iter)
        # overlap_r, diameter_r, length_r = eval_overlap_from_position(
            # position_r, "R", optimize_size, spine_tensor, image_shape, spacing
        # )

        # 只要找到更好的 loss（或你想用 loss+overlap 綜合排序也行）就更新 best
        # 這裡給你一個更安全的版本：優先選「合格解」；沒有合格解時才用 loss 最小的當備案
        # candidate_pos = (list(position_r[:5]) + [diameter_r, length_r]) if optimize_size else position_r

        # candidate_ok = is_solution_ok(loss_r, overlap_r, OVERLAP_THRESH)
        # best_ok = is_solution_ok(best_loss_r, best_overlap_r, OVERLAP_THRESH)

        # if candidate_ok and (not best_ok or loss_r < best_loss_r):
            # best_position_r = candidate_pos
            # best_loss_r = loss_r
            # best_overlap_r = overlap_r
            # print(f"[RIGHT][retry {retries+1}] ✅ ok | loss={loss_r:.4f}, overlap={overlap_r*100:.1f}%")
        # elif (not best_ok) and (loss_r < best_loss_r):
            # best 還不合格時，先用更小 loss 的當暫存（至少越來越好）
            # best_position_r = candidate_pos
            # best_loss_r = loss_r
            # best_overlap_r = overlap_r
            # print(f"[RIGHT][retry {retries+1}] ⚠️ not ok | loss improved={loss_r:.4f}, overlap={overlap_r*100:.1f}%")
        # else:
            # print(f"[RIGHT][retry {retries+1}] ❌ no improve | loss={loss_r:.4f}, overlap={overlap_r*100:.1f}%")

        # retries += 1
    
    end_time = time.time()
    total_time = end_time - start_time

    # 提取最終的 diameter 和 length
    if optimize_size:
        final_diameter_l = best_position_l[5]
        final_length_l = best_position_l[6]
        final_diameter_r = best_position_r[5]
        final_length_r = best_position_r[6]
        
        print(f"\n=== 最終結果 ===")
        print(f"Left - Diameter: {final_diameter_l} mm, Length: {final_length_l} mm")
        print(f"Right - Diameter: {final_diameter_r} mm, Length: {final_length_r} mm")
    else:
        final_diameter_l = diameter
        final_length_l = length
        final_diameter_r = diameter
        final_length_r = length

    res_plt_2_torch(
    spine_tensor,
    cortical_tensor,
    image_shape,
    image2_path,
    folder,
    label_str,
    final_diameter_l,
    final_length_l,
    final_diameter_r,
    final_length_r,
    best_position_l,
    best_position_r,
    swarm_size,
    max_iter,
    total_time,
    spacing,
    CBT,
    device,
    grid)
    
    return best_position_l, best_loss_l, best_position_r, best_loss_r, total_time

import time
import numpy as np
import SimpleITK as sitk
import torch
from scipy.optimize import differential_evolution
from scipy.optimize import minimize
from imaging.orientation import azimuth_rotation, analyze_vertebral_tilt_contour
from config.constant import ALLOWED_DIAMETERS, ALLOWED_LENGTHS
from core.cylinder import generate_cylinder_n_torch, snap_to_discrete_values, create_coordinate_grid
from core.scoring import compute_overlap_ratio_from_cylinder_mask, is_solution_ok
from config.constant import OVERLAP_THRESH
from visualization.res_plot_3d import res_plt_2_torch

def run_de_torch(
    label_str: str,
    image1_path: str,
    image2_path: str,
    image3_path: str,
    folder: str,
    swarm_size: int,
    max_iter: int,
    spacing: list,
    CBT: bool,
    device: torch.device,
    optimize_size: bool = True,
    grid=None
):
    """
    使用 Differential Evolution (DE) 進行最佳化
    """
    start_time = time.time()

    global image1_array, image2_array, image2_shape, image3_array
    global diameter, length  
    global spine_tensor, cortical_tensor, spine_roi_tensor

    image1 = sitk.ReadImage(image1_path)
    image2 = sitk.ReadImage(image2_path)
    image3 = sitk.ReadImage(image3_path)
    image1_array = sitk.GetArrayFromImage(image1)
    image2_array = sitk.GetArrayFromImage(image2)
    image3_array = sitk.GetArrayFromImage(image3)
    image2_shape = image2_array.shape
    image_shape = image2_shape

    cortical_tensor = torch.from_numpy(image1_array).to(device=device, dtype=torch.uint8)
    spine_tensor = torch.from_numpy(image2_array).to(device=device, dtype=torch.uint8)
    spine_roi_tensor = torch.from_numpy(image3_array).to(device=device, dtype=torch.uint8)

    # 建立明確的優化上下文（取代舊的跨檔案 global 注入；狀態全部顯式傳入目標函數）
    ctx = OptimizationContext(
        cortical_tensor=cortical_tensor,
        spine_tensor=spine_tensor,
        spine_roi_tensor=spine_roi_tensor,
        image1_array=image1_array,
        image2_array=image2_array,
        image3_array=image3_array,
        image2_shape=image2_shape,
        spacing=spacing,
        device=device,
        grid=grid,
        diameter=diameter if not optimize_size else None,
        length=length if not optimize_size else None,
    )
    objective_fn = make_objective_function(ctx)

    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']

    if CBT == True:    
        z_bounds = (0, image_shape[0] - 1)
        y_bounds = (image_shape[1]/5, image_shape[1]/2 - 1)
        x_bounds_right = (image_shape[2]/2 + image_shape[2]/10, image_shape[2] - 1)
        x_bounds_left  = (0, image_shape[2]/2 - image_shape[2]/10 - 1)
        # CBT 參數依據 references/：Santoni 2009 (Spine J 9:366) 冠狀面 25-30° cranial (caudo-cephalad)、
        # 軸狀面自正中線向外 (medial→lateral) ≤30°；Delgado-Fernandez 2017 (ASJ 11:817)、Kim 2022 (SSRR 6:1)
        azimuth_bounds_l = ((98-azi), (120-azi))
        azimuth_bounds_r = ((60-azi), (82-azi))
        altitude_bounds = ((60-alt), (70-alt))
    else:      
        z_bounds = (0, image_shape[0] - 1)   
        y_bounds = (image_shape[1]/5, image_shape[1]/2 - 1)   
        x_bounds_left = (0, image_shape[2]/2 - image_shape[2]/10 - 1)   
        x_bounds_right = (image_shape[2]/2 + image_shape[2]/10, image_shape[2] - 1)
        azimuth_bounds_l = (60-azi, 90-azi) 
        azimuth_bounds_r = (90-azi, 120-azi) 
        altitude_bounds = (65-alt, 80-alt)

    def eval_overlap_from_position(pos, side: str, optimize_size: bool, spine_tensor: torch.Tensor, image_shape, spacing):
        if optimize_size:
            # d, L = snap_to_discrete_values(pos[5], pos[6])
            d, L = snap_to_discrete_values_xfr(pos[5], pos[6])
            params_5 = pos[:5]
        else:
            d, L = diameter, length
            params_5 = pos

        cyl_mask = generate_cylinder_n_torch(
            d, L, params_5[0], params_5[1], params_5[2], params_5[3], params_5[4],
            image_shape, spacing, device, grid
        )
        overlap = compute_overlap_ratio_from_cylinder_mask(cyl_mask, spine_tensor)
        return overlap, d, L

    if optimize_size:
        print("=== DE 最佳化模式：最佳化位置、角度、直徑和長度 ===")
        diameter_bounds = (min(ALLOWED_DIAMETERS), max(ALLOWED_DIAMETERS))
        length_bounds = (min(ALLOWED_LENGTHS), max(ALLOWED_LENGTHS))
        
        bounds_l = [z_bounds, y_bounds, x_bounds_left, azimuth_bounds_l, altitude_bounds, diameter_bounds, length_bounds]
        bounds_r = [z_bounds, y_bounds, x_bounds_right, azimuth_bounds_r, altitude_bounds, diameter_bounds, length_bounds]
    else:
        print("=== DE 固定尺寸模式：最佳化位置和角度 ===")
        diameter = 4.5  
        length = 45     
        
        bounds_l = [z_bounds, y_bounds, x_bounds_left, azimuth_bounds_l, altitude_bounds]
        bounds_r = [z_bounds, y_bounds, x_bounds_right, azimuth_bounds_r, altitude_bounds]

    # DE 的 popsize 實際粒子數 = popsize * len(bounds)
    # 為了跟 PSO 公平比較，我們讓它轉換一下
    de_popsize = max(1, swarm_size // len(bounds_l))

    # --- 左側最佳化 ---
    print("\n=== 左側 (DE) ===")
    res_l = differential_evolution(objective_fn, bounds_l, popsize=de_popsize, maxiter=max_iter)
    position_l, loss_l = res_l.x, res_l.fun

    overlap_l, diameter_l, length_l = eval_overlap_from_position(position_l, "L", optimize_size, spine_tensor, image_shape, spacing)
    best_position_l = list(position_l[:5]) + [diameter_l, length_l] if optimize_size else list(position_l)
    best_loss_l, best_overlap_l = loss_l, overlap_l
    """
    retries = 0
    while (best_loss_l > 0 or best_overlap_l < OVERLAP_THRESH) and retries < 10:
        res_l = differential_evolution(objective_fn, bounds_l, popsize=de_popsize, maxiter=max_iter)
        position_l, loss_l = res_l.x, res_l.fun
        overlap_l, diameter_l, length_l = eval_overlap_from_position(position_l, "L", optimize_size, spine_tensor, image_shape, spacing)
        
        candidate_pos = (list(position_l[:5]) + [diameter_l, length_l]) if optimize_size else list(position_l)
        if is_solution_ok(loss_l, overlap_l, OVERLAP_THRESH) and (not is_solution_ok(best_loss_l, best_overlap_l, OVERLAP_THRESH) or loss_l < best_loss_l):
            best_position_l, best_loss_l, best_overlap_l = candidate_pos, loss_l, overlap_l
        elif (not is_solution_ok(best_loss_l, best_overlap_l, OVERLAP_THRESH)) and (loss_l < best_loss_l):
            best_position_l, best_loss_l, best_overlap_l = candidate_pos, loss_l, overlap_l
        retries += 1
    """
    # --- 右側最佳化 ---
    print("\n=== 右側 (DE) ===")
    res_r = differential_evolution(objective_fn, bounds_r, popsize=de_popsize, maxiter=max_iter)
    position_r, loss_r = res_r.x, res_r.fun

    overlap_r, diameter_r, length_r = eval_overlap_from_position(position_r, "R", optimize_size, spine_tensor, image_shape, spacing)
    best_position_r = list(position_r[:5]) + [diameter_r, length_r] if optimize_size else list(position_r)
    best_loss_r, best_overlap_r = loss_r, overlap_r
    """
    retries = 0
    while (best_loss_r > 0 or best_overlap_r < OVERLAP_THRESH) and retries < 10:
        res_r = differential_evolution(objective_fn, bounds_r, popsize=de_popsize, maxiter=max_iter)
        position_r, loss_r = res_r.x, res_r.fun
        overlap_r, diameter_r, length_r = eval_overlap_from_position(position_r, "R", optimize_size, spine_tensor, image_shape, spacing)
        
        candidate_pos = (list(position_r[:5]) + [diameter_r, length_r]) if optimize_size else list(position_r)
        if is_solution_ok(loss_r, overlap_r, OVERLAP_THRESH) and (not is_solution_ok(best_loss_r, best_overlap_r, OVERLAP_THRESH) or loss_r < best_loss_r):
            best_position_r, best_loss_r, best_overlap_r = candidate_pos, loss_r, overlap_r
        elif (not is_solution_ok(best_loss_r, best_overlap_r, OVERLAP_THRESH)) and (loss_r < best_loss_r):
            best_position_r, best_loss_r, best_overlap_r = candidate_pos, loss_r, overlap_r
        retries += 1
    """
    total_time = time.time() - start_time
    
    final_diameter_l = best_position_l[5] if optimize_size else diameter
    final_length_l = best_position_l[6] if optimize_size else length
    final_diameter_r = best_position_r[5] if optimize_size else diameter
    final_length_r = best_position_r[6] if optimize_size else length

    res_plt_2_torch(
        spine_tensor, cortical_tensor, image_shape, image2_path, 'Output', label_str,
        final_diameter_l, final_length_l, final_diameter_r, final_length_r,
        best_position_l, best_position_r, swarm_size, max_iter, total_time, spacing, CBT, device, grid
    )
    
    return best_position_l, best_loss_l, best_position_r, best_loss_r, total_time

def run_nm_torch(
    label_str: str,
    image1_path: str,
    image2_path: str,
    image3_path: str,
    folder: str,
    swarm_size: int, # NM 不用 swarm_size，但保留參數以維持介面統一
    max_iter: int,
    spacing: list,
    CBT: bool,
    device: torch.device,
    optimize_size: bool = True,
    grid=None
):
    """
    使用 Nelder-Mead 進行最佳化
    """
    start_time = time.time()

    global image1_array, image2_array, image2_shape, image3_array
    global diameter, length  
    global spine_tensor, cortical_tensor, spine_roi_tensor

    image1 = sitk.ReadImage(image1_path)
    image2 = sitk.ReadImage(image2_path)
    image3 = sitk.ReadImage(image3_path)
    image1_array = sitk.GetArrayFromImage(image1)
    image2_array = sitk.GetArrayFromImage(image2)
    image3_array = sitk.GetArrayFromImage(image3)
    image2_shape = image2_array.shape
    image_shape = image2_shape

    cortical_tensor = torch.from_numpy(image1_array).to(device=device, dtype=torch.uint8)
    spine_tensor = torch.from_numpy(image2_array).to(device=device, dtype=torch.uint8)
    spine_roi_tensor = torch.from_numpy(image3_array).to(device=device, dtype=torch.uint8)
    # 建立明確的優化上下文（取代舊的跨檔案 global 注入；狀態全部顯式傳入目標函數）
    ctx = OptimizationContext(
        cortical_tensor=cortical_tensor,
        spine_tensor=spine_tensor,
        spine_roi_tensor=spine_roi_tensor,
        image1_array=image1_array,
        image2_array=image2_array,
        image3_array=image3_array,
        image2_shape=image2_shape,
        spacing=spacing,
        device=device,
        grid=grid,
        diameter=diameter if not optimize_size else None,
        length=length if not optimize_size else None,
    )
    objective_fn = make_objective_function(ctx)

    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']

    if CBT == True:    
        z_bounds = (0, image_shape[0] - 1)
        y_bounds = (image_shape[1]/5, image_shape[1]/2 - 1)
        x_bounds_right = (image_shape[2]/2 + image_shape[2]/10, image_shape[2] - 1)
        x_bounds_left  = (0, image_shape[2]/2 - image_shape[2]/10 - 1)
        # CBT 參數依據 references/：Santoni 2009 (Spine J 9:366) 冠狀面 25-30° cranial (caudo-cephalad)、
        # 軸狀面自正中線向外 (medial→lateral) ≤30°；Delgado-Fernandez 2017 (ASJ 11:817)、Kim 2022 (SSRR 6:1)
        azimuth_bounds_l = ((98-azi), (120-azi))
        azimuth_bounds_r = ((60-azi), (82-azi))
        altitude_bounds = ((60-alt), (70-alt))
    else:      
        z_bounds = (0, image_shape[0] - 1)   
        y_bounds = (image_shape[1]/5, image_shape[1]/2 - 1)   
        x_bounds_left = (0, image_shape[2]/2 - image_shape[2]/10 - 1)   
        x_bounds_right = (image_shape[2]/2 + image_shape[2]/10, image_shape[2] - 1)
        azimuth_bounds_l = (60-azi, 90-azi) 
        azimuth_bounds_r = (90-azi, 120-azi) 
        altitude_bounds = (65-alt, 80-alt)

    def eval_overlap_from_position(pos, side: str, optimize_size: bool, spine_tensor: torch.Tensor, image_shape, spacing):
        if optimize_size:
            # d, L = snap_to_discrete_values(pos[5], pos[6])
            d, L = snap_to_discrete_values_xfr(pos[5], pos[6])
            params_5 = pos[:5]
        else:
            d, L = diameter, length
            params_5 = pos

        cyl_mask = generate_cylinder_n_torch(
            d, L, params_5[0], params_5[1], params_5[2], params_5[3], params_5[4],
            image_shape, spacing, device, grid
        )
        overlap = compute_overlap_ratio_from_cylinder_mask(cyl_mask, spine_tensor)
        return overlap, d, L

    if optimize_size:
        print("=== NM 最佳化模式 ===")
        bounds_l = [z_bounds, y_bounds, x_bounds_left, azimuth_bounds_l, altitude_bounds, 
                    (min(ALLOWED_DIAMETERS), max(ALLOWED_DIAMETERS)), (min(ALLOWED_LENGTHS), max(ALLOWED_LENGTHS))]
        bounds_r = [z_bounds, y_bounds, x_bounds_right, azimuth_bounds_r, altitude_bounds, 
                    (min(ALLOWED_DIAMETERS), max(ALLOWED_DIAMETERS)), (min(ALLOWED_LENGTHS), max(ALLOWED_LENGTHS))]
    else:
        print("=== NM 固定尺寸模式 ===")
        diameter, length = 4.5, 45     
        bounds_l = [z_bounds, y_bounds, x_bounds_left, azimuth_bounds_l, altitude_bounds]
        bounds_r = [z_bounds, y_bounds, x_bounds_right, azimuth_bounds_r, altitude_bounds]

    def get_random_x0(bounds):
        # 產生在 Bounds 內的隨機起始點
        return [np.random.uniform(b[0], b[1]) for b in bounds]

    # --- 左側最佳化 ---
    print("\n=== 左側 (Nelder-Mead) ===")
    x0_l = get_random_x0(bounds_l)
    res_l = minimize(objective_fn, x0_l, method='Nelder-Mead', bounds=bounds_l, options={'maxiter': max_iter})
    position_l, loss_l = res_l.x, res_l.fun

    overlap_l, diameter_l, length_l = eval_overlap_from_position(position_l, "L", optimize_size, spine_tensor, image_shape, spacing)
    best_position_l = list(position_l[:5]) + [diameter_l, length_l] if optimize_size else list(position_l)
    best_loss_l, best_overlap_l = loss_l, overlap_l
    
    retries = 0
    while (best_loss_l > 0 or best_overlap_l < OVERLAP_THRESH) and retries < 10:
        x0_l = get_random_x0(bounds_l) # 每次 retry 都換一個隨機起始點
        res_l = minimize(objective_fn, x0_l, method='Nelder-Mead', bounds=bounds_l, options={'maxiter': max_iter})
        position_l, loss_l = res_l.x, res_l.fun
        overlap_l, diameter_l, length_l = eval_overlap_from_position(position_l, "L", optimize_size, spine_tensor, image_shape, spacing)
        
        candidate_pos = (list(position_l[:5]) + [diameter_l, length_l]) if optimize_size else list(position_l)
        if is_solution_ok(loss_l, overlap_l, OVERLAP_THRESH) and (not is_solution_ok(best_loss_l, best_overlap_l, OVERLAP_THRESH) or loss_l < best_loss_l):
            best_position_l, best_loss_l, best_overlap_l = candidate_pos, loss_l, overlap_l
        elif (not is_solution_ok(best_loss_l, best_overlap_l, OVERLAP_THRESH)) and (loss_l < best_loss_l):
            best_position_l, best_loss_l, best_overlap_l = candidate_pos, loss_l, overlap_l
        retries += 1

    # --- 右側最佳化 ---
    print("\n=== 右側 (Nelder-Mead) ===")
    x0_r = get_random_x0(bounds_r)
    res_r = minimize(objective_fn, x0_r, method='Nelder-Mead', bounds=bounds_r, options={'maxiter': max_iter})
    position_r, loss_r = res_r.x, res_r.fun

    overlap_r, diameter_r, length_r = eval_overlap_from_position(position_r, "R", optimize_size, spine_tensor, image_shape, spacing)
    best_position_r = list(position_r[:5]) + [diameter_r, length_r] if optimize_size else list(position_r)
    best_loss_r, best_overlap_r = loss_r, overlap_r

    retries = 0
    while (best_loss_r > 0 or best_overlap_r < OVERLAP_THRESH) and retries < 10:
        x0_r = get_random_x0(bounds_r)
        res_r = minimize(objective_fn, x0_r, method='Nelder-Mead', bounds=bounds_r, options={'maxiter': max_iter})
        position_r, loss_r = res_r.x, res_r.fun
        overlap_r, diameter_r, length_r = eval_overlap_from_position(position_r, "R", optimize_size, spine_tensor, image_shape, spacing)
        
        candidate_pos = (list(position_r[:5]) + [diameter_r, length_r]) if optimize_size else list(position_r)
        if is_solution_ok(loss_r, overlap_r, OVERLAP_THRESH) and (not is_solution_ok(best_loss_r, best_overlap_r, OVERLAP_THRESH) or loss_r < best_loss_r):
            best_position_r, best_loss_r, best_overlap_r = candidate_pos, loss_r, overlap_r
        elif (not is_solution_ok(best_loss_r, best_overlap_r, OVERLAP_THRESH)) and (loss_r < best_loss_r):
            best_position_r, best_loss_r, best_overlap_r = candidate_pos, loss_r, overlap_r
        retries += 1

    total_time = time.time() - start_time
    
    final_diameter_l = best_position_l[5] if optimize_size else diameter
    final_length_l = best_position_l[6] if optimize_size else length
    final_diameter_r = best_position_r[5] if optimize_size else diameter
    final_length_r = best_position_r[6] if optimize_size else length

    res_plt_2_torch(
        spine_tensor, cortical_tensor, image_shape, image2_path, 'Output', label_str,
        final_diameter_l, final_length_l, final_diameter_r, final_length_r,
        best_position_l, best_position_r, swarm_size, max_iter, total_time, spacing, CBT, device, grid
    )
    
    return best_position_l, best_loss_l, best_position_r, best_loss_r, total_time