% 加载数据文件
load('idntfcnTrjctryCell_full2.mat');

% 获取数据矩阵
data = idntfcnTrjctryCell{7,1};
[num_samples, ~] = size(data);

% 定义关节列索引
joint_cols = 105:3:126; % J1到J8的列索引 (105,108,111,...,126)

% 创建时间向量（采样频率2000 Hz）
t = (0:num_samples-1)' / 2000; % 单位为秒

% 循环处理每个关节
for joint_idx = 1:8
    % 创建新图形窗口
    fig = figure('Position', [100, 100, 800, 500], 'Color', 'white');
    
    % 获取当前关节的数据
    col = joint_cols(joint_idx);
    if joint_idx == 8
        torque = data(:, joint_cols(joint_idx-1)+2);
    else
        torque = data(:, joint_cols(joint_idx));
    end
    % 计算最大绝对扭矩值及其位置
    [max_abs, max_idx] = max(abs(torque));
    max_time = t(max_idx);
    max_value = torque(max_idx);
    
    % 绘制力矩曲线
    plot(t, torque, 'b-', 'LineWidth', 1.5);
    
    % 标记最大绝对值点
    hold on;
    plot(max_time, max_value, 'ro', 'MarkerSize', 10, 'LineWidth', 2);
    
    % 在右上角添加标注文本
    annotation_text = sprintf('Max Abs Torque: %.3f Nm\\nAt Time: %.3f s', max_abs, max_time);
    text(0.98, 0.98, annotation_text, ...
        'Units', 'normalized', ...
        'VerticalAlignment', 'top', ...
        'HorizontalAlignment', 'right', ...
        'FontSize', 12, ...
        'BackgroundColor', [1 1 1 0.7], ...
        'Margin', 5, ...
        'EdgeColor', 'k');
    
    % 设置图形属性
    title(sprintf('J%d Torque vs Time', joint_idx), 'FontSize', 16, 'FontWeight', 'bold');
    xlabel('Time (s)', 'FontSize', 14);
    ylabel('Torque (Nm)', 'FontSize', 14);
    grid on;
    
    % 设置坐标轴范围
%     xlim([min(t), max(t)]);
%     y_range = max_abs * 1.3; % 基于最大绝对值设置Y轴范围
%     ylim([-y_range, y_range]);
    
    % 添加采样率信息
    text(0.98, 0.02, 'Sampling Rate: 2000 Hz', ...
        'Units', 'normalized', ...
        'FontSize', 10, ...
        'HorizontalAlignment', 'right', ...
        'BackgroundColor', [1 1 1 0.5]);
    
    % 保存图形为PNG文件
    filename = sprintf('Torque_joint_%d.png', joint_idx);
    saveas(fig, filename);
    fprintf('Saved: %s\n', filename);
    
    % 关闭图形
    close(fig);
end