function planPathPRM()
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%function planPathPRM()
%
% Task: Finding the shortest path between the start and goal points (using dijkstra algorithm)
%       in a map of linked random points that explores the q1 q2 space (using Probabilistic RoadMaps algorithm)
%       
%
% Inputs: 
%
% Outputs:
%
% Thomas OLIVE (thomas.olive@ecam.fr)
% 18/12/2021
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  [S, G, randVmin, randVmax, L1, L2, threshold] = setParams();
  
  [nbNode, visGraph, points] = buildPRM();
  [distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(nbNode, visGraph);
  addpath("C:/Users/Admin/Documents/ProjectMotionPlanning/motion_planning");
  Q1plot = [];
  Q2plot = [];
  X_plot = [];
  Y_plot = [];
  GapValue = 1;
  nodeTrajectory = [columns(points) nodeTrajectory]; % we add the index of the last (goal) point to that array

  for i=1:columns(nodeTrajectory) %runs through the nodeTrajectory array
    Q1plot = [Q1plot points(1, nodeTrajectory(i))]; %we note the q1 and q2 values that are to be plot as the shortest path
    Q2plot = [Q2plot points(2, nodeTrajectory(i))];
  
    % we define the lines composing the shortest path using the usual Ax+B method
    if i>1
      
          
      A = (Q2plot(i-1)- Q2plot(i))/(Q1plot(i-1) - Q1plot(i));
      B = Q2plot(i) - A * Q1plot(i);

      Q2 = @(Q1) A*Q1+B;
          
      % we sample the line
      if Q1plot(i) > Q1plot(i-1)
        gap = -GapValue;
      else
        gap = GapValue;
      endif
      for g=Q1plot(i):gap:Q1plot(i-1)
        Q1test = g;
        Q2test = Q2(g);
        
        %we compute the x y coordinates that are matching the sample points
        % [Xtest, Ytest]=MyFK(2,1,Q1test,Q2test); remove the % on that line and add it on the next ones for faster computation
    
        jTee=dh2ForwardKinematics([Q1test; Q2test], [0; 0], [L1; L2], [0; 0], 1);
        Xtest = jTee(1,4);
        Ytest = jTee(2,4);
        
        X_plot = [X_plot Xtest];% we note the x and y values of the sample points
        Y_plot = [Y_plot Ytest]; 
      endfor
    endif
    
  endfor
  
  figure 1
  axis([-180 180 -180 180]);
  title('q1 q2 Joint Space');
  hold on
  plot(Q1plot, Q2plot, 'Color', 'g', 'LineWidth', 1.5)% we plot the shortest path in the q1 q2 joint space
  
  figure 2
  title('X-Y Cartesian Space');
  axis([-3 3 -3 3]);
  hold all
  text(X_plot, Y_plot, '*', 'FontSize', 10, 'Color', 'g');% we plot the shortest path in the x y cartesian space
  
  for i=2:columns(nodeTrajectory)-1
    text(points(3, nodeTrajectory(i)), points(4, nodeTrajectory(i)), int2str(nodeTrajectory(i)-1), 'FontSize', 20); % we add the description of each node
  endfor
  
  % we draw the two circles defining the workspace
  x = 3*cos(0:0.01*pi:2*pi);
  y = 3*sin(0:0.01*pi:2*pi);
  plot(x,y, 'Color', 'k');
  
  x = cos(0:0.01*pi:2*pi);
  y = sin(0:0.01*pi:2*pi);
  plot(x,y, 'Color', 'k');

endfunction