% 随机种子 rng(42);
%% ================= 仿真参数 =================
sim.dt = 0.2; % s
sim.T = 200; % 迭代步数
sim.Hp = 12; % 预测域
sim.Hc = 3; % 控制域
sim.state_dim = 6; % [x y z vx vy vz]
sim.input_dim = 3; % [ax ay az]
sim.vmax = 4.0; % m/s 速度上限
sim.umin = -2.0*ones(3,1); % m/s^2
sim.umax = 2.0*ones(3,1); % 软约束 & 求解器
sim.safety_margin = 0.6; % 安全裕度
sim.obs_stride = 2; % 避障约束步长（加速）
sim.casadi.W_slack = 50.0; % slack 权重
sim.casadi.cache_version = 1; % 调整以强制重建模型
sim.casadi.ipopts = struct( ...
'print_time', false, ...
'ipopt', struct( ...
'print_level', 0, ...
'max_iter', 200, ...
'max_cpu_time', 0.2, ... % 单次求解 CPU 上限（秒）
'tol', 1e-3, ...
'constr_viol_tol', 1e-3, ...
'acceptable_tol', 5e-3, ...
'acceptable_obj_change_tol', 5e-3, ...
'warm_start_init_point', 'yes' ...
) ...
);
% 事件触发参数（通信）
sim.trigger.threshold = 0.6; % 误差范数阈值（相对尺度）
sim.trigger.dwell = 3; % 最小驻留步
sim.trigger.near_obst_gain = 0.5; % 近障降低阈值系数
sim.trigger.stall_speed = 0.1; % 低速保护（m/s）
% 代价权重（论文中可给出）
sim.W_track = blkdiag(2.0*eye(3), 0.2*eye(3)); % 位置>速度
sim.W_u = 0.05*eye(3);
sim.W_du = 0.05*eye(3); % 领航速度平滑，跟随偏置
sim.leader_speed = 2.5; % 目标点速度（m/s）
%% =============== 路径 & 环境 ===============
addpath_if_exist(cfg.casadi_path); % 工作空间（立方体）
ws.min = [0 0 0]';
ws.max = [30 18 12]'; % 起点在 (min) 邻域，终点在 (max) 邻域（对角）
N = 5; % 1 领航 + 4 跟随
[starts, goals, offsets] = make_diagonal_layout(ws, N); % 障碍（静态若干 + 1 个缓慢动态）
[obst, obst_dyn] = make_obstacles();
%% =============== 参考轨迹（领航） ===============
leader_traj = make_leader_reference(starts(:,1), goals(:,1), sim);
%% =============== 初始化 ===============
states = zeros(sim.state_dim, N, sim.T+1);
inputs = zeros(sim.input_dim, N, sim.T);
states(:, :, 1) = init_states(starts); % 事件触发状态
event.last_tx_k = -1e6*ones(1,N);
event.savings_solves = 0; % 避免求解的次数
comms = zeros(1,N); % 通信次数
% 指标累计
metrics.Evel = zeros(1, sim.T);
metrics.Eform= zeros(1, sim.T);
metrics.Eu = zeros(1, sim.T);
metrics.Esafe= zeros(1, sim.T);
%% =============== 双场景对比 ===============
S_sync = run_dmpc_scenario(states(:,:,1), leader_traj, offsets, obst, obst_dyn, sim, false);
S_et = run_dmpc_scenario(states(:,:,1), leader_traj, offsets, obst, obst_dyn, sim, true );
%% =============== 作图/导出 ===============
export_figures_and_tables(S_sync, S_et, ws, outdir);
fprintf('[Savings] Solves: %.1f%% | Comms: %.1f%%\n', 100*S_et.savings_solves, 100*S_et.savings_comms);
end
%% ================== 核心：运行单场景 ==================
function S = run_dmpc_scenario(states0, leader_traj, offsets, obst, obst_dyn, sim, use_event)
import casadi.*
N = size(states0,2);
T = sim.T; % 结果缓存
states = zeros(sim.state_dim, N, T+1);
inputs = zeros(sim.input_dim, N, T);
flags = zeros(1,T); % 1: 使用了求解器，0: 复用了上一次输入
states(:,:,1) = states0; % 通信与事件
last_tx_k = -1e6*ones(1,N);
comms = zeros(1,N); % 初值 u0
u_last = zeros(sim.input_dim, N); % 障碍缓冲（加速）：提前计算每个 k 的 OC/RAD 序列
[OC_all, RAD_all] = precompute_obstacles_over_horizon(obst, obst_dyn, sim);
for k=1:T % 近障检测用于事件阈值调节
near_obst = false(1,N);
for i=1:N
near_obst(i) = is_near_obstacle(states(1:3,i,k), obst, obst_dyn, sim);
end
for i=1:N % 构造参考（含编队偏置）
ref_now = make_reference(leader_traj, offsets(:,i), k, sim); % 事件触发：决定是否重算优化或复用 u 上次
do_solve = true; % 默认要求解
if use_event
err = states(:,i,k) - ref_now(:,1);
eps = sim.trigger.threshold * (1 + 0.0*norm(ref_now(1:3,1))); %#ok<*NASGU>
if near_obst(i); eps = eps * sim.trigger.near_obst_gain; end
if k - last_tx_k(i) < sim.trigger.dwell do_solve = false; % 驻留期内不给发
elseif norm(err(1:3)) < eps && norm(states(4:6,i,k)) > sim.trigger.stall_speed do_solve = false; % 误差小且未停滞
