kemm_dmoea_core.py

"""
===============================================================================
kemm_dmoea_core.py
KEMM-DMOEA 核心算法模块 + 船舶多目标动态路径规划 (完整高性能版)
===============================================================================
【文件职责】
  - 定义所有数据结构（ShipParams, AlgorithmParams, Obstacle, OceanCurrent）
  - 实现 KPO 运动学投影
  - 实现 EFF 环境场记忆（改进自 MMTL-DMOEA 的 Process 1 记忆机制）
  - 实现流形迁移学习 ManifoldTransfer（改进自 Process 3 的 SGF 方法）
  - 实现 FindBestSol（改进自 Process 2 的 SVR 估计 + 非支配选择）
  - 实现多目标评价器 MultiObjectiveEvaluator（4目标）
  - 实现进化算子 EvolutionaryOperators（NSGA-II 框架）
  - 实现 KEMM-DMOEA 核心引擎（完整 Process 1 流程）
  - 实现动态环境建模 DynamicEnvironment
  - 实现可视化模块 Visualizer
  - 提供独立运行的船舶规划主程序

【论文来源】(知识库: MMTL-DMOEA, IEEE TCYB, 2020)
  原文摘要:
    "combines the mechanism of memory to preserve the best individuals
     from the past with the feature of manifold TL to predict the optimal
     individuals at the new instance during the evolution"
  
  核心流程 (Process 1, lines 4-15):
    1. 检测环境变化
    2. FindBestSol: SVR估计 + 非支配排序选择精英 (Process 2)
    3. Transfer: LPCA聚类 + SGF测地流 + 内点法映射 (Process 3)
    4. 合并 LastBestSol ∪ TransSol → 初始种群
    5. SMOA 进化
    6. 记忆存储 (lines 10-14)

  本文 KEMM-DMOEA 改进点:
    1. KPO 运动学约束投影（面向船舶非完整约束）
    2. APF 环境指纹记忆（替代简单FIFO队列，实现跨时刻相似度检索）
    3. 自适应环境变化检测与响应比例调整
    4. 多源加权流形迁移（从多个相似历史环境加权迁移）

  参数设置 (论文 Section IV-A):
    "N=100; C=10×N; ns=30; L=4; p=5; nt=10; τt=10"

【运行方式】
  python kemm_dmoea_core.py
===============================================================================
"""

import numpy as np
import copy
import warnings
import time as time_module
from dataclasses import dataclass, field
from typing import List, Tuple, Optional, Dict
from scipy.spatial.distance import cdist

try:
    from sklearn.decomposition import PCA
    from sklearn.svm import SVR as SklearnSVR
    HAS_SKLEARN = True
except ImportError:
    HAS_SKLEARN = False

try:
    import matplotlib
    matplotlib.use('Agg')
    import matplotlib.pyplot as plt
    from matplotlib.patches import Circle
    HAS_MPL = True
except ImportError:
    HAS_MPL = False

warnings.filterwarnings('ignore')


# ╔═══════════════════════════════════════════════════════════════════════════╗
#  第 1 部分：全局配置与数据结构
# ╚═══════════════════════════════════════════════════════════════════════════╝

@dataclass
class ShipParams:
    """
    船舶物理参数
    ─────────────────────────────────────────
    r_min            : 最小转弯半径 (m)
    v_max            : 最大航速 (m/s)
    v_min            : 最小航速 (m/s)
    delta_rudder_max : 最大舵角变化率 (rad/s)
    ship_length      : 船舶长度 (m)
    fuel_coeff       : 燃油消耗系数 (kg/(m·(m/s)²))
    """
    r_min: float = 50.0
    v_max: float = 8.0
    v_min: float = 1.0
    delta_rudder_max: float = 0.05
    ship_length: float = 30.0
    fuel_coeff: float = 0.001


@dataclass
class AlgorithmParams:
    """
    算法超参数
    ─────────────────────────────────────────
    来源: 论文 Section IV-A
      "N=100; C=10×N; ns=30; L=4; p=5"
    """
    pop_size: int = 100
    n_waypoints: int = 12
    max_gen: int = 80
    crossover_prob: float = 0.9
    mutation_prob: float = 0.3
    eta_c: float = 20.0
    eta_m: float = 20.0
    # 论文 "external storage size C is set to 10 × N"
    memory_capacity: int = 30
    # 论文 "ns in FindBestSol is 30"
    n_svr_samples: int = 30
    # 论文 "L in Transfer is 4"
    n_clusters: int = 4
    # 论文 "p in Transfer is 5"
    n_subspaces: int = 5
    transfer_ratio: float = 0.5
    reinit_ratio_min: float = 0.1
    reinit_ratio_max: float = 0.6
    change_threshold: float = 5.0
    n_objectives: int = 4


@dataclass
class Obstacle:
    """障碍物（可移动），如岛礁或其他船舶"""
    position: np.ndarray
    radius: float = 15.0
    velocity: np.ndarray = field(default_factory=lambda: np.zeros(2))

    def predict_position(self, dt: float) -> np.ndarray:
        return self.position + self.velocity * dt

    def update(self, dt: float, bounds: Tuple[float, float] = (0, 500)):
        self.position = self.position + self.velocity * dt
        for dim in range(2):
            if self.position[dim] < bounds[0] + self.radius:
                self.position[dim] = bounds[0] + self.radius
                self.velocity[dim] *= -1
            elif self.position[dim] > bounds[1] - self.radius:
                self.position[dim] = bounds[1] - self.radius
                self.velocity[dim] *= -1


@dataclass
class OceanCurrent:
    """洋流场模型 — 高斯涡旋叠加 + 全局漂流"""
    centers: List[np.ndarray] = field(default_factory=list)
    strengths: List[float] = field(default_factory=list)
    sigmas: List[float] = field(default_factory=list)
    drift: np.ndarray = field(default_factory=lambda: np.zeros(2))

    def get_current(self, point: np.ndarray) -> np.ndarray:
        v = self.drift.copy()
        for c, s, sigma in zip(self.centers, self.strengths, self.sigmas):
            diff = point - c
            r = np.linalg.norm(diff) + 1e-8
            factor = s * np.exp(-r ** 2 / (2 * sigma ** 2))
            v += factor * np.array([-diff[1], diff[0]]) / r
        return v

    def get_current_batch(self, points: np.ndarray) -> np.ndarray:
        """向量化批量计算洋流"""
        v = np.tile(self.drift, (len(points), 1))  # (N, 2)
        for c, s, sigma in zip(self.centers, self.strengths, self.sigmas):
            diff = points - c  # (N, 2)
            r = np.linalg.norm(diff, axis=1, keepdims=True) + 1e-8  # (N, 1)
            factor = s * np.exp(-r ** 2 / (2 * sigma ** 2))  # (N, 1)
            rot = np.column_stack([-diff[:, 1], diff[:, 0]])  # (N, 2)
            v += factor * rot / r
        return v


# ╔═══════════════════════════════════════════════════════════════════════════╗
#  第 2 部分：动态环境建模
# ╚═══════════════════════════════════════════════════════════════════════════╝

