%% Matlab/Simulink 威胁源库集成示例
% 演示如何在Matlab中使用ThreatSource仿真库
% 
% 前置条件:
% 1. 将 ThreatSource.dll 复制到 Matlab 路径
% 2. 将 data/ 目录复制到当前工作目录
% 3. 确保 Matlab 支持 .NET 8.0

%% 清理环境
clear; clc; close all;

%% 加载 .NET 程序集
fprintf('正在加载 ThreatSource.dll...\n');
try
    % 假设 dll 文件在当前目录或 Matlab 路径中
    dll_path = 'ThreatSource.dll';
    if ~exist(dll_path, 'file')
        dll_path = fullfile(pwd, 'ThreatSource.dll');
    end
    
    if exist(dll_path, 'file')
        NET.addAssembly(dll_path);
        fprintf('成功加载威胁源库\n');
    else
        error('找不到 ThreatSource.dll 文件');
    end
catch ME
    fprintf('加载失败: %s\n', ME.message);
    return;
end

%% 创建适配器实例
fprintf('创建 Matlab 适配器...\n');
try
    % 注意：这里使用完整的命名空间路径
    adapter = NET.createGeneric('MatlabSimulinkAdapter', {});
    fprintf('适配器创建成功\n');
catch ME
    fprintf('适配器创建失败: %s\n', ME.message);
    return;
end

%% 初始化仿真
fprintf('初始化仿真系统...\n');
time_step = 0.01; % 10ms 时间步长
success = adapter.StartSimulation(time_step);

if ~success
    fprintf('仿真初始化失败\n');
    return;
end
fprintf('仿真系统初始化成功，时间步长: %.3f 秒\n', time_step);

%% 创建仿真场景
fprintf('\n=== 创建仿真场景 ===\n');

% 目标参数
target_id = 'target_001';
target_pos = [5000, 0, 1000];      % 位置: [x, y, z] 米
target_vel = [-50, 0, 0];          % 速度: [vx, vy, vz] 米/秒

% 创建目标
success = adapter.CreateTarget(target_id, ...
    target_pos(1), target_pos(2), target_pos(3), ...
    target_vel(1), target_vel(2), target_vel(3));

if success
    fprintf('成功创建目标: %s\n', target_id);
    fprintf('  位置: [%.0f, %.0f, %.0f] 米\n', target_pos);
    fprintf('  速度: [%.1f, %.1f, %.1f] 米/秒\n', target_vel);
else
    fprintf('目标创建失败\n');
    return;
end

% 导弹参数
missile_id = 'missile_001';
missile_pos = [0, 0, 100];         % 发射位置: [x, y, z] 米

% 创建红外制导导弹
success = adapter.CreateIRMissile(missile_id, ...
    missile_pos(1), missile_pos(2), missile_pos(3), target_id);

if success
    fprintf('成功创建红外制导导弹: %s\n', missile_id);
    fprintf('  发射位置: [%.0f, %.0f, %.0f] 米\n', missile_pos);
    fprintf('  目标: %s\n', target_id);
else
    fprintf('导弹创建失败\n');
    return;
end

%% 运行仿真循环
fprintf('\n=== 开始仿真 ===\n');

% 仿真参数
sim_duration = 5.0;                % 仿真持续时间（秒）
num_steps = int32(sim_duration / time_step);
update_interval = 50;              % 每50步显示一次状态

% 存储仿真数据
time_data = zeros(num_steps, 1);
missile_data = zeros(num_steps, 10); % [x,y,z,vx,vy,vz,roll,pitch,yaw,active]
target_data = zeros(num_steps, 10);

fprintf('仿真参数:\n');
fprintf('  持续时间: %.1f 秒\n', sim_duration);
fprintf('  时间步长: %.3f 秒\n', time_step);
fprintf('  总步数: %d\n', num_steps);

% 仿真主循环
for step = 1:num_steps
    % 更新仿真
    success = adapter.UpdateSimulation(time_step);
    if ~success
        fprintf('第 %d 步仿真更新失败\n', step);
        break;
    end
    
    % 记录时间
    current_time = adapter.GetCurrentTime();
    time_data(step) = current_time;
    
    % 获取实体状态
    missile_state = adapter.GetEntityState(missile_id);
    target_state = adapter.GetEntityState(target_id);
    
    % 转换 .NET 数组为 Matlab 数组
    if ~isempty(missile_state)
        missile_data(step, :) = double(missile_state);
    end
    if ~isempty(target_state)
        target_data(step, :) = double(target_state);
    end
    
    % 定期显示状态
    if mod(step, update_interval) == 0 || step == 1
        fprintf('时间: %6.2f 秒 | ', current_time);
        if ~isempty(missile_state) && length(missile_state) >= 10
            fprintf('导弹位置: [%6.0f, %6.0f, %6.0f] | ', ...
                missile_state(1), missile_state(2), missile_state(3));
            fprintf('活动: %s\n', missile_state(10) > 0.5 ? '是' : '否');
        else
            fprintf('导弹状态获取失败\n');
        end
    end
    
    % 检查导弹是否仍然活动
    if ~isempty(missile_state) && length(missile_state) >= 10
        if missile_state(10) < 0.5  % 导弹已失活
            fprintf('导弹在第 %.2f 秒失活\n', current_time);
            break;
        end
    end
