% 创建地图
%      1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0
map = [0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0;   % 1
       0 0 0 0 0 0 0 0 0 1 0 0 0 0 1 1 0 0 0 0;   % 2
       0 0 0 1 1 1 1 0 0 1 0 0 0 0 0 1 1 0 0 0;   % 3 
       0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 1 1 0 0;   % 4
       0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 1 1 0;   % 5
       1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 1 0 0 1 1;   % 6
       1 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 1;   % 7
       1 0 0 0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0;   % 8
       1 0 0 0 1 1 1 1 1 1 0 0 1 1 0 0 0 0 0 0;   % 9
       1 0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0;   %10
       0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0;   %11
       0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 0 0;   %12
       0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0;   %13
       0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0;   %14
       0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0;   %15
       0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0;   %16
       0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1;   %17
       0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;   %18
       0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;   %19
       0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;]; %20

% 设置起点和终点
start = [2, 2];
goal = [19, 19];

% 调用A*算法
path = Astar(map, start, goal);

visualize_path(map, start, goal, path);

function visualize_path(map, start, goal, path)
    % 可视化地图，0为空地，1为障碍
    imagesc(map);
    colormap(flipud(gray));          % 灰度图（0白 1黑）
    axis equal tight;
    hold on;

    % 蓝色起点
    plot(start(2), start(1), 'bo', 'MarkerSize', 10, 'LineWidth', 2);

    % 红色终点
    plot(goal(2), goal(1), 'ro', 'MarkerSize', 10, 'LineWidth', 2);

    if ~isempty(path)
    % 如果起点不等于路径首元素，插入
        if ~isequal(path(1,:), start)
            path = [start; path];
        end
    % 如果终点不等于路径末元素，插入
        if ~isequal(path(end,:), goal)
            path = [path; goal];
        end

    % 显示路径线
        plot(path(:,2), path(:,1), 'g-', 'LineWidth', 2); % 绿色路径线
        plot(path(:,2), path(:,1), 'go', 'MarkerSize', 4, 'MarkerFaceColor', 'g'); % 每个点画圈
    end

    % 网格线
    [rows, cols] = size(map);
    for i = 1:rows+1
        line([0.5, cols+0.5], [i-0.5, i-0.5], 'Color', 'k'); % 横线
    end
    for j = 1:cols+1
        line([j-0.5, j-0.5], [0.5, rows+0.5], 'Color', 'k'); % 竖线
    end

    set(gca, ...
        'XTick', 1:cols, ...
        'YTick', 1:rows, ...
        'XTickLabel', 1:cols, ...
        'YTickLabel', 1:rows, ...
        'XAxisLocation', 'top', ...      % x轴
        'YDir', 'reverse');              % y轴
    title('地图');
    hold off;
end