clear;
%snr=5;
%init=2055615866;
%[xref,xn]=wnoise(1,10,snr,init);
%load  origindata.mat;
%load sum2.txt;
load accel1.txt;
xref=accel1(1:4096);  %數據長度應為2的冪次


x = xref; 



signal=x;


points=4096;        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);
pdw=((posw(:,1:points-1)-posw(:,2:points))<0);
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;
wpeak(:,1)=wpeak(:,1)+1e-10;
wpeak(:,points)=wpeak(:,points)+1e-10;
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);

%模極大值的處理方式：
%在尺度j上極大值點位置，構造一個搜索區域，
%在尺度j-1中，極大值點落在該區域的點保留，其他的置0；
D3_wpeak=wpeak(level-1,:);
D4_p=(D4_wpeak~=0);
O_d4=10;%該參數確定在上一級搜索極大值的范圍，可以調整。
for P_d4=O_d4:(length(D4_wpeak)-O_d4);
    if D4_p(P_d4)==1; 
        for i=1:O_d4-1;
        D4_p(P_d4-i)=1;
        end ;
    end;     
end;
D3_wpeak=D3_wpeak.*D4_p;

D2_wpeak=wpeak(level-2,:);
D3_p=(D3_wpeak~=0);
O_d3=10;%該參數確定在上一級搜索極大值的范圍，可以調整。
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]);
all=pswa';
save denoiseddata56.txt all -ascii