%____模極大值法去噪：
%二進制小波變換（離散平穩小波變換）并找到模極大序列,對極大值序列處理后重建信號;
%參數c, O_d4, O_d3可以調節。
close all;
clc;
clear;
snr=2;
init=2055615866;
[xref,x]=wnoise(2,10,snr,init);
signal=x;
points=1024;        level=4;    sr=360;   num_inter=6;   wf='db3';
%所處理數據的長度    分解的級數   抽樣率    迭代次數        小波名稱
offset=0;


%____進行二進制小波變換（離散平穩小波變換），并給出各級波形：
[Lo_D,Hi_D,Lo_R,Hi_R]=wfilters(wf);
[swa,swd] = swt(signal,level,Lo_D,Hi_D);
figure;
subplot(level,1,1); plot(real(signal)); grid on;axis tight;
for i=1:level
    subplot(level+1,2,2*(i)+1);
    plot(swa(i,:)); axis tight;grid on;xlabel('time');
    ylabel(strcat('a   ',num2str(i)));
    subplot(level+1,2,2*(i)+2);
    plot(swd(i,:)); axis tight;grid on;
    ylabel(strcat('d   ',num2str(i)));
end


%____求小波變換的模極大值及其位置,并按級給出小波變換模極大的波形:
% swa:小波概貌;  swd:小波細節;
% ddw:局部極大位置; wpeak:小波變換的局部極大序列。
ddw=zeros(size(swd));
pddw=ddw;
nddw=ddw;
posw=swd.*(swd>0);%posw是swd大于零的部分組成的矩陣
pdw=((posw(:,1:points-1)-posw(:,2:points))<0);%pdw是posw的第i列與第i+1列向同行的數據相比較
                                             %比較結果小則為一，相反為零     
pddw(:,2:points-1)=((pdw(:,1:points-2)-pdw(:,2:points-1))>0);
negw=swd.*(swd<0);
ndw=((negw(:,1:points-1)-negw(:,2:points))>0);
nddw(:,2:points-1)=((ndw(:,1:points-2)-ndw(:,2:points-1))>0);
ddw=pddw|nddw;
ddw(:,1)=1;
ddw(:,points)=1;
wpeak=ddw.*swd;
%wpeak1=wpeak;%測試程序用
wpeak(:,1)=wpeak(:,1)+1e-10;
wpeak(:,points)=wpeak(:,points)+1e-10;
%wpeak2=wpeak;
%wpeak12=wpeak1-wpeak2;
figure;
subplot(level+1,1,1); plot(real(signal)); grid on;axis tight;
for i=1:level
    subplot(level+1,1,i+1);
    plot(wpeak(i,:)); axis tight;grid on;
    ylabel(strcat('j=   ',num2str(i)));
end


%____進行模極大值的處理：
C=0.8; 
%此參數需要調節，為了在最大尺度上設定合適閾值，以確定最大尺度上該保留的模極大值點。
D4_wpeak=wpeak(level,:);
M=max(D4_wpeak);
Thr=C*M/level; %閾值計算，可參考論文："3mm波段脈沖雷達系統研究和小波去噪分析"。
D4_wpeak=D4_wpeak.*(abs(D4_wpeak)>Thr);%D4_wpeak是D4_wpeak閾值處理后的模極大值點
%模極大值的處理方式：
%在尺度j上極大值點位置，構造一個搜索區域，
%在尺度j-1中，極大值點落在該區域的點保留，其他的置0；
D3_wpeak=wpeak(level-1,:);
D4_p=(D4_wpeak~=0);

O_d4=3;%該參數確定在上一級搜索極大值的范圍，可以調整。
for P_d4=O_d4:(length(D4_wpeak)-O_d4);
    if D4_p(P_d4)==1;%第四層的模極大值點位置P_d4 
        for i=1:O_d4-1;
            D4_p(P_d4-i)=1;%將第四層的模極大值點前面的第1，2個點變為零，確定搜索范圍
        end ;
    end;     
end;
D3_wpeak=D3_wpeak.*D4_p;%將第三層在模極大值點之外的點置0

D2_wpeak=wpeak(level-2,:);
D3_p=(D3_wpeak~=0);
O_d3=3;%該參數確定在上一級搜索極大值的范圍，可以調整。
for P_d3=O_d3:(length(D3_wpeak)-O_d3);
    if D3_p(P_d3)==1; 
        for i=1:O_d3-1;
        D3_p(P_d3-i)=1;
        end ;
    end;     
end;
D2_wpeak=D2_wpeak.*D3_p;

%第一層單獨處理，在第二層極大值點位置上，保留第一層相應極大值點：
D1_wpeak=wpeak(1,:);
D2_p=(D2_wpeak~=0);
D1_wpeak=D1_wpeak.*D2_p;

wpeak=[D1_wpeak' D2_wpeak' D3_wpeak' D4_wpeak'];
wpeak=wpeak';


%____重構信號：
pswa=swa(level,:); % pswa: 為待重建的信號
wframe=(wpeak~=0); %迭代初始化
w0=zeros(1,points);
[a,d]=swt(w0,level,Lo_D,Hi_D);
w2=d;  % w2為待重建小波
    for j=1:num_inter
       w2=Py_Pgama(d,wpeak,wframe,1,sr);   % 先進行Py投影和 Pgama投影
       w0=iswt(pswa,w2,Lo_R,Hi_R);         % 再進行Pv投影
       [a,d]=swt(w0,level,Lo_D,Hi_D);      % Pv
    end
     pswa=iswt(swa(level,:),w2,Lo_R,Hi_R); % 計算重建信號   
     
xcrr=xref-pswa; % 重建誤差
figure,
subplot(411)
plot(xref(1:points),'r');
axis([1 points -2 8]);
subplot(412)
plot(x(1:points),'r');
axis([1 points -2 8]);
subplot(413)
plot(pswa(1:points)); 
axis([1 points -2 8]);
subplot(414)
plot(xcrr(1:points)); 
axis([1 points -2 8]);


% %____分別計算重建小波以及原信號的信噪比
 werr=w2-swd;
% % 原信號的小波變換（swd)和重建后的小波變換（w2）的比較
 figure,
 for m=1:level
    wsnr(m)=20*log10(norm(swd(m,:))/norm(werr(m,:)))
    subplot(level+1,1,m);
    plot(swd(m,:)),hold on,
    plot(w2(m,:),'r');grid on;ylabel(strcat('j=',num2str(m))),axis tight;
 end
 err=pswa(1:points)-signal(1:points);
 snr=20*log10(norm(signal)/norm(err))