end

%% 显示仿真结果
fprintf('\n=== 仿真结果分析 ===\n');

% 获取所有实体ID
entity_ids = adapter.GetAllEntityIds();
fprintf('仿真中的实体数量: %d\n', length(entity_ids));
for i = 1:length(entity_ids)
    fprintf('  - %s\n', char(entity_ids(i)));
end

% 计算统计信息
valid_steps = sum(time_data > 0);
fprintf('有效仿真步数: %d / %d\n', valid_steps, num_steps);

if valid_steps > 0
    % 导弹轨迹分析
    missile_pos = missile_data(1:valid_steps, 1:3);
    target_pos_final = target_data(1:valid_steps, 1:3);
    
    % 计算导弹飞行距离
    if size(missile_pos, 1) > 1
        distances = sqrt(sum(diff(missile_pos).^2, 2));
        total_distance = sum(distances);
        fprintf('导弹总飞行距离: %.1f 米\n', total_distance);
    end
    
    % 计算最终距离
    if valid_steps > 1
        final_missile_pos = missile_pos(end, :);
        final_target_pos = target_pos_final(end, :);
        final_distance = norm(final_missile_pos - final_target_pos);
        fprintf('最终导弹-目标距离: %.1f 米\n', final_distance);
    end
end

%% 绘制仿真结果
if valid_steps > 10  % 确保有足够的数据点
    fprintf('\n绘制仿真轨迹...\n');
    
    figure('Name', '威胁源仿真轨迹', 'Position', [100, 100, 1200, 800]);
    
    % 3D轨迹图
    subplot(2, 2, 1);
    plot3(missile_data(1:valid_steps, 1), missile_data(1:valid_steps, 2), ...
          missile_data(1:valid_steps, 3), 'r-', 'LineWidth', 2);
    hold on;
    plot3(target_data(1:valid_steps, 1), target_data(1:valid_steps, 2), ...
          target_data(1:valid_steps, 3), 'b--', 'LineWidth', 2);
    xlabel('X (米)'); ylabel('Y (米)'); zlabel('Z (米)');
    title('3D 轨迹图');
    legend('导弹', '目标', 'Location', 'best');
    grid on; axis equal;
    
    % 高度vs时间
    subplot(2, 2, 2);
    plot(time_data(1:valid_steps), missile_data(1:valid_steps, 3), 'r-', 'LineWidth', 2);
    hold on;
    plot(time_data(1:valid_steps), target_data(1:valid_steps, 3), 'b--', 'LineWidth', 2);
    xlabel('时间 (秒)'); ylabel('高度 (米)');
    title('高度变化');
    legend('导弹', '目标', 'Location', 'best');
    grid on;
    
    % 速度vs时间
    subplot(2, 2, 3);
    missile_speed = sqrt(sum(missile_data(1:valid_steps, 4:6).^2, 2));
    target_speed = sqrt(sum(target_data(1:valid_steps, 4:6).^2, 2));
    plot(time_data(1:valid_steps), missile_speed, 'r-', 'LineWidth', 2);
    hold on;
    plot(time_data(1:valid_steps), target_speed, 'b--', 'LineWidth', 2);
    xlabel('时间 (秒)'); ylabel('速度 (米/秒)');
    title('速度变化');
    legend('导弹', '目标', 'Location', 'best');
    grid on;
    
    % 导弹-目标距离vs时间
    subplot(2, 2, 4);
    distances = sqrt(sum((missile_data(1:valid_steps, 1:3) - target_data(1:valid_steps, 1:3)).^2, 2));
    plot(time_data(1:valid_steps), distances, 'g-', 'LineWidth', 2);
    xlabel('时间 (秒)'); ylabel('距离 (米)');
    title('导弹-目标距离');
    grid on;
    
    % 保存图像
    saveas(gcf, 'threat_source_simulation_result.png');
    fprintf('仿真结果图已保存为: threat_source_simulation_result.png\n');
end

%% 清理资源
fprintf('\n=== 清理资源 ===\n');
try
    adapter.StopSimulation();
    fprintf('仿真已停止\n');
catch ME
    fprintf('停止仿真时出错: %s\n', ME.message);
end

fprintf('Matlab 威胁源库集成示例完成\n');

%% 辅助函数：导出数据到文件
function export_simulation_data(time_data, missile_data, target_data, filename)
    try
        % 创建数据表
        data_table = table(time_data, ...
                          missile_data(:,1), missile_data(:,2), missile_data(:,3), ...
                          missile_data(:,4), missile_data(:,5), missile_data(:,6), ...
                          target_data(:,1), target_data(:,2), target_data(:,3), ...
                          target_data(:,4), target_data(:,5), target_data(:,6), ...
                          'VariableNames', {'Time', ...
                          'Missile_X', 'Missile_Y', 'Missile_Z', ...
                          'Missile_VX', 'Missile_VY', 'Missile_VZ', ...
                          'Target_X', 'Target_Y', 'Target_Z', ...
                          'Target_VX', 'Target_VY', 'Target_VZ'});
        
        % 保存为CSV文件
        writetable(data_table, filename);
        fprintf('仿真数据已导出到: %s\n', filename);
    catch ME
        fprintf('导出数据失败: %s\n', ME.message);
    end
end