%% 五连杆机构腿长计算脚本
clear; clc;

%% 定义输入参数
% 连杆长度 (m)
l1 = 0.215;   % 连杆1长度
l2 = 0.258;  % 连杆2长度  
l3 = 0.258;  % 连杆3长度
l4 = 0.215;   % 连杆4长度
l5 = 0.0;   % 连杆5长度

% 关节角度 (弧度)
phi1 = 0.10;  % 30度
phi4 = 2.96;  % 45度

%% 调用腿长计算函数
fprintf('五连杆机构腿长计算\n');
fprintf('输入参数:\n');
fprintf('  连杆长度: l1=%.3f, l2=%.3f, l3=%.3f, l4=%.3f, l5=%.3f (m)\n', l1, l2, l3, l4, l5);
fprintf('  关节角度: phi1=%.3f (%.1f°), phi4=%.3f (%.1f°)\n', phi1, rad2deg(phi1), phi4, rad2deg(phi4));

try
    % 计算各关键点位置
    % B点 (连杆1末端)
    x_B = l1 * cos(phi1);
    y_B = l1 * sin(phi1);
    
    % D点 (连杆4末端)
    x_D = l5 + l4 * cos(phi4);
    y_D = l4 * sin(phi4);
    
    % 计算BD距离
    BD_dist = sqrt((x_D - x_B)^2 + (y_D - y_B)^2);
    
    % 检查三角形约束条件
    if BD_dist > (l2 + l3) || BD_dist < abs(l2 - l3)
        warning('无法形成闭合五连杆机构！BD距离=%.3f, 需要满足: %.3f < BD < %.3f', ...
                BD_dist, abs(l2-l3), l2+l3);
        leg_length = NaN;
        foot_pos = [NaN, NaN];
        return;
    end
    
    % 使用余弦定理求解phi2
    cos_phi2_BD = (l2^2 + BD_dist^2 - l3^2) / (2 * l2 * BD_dist);
    
    % 检查余弦值是否在有效范围内
    if abs(cos_phi2_BD) > 1
        warning('余弦值超出范围: %.3f', cos_phi2_BD);
        leg_length = NaN;
        foot_pos = [NaN, NaN];
        return;
    end
    
    % 计算BD方向角
    alpha_BD = atan2(y_D - y_B, x_D - x_B);
    
    % 计算phi2 (两个解，选择合理的一个)
    angle_offset = acos(cos_phi2_BD);
    phi2_solution1 = alpha_BD + angle_offset;
    phi2_solution2 = alpha_BD - angle_offset;
    
    % 选择合理的解 (通常选择使机构在合理配置的解)
    phi2 = phi2_solution1;  % 可以根据实际需要调整选择逻辑
    
    % 计算C点位置 (足端位置)
    x_C = x_B + l2 * cos(phi2);
    y_C = y_B + l2 * sin(phi2);
    
    % 验证C点到D点距离是否等于l3
    CD_dist = sqrt((x_D - x_C)^2 + (y_D - y_C)^2);
    error_CD = abs(CD_dist - l3);
    
    if error_CD > 1e-6
        fprintf('警告: CD距离误差较大 (%.6f)\n', error_CD);
    end
    
    % 计算腿长 (假设为原点到C点的距离)
    leg_length = sqrt(x_C^2 + y_C^2);
    foot_pos = [x_C, y_C];
    
    % 输出结果
    fprintf('\n计算结果:\n');
    fprintf('  B点位置: (%.3f, %.3f)\n', x_B, y_B);
    fprintf('  C点位置: (%.3f, %.3f)\n', x_C, y_C);
    fprintf('  D点位置: (%.3f, %.3f)\n', x_D, y_D);
    fprintf('  phi2角度: %.3f rad (%.1f°)\n', phi2, rad2deg(phi2));
    fprintf('  腿长: %.3f m\n', leg_length);
    fprintf('  BD距离: %.3f m\n', BD_dist);
    fprintf('  CD验证: %.6f m (应该等于l3=%.3f)\n', CD_dist, l3);
    
    %% 可视化结果
    figure;
    plot([0, x_B], [0, y_B], 'b-', 'LineWidth', 2); % 连杆1
    hold on;
    plot([x_B, x_C], [y_B, y_C], 'r-', 'LineWidth', 2); % 连杆2
    plot([x_C, x_D], [y_C, y_D], 'g-', 'LineWidth', 2); % 连杆3
    plot([l5, x_D], [0, y_D], 'm-', 'LineWidth', 2); % 连杆4
    plot([0, l5], [0, 0], 'k-', 'LineWidth', 3); % 连杆5 (基座)
    
    % 标记关键点
    plot(0, 0, 'ko', 'MarkerSize', 8, 'MarkerFaceColor', 'k'); % A点
    plot(x_B, y_B, 'bo', 'MarkerSize', 8, 'MarkerFaceColor', 'b'); % B点
    plot(x_C, y_C, 'ro', 'MarkerSize', 8, 'MarkerFaceColor', 'r'); % C点
    plot(x_D, y_D, 'go', 'MarkerSize', 8, 'MarkerFaceColor', 'g'); % D点
    plot(l5, 0, 'ko', 'MarkerSize', 8, 'MarkerFaceColor', 'k'); % E点
    
    % 标记腿长
    plot([0, x_C], [0, y_C], 'k--', 'LineWidth', 1); % 腿长线
    
    % 图形设置
    grid on;
    axis equal;
    xlabel('X (m)');
    ylabel('Y (m)');
    title('五连杆机构示意图');
    legend('连杆1', '连杆2', '连杆3', '连杆4', '基座', 'Location', 'best');
    
    % 添加文本标注
    text(0, 0-0.02, 'A', 'HorizontalAlignment', 'center');
    text(x_B, y_B+0.02, 'B', 'HorizontalAlignment', 'center');
    text(x_C, y_C+0.02, 'C(足端)', 'HorizontalAlignment', 'center');
    text(x_D, y_D+0.02, 'D', 'HorizontalAlignment', 'center');
    text(l5, 0-0.02, 'E', 'HorizontalAlignment', 'center');
    text(x_C/2, y_C/2, sprintf('腿长=%.3fm', leg_length), 'HorizontalAlignment', 'center');
    
