function buildPRM(L1, L2, nbPoints)
%%%%%%%%%%%%%%%%%%
%function buildPRM(L1, L2, nbPoints)
% ex. buildPRM(2000, 1000, 10)
%
% Inputs:
%	-L1: lenght of the first link (in mm)
%	-L2: length of the second link (in mm)
%	-nbPoints: the number of valid points in the roadmap
%
% Outputs: None
%
% author: Camille CONJAT, camille.conjat@ecam.fr
% date: 03/12/2023
%%%%%%%%%%%%%%%%%%

q1q2_valid = [];
counter = 0;

% creates a figure
figure;

while (size(q1q2_valid,2) < nbPoints)
	% samples randomly the joint space
	q1 = rand()*360.0;
	q2 = rand()*360.0;

	% creates the DH table
	theta = [q1; q2];
	d = [0; 0];
	a = [L1; L2];
	alpha = [0; 0];

	% computes the FK
	wTee = dh2ForwardKinematics(theta, d, a, alpha, 1);

	% determines the position of the end-effector
	position_ee = wTee(1:2,end);

	% checks if the end-effector is not hitting any obstacle
	eeHittingObstacle = 0;
	if (position_ee(2) >= L1)
		eeHittingObstacle = 1;
	end
	if (position_ee(2) <= -L1)
		eeHittingObstacle = 1;
	end
	if (position_ee(1) >= -L2 && position_ee(1) <= L2 && position_ee(2) >= -L2 && position_ee(2) <= L2)
		eeHittingObstacle = 1;
	end

	% stores these random values
	if (eeHittingObstacle == 0)
		q1q2_valid = [q1q2_valid theta];

		% plots the new valid sample
		subplot(1,2,1);
		plot(q1,q2, '+g'); hold on;
		subplot(1,2,2);
		plot(position_ee(1),position_ee(2), '+g'); hold on;
	else
		% plots the new invalid sample
		subplot(1,2,1);
		plot(q1,q2, '*r'); hold on;
		subplot(1,2,2);
		plot(position_ee(1),position_ee(2), '*r'); hold on;
	end

	counter = counter + 1;
end

% displays stats
fprintf("%d points were sorted to achieve %d valid points\n", counter, nbPoints)

% add limits, labels and obstacles to the map
subplot(1,2,1);
	xlim([0 360]);
	ylim([0 360]);

	xlabel('q1(deg)');
	ylabel('q2(deg)');
	title('Joint space');


subplot(1,2,2);
	plot([-L2; L2], [L2; L2], 'r'); hold on;
	plot([L2; L2], [L2; -L2], 'r'); hold on;
	plot([L2; -L2], [-L2; -L2], 'r'); hold on;
	plot([-L2; -L2], [-L2; L2], 'r'); hold on;

	plot([-(L1+L2); L1+L2], [L1 ; L1], 'r'); hold on;
	plot([-(L1+L2); L1+L2], [-L1 ; -L1], 'r'); hold on;

	drawCircle(0,0, L1+L2);
	drawCircle(0,0, L1-L2);

	xlim([-(L1+L2) (L1+L2)]);
	ylim([-(L1+L2) (L1+L2)]);

	xlabel('x(mm)');
	ylabel('y(mm)');
	title('Cartesian space');

% creates a connection map
connectionMap = zeros(nbPoints, nbPoints);

for l = 1:size(q1q2_valid, 2)
    for m = 1:size(q1q2_valid, 2)
        if (l ~= m)
            % creates the DH table
            thetaA = q1q2_valid(:, l);
            d = [0; 0];
            a = [L1; L2];
            alpha = [0; 0];

            % computes the FK
            wTeeA = dh2ForwardKinematics(thetaA, d, a, alpha, 1);
            pointA = wTeeA(1:2, end);

            % creates the DH table
            thetaB = q1q2_valid(:, m);

            % computes the FK
            wTeeB = dh2ForwardKinematics(thetaB, d, a, alpha, 1);
            pointB = wTeeB(1:2, end);

            % Check for collisions along the line segment
            collision = checkCollisionAlongLine(pointA, pointB, L1, L2);

            % if no collision
            if ~collision
                connectionMap(l, m) = 1;

                % Plot the line between connected points with color representing the slope
                color = [rand(), rand(), rand()]; % Random color for each line
                subplot(1,2,2);
                plot([pointA(1), pointB(1)], [pointA(2), pointB(2)], 'color', color); hold on;
            end
        end
    end
end

% Add limits, labels, and obstacles to the joint space map
subplot(1,2,1);
xlim([0 360]);
ylim([0 360]);
xlabel('q1(deg)');
ylabel('q2(deg)');
title('Joint space');


% Plot connection links in joint space
for l = 1:size(q1q2_valid, 2)
    for m = 1:size(q1q2_valid, 2)
        if connectionMap(l, m)
            % Plot the line between connected points in joint space
            plot([q1q2_valid(1, l), q1q2_valid(1, m)], [q1q2_valid(2, l), q1q2_valid(2, m)], 'color', [0.5, 0.5, 0.5]); hold on;
        end
    end
end

% Save the relevant variables to a MAT file
save('prm_data.mat', 'q1q2_valid', 'connectionMap', 'L1', 'L2', 'nbPoints');
end