function [UnwrappedPhase] = Diamond(WrappedPhase,Mask,ptstart)
% Diamond - Unwrapping the wrapped phase using DIAMOND method;-
%
%Descriptions:
% [UnwrappedPhase] = Diamond(WrappedPhase,mask,ptstart);
%Examples:
% [UnwrappedPhase] = Diamond(WrappedPhase,mask,ptstart);
%    WrappedPhase : The wrapped phase;
%                   mask : The [0 1] binary mask plate using in phase unwrapping;
%                 ptstart : the coordinates of start point; it MUST be a 1*2 array; 
% UnwrappedPhase : The unwrapped phase;
%See also:   Diamond

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  Author - Qc Zhang & Y Li 16-Nov-2004
%  Last Revised - Qc Zhang & Y Li 16-Nov-2004
%  Copyright 2004-2004 Qc Zhang & Y Li.  All Rights Reserved.
%  $Revision: 1.001.001 $  $Date: 16-Nov-2004 16:34:27 $
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

error(nargchk(1, 3, nargin));
error(nargchk(0, 1, nargout));

if nargin == 1
    [r c] = size(WrappedPhase);
    Mask = ones(r,c);
    ptstart = [round(r/2),round(c/2)];
elseif nargin == 2
    [r c] = size(WrappedPhase);
    ptstart = [round(r/2),round(c/2)];
elseif nargin == 0 | nargin > 3
   disp('Error using! Not enough OR too many parameters.')
   return
end

t0 = clock;

J = ptstart(1);
I = ptstart(2);

disp('Unwrapping the Phase Using Diamond Method...');

[nRow, nCol] = size(WrappedPhase);

UnwrappedPhase = zeros(nRow, nCol);
HasPush = zeros(nRow,nCol);
Track = zeros(2,nRow*nCol);
TrackCont = 1;

%構(gòu)造一個(gè)循環(huán)隊(duì)列，四個(gè)值域分別放置待展開點(diǎn)和已展開點(diǎn)的x,y坐標(biāo)，
%一個(gè)指針域存放下一個(gè)待展開點(diǎn)的地址；
for i = 1:1*(nRow+nCol)   
    CycQue(i).x = 0; %待展開的點(diǎn)x坐標(biāo)；
    CycQue(i).y = 0; %待展開的點(diǎn)y坐標(biāo)；　
    CycQue(i).px = 0; %已展開的前一點(diǎn)x坐標(biāo)；
    CycQue(i).py = 0; %已展開的前一點(diǎn)y坐標(biāo)；　
    CycQue(i).ni = i+1; %待展開的下一點(diǎn)隊(duì)列位置；　
end

t0 = clock;

CycQue(i).ni = 1;%隊(duì)列尾部單元的指針指向隊(duì)頭，形成循環(huán)隊(duì)列；

head = zeros(1,1); 
head = 1; %指向隊(duì)列起始點(diǎn)；
rear = zeros(1,1);
rear = 2;%指向隊(duì)列的尾部；初始時(shí)為head的下一個(gè)單元；

CycQue(head).x = I; %展開起始點(diǎn)；
CycQue(head).y = J;
CycQue(head).px = I;%已展開點(diǎn)；
CycQue(head).py = J;

HasPush(J,I) = 1; %入隊(duì)標(biāo)志

UnwrappedPhase(J,I) = WrappedPhase(J,I); %已展開相位；

%idisp(Mod);set(gcf,'Color',[1, 1, 1]);axis off
%hold on;