catch ME
    fprintf('计算过程中出现错误: %s\n', ME.message);
    leg_length = NaN;
    foot_pos = [NaN, NaN];
end

%% 多组参数测试
fprintf('\n\n=== 多组参数测试 ===\n');
test_cases = [
    pi/6,  pi/4;   % 30°, 45°
    pi/4,  pi/3;   % 45°, 60°
    -pi/6, pi/2;   % -30°, 90°
    0,     pi/6;   % 0°, 30°
];

for i = 1:size(test_cases, 1)
    phi1_test = test_cases(i, 1);
    phi4_test = test_cases(i, 2);
    
    fprintf('\n测试 %d: phi1=%.1f°, phi4=%.1f°\n', i, rad2deg(phi1_test), rad2deg(phi4_test));
    
    % 重复计算过程（简化版）
    x_B_test = l1 * cos(phi1_test);
    y_B_test = l1 * sin(phi1_test);
    x_D_test = l5 + l4 * cos(phi4_test);
    y_D_test = l4 * sin(phi4_test);
    BD_dist_test = sqrt((x_D_test - x_B_test)^2 + (y_D_test - y_B_test)^2);
    
    if BD_dist_test <= (l2 + l3) && BD_dist_test >= abs(l2 - l3)
        cos_phi2_BD_test = (l2^2 + BD_dist_test^2 - l3^2) / (2 * l2 * BD_dist_test);
        if abs(cos_phi2_BD_test) <= 1
            alpha_BD_test = atan2(y_D_test - y_B_test, x_D_test - x_B_test);
            angle_offset_test = acos(cos_phi2_BD_test);
            phi2_test = alpha_BD_test + angle_offset_test;
            x_C_test = x_B_test + l2 * cos(phi2_test);
            y_C_test = y_B_test + l2 * sin(phi2_test);
            leg_length_test = sqrt(x_C_test^2 + y_C_test^2);
            fprintf('  结果: 腿长=%.3f m, 足端位置=(%.3f, %.3f)\n', leg_length_test, x_C_test, y_C_test);
        else
            fprintf('  结果: 余弦值超出范围\n');
        end
    else
        fprintf('  结果: 无法形成闭合机构\n');
    end
end

fprintf('\n计算完成！\n');