```close all; clear; clc

robot = importrobot('/home/saturn/Downloads/PJ4/PJ4_W3_six_link_robot_4REALZ_correct.urdf', 'DataFormat', 'column');
%fingy = importrobot('/home/saturn/Downloads/PJ4/cilly.urdf', 'DataFormat', 'column');

end_effector_name = robot.BodyNames{end};
joint_limits = [-pi/2, pi/2; -pi/4, pi/4; -pi/9, pi/9; -pi/2, pi/2; -pi/12, pi/2; -pi, pi;];

%%%%%%% PICK FUNCTION TO USE (systemic, rand_1, rand_2)
workspace_points = rand_2(500, joint_limits, robot, end_effector_name);
%%%%%%%

figure;
hold on;

% POINTS
scatter3(workspace_points(:,1), workspace_points(:,2), workspace_points(:,3), ...
    15, 'g', 'filled', 'MarkerFaceAlpha', 0.8);

% HULL
% Below is convex which is less accurate
%shp = alphaShape(workspace_points(:,1), workspace_points(:,2), workspace_points(:,3), 10);
%plot(shp, 'FaceColor', 'r', 'FaceAlpha', 0.5, 'EdgeColor', 'none');

[k, V] = convhull(workspace_points(:,1), workspace_points(:,2), workspace_points(:,3));
trisurf(k, workspace_points(:,1), workspace_points(:,2), workspace_points(:,3), ...
            'FaceColor', 'cyan', 'FaceAlpha', 0, 'EdgeColor', 'none');

liters = double(V)/1000 %urdf is in cm so convert from cm^2 to liters


% ROBOT +  fingy
q0 = [0; 0; 0; 0; 0; 0];
show(robot, q0, 'Visuals', 'on', 'PreservePlot', false);
%show(fingy, [0], 'Visuals', 'on', 'PreservePlot', false);

% PLOTTING
set(gcf, 'Color', 'w')
set(gcf, 'Position', [100, 100, 800, 800])
title('Robot Workspace Visualization', 'FontSize', 16)
xlabel('$x$', 'Interpreter', 'latex', 'FontSize', 16)
ylabel('$y$', 'Interpreter', 'latex', 'FontSize', 16)
zlabel('$z$', 'Interpreter', 'latex', 'FontSize', 16)

ax = gca;
ax.Projection = 'perspective';
grid on;
axis equal;
axis vis3d;
light;
rotate3d on
shg

% SYSTEMIC
function [workspace_points] = systemic(num_samples, joint_limits, robot, end_effector_name)
    workspace_points = zeros(num_samples^6, 3);
    valid_point_count = 0;

    q1 = linspace(joint_limits(1,1), joint_limits(1,2), num_samples);
    q2 = linspace(joint_limits(2,1), joint_limits(2,2), num_samples);
    q3 = linspace(joint_limits(3,1), joint_limits(3,2), num_samples);
    q4 = linspace(joint_limits(4,1), joint_limits(4,2), num_samples);
    q5 = linspace(joint_limits(5,1), joint_limits(5,2), num_samples);
    q6 = linspace(joint_limits(6,1), joint_limits(6,2), num_samples);
    
    for i = 1:length(q1)
        for j = 1:length(q2)
            for k = 1:length(q3)
                for l = 1:length(q4)
                    for m = 1:length(q5)
                        for n = 1:length(q6)
                            q = [q1(i); q2(j); q3(k); q4(l); q5(m); q6(n)];
    
                            try
                                T = getTransform(robot, q, end_effector_name, robot.BaseName);
                                valid_point_count = valid_point_count + 1;
                                workspace_points(valid_point_count, :) = T(1:3,4)';
                            catch
                                % Skip invalid configurations
                            end
                        end
                    end
                end
            end
        end
    end
end

% RANDOM 1
function [workspace_points] = rand_1(num_samples, joint_limits, robot, end_effector_name)
    workspace_points = zeros(num_samples, 3);
    valid_point_count = 0;

    for i = 1:num_samples
        % Generate random joint angles within limits
        q_random = zeros(6,1);
        for j = 1:6
            q_random(j) = joint_limits(j,1) + (joint_limits(j,2)-joint_limits(j,1))*rand();
        end

        % Get end-effector position
        try
            T = getTransform(robot, q_random, end_effector_name, robot.BaseName);
            % Store the valid point
            valid_point_count = valid_point_count + 1;
            workspace_points(valid_point_count, :) = T(1:3,4)';
        catch
            
        end
    end

    % Trim the array to only include valid points
    workspace_points = workspace_points(1:valid_point_count, :);
end


% RANDOM 2 
function [workspace_points] = rand_2(num_samples, joint_limits, robot, end_effector_name)
    workspace_points = zeros(num_samples, 3);

    for i = 1:num_samples
        q_random = randomConfiguration(robot);
        T_ee = getTransform(robot, q_random, end_effector_name);
        workspace_points(i, :) = T_ee(1:3, 4)';
    end
end```  I want to check whether the ee fully covers all positions inside the urdf of fingy