while (head ~= rear) % 如果該循環(huán)隊(duì)列為空，停止展開；
    a = UnwrappedPhase(CycQue(head).py,CycQue(head).px); %展開點(diǎn)相位值；
    b = WrappedPhase(CycQue(head).y,CycQue(head).x); %待展開點(diǎn)相位值；
    %         disp('Unwrapping...')
    b = b + ((b-a) < -pi).*2*pi*round(-(b-a)/(2*pi)) -  ((b-a) > pi).*2*pi*round((b-a)/(2*pi)); %相位展開
    
    UnwrappedPhase(CycQue(head).y,CycQue(head).x) = b; %展開后賦值交回；
    
    tempx = CycQue(head).x;%記錄當(dāng)前展開點(diǎn)；
    tempy = CycQue(head).y;
    
    Track(1,TrackCont) = tempx;%記錄展開路徑
    Track(2,TrackCont) = tempy;
    TrackCont = TrackCont+1;       
    
    %        pause(0.03);
    %        plot(tempy,tempx,'r*');
    
    head = CycQue(head).ni; %下一個(gè)結(jié)點(diǎn)；
    
    if (tempy > 1) & (HasPush(tempy-1,tempx) == 0) & (Mask(tempy-1,tempx) == 1) %上一點(diǎn)：(x,y-1)
        CycQue(rear).x = tempx; %展開點(diǎn)
        CycQue(rear).y = tempy-1;
        CycQue(rear).px = tempx;%已展開點(diǎn)；
        CycQue(rear).py = tempy;
        %             fprintf('Up : (%d,%d)\n',CycQue(rear).x,CycQue(rear).y);
        
        HasPush(CycQue(rear).y,CycQue(rear).x) = 1; %修改入隊(duì)標(biāo)志；
        rear = CycQue(rear).ni; %指向下一個(gè)結(jié)點(diǎn)，準(zhǔn)備進(jìn)行下一次展開；
    end
    
    if (tempx < nCol) & (HasPush(tempy,tempx+1) == 0) & (Mask(tempy,tempx+1) == 1) %右一點(diǎn)：(x+1,y)
        CycQue(rear).x = tempx+1; %展開點(diǎn)；
        CycQue(rear).y = tempy;
        CycQue(rear).px = tempx;%已展開點(diǎn)；
        CycQue(rear).py = tempy;
        
        %             fprintf('Right : (%d,%d)\n',CycQue(rear).x,CycQue(rear).y);
        
        HasPush(CycQue(rear).y,CycQue(rear).x) = 1; %修改入隊(duì)標(biāo)志； 
        rear = CycQue(rear).ni; %指向下一個(gè)結(jié)點(diǎn)，準(zhǔn)備進(jìn)行下一次展開；
    end
    
    if (tempy < nRow) & (HasPush(tempy+1,tempx) == 0) & (Mask(tempy+1,tempx) == 1)  %下一點(diǎn)：(x,y+1)
        CycQue(rear).x = tempx; %展開點(diǎn)；
        CycQue(rear).y = tempy+1;
        CycQue(rear).px = tempx;%已展開點(diǎn)；
        CycQue(rear).py = tempy;
        
        %             fprintf('Down : (%d,%d)\n',CycQue(rear).x,CycQue(rear).y);
        
        HasPush(CycQue(rear).y,CycQue(rear).x) = 1; %修改入隊(duì)標(biāo)志；
        rear = CycQue(rear).ni;%指向下一個(gè)結(jié)點(diǎn)，準(zhǔn)備進(jìn)行下一次展開；
    end       
    
    if (tempx > 1) & (HasPush(tempy,tempx-1) == 0) & (Mask(tempy,tempx-1) == 1) %左一點(diǎn)：(x-1,y)
        CycQue(rear).x = tempx-1; %展開點(diǎn)；
        CycQue(rear).y = tempy;
        CycQue(rear).px = tempx;%已展開點(diǎn)；
        CycQue(rear).py = tempy;
        
        %             fprintf('Left : (%d,%d)\n',CycQue(rear).x,CycQue(rear).y);
        
        HasPush(CycQue(rear).y,CycQue(rear).x) = 1; %修改入隊(duì)標(biāo)志；
        rear = CycQue(rear).ni;%指向下一個(gè)結(jié)點(diǎn)，準(zhǔn)備進(jìn)行下一次展開；
    end      
end

% disp('Diamondunwrapping job finished');

Time=etime(clock,t0);
disp(' ');
fprintf('The total consumption of time is %2.2f second\n',Time);
disp(' ');