function path = planPathPRM(prmDataFile, startPoint, goalPoint)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% function path = planPathPRM(prmDataFile, startPoint, goalPoint)
%
% Task:
%   Plan a path from start point S to goal point G using the PRM approach using a mat file.
%
% Inputs:
%   - prmDataFile: MAT file containing PRM data (e.g., 'prm_data.mat')
%   - start: Start point coordinates in cartesian space [x_start, y_start]
%   - goal: Goal point coordinates in cartesian space [x_goal, y_goal]
%
% Outputs:
%   - path: The planned path as a sequence of waypoints
%
% author: Camille CONJAT, camille.conjat@ecam.fr
% date: 03/12/2023
%
% Does not work correctly(an issue with the function inverseKinematics)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Load PRM data from the MAT file
load('prm_data.mat', 'q1q2_valid', 'connectionMap', 'L1', 'L2', 'nbPoints');

% Add start and goal points to the map
startNode = inverseKinematics(startPoint(1), startPoint(2), L1, L2);
goalNode = inverseKinematics(goalPoint(1), goalPoint(2), L1, L2);

% Append start and goal nodes to the PRM map
q1q2_valid = [q1q2_valid startNode goalNode];

% Create visibility graph
visibilityGraph = zeros(nbPoints + 2, nbPoints + 2);

% Connect start and goal nodes to the existing nodes in the map
for i = 1:nbPoints + 2
    % Connect start node
    collisionStart = checkCollisionAlongLine(startNode, q1q2_valid(:, i), L1, L2);
    if ~collisionStart
        visibilityGraph(nbPoints + 1, i) = 1;
        visibilityGraph(i, nbPoints + 1) = 1;
    end

    % Connect goal node
    collisionGoal = checkCollisionAlongLine(goalNode, q1q2_valid(:, i), L1, L2);
    if ~collisionGoal
        visibilityGraph(nbPoints + 2, i) = 1;
        visibilityGraph(i, nbPoints + 2) = 1;
    end

    % Connect other nodes in the map
    for j = 1:nbPoints + 2
        if i ~= j && connectionMap(i, j)
            visibilityGraph(i, j) = 1;
            visibilityGraph(j, i) = 1;
        end
    end
end

% Use Dijkstra algorithm to find the shortest path
[~, pathIndices] = dijkstra(visibilityGraph, nbPoints + 1, nbPoints + 2);

% Display the resulting path on the graph
subplot(1, 2, 2);
for i = 1:length(pathIndices) - 1
    nodeA = pathIndices(i);
    nodeB = pathIndices(i + 1);
    pointA = q1q2_valid(1:2, nodeA);
    pointB = q1q2_valid(1:2, nodeB);
    plot([pointA(1), pointB(1)], [pointA(2), pointB(2)], 'b', 'LineWidth', 2); hold on;
end

% Display start and goal points
plot(startPoint(1), startPoint(2), 'mo', 'MarkerSize', 10, 'LineWidth', 2);
plot(goalPoint(1), goalPoint(2), 'co', 'MarkerSize', 10, 'LineWidth', 2);

% Add legend and title
legend('Obstacles', 'Valid points', 'Path', 'Start', 'Goal', 'Location', 'Best');
title('Path Planning using PRM');

% Update axis limits
xlim([-(L1 + L2) (L1 + L2)]);
ylim([-(L1 + L2) (L1 + L2)]);
end