function costmap = GridAStar(obstlist,goal,gres)
    [minx,miny,obmap] = CalcObstMap(obstlist,gres);//minx miny其实是障碍物在普通地图的坐标，不是栅格地图的坐标，利用CalcObstMap函数生成障碍物栅格地图
    col = goal(1);
    row = goal(2);
    col = ceil((col-minx)/gres);// 将goal（目标点）的普通地图坐标转换为栅格坐标，四舍五入为整数，x坐标对应列数，y对应行数
    row = ceil((row-miny)/gres);
//     goal = [gx,gy];
//     goal.parent = [gx,gy];
//     goal.precost = 0;
//     goal.postcost = inf;
    goal = [row,col];
    costmap = 0*obmap; //生成cost地图的0矩阵
    dim = size(obmap);  //计算矩阵尺寸，方便给cost填值   
     // 对costmap填值，如果障碍物栅格地图的值是1，则cost地图上填入inf，如果不是1，且不是目标点的行列值，则运行Astarsearch
    for i = 1:dim(1)
        for j = 1:dim(2)
            if obmap(i,j) == 1
                costmap(i,j) = inf;
                continue
            elseif i == col && j == row
                continue
            end            
            start = [i,j];//将每个栅格都当做起点，
            cost = AStarSearch(start,goal,obmap);//调用搜索主程序
            costmap(i,j) = cost;//每一个点到目标点做一次Astar，然后得到每个点到终点的最短cost，生成整张地图的cost地图
        end
    end    
end

function cost = AStarSearch(start,goal,obmap)
    dim = size(obmap);// 计算栅格地图尺寸，返回m*n，m行，n列
    // Grids(i,j,1) - x of parent pos; 2 - y of parent pos; 3 - precost; 4 -
    // postcost
    Grids = zeros(dim(1),dim(2),4);//生成四个dim尺寸的0矩阵
    for i = 1:dim(1)
        for j = 1:dim(2)
            Grids(i,j,1) = i;// 栅格地图中点(i,j)的父节点的栅格行坐标，每格都是该格子行坐标值
            Grids(i,j,2) = j;// 栅格地图中点(i,j)的父节点的栅格列坐标，每格都是该格子列坐标值
            Grids(i,j,3) = norm(([i,j]-goal));// 构造栅格的启发值h，每个点到目标点的向量的模
            Grids(i,j,4) = inf;// 从起始点到当前点i,j走过的距离，实际就是g值
        end
    end
    Open = start; // Open集合，存放待拓展的栅格节点，不是实际坐标
    Grids(start(1),start(2),4) = 0; // 从起始点到该点的距离
    Close = [];// Close集合，存放拓展过的栅格节点，不是实际坐标
    while ~isempty(Open)// 当open为空时，搜索完毕，为A*核心搜索代码的三个过程1.找出最小的点 2.拓展点 3.把点放入close集合中
        [wknode,Open] = PopOpen(Open,Grids);//计算cost最小点wknode，同时open中剔除wknode点。
        [Grids,Open,Close] = Update(wknode,goal,obmap,Grids,Open,Close);//调用update函数，将扩展点加入open中，更新open集
        Close(end+1,:) = wknode;//wknode已经处理过，加入close中
    end
    cost = Grids(goal(1),goal(2),3)+Grids(goal(1),goal(2),4);//得到起点到终点的最小cost
end

function [Grids,Open,Close] = Update(wknode,goal,obmap,Grids,Open,Close)
    dim = size(obmap);
    for i = -1:1
        for j = -1:1
            adjnode = wknode+[i,j];//adjnode是扩展点，wknode栅格周围一共八个扩展点，
            row = adjnode(1);
            col = adjnode(2);
            if i == 0 && j == 0//扩展点是wknode自身，不处理，继续循环
                continue
            elseif row < 1 || row > dim(1)//扩展点行超出栅格地图边界，不处理，继续循环
                continue
            elseif col < 1 || col > dim(2)//扩展点列超出栅格地图边界，不处理，继续循环
                continue
            elseif obmap(row,col) == 1//扩展点是障碍物占据栅格，不处理，继续循环
                continue
            end
            tcost = Grids(wknode(1),wknode(2),4)+norm([i,j]);//计算扩展点adjnode的G值
            if Grids(row,col,4) > tcost
                Grids(row,col,1) = wknode(1);
                Grids(row,col,2) = wknode(2);//记录该扩展点的父节点到grids中
                Grids(row,col,4) = tcost;//记录扩展点的G值
                // add adjnode to Open except wknode is goal
                if ~ismember(adjnode,Open,'rows') && ~isequal(adjnode,goal)
                    Open(end+1,:) = adjnode;//如果扩展点不在open中，且扩展点不是终点，则把扩展点加入到open中
                end
                // if adjnode is in Close remove it
                if isempty(Close)
                    // do nothing
                elseif ismember(adjnode,Close,'rows')//判断扩展点在不在close中
                    [~,rIdx] = ismember(adjnode,Close,'rows');
                    Close(rIdx,:) = [];//扩展点在close中则从close中删除此点
                end
            end
        end
    end
end

function [wknode,Open] = PopOpen(Open,Grids)
    mincost = inf;
    minidx = 1;//初始化mincost，minidx
    for i = 1:size(Open,1)//遍历open集，找到cost最小的栅格节点
        node = Open(i,:);
        tcost = Grids(node(1),node(2),3)+Grids(node(1),node(2),4);
        if tcost < mincost
            minidx = i;
            mincost = tcost;
        end
    end
    wknode = Open(minidx,:);//提出cost最小的节点记为wknode
    Open(minidx,:) = [];//open集中删除wknode。
end

function [minx,miny,obmap] = CalcObstMap(obstlist,gres)// 使用障碍物中点的坐标构建地图
    minx = min(obstlist(:,1));
    maxx = max(obstlist(:,1));
    miny = min(obstlist(:,2));
    maxy = max(obstlist(:,2));
    xwidth = maxx - minx;// 计算普通地图的x向距离
    xwidth = ceil(xwidth/gres);//转换为栅格地图的个数
    ywidth = maxy - miny;// 计算普通地图的y向距离
    ywidth = ceil(ywidth/gres);//转换为栅格地图的个数
    obmap = zeros(ywidth,xwidth);//结合栅格行列数，生成空矩阵
    for i = 1:ywidth
        for j = 1:xwidth
            ix = minx+(j-1/2)*gres;// 把离散栅格转换为实际位置坐标，单位是米
            iy = miny+(i-1/2)*gres;
            [~,D] = knnsearch(obstlist,[ix,iy]); //计算点(ix, iy)距离obstlist中点最近的一个点的距离
            if D < 0.5 // 距离小于0.5则认为是障碍物的一部分，障碍物的边长，形状为正方形
                obmap(i,j) = 1;
            end
        end
    end
end

// function [xidx,yidx] = CalcIdx(x,y,minx,miny,gres)
//     xidx = ceil((x-minx)/gres);
//     yidx = ceil((y-miny)/gres);
// end