class DynamicEnvironment:
    """管理动态海洋环境：障碍物、洋流、边界"""

    def __init__(self, bounds: Tuple[float, float] = (0, 500)):
        self.bounds = bounds
        self.obstacles: List[Obstacle] = []
        self.current_field = OceanCurrent()
        self.start = np.array([20.0, 20.0])
        self.goal = np.array([480.0, 480.0])
        self.time = 0.0
        # 缓存
        self._obs_pos_cache = None
        self._obs_rad_cache = None
        self._cache_valid = False

    def add_obstacle(self, pos, radius=15.0, vel=None):
        vel = vel if vel is not None else np.zeros(2)
        self.obstacles.append(
            Obstacle(np.array(pos, dtype=float), radius, np.array(vel, dtype=float)))
        self._cache_valid = False

    def setup_currents(self, centers, strengths, sigmas, drift=None):
        self.current_field = OceanCurrent(
            centers=[np.array(c, dtype=float) for c in centers],
            strengths=strengths, sigmas=sigmas,
            drift=np.array(drift, dtype=float) if drift is not None else np.zeros(2))

    def step(self, dt: float):
        self.time += dt
        for obs in self.obstacles:
            obs.update(dt, self.bounds)
        self._cache_valid = False

    def _update_cache(self):
        if not self._cache_valid:
            if self.obstacles:
                self._obs_pos_cache = np.array([o.position for o in self.obstacles])
                self._obs_rad_cache = np.array([o.radius for o in self.obstacles])
            else:
                self._obs_pos_cache = np.empty((0, 2))
                self._obs_rad_cache = np.empty(0)
            self._cache_valid = True

    def get_obstacle_positions(self) -> np.ndarray:
        self._update_cache()
        return self._obs_pos_cache

    def get_obstacle_radii(self) -> np.ndarray:
        self._update_cache()
        return self._obs_rad_cache


# ╔═══════════════════════════════════════════════════════════════════════════╗
#  第 3 部分：运动学约束投影 (KPO)
# ╚═══════════════════════════════════════════════════════════════════════════╝

class KinematicProjectionOperator:
    """
    将任意航路点序列投影到运动学可行空间
    ─────────────────────────────────────────
    (a) 最大/最小航速约束
    (b) 最小转弯半径约束 (曲率 ≤ 1/R_min)
    支持批量处理 apply_batch
    """

    def __init__(self, ship: ShipParams, delta_t: float = 10.0):
        self.ship = ship
        self.delta_t = delta_t
        self.max_curvature = 1.0 / ship.r_min
        self.max_step = ship.v_max * delta_t
        self.min_step = ship.v_min * delta_t

    def apply(self, path: np.ndarray, n_iterations: int = 3) -> np.ndarray:
        """单条路径的 KPO 投影"""
        fp = np.copy(path)
        n = len(fp)
        if n < 3:
            return fp
        for _ in range(n_iterations):
            for i in range(1, n):
                fp[i] = self._project_step(fp[i - 1], fp[i],
                                           fp[i - 2] if i >= 2 else None)
            for i in range(n - 2, 0, -1):
                fp[i] = self._project_step(fp[i + 1], fp[i],
                                           fp[i + 2] if i <= n - 3 else None)
        return fp

    def apply_batch(self, population: np.ndarray, n_iterations: int = 2) -> np.ndarray:
        """
        批量 KPO 投影 — 向量化加速
        population: (pop_size, n_pts, 2)
        """
        pop = population.copy()
        N, n_pts, _ = pop.shape
        if n_pts < 3:
            return pop

        for _ in range(n_iterations):
            # 前向扫描
            for i in range(1, n_pts):
                diff = pop[:, i] - pop[:, i - 1]  # (N, 2)
                dist = np.linalg.norm(diff, axis=1, keepdims=True)  # (N, 1)
                dist_safe = np.maximum(dist, 1e-8)
                direction = diff / dist_safe

                # 速度约束
                dist_clamped = np.clip(dist, self.min_step, self.max_step)
                pop[:, i] = pop[:, i - 1] + direction * dist_clamped

                # 曲率约束 (需要前前点)
                if i >= 2:
                    v_in = pop[:, i - 1] - pop[:, i - 2]  # (N, 2)
                    v_out = pop[:, i] - pop[:, i - 1]
                    len_in = np.linalg.norm(v_in, axis=1)
                    len_out = np.linalg.norm(v_out, axis=1)
                    valid = (len_in > 1e-6) & (len_out > 1e-6)
                    if np.any(valid):
                        a_in = np.arctan2(v_in[valid, 1], v_in[valid, 0])
                        a_out = np.arctan2(v_out[valid, 1], v_out[valid, 0])
                        dpsi = np.arctan2(np.sin(a_out - a_in), np.cos(a_out - a_in))
                        max_d = self.max_curvature * len_out[valid]
                        exceed = np.abs(dpsi) > max_d
                        if np.any(exceed):
                            new_a = a_in[exceed] + np.sign(dpsi[exceed]) * max_d[exceed]
                            lo = len_out[valid][exceed]
                            new_pts = pop[valid][exceed, i - 1] + np.column_stack(
                                [lo * np.cos(new_a), lo * np.sin(new_a)])
                            idx_valid = np.where(valid)[0][exceed]
                            pop[idx_valid, i] = new_pts

            # 后向扫描 (简化: 仅速度约束)
            for i in range(n_pts - 2, 0, -1):
                diff = pop[:, i] - pop[:, i + 1]
                dist = np.linalg.norm(diff, axis=1, keepdims=True)
                dist_safe = np.maximum(dist, 1e-8)
                direction = diff / dist_safe
                dist_clamped = np.clip(dist, self.min_step, self.max_step)
                pop[:, i] = pop[:, i + 1] + direction * dist_clamped

        return pop

    def calculate_penalty_batch(self, paths: np.ndarray) -> np.ndarray:
        """批量计算运动学惩罚 — 向量化"""
        N, n_pts, _ = paths.shape
        penalty = np.zeros(N)

        # 速度约束
        diffs = np.diff(paths, axis=1)  # (N, n_pts-1, 2)
        speeds = np.linalg.norm(diffs, axis=2) / self.delta_t  # (N, n_pts-1)
        over = np.maximum(speeds - self.ship.v_max, 0)
        under = np.maximum(self.ship.v_min - speeds, 0)
        penalty += np.sum(over ** 2 + under ** 2, axis=1)

        # 曲率约束
        if n_pts >= 3:
            v1 = diffs[:, :-1]  # (N, n_pts-2, 2)
            v2 = diffs[:, 1:]   # (N, n_pts-2, 2)
            l1 = np.linalg.norm(v1, axis=2)
            l2 = np.linalg.norm(v2, axis=2)
            a1 = np.arctan2(v1[:, :, 1], v1[:, :, 0])
            a2 = np.arctan2(v2[:, :, 1], v2[:, :, 0])
            dpsi = np.abs(np.arctan2(np.sin(a2 - a1), np.cos(a2 - a1)))
            safe_l2 = np.maximum(l2, 1e-8)
            curv = dpsi / safe_l2
            exceed = np.maximum(curv - self.max_curvature, 0)
            penalty += np.sum(exceed ** 2 * 100, axis=1)

        return penalty


# ╔═══════════════════════════════════════════════════════════════════════════╗
#  第 4 部分：环境场记忆 (EFF) — 改进的 Process 1 记忆机制
# ╚═══════════════════════════════════════════════════════════════════════════╝

