function [joint_angles, success] = robot_inverse_kinematics_all_solutions(initial_theta,target_pose)
%   ROBOT_INVERSE_KINEMATICS 求解六关节机器人逆运动学
%   输入参数：
%       initial_theta: 1x6向量，关节角度初始值（单位：弧度），用于迭代法求解的初始点
%       target_pose: 4x4矩阵，期望末端执行器位姿（齐次变换矩阵，包含位置和姿态）
%   输出参数：
%       joint_angles: 1x6向量，求解得到的关节角度（单位：弧度）
%       success: 逻辑值，true表示求解成功，false表示求解失败

    % 1. 验证输入合法性
    if length(initial_theta) ~= 6
        error('关节角度初始值必须为1x6向量');
    end
    if size(target_pose) ~= [4,4]
        error('期望末端位姿必须为4x4齐次变换矩阵');
    end

    % 提取当前各关节角度（用于后续选择最优解）
    robot_current_joints = [initial_theta(1),initial_theta(2),initial_theta(3),initial_theta(4),initial_theta(5),initial_theta(6)]

    % 2. 机器人DH结构参数（固定参数）
    d1 = 0;      a1 = 0.00;   alfa1 = -pi/2;  seta1_offset = 0.00;
    d2 = 305;    a2 = 0;      alfa2 = pi/2;   seta2_offset = pi/2;
    d3 = 0;      a3 = 785;    alfa3 = 0;      seta3_offset = pi/2;
    d4 = 0;      a4 = 774;    alfa4 = 0;      seta4_offset = 0;
    d5 = 0;      a5 = 0;      alfa5 = pi/2;   seta5_offset = pi/2;
    d6 = 485;    a6 = 0;      alfa6 = 0;      seta6_offset = 0;        
    
    % 3. 处理目标位姿：去除末端d6的影响
    TransZ_d6 = [1 0 0 0; 0 1 0 0; 0 0 1 d6; 0 0 0 1];  
    NPOS_Mod = target_pose * inv(TransZ_d6);  

    % 4. 提取姿态向量（目标位姿的旋转矩阵列向量）
    % 法线向量(nx, ny, nz) - 旋转矩阵第一列
    nx = NPOS_Mod(1,1);
    ny = NPOS_Mod(2,1);
    nz = NPOS_Mod(3,1);
    % 方向向量(ox, oy, oz) - 旋转矩阵第二列
    ox = NPOS_Mod(1,2);
    oy = NPOS_Mod(2,2);
    oz = NPOS_Mod(3,2);
    % 接近向量(ax, ay, az) - 旋转矩阵第三列
    ax = NPOS_Mod(1,3);
    ay = NPOS_Mod(2,3);
    az = NPOS_Mod(3,3);
    % 位置坐标(p_x, p_y, p_z)
    px = NPOS_Mod(1,4);
    py = NPOS_Mod(2,4);
    pz = NPOS_Mod(3,4);
    %% 求解逆运动学
    %% 1.第一关节位置
    seat1_Iv_1=atan((ax*NPOS_Mod(3,4) - az*NPOS_Mod(1,4)  )/ (-ay*NPOS_Mod(3,4)+az*NPOS_Mod(2,4)));
    seat1_Iv_2 = seat1_Iv_1 + pi;
    % 确保 seat1_Iv_2 落在 [-π, π] 内
    if seat1_Iv_2 > pi
        seat1_Iv_2 = seat1_Iv_2 - 2*pi;
    end
    seat1_Iv = [seat1_Iv_1,seat1_Iv_2];

    %% 2. 第二关节位置（基于seat1_Iv的2个解，每个生成2个解，共4个解）
    seat2_Iv_temp = zeros(1,4);  % 初始化4个解的数组
    for i = 1:2
        seat1 = seat1_Iv(i);  % 依次取seat1的两个解
        % 计算当前seat1对应的第一个解
        seat2_i1 = atan((NPOS_Mod(1,4)*cos(seat1) + NPOS_Mod(2,4)*sin(seat1)) / NPOS_Mod(3,4));
        % 计算当前seat1对应的第二个解（相差pi）
        seat2_i2 = seat2_i1 + pi;
        % 确保解落在[-pi, pi]内
        if seat2_i2 > pi
            seat2_i2 = seat2_i2 - 2*pi;
        end
        % 存入结果数组
        seat2_Iv_temp(2*i-1) = seat2_i1;
        seat2_Iv_temp(2*i) = seat2_i2;
    end

    seat2_Iv = seat2_Iv_temp;
    
    %% 3.第六关节位置（基于seat1_Iv的2个解，每个生成2个解，共4个解）
    seat6_Iv_temp = zeros(1,4);  % 初始化4个解的数组
    for i = 1:2
        seat1 = seat1_Iv(i);  % 依次取seat1的两个解
        % 计算当前seat1对应的第一个解
        numerator = NPOS_Mod(2,2)*cos(seat1) - NPOS_Mod(1,2)*sin(seat1);
        denominator = NPOS_Mod(1,1)*sin(seat1) - NPOS_Mod(2,1)*cos(seat1);
        seat6_i1 = atan(numerator / denominator);
        % 计算当前seat1对应的第二个解（相差pi）
        seat6_i2 = seat6_i1 + pi;
        % 确保解落在[-pi, pi]内
        if seat6_i2 > pi
            seat6_i2 = seat6_i2 - 2*pi;
        end
        % 存入结果数组
        seat6_Iv_temp(2*i-1) = seat6_i1;
        seat6_Iv_temp(2*i) = seat6_i2;
    end
        seat6_Iv = seat6_Iv_temp;
    

    %% 4. 第四关节位置：按(seat1, seat2)四个组合计算8个解
    seat4_Iv_temp = zeros(1,8);  % 存储8个解
    idx = 1;  % 解索引计数器
    
    % 组合1：seat1_Iv(1) + seat2_Iv(1)
    seat1 = seat1_Iv(1);
    seat2 = seat2_Iv(1);
    m = NPOS_Mod(3,4)*cos(seat2) + NPOS_Mod(1,4)*cos(seat1)*sin(seat2) + NPOS_Mod(2,4)*sin(seat1)*sin(seat2);
    n = NPOS_Mod(2,4)*cos(seat1) - NPOS_Mod(1,4)*sin(seat1) - d2;
    radicand = (m^2 + n^2 - a3^2 - a4^2) / (2*a3*a4);
    radicand = max(min(radicand, 1), -1);  % 限制输入范围
    seat4_Iv_temp(idx) = acos(radicand);
    seat4_Iv_temp(idx+1) = -acos(radicand);
    idx = idx + 2;
    
    % 组合2：seat1_Iv(1) + seat2_Iv(2)
    seat1 = seat1_Iv(1);
    seat2 = seat2_Iv(2);
    m = NPOS_Mod(3,4)*cos(seat2) + NPOS_Mod(1,4)*cos(seat1)*sin(seat2) + NPOS_Mod(2,4)*sin(seat1)*sin(seat2);
    n = NPOS_Mod(2,4)*cos(seat1) - NPOS_Mod(1,4)*sin(seat1) - d2;
    radicand = (m^2 + n^2 - a3^2 - a4^2) / (2*a3*a4);
    radicand = max(min(radicand, 1), -1);
    seat4_Iv_temp(idx) = acos(radicand);
    seat4_Iv_temp(idx+1) = -acos(radicand);
    idx = idx + 2;
    
    % 组合3：seat1_Iv(2) + seat2_Iv(1)
    seat1 = seat1_Iv(2);
    seat2 = seat2_Iv(1);
    m = NPOS_Mod(3,4)*cos(seat2) + NPOS_Mod(1,4)*cos(seat1)*sin(seat2) + NPOS_Mod(2,4)*sin(seat1)*sin(seat2);
    n = NPOS_Mod(2,4)*cos(seat1) - NPOS_Mod(1,4)*sin(seat1) - d2;
    radicand = (m^2 + n^2 - a3^2 - a4^2) / (2*a3*a4);
    radicand = max(min(radicand, 1), -1);
    seat4_Iv_temp(idx) = acos(radicand);
    seat4_Iv_temp(idx+1) = -acos(radicand);
    idx = idx + 2;
    
    % 组合4：seat1_Iv(2) + seat2_Iv(2)
    seat1 = seat1_Iv(2);
    seat2 = seat2_Iv(2);
    m = NPOS_Mod(3,4)*cos(seat2) + NPOS_Mod(1,4)*cos(seat1)*sin(seat2) + NPOS_Mod(2,4)*sin(seat1)*sin(seat2);
    n = NPOS_Mod(2,4)*cos(seat1) - NPOS_Mod(1,4)*sin(seat1) - d2;
    radicand = (m^2 + n^2 - a3^2 - a4^2) / (2*a3*a4);
    radicand = max(min(radicand, 1), -1);
    seat4_Iv_temp(idx) = acos(radicand);
    seat4_Iv_temp(idx+1) = -acos(radicand);
    idx = idx + 2;

    seat4_Iv = seat4_Iv_temp;

    %% 5. 第三关节位置：按指定(seat1, seat2)组合与对应seat4解计算，生成16个解（每个seat4对应2个[-pi,pi]范围内的解）
    seat3_Iv_temp = zeros(1, 16);  % 存储最终16个解
    seat3_idx = 1;  % 解索引计数器
    
    % 组合1：seat1_Iv(1) + seat2_Iv(1)，对应seat4_Iv(1)、seat4_Iv(2)
    seat1 = seat1_Iv(1);
    seat2 = seat2_Iv(1);
    m = NPOS_Mod(3,4)*cos(seat2) + NPOS_Mod(1,4)*cos(seat1)*sin(seat2) + NPOS_Mod(2,4)*sin(seat1)*sin(seat2);
    n = NPOS_Mod(2,4)*cos(seat1) - NPOS_Mod(1,4)*sin(seat1) - d2;
    % 处理seat4_Iv(1)
    seat4 = seat4_Iv(1);
    numerator = (a3 + a4*cos(seat4))*m - a4*n*sin(seat4);
    denominator = (a3 + a4*cos(seat4))*n + a4*m*sin(seat4);
    tan_val = numerator / denominator;
    % 计算第一个解（主值）并调整范围
    angle1 = atan(tan_val);
    % 计算第二个解（主值+pi）并调整范围
    angle2 = angle1 + pi;
    if angle2 > pi
        angle2 = angle2 - 2*pi;
    end
    seat3_Iv_temp(seat3_idx:seat3_idx+1) = [angle1, angle2];
    seat3_idx = seat3_idx + 2;
    % 处理seat4_Iv(2)
    seat4 = seat4_Iv(2);
    numerator = (a3 + a4*cos(seat4))*m - a4*n*sin(seat4);
    denominator = (a3 + a4*cos(seat4))*n + a4*m*sin(seat4);
    tan_val = numerator / denominator;
    angle1 = atan(tan_val);
    angle2 = angle1 + pi;
    if angle2 > pi
        angle2 = angle2 - 2*pi;
    end
    seat3_Iv_temp(seat3_idx:seat3_idx+1) = [angle1, angle2];
    seat3_idx = seat3_idx + 2;
    
    % 组合2：seat1_Iv(1) + seat2_Iv(2)，对应seat4_Iv(3)、seat4_Iv(4)
    seat1 = seat1_Iv(1);
    seat2 = seat2_Iv(2);
    m = NPOS_Mod(3,4)*cos(seat2) + NPOS_Mod(1,4)*cos(seat1)*sin(seat2) + NPOS_Mod(2,4)*sin(seat1)*sin(seat2);
    n = NPOS_Mod(2,4)*cos(seat1) - NPOS_Mod(1,4)*sin(seat1) - d2;
    % 处理seat4_Iv(3)
    seat4 = seat4_Iv(3);
    numerator = (a3 + a4*cos(seat4))*m - a4*n*sin(seat4);
    denominator = (a3 + a4*cos(seat4))*n + a4*m*sin(seat4);
    tan_val = numerator / denominator;
    angle1 = atan(tan_val);
    angle2 = angle1 + pi;
    if angle2 > pi
        angle2 = angle2 - 2*pi;
    end
    seat3_Iv_temp(seat3_idx:seat3_idx+1) = [angle1, angle2];
    seat3_idx = seat3_idx + 2;
    % 处理seat4_Iv(4)
    seat4 = seat4_Iv(4);
    numerator = (a3 + a4*cos(seat4))*m - a4*n*sin(seat4);
    denominator = (a3 + a4*cos(seat4))*n + a4*m*sin(seat4);
    tan_val = numerator / denominator;
    angle1 = atan(tan_val);
    angle2 = angle1 + pi;
    if angle2 > pi
        angle2 = angle2 - 2*pi;
    end
    seat3_Iv_temp(seat3_idx:seat3_idx+1) = [angle1, angle2];
    seat3_idx = seat3_idx + 2;
    
    % 组合3：seat1_Iv(2) + seat2_Iv(1)，对应seat4_Iv(5)、seat4_Iv(6)
    seat1 = seat1_Iv(2);
    seat2 = seat2_Iv(1);
    m = NPOS_Mod(3,4)*cos(seat2) + NPOS_Mod(1,4)*cos(seat1)*sin(seat2) + NPOS_Mod(2,4)*sin(seat1)*sin(seat2);
    n = NPOS_Mod(2,4)*cos(seat1) - NPOS_Mod(1,4)*sin(seat1) - d2;
    % 处理seat4_Iv(5)
    seat4 = seat4_Iv(5);
    numerator = (a3 + a4*cos(seat4))*m - a4*n*sin(seat4);
    denominator = (a3 + a4*cos(seat4))*n + a4*m*sin(seat4);
    tan_val = numerator / denominator;
    angle1 = atan(tan_val);
    angle2 = angle1 + pi;
    if angle2 > pi
        angle2 = angle2 - 2*pi;
    end
    seat3_Iv_temp(seat3_idx:seat3_idx+1) = [angle1, angle2];
    seat3_idx = seat3_idx + 2;
    % 处理seat4_Iv(6)
    seat4 = seat4_Iv(6);
    numerator = (a3 + a4*cos(seat4))*m - a4*n*sin(seat4);
    denominator = (a3 + a4*cos(seat4))*n + a4*m*sin(seat4);
    tan_val = numerator / denominator;
    angle1 = atan(tan_val);
    angle2 = angle1 + pi;
    if angle2 > pi
        angle2 = angle2 - 2*pi;
    end
    seat3_Iv_temp(seat3_idx:seat3_idx+1) = [angle1, angle2];
    seat3_idx = seat3_idx + 2;
    
    % 组合4：seat1_Iv(2) + seat2_Iv(2)，对应seat4_Iv(7)、seat4_Iv(8)
    seat1 = seat1_Iv(2);
    seat2 = seat2_Iv(2);
    m = NPOS_Mod(3,4)*cos(seat2) + NPOS_Mod(1,4)*cos(seat1)*sin(seat2) + NPOS_Mod(2,4)*sin(seat1)*sin(seat2);
    n = NPOS_Mod(2,4)*cos(seat1) - NPOS_Mod(1,4)*sin(seat1) - d2;
    % 处理seat4_Iv(7)
    seat4 = seat4_Iv(7);
    numerator = (a3 + a4*cos(seat4))*m - a4*n*sin(seat4);
    denominator = (a3 + a4*cos(seat4))*n + a4*m*sin(seat4);
    tan_val = numerator / denominator;
    angle1 = atan(tan_val);
    angle2 = angle1 + pi;
    if angle2 > pi
        angle2 = angle2 - 2*pi;
    end
    seat3_Iv_temp(seat3_idx:seat3_idx+1) = [angle1, angle2];
    seat3_idx = seat3_idx + 2;
    % 处理seat4_Iv(8)
    seat4 = seat4_Iv(8);
    numerator = (a3 + a4*cos(seat4))*m - a4*n*sin(seat4);
    denominator = (a3 + a4*cos(seat4))*n + a4*m*sin(seat4);
    tan_val = numerator / denominator;
    angle1 = atan(tan_val);
    angle2 = angle1 + pi;
    if angle2 > pi
        angle2 = angle2 - 2*pi;
    end
    seat3_Iv_temp(seat3_idx:seat3_idx+1) = [angle1, angle2];
    seat3_idx = seat3_idx + 2;
    
    seat3_Iv = seat3_Iv_temp;

    %% 6. 第五关节位置：按要求重新计算，生成所有合法解
    seat5_Iv_temp = [];  % 存储所有第五关节解
    seat5_idx = 1;       % 解索引计数器
    
    % 遍历所有seat2_Iv解（共4个）
    for s2_idx = 1:length(seat2_Iv)
        seat2 = seat2_Iv(s2_idx);
        
        % 计算cos(seat2)，避免除零错误
        cos_seat2 = cos(seat2);
        if abs(cos_seat2) < 1e-6
            error(['第', num2str(s2_idx), '个seat2解的cos值接近零，无法计算asin']);
        end
        
        % 计算asin输入并做数值稳定性处理
        asin_input = NPOS_Mod(3,3) / cos_seat2;
        asin_input = max(min(asin_input, 1), -1);  % 限制在[-1,1]避免数值错误
        
        % 生成asin解在[-pi, pi]内的两个解
        asin_sol1 = asin(asin_input);          % 主值解（[-pi/2, pi/2]）
        asin_sol2 = pi - asin_sol1;            % 对称解
        % 确保第二个解落在[-pi, pi]区间
        if asin_sol2 > pi
            asin_sol2 = asin_sol2 - 2*pi;
        end
        current_asin_sols = [asin_sol1, asin_sol2];  % 每个seat2对应2个asin解
        
        % 遍历当前seat2对应的2个asin解
        for a_idx = 1:length(current_asin_sols)
            current_asin = current_asin_sols(a_idx);
            
            % 遍历所有seat4_Iv解（共8个）
            for s4_idx = 1:length(seat4_Iv)
                current_seat4 = seat4_Iv(s4_idx);
                
                % 遍历所有seat3_Iv解（共16个）
                for s3_idx = 1:length(seat3_Iv)
                    current_seat3 = seat3_Iv(s3_idx);
                    
                    % 计算seat5基础值
                    seat5_base = current_asin + current_seat3 - current_seat4;
                    
                    % 生成seat5在[-pi, pi]内的两个解（相差2pi，取落在区间内的两个）
                    seat5_sol1 = mod(seat5_base + pi, 2*pi) - pi;  % 主解
                    seat5_sol2 = mod(seat5_sol1 + pi, 2*pi) - pi;  % 相差pi的对称解（确保在区间内）
                    
                    % 将两个解存入结果数组（避免重复，仅当两解不同时都存入）
                    if abs(seat5_sol1 - seat5_sol2) > 1e-6  % 排除数值误差导致的重复解
                        seat5_Iv_temp(seat5_idx) = seat5_sol1;
                        seat5_idx = seat5_idx + 1;
                        seat5_Iv_temp(seat5_idx) = seat5_sol2;
                        seat5_idx = seat5_idx + 1;
                    else
                        % 两解数值相同，仅存入一个
                        seat5_Iv_temp(seat5_idx) = seat5_sol1;
                        seat5_idx = seat5_idx + 1;
                    end
                end
            end
        end
    end

    % 定义seat5_Iv用于后续验算（去重后保留所有唯一解）
    % 去除重复解（因不同组合可能生成相同解）
    [~, unique_idx] = unique(round(seat5_Iv_temp, 6));  % 保留6位小数去重
    seat5_Iv = seat5_Iv_temp(unique_idx);
    % disp(['第五关节共生成 ', num2str(length(seat5_Iv)), ' 个唯一解']);
    

