function retval = planPathPRM ()
%%%%%%%%%%%%%%%%%%%
%planPathPRM ()
%ex. planPathPRM()
%
%Inputs:
%
%
%Outputs:
%
%Author : Thomas Périn, thomas.perin@ecam.fr
%Date/ 22/11/2023
%%%%%%%%%%%%%%%%%%%

load('dataFile.mat')

start =  [2000; 0];
goal = [-2000; 0];

nbNodes = size(xy_valid,2);

visibilityGraph = zeros(size(xy_valid,2)+2,size(xy_valid,2)+2);

for l = 1:(size(visibilityGraph,2)-1)
  for m = 1:(size(visibilityGraph,2)-1)
    if l ~= m
      if l == 1
        visibilityGraph(l , m) = sqrt((start(1)-xy_valid(1, m-1))^2 + xy_valid(2, m-1)^2);
      elseif m == 1
        visibilityGraph(l , m) = sqrt((start(1)-xy_valid(1, l-1))^2 + xy_valid(2, l-1)^2);
      elseif connectionMap(l-1, m-1) == 1
        visibilityGraph(l , m) = sqrt((xy_valid(1, m-1)-xy_valid(1, l-1))^2 + (xy_valid(2, m-1)-xy_valid(2, l-1))^2);
      end
    end
  end
end
for s = 2:(size(visibilityGraph,2)-1)
  visibilityGraph(s , 12) = sqrt((goal(1)-xy_valid(1, s-1))^2 + xy_valid(2, s-1)^2);
  visibilityGraph(12 , s) = sqrt((goal(1)-xy_valid(1, s-1))^2 + xy_valid(2, s-1)^2);
end
visibilityGraph
[distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(nbNodes, visibilityGraph);

endfunction