class EnvironmentFieldMemory:
    """
    APF 指纹记忆库
    ─────────────────────────────────────────
    来源: 论文 Process 1, lines 10-14:
      "When the external memory overflows, the algorithm replaces
       the earliest stored individuals with the newly generated ones."
      "external storage size C is set to 10 × N"
    
    本算法改进:
      使用 APF 势场统计指纹进行相似度检索（12维），
      而非简单 FIFO 队列，实现更精准的跨时刻知识匹配。
      论文原文仅按时间先后替换，我们按指纹相似度检索。
    """

    def __init__(self, capacity: int = 30, grid_res: int = 30,
                 bounds: Tuple[float, float] = (0, 500)):
        self.capacity = capacity
        self.grid_res = grid_res
        self.bounds = bounds
        self.memory: List[Dict] = []
        self._age = 0
        # 预计算网格
        lo, hi = bounds
        x = np.linspace(lo, hi, grid_res)
        y = np.linspace(lo, hi, grid_res)
        self._X, self._Y = np.meshgrid(x, y)

    def compute_fingerprint(self, env: DynamicEnvironment) -> np.ndarray:
        """计算 12 维 APF 势场统计指纹"""
        X, Y = self._X, self._Y

        # 引力场
        U_att = 0.5 * np.sqrt((X - env.goal[0]) ** 2 + (Y - env.goal[1]) ** 2)

        # 斥力场
        U_rep = np.zeros_like(X)
        obs_pos = env.get_obstacle_positions()
        obs_rad = env.get_obstacle_radii()
        for pos, rad in zip(obs_pos, obs_rad):
            dist = np.sqrt((X - pos[0]) ** 2 + (Y - pos[1]) ** 2) + 1e-8
            mask = dist < rad * 5
            U_rep[mask] += (rad * 100.0) / dist[mask]

        U = U_att + U_rep
        gy, gx = np.gradient(U)
        grad_mag = np.sqrt(gx ** 2 + gy ** 2)

        n_obs = len(obs_pos)
        if n_obs > 0:
            d2g = float(np.mean(np.linalg.norm(obs_pos - env.goal, axis=1)))
            if n_obs > 1:
                inter = cdist(obs_pos, obs_pos)
                np.fill_diagonal(inter, np.inf)
                m_inter = float(np.mean(np.min(inter, axis=1)))
            else:
                m_inter = 0.0
        else:
            d2g, m_inter = 0.0, 0.0

        flat = U.ravel()
        m = np.mean(flat)
        s = np.std(flat) + 1e-12
        skew = float(np.mean(((flat - m) / s) ** 3))

        fp = np.array([
            np.mean(U), np.std(U), np.max(U), np.min(U), np.median(U), skew,
            np.mean(grad_mag), np.std(grad_mag),
            float(n_obs), d2g, m_inter, 0.0
        ])
        return fp

    def store(self, fp, solutions, fitness):
        """
        存储到记忆库
        来源: Process 1, lines 10-14
          "if size(P ∪ Solutionst) ≤ C then Store;
           else Replace the earliest stored individuals"
        """
        self._age += 1
        self.memory.append({
            'fingerprint': fp.copy(),
            'solutions': solutions.copy(),
            'fitness': fitness.copy(),
            'age': self._age
        })
        # 溢出时替换最早的 (论文原文策略)
        if len(self.memory) > self.capacity:
            self.memory.pop(0)

    def retrieve(self, query_fp, top_k=3):
        """
        按指纹相似度检索 (本文改进: 非FIFO, 而是相似度排序)
        """
        if not self.memory:
            return None
        fps = np.array([m['fingerprint'] for m in self.memory])
        all_fp = np.vstack([fps, query_fp.reshape(1, -1)])
        mu = all_fp.mean(0) + 1e-12
        sig = all_fp.std(0) + 1e-12
        dists = cdist(((query_fp - mu) / sig).reshape(1, -1),
                      (fps - mu) / sig, 'euclidean').ravel()
        k = min(top_k, len(dists))
        return [self.memory[i] for i in np.argsort(dists)[:k]]

    def compute_change_magnitude(self, fp_old, fp_new):
        return float(np.mean(np.abs(fp_new - fp_old) / (np.abs(fp_old) + 1e-12)))


# ╔═══════════════════════════════════════════════════════════════════════════╗
#  第 5 部分：FindBestSol — 改进的 Process 2
# ╚═══════════════════════════════════════════════════════════════════════════╝

class FindBestSol:
    """
    从外部记忆中选择精英个体
    ─────────────────────────────────────────
    来源: 论文 Process 2
      "1: Uniformly sample ns solutions XT from the decision space;
       2: Call a SVR to construct the estimator E with {XT, F(XT, t)};
       3: Estimate objectives of P: Y = E(P);
       4: Find non-dominated solutions LastBestSol in Y;
       5-10: 调整 LastBestSol 大小至 N/2"
    
    计算复杂度 (论文):
      "constructing the SVR estimator with ns samples needs O(ns²d);
       Using SVR to estimate consumes O(N²d);
       fast nondominated sorting is O(N²m)"
    
    本文改进:
      - 对船舶路径问题，SVR 在展平的路径空间上构建
      - 支持多目标 SVR (每个目标一个 SVR)
    """

    def __init__(self, evaluator, n_samples: int = 30):
        self.evaluator = evaluator
        self.n_samples = n_samples

    def select(self, memory_solutions: np.ndarray, memory_fitness: np.ndarray,
               target_size: int, env_evaluator=None) -> Tuple[np.ndarray, np.ndarray]:
        """
        实现 Process 2 的完整流程
        
        Args:
            memory_solutions: 记忆中的所有解 (N_mem, n_pts, 2)
            memory_fitness: 记忆中的适应度 (N_mem, n_obj)
            target_size: 目标选出个体数 (论文中为 N/2)
            env_evaluator: 当前环境评价器 (用于构建SVR)
        
        Returns:
            selected_solutions, selected_fitness
        """
        N_mem = len(memory_solutions)

        if env_evaluator is not None and HAS_SKLEARN and N_mem > self.n_samples:
            # ── Process 2, Step 1-3: SVR 估计 ──
            # "Uniformly sample ns solutions XT from the decision space"
            sample_idx = np.random.choice(N_mem, min(self.n_samples, N_mem), replace=False)
            X_train = memory_solutions[sample_idx]
            # 重新在当前环境评价
            Y_train = env_evaluator.evaluate_population(X_train)

            # 展平路径用于 SVR
            X_flat = memory_solutions.reshape(N_mem, -1)
            X_train_flat = X_train.reshape(len(X_train), -1)

            # 为每个目标构建 SVR
            n_obj = Y_train.shape[1]
            Y_estimated = np.zeros((N_mem, n_obj))
            for obj_i in range(n_obj):
                try:
                    svr = SklearnSVR(kernel='rbf', C=1.0, epsilon=0.1, max_iter=500)
                    svr.fit(X_train_flat, Y_train[:, obj_i])
                    Y_estimated[:, obj_i] = svr.predict(X_flat)
                except Exception:
                    Y_estimated[:, obj_i] = memory_fitness[:, obj_i]
        else:
            # 无 SVR 时直接使用历史适应度
            Y_estimated = memory_fitness.copy()

        # ── Process 2, Step 4: 非支配排序 ──
        fronts = self._fast_nds(Y_estimated)
        nd_idx = fronts[0] if fronts else list(range(min(target_size, N_mem)))

        # ── Process 2, Steps 5-10: 调整大小 ──
        if len(nd_idx) > target_size:
            # "Delete individual in LastBestSol" — 按拥挤度删除
            cd = self._crowding_distance(Y_estimated, nd_idx)
            keep = np.argsort(-cd)[:target_size]
            nd_idx = [nd_idx[k] for k in keep]
        elif len(nd_idx) < target_size:
            # "Add Gaussian noise with individuals in LastBestSol"
            existing = memory_solutions[nd_idx]
            n_add = target_size - len(nd_idx)
            noise_idx = np.random.choice(len(existing), n_add, replace=True)
            noisy = existing[noise_idx].copy()
            noisy[:, 1:-1] += np.random.normal(0, 5.0, noisy[:, 1:-1].shape)
            extra_solutions = noisy
            extra_fitness = Y_estimated[np.array(nd_idx)[noise_idx]]

            sel_sol = np.concatenate([memory_solutions[nd_idx], extra_solutions])
            sel_fit = np.concatenate([Y_estimated[nd_idx], extra_fitness])
            return sel_sol[:target_size], sel_fit[:target_size]

        nd_idx = np.array(nd_idx)
        return memory_solutions[nd_idx], Y_estimated[nd_idx]

    @staticmethod
    def _fast_nds(fitness):
        n = len(fitness)
        if n == 0:
            return []
        F = fitness
        leq = F[:, None, :] <= F[None, :, :]
        lt = F[:, None, :] < F[None, :, :]
        dom_matrix = np.all(leq, axis=2) & np.any(lt, axis=2)
        dom_count = dom_matrix.sum(axis=0).astype(int)
        fronts = []
        remaining = np.ones(n, dtype=bool)
        while np.any(remaining):
            current = np.where(remaining & (dom_count == 0))[0].tolist()
            if not current:
                current = np.where(remaining)[0][:1].tolist()
            fronts.append(current)
            for i in current:
                remaining[i] = False
                dominated = np.where(dom_matrix[i] & remaining)[0]
                dom_count[dominated] -= 1
        return fronts

    @staticmethod
    def _crowding_distance(fitness, front):
        n = len(front)
        if n <= 2:
            return np.full(n, np.inf)
        f = fitness[front]
        dist = np.zeros(n)
        for m in range(f.shape[1]):
            order = np.argsort(f[:, m])
            dist[order[0]] = dist[order[-1]] = np.inf
            rng = f[order[-1], m] - f[order[0], m]
            if rng < 1e-14:
                continue
            dist[order[1:-1]] += (f[order[2:], m] - f[order[:-2], m]) / rng
        return dist


