function planPathRRT
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%function planPathPRM()
%
% Task: Finding the shortest path between the start and goal points (using dijkstra algorithm)
%       in a tree of linked random points that explores the q1 q2 space (using Rapidly-exploring Random Trees algorithm)
%       
%
% Inputs: 
%
% Outputs:
%
% Thomas OLIVE (thomas.olive@ecam.fr)
% 19/12/2021
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  addpath("C:/Users/Admin/Documents/ProjectMotionPlanning/motion_planning"); % gets access to the scripts that are stored in that folder
  
  [S, G, randVmin, randVmax, L1, L2, threshold] = setParams();
  
  GapValue=1;
  [nbNode, visGraph, points] = buildRRT();
  
  for i=1:columns(visGraph)
    for j=1:columns(visGraph)
      if visGraph(i,j)==0 % if a link has not been noted with the distance
        visGraph(i,j) = NaN; % we set it as invalid
      endif
    endfor
  endfor
  
  [distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(nbNode, visGraph);
  
  Q1plot = [];
  Q2plot = [];
  X_plot = [];
  Y_plot = [];
  
  nodeTrajectory = [columns(points) nodeTrajectory]; % we add the index of the last (goal) point to that array
  shortcutPath = [1]; % we create an array similar to nodeTrajectory but without the nodes that can be avoided with shortcuts
  loopAgain = 1; %boolean value indicating wether or not we keep running the loop looking for shortcuts
  prevMaxIndex = columns(nodeTrajectory); % the index of the previous shortcut node we have in memory, the start point being at the end of nodeTrajectory
  while loopAgain
    maxShortcut = 0; % we reset the shortcut point we have in memory
    for i=prevMaxIndex:-1:1 % we loop from the last saved shortcut to the goal point
      if ~isnan(checkingLine(GapValue, L1, L2, nodeTrajectory(prevMaxIndex), nodeTrajectory(i), points, threshold)) % if a line can be drawn between the last saved shortcut and the node of index i
        maxShortcut = nodeTrajectory(i); % we update that point of index i as the maximum shortcut reachable
        maxIndex = i; % we save the index of that point of index i that will be the next shortcut
      endif
    endfor
    prevMaxIndex = maxIndex;
    if shortcutPath(end) == columns(points) || maxShortcut == 0 % if we reached the goal point with the shortcuts
      loopAgain = 0; % we stop looping
    else
      loopAgain = 1;
      shortcutPath = [shortcutPath maxShortcut];
    endif
  endwhile  
  for i=1:columns(shortcutPath) % running through the new nodeTrajectory array
    Q1plot = [Q1plot points(1, shortcutPath(i))]; %we note the q1 and q2 values that are to be plot as the shortest path
    Q2plot = [Q2plot points(2, shortcutPath(i))];
    
    if i>1 % for the points that are not the start one
      
      % we define the lines composing the shortest path using the usual Ax+B method
      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);
        if g==Q1plot(i) % as the sampling starts from a node that condition means that we are computing about a node
          points(3, shortcutPath(i)) = Xtest; % we note the x and y values of the nodes
          points(4, shortcutPath(i)) = Ytest;
        endif
        
        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(shortcutPath)-1
      text(points(3, shortcutPath(i)), points(4, shortcutPath(i)), int2str(shortcutPath(i)-1), 'FontSize', 20); % we add the description of each node
    %endif
 endfor
  
  text(S(1), S(2), 'S', 'FontSize', 20);
  text(G(1), G(2), 'G', 'FontSize', 20);
  
  % 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