%% 正向运动学验算：按关节解对应关系组合，筛选有效解
    joint_angles_temp = [];  % 存储符合要求的关节角度组合
    success = false;         % 初始化成功标志
    
    % 关键：明确关节解的对应关系（按前文求解逻辑的组合依赖）
    % seat1（2个解）→ 每个seat1对应2个seat2解 → 每个seat2对应2个seat4解 → 每个seat4对应2个seat3解
    % seat1（2个解）→ 每个seat1对应2个seat6解
    % seat5为独立去重解（所有组合共用）
    for s1_idx = 1:length(seat1_Iv)
        theta1 = seat1_Iv(s1_idx);  % 取当前seat1解
        
        % 对应seat2解：seat1第1个解对应seat2(1-2)，seat1第2个解对应seat2(3-4)
        s2_start = (s1_idx - 1)*2 + 1;
        s2_end = s1_idx*2;
        for s2_idx = s2_start:s2_end
            theta2 = seat2_Iv(s2_idx);  % 取当前seat2解
            
            % 对应seat4解：seat1+seat2组合对应2个seat4解（共4组组合，对应seat4(1-8)）
            s4_start = ( (s1_idx - 1)*2 + (s2_idx - s2_start + 1) - 1 )*2 + 1;
            s4_end = s4_start + 1;
            for s4_idx = s4_start:s4_end
                theta4 = seat4_Iv(s4_idx);  % 取当前seat4解
                
                % 对应seat3解：每个seat4对应2个seat3解
                s3_start = (s4_idx - 1)*2 + 1;
                s3_end = s4_idx*2;
                for s3_idx = s3_start:s3_end
                    theta3 = seat3_Iv(s3_idx);  % 取当前seat3解
                    
                    % 对应seat6解：每个seat1对应2个seat6解
                    s6_start = (s1_idx - 1)*2 + 1;
                    s6_end = s1_idx*2;
                    for s6_idx = s6_start:s6_end
                        theta6 = seat6_Iv(s6_idx);  % 取当前seat6解
                        
                        % 遍历所有独立的seat5解（去重后的所有合法解）
                        for s5_idx = 1:length(seat5_Iv)
                            theta5 = seat5_Iv(s5_idx);  % 取当前seat5解
                            
                            % 组合完整6关节角度
                            current_thetas = [theta1, theta2, theta3, theta4, theta5, theta6];
                            
                            % 计算正向运动学（需确保robot_forward_kinematics函数正确实现DH变换）
                            current_pose = robot_forward_kinematics(current_thetas);
                            
                            % 位姿对比：允许微小浮点误差（5位小数精度）
                            pose_diff = abs(target_pose - current_pose);
                            if all(pose_diff(:) < 1e-5)  % 所有元素差值小于1e-5判定为一致
                                % 去重：避免相同关节组合重复存入
                                if isempty(joint_angles_temp)
                                    joint_angles_temp = [joint_angles_temp; current_thetas];
                                    success = true;
                                else
                                    % 检查当前组合是否与已有组合重复（误差1e-6）
                                    diff_matrix = abs(joint_angles_temp - current_thetas);
                                    is_duplicate = any(all(diff_matrix < 1e-6, 2));
                                    if ~is_duplicate
                                        joint_angles_temp = [joint_angles_temp; current_thetas];
                                        success = true;
                                    end
                                end
                            end
                        end
                    end
                end
            end
        end
    end
    
    % 输出结果处理
    if success
