## Author: adril <adril@LAPTOP-EJ1AIJHT>
## Created: 2022-12-06

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%function buildPRM (rangeQ1Q2, nbPoints, L1, L2, MapFilename)
%
% Task:
%
% Inputs:
%	- rangeQ1Q2 : range of values (in degrees) acceptable for joints Q1 and Q2
% - nbPoints : number of points required
% - L1, L2 : lengths of the links (in m)
% - MapFilename : the name of the file to be saved for the map
%
% Outputs:
%	- None
%
% Adrien Lasserre (adrien.lasserre@ecam.fr) & Gwenn Durpoix-Espinasson (g.durpoix-espinasson@ecam.fr)
% 06/12/2022
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function buildPRM (rangeQ1Q2, nbPoints, L1, L2, MapFilename)
  Points=zeros(1, nbPoints);
  MatrixOfLinks=zeros(nbPoints, nbPoints);
  alpha=[0;0];
  d=[0;0];
  a=[L1;L2];
  jointNumber=[1;2];

  for i=1:nbPoints

    [q1;q2]=[rand()*(rangeQ1Q2(1,2)-rangeQ1Q2(1,1))+rangeQ1Q2(1,1);rand()*(rangeQ1Q2(2,2)-rangeQ1Q2(2,1))+rangeQ1Q2(2,1)];

    theta=[q1;q2];
    OutOfRange=0;

    bTee=dh2ForwardKinematics(theta, d, a, alpha, jointNumber);
    jTee=bTee(1:2, 4);

    if (jTee(2,1)>=L1)
      OutOfRange=1;
    else if (jTee(2,1)<=-L1)
      OutOfRange=1;
    else if (abs(jTee(1,1)) <= L2 && abs(jTee(2,1)) <=L2)
      OutOfRange=1;
    endif

    if (OutOfRange==0)
      Points(i)=jTee;
      MatrixOfLinks(i, i)=1;
      for j=1:i
        intersect=0;
        if

          intersect=1;

        else

          MatrixOfLinks(i, j)=1;
          MatrixOfLinks(j,i)=1;

        endif



      endfor
    endif

  endfor


endfunction