function BUILDPRM(L1, L2, nbPoints)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
% FUNCTION BUILDPRM
%
% INPUTS : 
%   - L1 : length of first link in mm
%   - L2 : length of first link in mm
%   - nbPoints : number of valid points in the RoadMap
%
% OUTPUTS : 
%   - None
%
% Author : Rémi Bussiere, remi.bussiere@ecam.fr
% Date : 03/12/2023
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

q1q2_valid = [];
counter = 0;
points = [];
valid  = zeros(12, 12);
points = [ points ; [2, 0.01]];

while(size(q1q2_valid) < nbPoints)
  % chooses random values for q1 and q2
  q1 = rand()*360.0;
  q2 = rand()*360.0;

  % Create D-H table
  theta = [q1, q2];
  d =     [0, 0];
  a =     [L1, L2];
  alpha = [0, 0];

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

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

  % check if it hits obstacles
  eeHittingObstacle = 0;
  if (position_ee(2) >= L1 )
    eeHittingObstacle = 1;
    endif
  if (position_ee(2) <= -L1 )
    eeHittingObstacle = 1;
    endif 
  if (position_ee(1) >= -L2 && position_ee(1) <= L2)
    eeHittingObstacle = 1;
  endif
  if (position_ee(2) >= -L2 && position_ee(2) <= L2)
    eeHittingObstacle = 1;
  endif
  
  % stores the position of the end-effector
  if(eeHittingObstacle == 0)
    q1q2_valid = [q1q2_valid; theta];
    x = position_ee(1);
    y = position_ee(2);
    points = [ points ; [x, y]];
  endif
  
endwhile
  points = [ points ; [-2, 0.01]];
  disp(points);
  for i = 1:length(points)
    x_old = points(i, 1);
    y_old = points(i, 2);
    for j = 1:length(points)
      x = points(j, 1);
      y = points(j, 2);
      a_eq = (y_old - y)/(x_old - x);
      b_eq = y - a_eq * x;
      intersectest = 0;
      
      if ( a_eq * L2 + b_eq >= - L2 && a_eq * L2 + b_eq <= L2)
        intersectest = 1;
      endif
      if ( a_eq * (-L2) + b_eq >= - L2 && a_eq * (-L2) + b_eq <= L2 )
        intersectest = 1;
      endif
      if ( ( L2 - b_eq )/a_eq >= -L2 && ( L2 - b_eq )/a_eq <= L2)
        intersectest = 1;
      endif
      if ( ( (-L2) - b_eq )/a_eq >= -L2 && ( (-L2) - b_eq )/a_eq <= L2)
        intersectest = 1;
      endif
      
      if( intersectest == 0 )
        valid(i, j) = 1;
        x_old = x;
        y_old = y;

      endif
    endfor
  endfor
  %compute the djisktra algorithm
  [nbNodes, visibilityGraph] = createVisibilityGraph(valid, points);
  dijkstra(nbNodes, visibilityGraph);
  
  % Plot the points with text labels
  figure;
  plot(points(:, 1), points(:, 2), 'o', 'MarkerSize', 2);
  hold on;
  title('Set of Points');
  xlabel('X-axis');
  ylabel('Y-axis');
  grid on;

  % Add text labels next to each point
  for i = 1:size(points, 1)
      text(points(i, 1), points(i, 2) + 0.1, num2str(i), 'HorizontalAlignment', 'center', 'FontSize', 12);
  end
  
  side_length = L2;
  half_side = side_length;
  x = [-half_side, -half_side, half_side, half_side, -half_side];
  y = [-half_side, half_side, half_side, -half_side, -half_side];
  plot(x, y, '-o');
endfunction