%         disp(['求解成功，共找到 ', num2str(size(joint_angles_temp, 1)), ' 组唯一有效解']);
    else
        disp('未找到符合要求的解');
        joint_angles_temp = [];  % 无有效解时返回空
    end

%% 最小变动量：保存返回success=true的关节角度
%%最小变动量：筛选与初始关节角度差值最小的关节组合
joint_angles_final = [];  % 存储差值最小的最优关节角度
if success && ~isempty(joint_angles_temp)
%     disp('开始计算各关节组合与初始角度的差值，筛选最优解...');
    
    % 初始化差值存储数组（每行对应一组关节角度的总差值）
    total_diffs = zeros(size(joint_angles_temp, 1), 1);
    
    % 遍历所有有效关节组合，计算每组与初始角度的绝对差值总和
    for i = 1:size(joint_angles_temp, 1)
        % 计算当前组与初始关节角度的对应位置绝对差值
        joint_diff = abs(joint_angles_temp(i, :) - robot_current_joints);
        % 计算差值总和（表征整体变动量）
        total_diffs(i) = sum(joint_diff);
    end
    
    % 找到差值总和最小的索引（若有多个最小值，取第一个）
    [min_diff, min_idx] = min(total_diffs);
    
    % 提取差值最小的关节角度组合
    joint_angles_final = joint_angles_temp(min_idx, :);
    
    % 输出筛选结果
%     disp(['筛选完成！最优关节角度组合为第', num2str(min_idx), '组']);
%     disp(['与初始角度的总变动量（弧度）：', num2str(min_diff)]);
%     disp(['最优关节角度：[', num2str(joint_angles_final(1)), ', ', num2str(joint_angles_final(2)), ', ', ...
%           num2str(joint_angles_final(3)), ', ', num2str(joint_angles_final(4)), ', ', ...
%           num2str(joint_angles_final(5)), ', ', num2str(joint_angles_final(6)), ']']);

    disp('求解成功');
    % 更新函数输出，将最优解作为返回值
    joint_angles = joint_angles_final
    success = ~isempty(joint_angles_final);
else
    error('无有效关节角度组合，无法筛选最优解');
    joint_angles_final = [];
end
   
end