%--------------------------------------------------------------------------
% Plus court chemin sur une grille avec obstacle(s). Algorithme A*
%--------------------------------------------------------------------------
clear all

% Dimension de la grille
xmin = -1; xmax = 1; nx = 50;
ymin = -1; ymax = 1; ny = 50;

%------------------------
% Construction du graphe
%------------------------
diag = true;
%[G,MG] = make_graphe(xmin,xmax,nx,ymin,ymax,ny,{},diag);
[G,MG] = make_graphe(xmin,xmax,nx,ymin,ymax,ny, {@cercle1,@rect1},diag);
%[G,MG] = make_graphe(xmin,xmax,nx,ymin,ymax,ny, {@recta,@rectb,@rectc},diag);
%[G,MG] = make_graphe3D(xmin,xmax,nx,ymin,ymax,ny,{},diag);

% Sommets de départ et d'arrivée
x_start = xmin;
y_start= ymin;
s_start = find_node(x_start,y_start,xmax,xmin,nx,ymin,ymax,ny,G);

x_end=0.2;
y_end=0.6;
s_end=find_node(x_end,y_end,xmax,xmin,nx,ymin,ymax,ny,G);
%s_end = randi(G.n,1); 

%------------------------
% Resolution par A-star
%------------------------
fprintf('-----------------\n');
fprintf('Resolution par A*\n');
fprintf('-----------------\n');
t1=tic();

[d_opt, c_opt] = astar(G,s_start,s_end);

fprintf('longueur du chemin = %g\n',d_opt);
fprintf('temps de résolution A* : %g (s)\n',toc(t1));

%------------------------
% Resolution par Dijkstra
%------------------------
fprintf('-----------------------\n');
fprintf('Resolution par Dijkstra\n');
fprintf('-----------------------\n');
t2=tic();

[dist, path_dijkstra] = graphshortestpath(MG,s_start,s_end,'Method','Dijkstra');

fprintf('longueur du chemin = %g\n',dist);
fprintf('temps de résolution Dijkstra : %g (s)\n',toc(t2));

%------------------------
% Affichage
%------------------------
rect=[xmin xmax ymin ymax];

path{1}=c_opt; color_path{1}='black';
path{2}=path_dijkstra; color_path{2}='blue';

print_path(nx,ny,rect,G,s_start,s_end,path,color_path);
title('A-star / Dijkstra');