# ╔═══════════════════════════════════════════════════════════════════════════╗
#  第 6 部分：多目标评价器 — 向量化批量计算
# ╚═══════════════════════════════════════════════════════════════════════════╝

class MultiObjectiveEvaluator:
    """
    四目标评价函数 — 向量化
    ─────────────────────────────────────────
    f1: 路径总长度       (最小化)
    f2: 碰撞风险         (最小化)
    f3: 燃油消耗         (最小化)
    f4: 航行平稳性       (最小化) — 累计转角
    + 运动学约束惩罚 + 边界惩罚
    """

    def __init__(self, env: DynamicEnvironment, ship: ShipParams,
                 kpo: KinematicProjectionOperator):
        self.env = env
        self.ship = ship
        self.kpo = kpo
        self.safety_multiplier = 2.0

    def evaluate_population(self, population: np.ndarray) -> np.ndarray:
        """批量评价 (pop_size, n_pts, 2) -> (pop_size, 4)"""
        N, n_pts, _ = population.shape
        obs_pos = self.env.get_obstacle_positions()
        obs_rad = self.env.get_obstacle_radii()
        lo, hi = self.env.bounds

        # 路径段
        diffs = np.diff(population, axis=1)  # (N, n_pts-1, 2)
        seg_lens = np.linalg.norm(diffs, axis=2)  # (N, n_pts-1)
        midpoints = (population[:, :-1] + population[:, 1:]) / 2.0  # (N, n_pts-1, 2)

        # f1: 路径总长度
        f1 = np.sum(seg_lens, axis=1)

        # f2: 碰撞风险
        f2 = np.zeros(N)
        if len(obs_pos) > 0:
            safe = obs_rad * self.safety_multiplier + self.ship.ship_length
            for seg_i in range(n_pts - 1):
                mid = midpoints[:, seg_i, :]  # (N, 2)
                dists = cdist(mid, obs_pos)  # (N, M)
                for oi in range(len(obs_pos)):
                    close = dists[:, oi] < safe[oi]
                    f2[close] += ((safe[oi] - dists[close, oi]) / safe[oi]) ** 2 * 100
                    mid_range = (~close) & (dists[:, oi] < safe[oi] * 2)
                    f2[mid_range] += 1.0 / (dists[mid_range, oi] - safe[oi] + 1.0)

        # f3: 燃油消耗
        f3 = np.zeros(N)
        for seg_i in range(n_pts - 1):
            mid = midpoints[:, seg_i, :]  # (N, 2)
            cv = self.env.current_field.get_current_batch(mid)  # (N, 2)
            seg = diffs[:, seg_i, :]  # (N, 2)
            sl = seg_lens[:, seg_i]  # (N,)
            safe_sl = np.maximum(sl, 1e-8)
            heading = seg / safe_sl[:, None]
            cp = np.sum(cv * heading, axis=1)
            ws = sl / self.kpo.delta_t
            es = np.maximum(ws - cp, self.ship.v_min)
            f3 += self.ship.fuel_coeff * es ** 2 * sl

        # f4: 航行平稳性 (累计转角)
        f4 = np.zeros(N)
        if n_pts >= 3:
            v1 = diffs[:, :-1]  # (N, n_pts-2, 2)
            v2 = diffs[:, 1:]
            a1 = np.arctan2(v1[:, :, 1], v1[:, :, 0])
            a2 = np.arctan2(v2[:, :, 1], v2[:, :, 0])
            turns = np.abs(np.arctan2(np.sin(a2 - a1), np.cos(a2 - a1)))
            f4 = np.sum(turns, axis=1)

        # 运动学惩罚
        pen = self.kpo.calculate_penalty_batch(population) * 10

        # 边界惩罚
        below = np.maximum(lo - population, 0)  # (N, n_pts, 2)
        above = np.maximum(population - hi, 0)
        pen += np.sum((below ** 2 + above ** 2) * 5, axis=(1, 2))

        return np.column_stack([f1 + pen, f2 + pen, f3 + pen, f4 + pen])


# ╔═══════════════════════════════════════════════════════════════════════════╗
#  第 7 部分：进化算子（NSGA-II 框架）— 向量化
# ╚═══════════════════════════════════════════════════════════════════════════╝

