function [nbNodes, obstacle, points] = buildPRM()
  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%function [nbNodes, obstacle, points] = buildPRM()
%
% Task: Implement a code that creates a map of random points in the q1 q2 joint space
%       and checks the presence of an obstacle between each of them in the x y cartesian space
%       That algorithm is called Probabilistic RoadMaps (PRM)
%
% Inputs: q1 random variable
%	        q2 random variable
%
% Outputs:
%	-nbNodes: number of nodes of the graph excluding the starting and goal points
%	-obstacle: a matrix containing the distance between connected nodes 
%		(NaN refers to not connected nodes)
%		The matrix has a size of (nbNodes+2)x(nbNodes+2)
%-points: a matrix saving the q1 q2 and x y coordinates of each point,
%         the 1st column describing the 1st (start) point
%	
%		
%		
%
% Thomas OLIVE (thomas.olive@ecam.fr)
% 18/12/2021
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  clc
  close all
  addpath("C:/Users/Admin/Documents/ProjectMotionPlanning/motion_planning");
  
  [S, G, randVmin, randVmax, L1, L2, threshold] = setParams();
  
  
  threshold = 0.000001;
  
  [nbSol, qi] = solveIK2LinkPlanarRobot(2, 1, 2, 0);
  Sq1 = qi(1,1)
  Sq2 = qi(1,2)
  
  [nbSol, qi] = solveIK2LinkPlanarRobot(2, 1, -2, 0);
  Gq1 = qi(1,1)
  Gq2 = qi(1,2)
  
  points = [Sq1 Sq2 S(1) S(2)]';
  obstacle = [];
  GapValue = 10;
  X_space = [S(1) G(1)];
  Y_space = [S(2) G(2)]; 
  
  n=columns(points); % represents the number of validated points
  WhileCond = 12; % number of desired points in the tree
  while n<WhileCond
    
    q1 = randVmin + (randVmax - randVmin)* rand();% setting a new random point r
    q2 = randVmin + (randVmax - randVmin)* rand();
    
    %[x, y]=MyFK(L1,L2,q1,q2); remove the '%' on that line and add it on the next ones for faster computation
    jTee=dh2ForwardKinematics([q1; q2], [0; 0], [L1; L2], [0; 0], 1);
    x= jTee(1,4);
    y= jTee(2,4);
    %Checking if the new random point (end effector) is part of a known obstacle in X Y cartesian space
    if not((y >= L1 - threshold || y <= -L1 + threshold || (x>=-L2 - threshold && x<=L2 - threshold && y>=-L2 - threshold && y<=L2 + threshold))
    
      n=columns(points);
      
      if n==WhileCond-1    % if it is the last iteration (concerns the goal point)   
        figure 1
        hold on
        text(Gq1, Gq2, 'G', 'FontSize', 20); %text function is used to plot points with their description
        
        
        figure 2
        hold on
        text(G(1), G(2), 'G', 'FontSize', 20);
        
        q1 = Gq1; % the values are not random but known
        q2 = Gq2;
        x = G(1);
        y = G(2);
        
        points = [points [q1;q2;x;y]]; %Store q1 q2 x y values
        
        obstacle(n+1,n+1) = NaN;
      elseif n==1    % if it is the first iteration (concerns the start point)     
        figure 1
        hold on
        text(Sq1, Sq2, 'S', 'FontSize', 20);
        text(q1, q2, int2str(n), 'FontSize', 20);
        
        
        figure 2
        hold on
        text(S(1), S(2), 'S', 'FontSize', 20);
        
        obstacle(n,n) = NaN; % enlarges the obstacle matrix so we can write later the valid links computed around the new point
        obstacle(n+1,n+1) = NaN;
      
        points = [points [q1;q2;x;y]];
      elseif n<WhileCond      
        figure 1
        hold on
        text(q1, q2, int2str(n), 'FontSize', 20);
        
        points = [points [q1;q2;x;y]];
        obstacle(n+1,n+1) = NaN;
      endif
      drawnow
      

      for j=1:n % for each point of the map
        q1 = points(1,n); % we are going to compute links between the new point (q1 q2)
        q2 = points(2,n);
        q1_stored = points(1,j); % and each one that is stored (q1_stored q2_stored)
        q2_stored = points(2,j);
        
        
        obstacle(j,n) = checkingLine(GapValue, L1, L2, n, j, points, threshold); % we store the distance (NaN if invalid) between the points in a matrix
        obstacle(n,j) = checkingLine(GapValue, L1, L2, n, j, points, threshold); % that is designed as a double entry table
        
        if ~isnan(obstacle(j, n)) && n!=j % if the points of index n and j can be linked
            Xplot = [q1, q1_stored];
            Yplot = [q2, q2_stored];
            figure 1
            plot(Xplot, Yplot, 'Color', 'b') % we plot the link between them
            drawnow
         endif
      endfor
      
    endif
  endwhile
  nbNodes = WhileCond-2;
endfunction