function path = RectWave(map, startPos, endPos)
%map——二維數(shù)組圖 0: passable 1: obstacle
% startPos: Original Position
% endPos:   Target Postion
% Search the shortest path by rect wave propagation algorithm
% Rectwave should search the path from the DTM begin with principal
% neibourgh grids, else it will make zigzag path


%路徑規(guī)劃
    
    %s->Start, G->Goal
    S.x = startPos(1);
    S.y = startPos(2);

    G.x = endPos(1);
    G.y = endPos(2);
    
    DTM = GetDistanceTransform(map, S, G);
    path = FindPathFromDTM(DTM, S, G);
    return;
    
%%%%%%%%%%%%%
%make DistanceTransformMatrix
%+++++++++++++++++++++++++++++++++++++++++++++++++
function  DTM = GetDistanceTransform(map, S, G)

    DTM = Inf * ones(size(map));
    DTM(G.y, G.x) = 0;
    G.cost = 0;
   
    WFT = [];
    WFT = INSERT_NODE(WFT, G);
    cur = G;
 
    while ~(cur.x==S.x & cur.y==S.y) & length(WFT)~=0
        [cur WFT] = GetTail(WFT);
%         plot(cur.x, cur.y, 'k+'); drawnow;
        [WFT DTM] = PropagateWave(map, DTM, WFT, cur);
    end
    return;
%=================================================


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Find path from Distance Transform Matrix
%+++++++++++++++++++++++++++++++++++++++++++++++
function path = FindPathFromDTM(DTM, S, G)

    if DTM(S.y, S.x) == Inf%尋路失敗
        path = [];
        return;
    end
    
    [Ymax Xmax] = size(DTM);
    p = S;
    path = S;

    j = 0; % 0 : start with N4(p) or principal grids;  4: start with ND(p) or diagonal grids
    
    while DTM(p.y, p.x)~=0
        for k = j : j+7
            x = p.x; y = p.y;
            switch(mod(k,8))
                case 0
                    x = x + 1;
                case 1
                    x = x - 1;
                case 2
                    y = y - 1;
                case 3
                    y = y + 1;
                case 4
                    x = x + 1;
                    y = y + 1;
                case 5
                    x = x - 1;
                    y = y - 1;
                case 6
                    x = x + 1;
                    y = y - 1;
                case 7
                    x = x - 1;
                    y = y + 1;
            end
            if x<1 | y<1 | x>Xmax | y>Ymax
                continue;
            end
            if DTM(y,x) == DTM(p.y, p.x)-1
                p.x = x; p.y = y;
%                 j = k;
                break;
            end
        end
        path = [path, p];
    end
%=================================================
    

 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%name: insert_node(): 將節(jié)點插入列表并排序

%%%%%%%%%%=======begining of insert_node=========%%%%%%%%%%%%%%%%%%%%  
function NewWFT = INSERT_NODE(WFT, p)
% p可能在WFT里有重復(fù)，卍代碼是刪除WFT重復(fù)p，但實驗表明刪除重復(fù)點反而增加耗時
%     global tag;
%     if tag(p.y, p.x) > 0
%         WFT = Delete(p, WFT);
%     else
%         tag(p.y, p.x) = tag(p.y, p.x)+1;
%     end

%以下代碼是對WFT進行排序，但實驗表明排序也使算法耗時增加
%     if isempty(WFT)    %a new or null list
%         NewWFT = p;
%     else
%         num = length(WFT);
%         while num~=0 & WFT(num).cost<p.cost
%             num = num-1;
%         end
%         NewWFT = [WFT(1:num), p, WFT(num+1:end)];
%     end

%以下代碼是將p直接插入WFT前面，既不排序也不刪除重復(fù)點，但實驗表明耗時反而最少
    NewWFT = [p WFT];
    

%%%%%%%%%%=======end of insert_node=========%%%%%%%%%%%%%%%%%%%%%%


function NewList = Delete(X, List)
    num = length(List);
    while num~=0 & ~(List(num).x==X.x & List(num).y==X.y)
        num = num - 1;
    end
    % 不考慮狀態(tài)增大或者減小嗎？
    NewList = [List(1:num-1), List(num+1:end)];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%function: GetTail(): 從List中取出末尾節(jié)點并修改List
%%%%%%%%%%======= begining =========%%%%%%%%%%%%%%%%%%%%
function [y, newlist] = GetTail(list)
    if isempty(list)
        y = [];
        newlist = [];
    else
        y = list(end);
        newlist = list(1 : end-1);
    end

 %%%%%%%%%%=======end of get_node()=========%%%%%%%%%%%%%%%%%%%%%%     
    
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%function: Ex_nods(): 擴展節(jié)點
%%%%%%%%%%======= begining =========%%%%%%%%%%%%%%%%%%%%
function [newlist newVmap] = PropagateWave(map, Vmap, openlist, p)
    global tag;
    [Ymax Xmax] = size(map);
    for k = 1 : 8
        x = p.x; y = p.y; d=1;
        switch(k)
            case 1
                x = x - 1;
            case 2
                x = x + 1;
            case 3
                y = y - 1;
            case 4
                y = y + 1;
            case 5
                x = x - 1;
                y = y - 1;
            case 6
                x = x + 1;
                y = y + 1;
            case 7
                x = x - 1;
                y = y + 1;
            case 8
                x = x + 1;
                y = y - 1;
        end
        
        if x<1 | y<1 | x>Xmax | y>Ymax | map(y, x)==1 | Vmap(y, x)<=Vmap(p.y, p.x)+d
            continue;
        else
            Vmap(y, x) = Vmap(p.y, p.x)+d;
            temp.x = x; temp.y = y;
            temp.cost = Vmap(y,x);
            openlist = INSERT_NODE(openlist, temp); 
        end
    end
     newlist = openlist;
     newVmap = Vmap;
    return;
%%%%%%%%%%=======   end  =========%%%%%%%%%%%%%%%%%%%%%%