class EvolutionaryOperators:
    """NSGA-II 风格的进化算子 — 向量化 SBX/PM"""

    def __init__(self, params: AlgorithmParams,
                 kpo: KinematicProjectionOperator,
                 bounds: Tuple[float, float]):
        self.params = params
        self.kpo = kpo
        self.lo, self.hi = bounds

    @staticmethod
    def fast_non_dominated_sort(fitness: np.ndarray) -> List[List[int]]:
        """向量化非支配排序"""
        n = len(fitness)
        if n == 0:
            return []
        F = fitness
        leq = F[:, None, :] <= F[None, :, :]
        lt = F[:, None, :] < F[None, :, :]
        dom_matrix = np.all(leq, axis=2) & np.any(lt, axis=2)
        dom_count = dom_matrix.sum(axis=0).astype(int)
        fronts = []
        remaining = np.ones(n, dtype=bool)
        while np.any(remaining):
            current = np.where(remaining & (dom_count == 0))[0].tolist()
            if not current:
                current = np.where(remaining)[0][:1].tolist()
            fronts.append(current)
            for i in current:
                remaining[i] = False
                dominated = np.where(dom_matrix[i] & remaining)[0]
                dom_count[dominated] -= 1
        return fronts

    @staticmethod
    def crowding_distance(fitness, front):
        n = len(front)
        if n <= 2:
            return np.full(n, np.inf)
        f = fitness[front]
        dist = np.zeros(n)
        for m in range(f.shape[1]):
            order = np.argsort(f[:, m])
            dist[order[0]] = dist[order[-1]] = np.inf
            rng = f[order[-1], m] - f[order[0], m]
            if rng < 1e-14:
                continue
            dist[order[1:-1]] += (f[order[2:], m] - f[order[:-2], m]) / rng
        return dist

    def environmental_selection(self, pop, fit, size):
        fronts = self.fast_non_dominated_sort(fit)
        sel = []
        for front in fronts:
            if len(sel) + len(front) <= size:
                sel.extend(front)
            else:
                rem = size - len(sel)
                cd = self.crowding_distance(fit, front)
                sel.extend([front[i] for i in np.argsort(-cd)[:rem]])
                break
        idx = np.array(sel[:size])
        return pop[idx], fit[idx]

    def reproduce(self, population, fitness, start, goal):
        """
        生成子代种群 — 向量化 SBX + PM + 批量 KPO
        """
        pop_size, n_pts, _ = population.shape
        lo, hi = self.lo, self.hi
        eta_c = self.params.eta_c
        eta_m = self.params.eta_m

        # 锦标赛选择配对
        idx1 = self._tournament_batch(fitness, pop_size)
        idx2 = self._tournament_batch(fitness, pop_size)
        p1 = population[idx1].copy()  # (pop_size, n_pts, 2)
        p2 = population[idx2].copy()

        # SBX 交叉 (仅内部航路点)
        inner1 = p1[:, 1:-1].reshape(pop_size, -1)  # (pop_size, (n_pts-2)*2)
        inner2 = p2[:, 1:-1].reshape(pop_size, -1)
        D = inner1.shape[1]

        cx_mask = (np.random.rand(pop_size, 1) < self.params.crossover_prob)
        gene_mask = (np.random.rand(pop_size, D) < 0.5) & cx_mask
        diff = np.abs(inner1 - inner2)
        active = gene_mask & (diff > 1e-14)

        if np.any(active):
            y1 = np.minimum(inner1, inner2)
            y2 = np.maximum(inner1, inner2)
            u = np.random.rand(pop_size, D)
            beta = 1.0 + 2.0 * (y1 - lo) / (diff + 1e-14)
            alpha = 2.0 - beta ** (-(eta_c + 1))
            mask_u = u <= 1.0 / alpha
            bq = np.where(mask_u,
                          (u * alpha) ** (1.0 / (eta_c + 1)),
                          (1.0 / (2.0 - u * alpha + 1e-30)) ** (1.0 / (eta_c + 1)))
            c1 = np.clip(0.5 * ((y1 + y2) - bq * (y2 - y1)), lo, hi)
            c2 = np.clip(0.5 * ((y1 + y2) + bq * (y2 - y1)), lo, hi)
            inner1 = np.where(active, c1, inner1)
            inner2 = np.where(active, c2, inner2)

        # 合并子代
        offspring_inner = np.vstack([inner1[:pop_size // 2], inner2[:pop_size // 2]])
        if len(offspring_inner) < pop_size:
            offspring_inner = np.vstack([offspring_inner, inner1[:pop_size - len(offspring_inner)]])
        offspring_inner = offspring_inner[:pop_size]

        # PM 变异
        pm_rate = self.params.mutation_prob / max(D, 1)
        mut_mask = np.random.rand(pop_size, D) < pm_rate
        if np.any(mut_mask):
            val = offspring_inner[mut_mask]
            cols = np.where(mut_mask)[1]
            d1 = (val - lo) / (hi - lo + 1e-14)
            d2 = (hi - val) / (hi - lo + 1e-14)
            u_m = np.random.rand(len(val))
            left = u_m < 0.5
            dq = np.empty(len(val))
            if np.any(left):
                xy_l = 1.0 - d1[left]
                dq[left] = (2 * u_m[left] + (1 - 2 * u_m[left]) * (xy_l ** (eta_m + 1))) ** (
                        1 / (eta_m + 1)) - 1.0
            if np.any(~left):
                xy_r = 1.0 - d2[~left]
                dq[~left] = 1.0 - (2 * (1 - u_m[~left]) + 2 * (u_m[~left] - 0.5) * (
                        xy_r ** (eta_m + 1))) ** (1 / (eta_m + 1))
            offspring_inner[mut_mask] = np.clip(val + dq * (hi - lo), lo, hi)

        # 重组路径
        n_inner = (n_pts - 2) * 2
        offspring = np.zeros((pop_size, n_pts, 2))
        offspring[:, 0] = start
        offspring[:, -1] = goal
        offspring[:, 1:-1] = offspring_inner.reshape(pop_size, n_pts - 2, 2)

        # 批量 KPO
        offspring = self.kpo.apply_batch(offspring, n_iterations=1)
        offspring[:, 0] = start
        offspring[:, -1] = goal
        return offspring

    def _tournament_batch(self, fitness, size, k=2):
        """批量锦标赛选择"""
        n = len(fitness)
        candidates = np.random.randint(0, n, (size, k))
        # 简单策略: 选 fitness 第一目标较小的
        best = candidates[:, 0]
        for i in range(1, k):
            better = fitness[candidates[:, i], 0] < fitness[best, 0]
            best[better] = candidates[better, i]
        return best


# ╔═══════════════════════════════════════════════════════════════════════════╗
#  第 8 部分：流形迁移学习 — 改进的 Process 3 (Transfer)
# ╚═══════════════════════════════════════════════════════════════════════════╝

class ManifoldTransfer:
    """
    流形迁移学习模块 — 完整实现
    ─────────────────────────────────────────
    来源: 论文 Process 3 (Transfer)
      "1: dimension d = m − 1; TransSol = ∅;
       2: Clustering LastBestSol: LastBestSol1,...,LastBestSolL by LPCA;
       3: for j = 1 to L do
       4:   Use PCA for LastBestSolj to get PS;
       5:   Generate set T containing N individuals of F(x, t);
       6:   Use PCA for T to get PT ∈ R^d×N;
       7:   Construct the geodesic flow φ(k) = PS U1Γ(k) − RS U2Σ(k);
       8:   for x ∈ LastBestSolj do
       9:     Project x to φ(·) and get x̄;
       10:    x̂ = arg min_x ||x^T φ(·) − x̄||;
       11:    TransSol = x̂ ∪ TransSol;
       12:  end for
       13: end for"
    
    参数 (论文):
      "manifold segments L=4; intermediate subspaces p=5"
    
    计算复杂度 (论文):
      "Clustering by LPCA: O(d²);
       Constructing geodesic flow: O(d²) and O(1);
       Interior-point: O(m³n);
       Total: O(m³n)"
    
    本文改进:
      1. 使用 k-means 替代 LPCA 聚类 (更鲁棒)
      2. SGF 中使用 QR 正交化替代严格 SVD (加速)
      3. 多源加权: 从多个相似历史环境加权迁移
    """

    def __init__(self, n_clusters: int = 4, n_subspaces: int = 5):
        self.n_clusters = n_clusters  # 论文 L=4
        self.n_subspaces = n_subspaces  # 论文 p=5

    def transfer(self, source_pop: np.ndarray, source_fitness: np.ndarray,
                 target_samples: np.ndarray, transfer_size: int,
                 weights: np.ndarray = None) -> np.ndarray:
        """
        完整的 Transfer 流程 (Process 3)
        
        Args:
            source_pop: 源域精英解 (N_src, n_pts, 2)
            source_fitness: 源域适应度 (N_src, n_obj)
            target_samples: 目标域采样 (N_tgt, n_pts, 2)
            transfer_size: 需要迁移的个体数
            weights: 多源权重 (可选)
        
        Returns:
            迁移后的个体 (transfer_size, n_pts, 2)
        """
        if not HAS_SKLEARN:
            return self._perturb(source_pop, transfer_size)

        n_pts = source_pop.shape[1]
        S = source_pop.reshape(len(source_pop), -1)  # 展平
        T = target_samples.reshape(len(target_samples), -1)

        dim = S.shape[1]
        n_src = len(S)

        if n_src < 4 or dim < 2:
            return self._perturb(source_pop, transfer_size)

        # ── Step 2: 聚类 (论文: LPCA, 本文: k-means) ──
        L = min(self.n_clusters, n_src // 2)
        if L < 1:
            L = 1

        try:
            from sklearn.cluster import KMeans
            kmeans = KMeans(n_clusters=L, n_init=3, max_iter=50, random_state=0)
            labels = kmeans.fit_predict(S)
        except Exception:
            labels = np.random.randint(0, L, n_src)

        all_transferred = []

        # ── Step 3-12: 对每个聚类执行 SGF 迁移 ──
        for j in range(L):
            cluster_mask = labels == j
            S_j = S[cluster_mask]
            if len(S_j) < 2:
                continue

            # Step 4: PCA for source cluster → PS
            d = min(self.n_subspaces, dim, len(S_j) - 1, len(T) - 1)
            if d < 1:
                continue

            try:
                pca_s = PCA(n_components=d).fit(S_j)
                pca_t = PCA(n_components=d).fit(T)
            except Exception:
                continue

            PS = pca_s.components_.T  # (dim, d) — 源子空间基
            PT = pca_t.components_.T  # (dim, d) — 目标子空间基

            # Step 7: 构建测地流 φ(k)
            # 论文公式 (3): φ(k) = PS U1 Γ(k) − RS U2 Σ(k)
            # 简化实现: 使用 p 个中间子空间插值
            transferred_j = self._geodesic_transfer(S_j, PS, PT, pca_s.mean_,
                                                    pca_t.mean_, d, dim)
            all_transferred.append(transferred_j)

        if not all_transferred:
            return self._perturb(source_pop, transfer_size)

        all_trans = np.vstack(all_transferred)

        # 选取指定数量
        if len(all_trans) >= transfer_size:
            idx = np.random.choice(len(all_trans), transfer_size, replace=False)
        else:
            idx = np.random.choice(len(all_trans), transfer_size, replace=True)

        result = all_trans[idx].reshape(-1, n_pts, 2)

        # 应用多源权重 (如果有)
        if weights is not None and len(weights) == transfer_size:
            # 加权微调
            noise_scale = (1.0 - weights[:, None, None]) * 3.0
            result += np.random.normal(0, 1, result.shape) * noise_scale

        return result

    def _geodesic_transfer(self, S_j, PS, PT, mean_s, mean_t, d, dim):
        """
        SGF 测地流迁移
        
        来源: 论文公式 (3)-(4)
          "φ(k) = PS U1 Γ(k) − RS U2 Σ(k), k ∈ (0,1)"
          "xk = x^T φ(k)"
        
        简化实现:
          使用 p 个中间子空间线性插值 + QR 正交化
          等价于在 Grassmann 流形上的近似测地线
        """
        all_mapped = []
        p = self.n_subspaces  # 论文 p=5

        for k_idx in range(1, p + 1):
            alpha = k_idx / (p + 1)  # 0 < k < 1

            # 构建中间子空间 (Grassmann 流形插值)
            P_mid = (1 - alpha) * PS + alpha * PT
            # QR 正交化 → 确保子空间基正交
            P_mid, _ = np.linalg.qr(P_mid)

            # 投影源数据到中间子空间 (论文 Step 9)
            S_centered = S_j - mean_s
            projected = S_centered @ P_mid  # (n_cluster, d)

            # 反投影到目标空间 (论文 Step 10: 最小化映射误差)
            # x̂ = arg min_x ||x^T φ(·) − x̄||
            # 简化: 直接反投影 + 用目标域均值偏移
            reconstructed = projected @ P_mid.T + mean_t

            all_mapped.append(reconstructed)

        return np.vstack(all_mapped)

    def transfer_multi_source(self, sources: List[Dict], target_samples: np.ndarray,
                              transfer_size: int) -> np.ndarray:
        """
        多源加权流形迁移 (KEMM-DMOEA 改进)
        ─────────────────────────────────────────
        从多个相似历史环境按相似度加权迁移
        
        Args:
            sources: list of {solutions, fitness, similarity}
            target_samples: 目标域采样
            transfer_size: 总迁移数
        """
        if not sources:
            return self._perturb(target_samples, transfer_size)

        total_sim = sum(s.get('similarity', 1.0) for s in sources)
        all_transferred = []

        for src in sources:
            sim = src.get('similarity', 1.0)
            n_from_src = max(1, int(transfer_size * sim / (total_sim + 1e-12)))

            trans = self.transfer(
                src['solutions'], src['fitness'],
                target_samples, n_from_src)
            all_transferred.append(trans)

        result = np.concatenate(all_transferred, axis=0)
        if len(result) >= transfer_size:
            idx = np.random.choice(len(result), transfer_size, replace=False)
        else:
            idx = np.random.choice(len(result), transfer_size, replace=True)
        return result[idx]

    @staticmethod
    def _perturb(pop, n):
        idx = np.random.choice(len(pop), n, replace=True)
        result = pop[idx].copy()
        result[:, 1:-1] += np.random.normal(0, 3.0, result[:, 1:-1].shape)
        return result


# ╔═══════════════════════════════════════════════════════════════════════════╗
#  第 9 部分：KEMM-DMOEA 核心引擎 — 完整 Process 1 实现
# ╚═══════════════════════════════════════════════════════════════════════════╝

class KEMMDMOEA:
    """
    KEMM-DMOEA 主控制器 — 完整实现
    ─────────────────────────────────────────
    完整实现论文 Process 1 的流程:
    
    ```
    Process 1 MMTL-DMOEA (本文改进版 KEMM-DMOEA)
    1: Initialization;
    2: Solutions0 = SMOA(POP0, F(x, 0), N);
    3: Store Solutions0 into P;
    4: while Change detected do
    5:   t = t + 1;
    6:   LastBestSol = FindBestSol(P, F(x,t), N);     ← Process 2
    7:   TransSol = Transfer(LastBestSol, F(x,t), N, L); ← Process 3
    8:   POPt = LastBestSol ∪ TransSol;
    9:   (Solutionst) = SMOA(POPt, F(x,t), N);
    10:  if size(P ∪ Solutionst) ≤ C then
    11:    Store Solutionst into P;
    12:  else
    13:    Replace earliest stored with Solutionst;
    14:  end if
    15: end while
    ```
    
    KEMM-DMOEA 在此基础上的改进:
      1. FindBestSol: APF指纹相似度检索 (替代时间顺序)
      2. Transfer: 多源加权SGF (替代单源)
      3. KPO: 运动学约束投影
      4. 自适应变化检测: 根据指纹距离调整迁移/重初始化比例
    """

    def __init__(self, env: DynamicEnvironment,
                 ship_params: ShipParams = None,
                 algo_params: AlgorithmParams = None):
        self.env = env
        self.ship = ship_params or ShipParams()
        self.params = algo_params or AlgorithmParams()

        self.kpo = KinematicProjectionOperator(self.ship, delta_t=10.0)
        self.memory = EnvironmentFieldMemory(
            capacity=self.params.memory_capacity,
            grid_res=30,
            bounds=self.env.bounds)
        self.evaluator = MultiObjectiveEvaluator(self.env, self.ship, self.kpo)
        self.operators = EvolutionaryOperators(
            self.params, self.kpo, self.env.bounds)
        self.transfer = ManifoldTransfer(
            n_clusters=self.params.n_clusters,
            n_subspaces=self.params.n_subspaces)
        self.find_best = FindBestSol(
            self.evaluator, n_samples=self.params.n_svr_samples)

        self.population = None
        self.fitness = None
        self.prev_fingerprint = None
        self.history_pareto = []
        self.n_total_pts = self.params.n_waypoints + 2

    def _initialize_population(self):
        """初始化种群 — 三策略混合"""
        N = self.params.pop_size
        n_wp = self.params.n_waypoints
        n_total = self.n_total_pts
        lo, hi = self.env.bounds
        start, goal = self.env.start, self.env.goal

        pop = np.zeros((N, n_total, 2))
        pop[:, 0] = start
        pop[:, -1] = goal

        n1 = N // 3
        t_vals = np.linspace(0, 1, n_wp + 2)[1:-1]
        linear = (1 - t_vals[:, None]) * start + t_vals[:, None] * goal
        pop[:n1, 1:-1] = linear + np.random.normal(0, 30.0, (n1, n_wp, 2))

        n2 = N // 3
        for i in range(n1, n1 + n2):
            pop[i, 1:-1] = self._apf_guided(start, goal, n_wp)

        pop[n1 + n2:, 1:-1] = np.random.uniform(lo, hi, (N - n1 - n2, n_wp, 2))

        pop[:, 1:-1] = np.clip(pop[:, 1:-1], lo + 5, hi - 5)
        pop = self.kpo.apply_batch(pop, n_iterations=2)
        pop[:, 0] = start
        pop[:, -1] = goal
        return pop

    def _apf_guided(self, start, goal, n_wp):
        """APF引导单条路径"""
        path = np.zeros((n_wp, 2))
        current = start.copy()
        step = np.linalg.norm(goal - start) / (n_wp + 1)
        obs_pos = self.env.get_obstacle_positions()
        obs_rad = self.env.get_obstacle_radii()

        for j in range(n_wp):
            diff_g = goal - current
            dist_g = np.linalg.norm(diff_g) + 1e-8
            f_att = diff_g / dist_g
            f_rep = np.zeros(2)
            if len(obs_pos) > 0:
                diff_o = current - obs_pos
                dist_o = np.linalg.norm(diff_o, axis=1) + 1e-8
                close = dist_o < obs_rad * 4
                if np.any(close):
                    force = (obs_rad[close] / dist_o[close]) ** 2
                    f_rep = np.sum(force[:, None] * diff_o[close] / dist_o[close, None], axis=0)
            f_total = f_att + 0.5 * f_rep + np.random.normal(0, 0.3, 2)
            f_total = f_total / (np.linalg.norm(f_total) + 1e-8)
            current = current + f_total * step
            path[j] = current
        return path

    def evolve_one_step(self, verbose=False):
        """
        执行单个时刻的完整进化
        ─────────────────────────────────────────
        实现 Process 1 的完整流程
        """
        current_fp = self.memory.compute_fingerprint(self.env)

        if self.population is None:
            # ── Process 1, Line 1-2: 初始化 ──
            self.population = self._initialize_population()
            if verbose:
                print("  [INIT] Cold start — Process 1 Line 1-2")
        else:
            # ── Process 1, Line 4: 变化检测 ──
            change_mag = self.memory.compute_change_magnitude(
                self.prev_fingerprint, current_fp)
            reinit_ratio = np.clip(
                change_mag / self.params.change_threshold,
                self.params.reinit_ratio_min, self.params.reinit_ratio_max)
            if verbose:
                print(f"  [DETECT] Process 1 Line 4 — Change: {change_mag:.4f}, "
                      f"reinit: {reinit_ratio:.2%}")

            # ── Process 1, Line 6: FindBestSol (改进版 Process 2) ──
            retrieved = self.memory.retrieve(current_fp, top_k=3)
            n_transfer = int(self.params.pop_size * self.params.transfer_ratio)
            n_reinit = int(self.params.pop_size * reinit_ratio)
            n_keep = self.params.pop_size - n_transfer - n_reinit

            parts = []

            # (a) 从记忆中选择精英 — FindBestSol (Process 2)
            if n_keep > 0 and retrieved is not None:
                best_match = retrieved[0]
                last_best_sol, last_best_fit = self.find_best.select(
                    best_match['solutions'], best_match['fitness'],
                    target_size=n_keep,
                    env_evaluator=self.evaluator
                )
                if verbose:
                    print(f"  [FIND] Process 2 — Selected {len(last_best_sol)} elites "
                          f"via SVR estimation")
                # KPO 投影
                last_best_sol = self.kpo.apply_batch(last_best_sol, n_iterations=1)
                last_best_sol[:, 0] = self.env.start
                last_best_sol[:, -1] = self.env.goal
                parts.append(last_best_sol)
            elif n_keep > 0 and self.fitness is not None:
                # 回退: 保留当前精英
                fronts = EvolutionaryOperators.fast_non_dominated_sort(self.fitness)
                keep_idx = []
                for f in fronts:
                    keep_idx.extend(f)
                    if len(keep_idx) >= n_keep:
                        break
                parts.append(self.population[keep_idx[:n_keep]])

            # (b) ── Process 1, Line 7: Transfer (改进版 Process 3) ──
            if n_transfer > 0 and retrieved is not None:
                if len(retrieved) >= 2:
                    # 多源加权迁移 (KEMM-DMOEA 改进)
                    sources = []
                    for ri, ret in enumerate(retrieved):
                        sim = 1.0 / (ri + 1)  # 按排名衰减权重
                        sources.append({
                            'solutions': ret['solutions'],
                            'fitness': ret['fitness'],
                            'similarity': sim
                        })
                    target_samples = self.population[:min(20, len(self.population))] \
                        if self.population is not None else self._initialize_population()[:20]
                    transferred = self.transfer.transfer_multi_source(
                        sources, target_samples, n_transfer)
                    if verbose:
                        print(f"  [TRANSFER] Process 3 — Multi-source SGF from "
                              f"{len(sources)} memories, {n_transfer} individuals")
                else:
                    # 单源迁移
                    best_match = retrieved[0]
                    target_samples = self.population[:min(20, len(self.population))] \
                        if self.population is not None else self._initialize_population()[:20]
                    transferred = self.transfer.transfer(
                        best_match['solutions'], best_match['fitness'],
                        target_samples, n_transfer)
                    if verbose:
                        print(f"  [TRANSFER] Process 3 — Single-source SGF, "
                              f"{n_transfer} individuals")

                lo, hi = self.env.bounds
                transferred = np.clip(transferred, lo, hi)
                transferred[:, 0] = self.env.start
                transferred[:, -1] = self.env.goal
                transferred = self.kpo.apply_batch(transferred, n_iterations=1)
                transferred[:, 0] = self.env.start
                transferred[:, -1] = self.env.goal
                parts.append(transferred)
            else:
                n_reinit += n_transfer

            # (c) 随机重初始化
            if n_reinit > 0:
                parts.append(self._initialize_population()[:n_reinit])

            # ── Process 1, Line 8: 合并 ──
            self.population = np.concatenate(parts, axis=0)[:self.params.pop_size]
            if len(self.population) < self.params.pop_size:
                extra = self._initialize_population()[
                        :self.params.pop_size - len(self.population)]
                self.population = np.concatenate([self.population, extra])

        # ── Process 1, Line 9: SMOA 进化 ──
        self.fitness = self.evaluator.evaluate_population(self.population)
        igd_curve = []

        for gen in range(self.params.max_gen):
            offspring = self.operators.reproduce(
                self.population, self.fitness, self.env.start, self.env.goal)
            off_fit = self.evaluator.evaluate_population(offspring)

            merged_pop = np.concatenate([self.population, offspring])
            merged_fit = np.concatenate([self.fitness, off_fit])
            self.population, self.fitness = self.operators.environmental_selection(
                merged_pop, merged_fit, self.params.pop_size)

            if (gen + 1) % 5 == 0:
                fronts = EvolutionaryOperators.fast_non_dominated_sort(self.fitness)
                igd_curve.append(float(np.mean(self.fitness[fronts[0]])))
                if verbose and (gen + 1) % 20 == 0:
                    pf_fit = self.fitness[fronts[0]]
                    print(f"    Gen {gen+1:3d}/{self.params.max_gen} | "
                          f"PF: {len(fronts[0])} | Mean: {np.mean(pf_fit, axis=0).round(1)}")

        # ── Process 1, Lines 10-14: 记忆存储 ──
        fronts = EvolutionaryOperators.fast_non_dominated_sort(self.fitness)
        pf_idx = fronts[0]
        pareto_pop = self.population[pf_idx]
        pareto_fit = self.fitness[pf_idx]
        best_idx = self._select_compromise(pareto_fit)

        self.memory.store(current_fp, self.population, self.fitness)
        self.prev_fingerprint = current_fp.copy()

        if verbose:
            print(f"  [STORE] Process 1 Lines 10-14 — Memory size: {len(self.memory.memory)}")

        result = {
            'pareto_front': pareto_pop,
            'pareto_fitness': pareto_fit,
            'best_compromise': pareto_pop[best_idx],
            'best_fitness': pareto_fit[best_idx],
            'igd_curve': igd_curve,
            'population': self.population,
            'fitness': self.fitness
        }
        self.history_pareto.append(result)
        return result

    @staticmethod
    def _select_compromise(pf):
        f = pf.copy()
        f_min, f_max = f.min(0), f.max(0)
        f_norm = (f - f_min) / (f_max - f_min + 1e-12)
        return int(np.argmin(np.sqrt(np.sum(f_norm ** 2, axis=1))))


# ╔═══════════════════════════════════════════════════════════════════════════╗
#  第 10 部分：可视化
# ╚═══════════════════════════════════════════════════════════════════════════╝

class Visualizer:
    @staticmethod
    def plot_paths(env, pareto_paths, best_path, title="", save_path=None):
        if not HAS_MPL:
            return
        fig, ax = plt.subplots(1, 1, figsize=(10, 10))
        lo, hi = env.bounds
        for obs in env.obstacles:
            ax.add_patch(Circle(obs.position, obs.radius, color='red', alpha=0.4))
            ax.add_patch(Circle(obs.position, obs.radius * 2,
                                color='red', alpha=0.1, fill=False, linestyle='--'))
        for path in pareto_paths[:30]:
            ax.plot(path[:, 0], path[:, 1], 'b-', alpha=0.15, linewidth=0.8)
        ax.plot(best_path[:, 0], best_path[:, 1], 'g-', linewidth=2.5, zorder=5)
        ax.plot(best_path[:, 0], best_path[:, 1], 'go', markersize=4, zorder=5)
        ax.plot(*env.start, 'k^', markersize=15, label='Start', zorder=6)
        ax.plot(*env.goal, 'k*', markersize=18, label='Goal', zorder=6)
        ax.set_xlim(lo - 10, hi + 10)
        ax.set_ylim(lo - 10, hi + 10)
        ax.set_aspect('equal')
        ax.legend()
        ax.set_title(title)
        ax.grid(True, alpha=0.3)
        plt.tight_layout()
        if save_path:
            plt.savefig(save_path, dpi=120, bbox_inches='tight')
        plt.close(fig)


# ╔═══════════════════════════════════════════════════════════════════════════╗
#  第 11 部分：独立运行入口
# ╚═══════════════════════════════════════════════════════════════════════════╝

def create_complex_scenario():
    env = DynamicEnvironment(bounds=(0, 500))
    env.start = np.array([30.0, 30.0])
    env.goal = np.array([470.0, 470.0])
    for pos, rad in [([120, 100], 20), ([250, 200], 25), ([350, 150], 18),
                     ([150, 350], 22), ([400, 350], 20), ([300, 400], 15)]:
        env.add_obstacle(pos, radius=rad, vel=[0, 0])
    for pos, rad, vel in [([200, 80], 12, [2, 1.5]), ([80, 250], 10, [1.5, -1]),
                          ([320, 280], 14, [-1, 2]), ([180, 420], 11, [2.5, -0.5]),
                          ([420, 200], 13, [-1.5, 1.5]), ([280, 100], 10, [0.5, 2.5])]:
        env.add_obstacle(pos, radius=rad, vel=vel)
    env.setup_currents(
        centers=[[150, 150], [350, 300], [250, 450]],
        strengths=[30.0, -25.0, 20.0],
        sigmas=[80.0, 100.0, 70.0],
        drift=[0.3, 0.2])
    return env


def main_ship_planning():
    print("=" * 70)
    print("  KEMM-DMOEA: Ship Path Planning (Complete + Optimized)")
    print("=" * 70)
    env = create_complex_scenario()
    ship = ShipParams(r_min=40, v_max=8, v_min=1, ship_length=25, fuel_coeff=0.001)
    algo = AlgorithmParams(pop_size=80, n_waypoints=14, max_gen=60,
                           n_clusters=4, n_subspaces=5, n_svr_samples=30)
    planner = KEMMDMOEA(env, ship, algo)
    viz = Visualizer()

    for t in range(5):
        print(f"\n--- Time Step {t} | t={env.time:.0f}s ---")
        t0 = time_module.time()
        result = planner.evolve_one_step(verbose=True)
        elapsed = time_module.time() - t0
        bf = result['best_fitness']
        print(f"  ★ Best: Len={bf[0]:.1f} Risk={bf[1]:.2f} "
              f"Fuel={bf[2]:.3f} Smooth={bf[3]:.3f}  ({elapsed:.2f}s)")
        viz.plot_paths(env, result['pareto_front'], result['best_compromise'],
                       title=f"Time Step {t}", save_path=f"ship_t{t}.png")
        env.step(30.0)
    print("\n  Done.")


if __name__ == "__main__":
    main_ship_planning()