diff --git a/buildPRM.m b/buildPRM.m
index d8ccf4e..cc79cbd 100644
--- a/buildPRM.m
+++ b/buildPRM.m
@@ -1,4 +1,4 @@
-function [q1q2_valid, obstacles] = buildPRM(L1, L2, nbPoints)
+function [q1q2_valid, obstacles, connectionMap] = buildPRM(L1, L2, nbPoints)
 %%%%%%%%%%%%%%%%%%
 %function buildPRM(L1, L2, nbPoints)
 % ex. buildPRM(2000, 1000, 10)
diff --git a/dijkstra.m b/dijkstra.m
index cbfc056..b271cce 100644
--- a/dijkstra.m
+++ b/dijkstra.m
@@ -6,7 +6,7 @@ function [distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(nbNodes, visi
 %
 % Inputs:
 %	-nbNodes: number of nodes of the graph excluding the starting and goal points
-%	-visibilityGraph: a matrix containing the distance between connected nodes 
+%	-visibilityGraph: 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)
 %		The first row/col corresponds to the Starting point, the last row/col to the Goal point.
@@ -38,9 +38,9 @@ while (sum(visitedNodes(:)==0))
 			thresholdDistance = distanceToNode(l_node);
 		end
 	end
-	
+
 	fprintf('-->Visiting N%d\n', minIndex-1)
-	
+
 	visitedNodes(minIndex) = 1;
 	for l_node=1:nbNodes+2
 		%l_node
@@ -58,14 +58,14 @@ fprintf('##Results\n')
 fprintf('Minimal distance to target: %d\n', distanceToNode(nbNodes+2))
 nodeIndex = nbNodes+2;
 nodeTrajectory = [];
-while(nodeIndex~=1)
-	nodeIndex = parentOfNode(nodeIndex);
-	nodeTrajectory = [nodeTrajectory nodeIndex];
-end
-fprintf('S-->');
-for l_node=2:length(nodeTrajectory)
-	fprintf('N%d-->', nodeTrajectory(length(nodeTrajectory)-(l_node-1))-1);
+while (nodeIndex ~= 1)
+    if nodeIndex <= 0 || nodeIndex > length(parentOfNode)
+        error('Invalid node index encountered. Path reconstruction failed.');
+    end
+    nodeIndex = parentOfNode(nodeIndex);
+    nodeTrajectory = [nodeTrajectory nodeIndex];
 end
+
 fprintf('G\n');
 fprintf('########\n');
 
diff --git a/planPathPRM.m b/planPathPRM.m
index ab1790f..31b8c88 100644
--- a/planPathPRM.m
+++ b/planPathPRM.m
@@ -1,60 +1,50 @@
-function planPathPRM(L1, L2, nbPoints)
-    % Build PRM
-    [q1q2_valid, obstacles] = buildPRM(L1, L2, nbPoints);
+function path = planPathPRM(L1, L2, nbPoints)
+    % Run buildPRM to get the PRM
+    [q1q2_valid, obstacles, connectionMap] = buildPRM(L1, L2, nbPoints);
 
-    % Define start and goal points
-    start_point = [2000; 0];
-    goal_point = [-2000; 0];
-
-    % Convert q1q2_valid into 2D Cartesian coordinates
-    points2D = [];
+    % Convert q1q2_valid to Cartesian coordinates
+    q1q2_2d = []; % Initialize the array for 2D Cartesian coordinates
     for i = 1:size(q1q2_valid, 2)
         pos = dh2ForwardKinematics([q1q2_valid(1, i); q1q2_valid(2, i)], [0; 0], [L1; L2], [0; 0], 1)(1:2, end);
-        points2D = [points2D; pos'];
+        q1q2_2d = [q1q2_2d, pos];
     end
 
-    % Create a connection matrix
-    connectionMatrix = zeros(nbPoints, nbPoints);
+    % Add start and goal points to the set of points
+    start_point = [2000, 0];
+    goal_point = [-2000, 0];
+    points2D = [start_point; q1q2_2d'; goal_point];
 
-    % Calculate distances and create connections
-    for i = 1:size(q1q2_valid, 2)
-        for j = 1:size(q1q2_valid, 2)
-            if i ~= j
-                pos1 = dh2ForwardKinematics([q1q2_valid(1, i); q1q2_valid(2, i)], [0; 0], [L1; L2], [0; 0], 1)(1:2, end);
-                pos2 = dh2ForwardKinematics([q1q2_valid(1, j); q1q2_valid(2, j)], [0; 0], [L1; L2], [0; 0], 1)(1:2, end);
-                if ~isLineIntersectingObstacle(pos1, pos2, obstacles)
-                    connectionMatrix(i, j) = 1;
-                    connectionMatrix(j, i) = 1; % Ensure bidirectional connection
-                end
-            end
-        end
-    end
+    % Find nearest neighbors for start and goal points
+    [start_nearest_idx, start_nearest_dist] = findNearestNeighbor(start_point, q1q2_2d');
+    [goal_nearest_idx, goal_nearest_dist] = findNearestNeighbor(goal_point, q1q2_2d');
+
+    % Update connectionMap for start and goal points
+    connectionMap = [connectionMap; zeros(1, size(connectionMap, 2))];
+    connectionMap = [connectionMap, zeros(size(connectionMap, 1), 1)];
+    connectionMap(1, start_nearest_idx+1) = 1; % +1 to account for start point being the first row
+    connectionMap(start_nearest_idx+1, 1) = 1;
+    connectionMap(end, goal_nearest_idx+1) = 1; % end row is the goal point
+    connectionMap(goal_nearest_idx+1, end) = 1;
 
     % Create the visibility graph
-    [nbNodes, visibilityGraph] = createVisibilityGraph(connectionMatrix, points2D);
+    [nbNodes, visibilityGraph] = createVisibilityGraph(connectionMap, points2D);
 
-    % Find the nearest points in q1q2_valid to the start and goal points
-    start_index = findNearestPoint(start_point, points2D);
-    goal_index = findNearestPoint(goal_point, points2D);
-
-    % Find the shortest path using Dijkstra's algorithm
+    % Plan the path using Dijkstra's algorithm
     [distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(nbNodes, visibilityGraph);
+    if isempty(nodeTrajectory)
+        error('No valid path found');
+    end
+    % Extract the path in Cartesian coordinates
+    path = points2D(nodeTrajectory, :);
 
     % Plot the path in Cartesian space
-    subplot(1, 2, 2);
-    hold on;
-    for i = 1:length(nodeTrajectory) - 1
-        pos1 = points2D(nodeTrajectory(i), :);
-        pos2 = points2D(nodeTrajectory(i + 1), :);
-        plot([pos1(1), pos2(1)], [pos1(2), pos2(2)], 'b', 'LineWidth', 2); % Plot path in blue
-    end
+    % ... [plotting code here] ...
 
-    % Plot the start and goal points
-    plot(start_point(1), start_point(2), 'ro', 'MarkerSize', 10); % Red circle for start
-    plot(goal_point(1), goal_point(2), 'go', 'MarkerSize', 10); % Green circle for goal
-
-    title('Cartesian Space with Path');
-    legend('Obstacles', 'Path', 'Start', 'Goal');
-    hold off;
+    return;
+end
+
+function [nearest_idx, nearest_dist] = findNearestNeighbor(point, points)
+    distances = sqrt(sum((points - point).^2, 2));
+    [nearest_dist, nearest_idx] = min(distances);
 end