% Given the Observation, the noise variance, the PSD of the input, the
% blurring function and the wavelet system setup (defined in Expt_Setup and
% WaRD_Setup), this program now runs the actual WaRD algorithm to perform
% the deconvolution.


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Wiener Estimate
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Wiener_filter_Est = (conj(G) .* (Weight.^2)) ./ ((abs(G).^2 .* (Weight.^2)) + N^2*(sigma^2));
Wiener_Estimate  = real(ifft2(Wiener_filter_Est .* fft2(Y)));
clear Wiener_filter_Est
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Regularized inversion in WaRD. This corresponds to the FoRD step.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Reg_Inv_filter = (conj(G) .* (Weight.^2)) ./ ((abs(G).^2 .* (Weight.^2)) + reg_param*N^2*(sigma^2));
x_ford = real(ifft2(Reg_Inv_filter .* fft2(Y)));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Wavelet domian wiener filtering 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Take the RDWT of the x_ford
[x_fordw, x_fords] = mrdwt_cycle2D(x_ford,Wavelet_Filt_type1,Decomp_levels);

% Calculate the variances of the noise at different levels
temp_filt = real(ifft2(Reg_Inv_filter));
[temp_w,temp_s] = mrdwt_cycle2D(temp_filt,Wavelet_Filt_type1,Decomp_levels);
for lev = 1:Decomp_levels
  for sub = 1:3
    noise_varw(lev,sub) = matrix_norm(temp_w(:,:,lev,sub),2)*sigma;
  end
end
noise_vars = matrix_norm(temp_s,2)*sigma;
clear temp_w temp_s 

% Step 1 in WDWF: Get the hard thresholded estimate

for lev = 1:Decomp_levels
  for sub = 1:3
    temp_w(:,:,lev,sub)=HardTh(x_fordw(:,:,lev,sub),...
	Thresh_Factor*noise_varw(lev,sub)); %thresh difft wavelet subbands
  end
end
temp_s = HardTh(x_fords,Thresh_Factor*noise_vars); % thresh scaling subband
clear x_fordw x_fords
x_ref = mirdwt_cycle2D(temp_w,temp_s,Wavelet_Filt_type1,Decomp_levels);
clear temp_w temp_s

% Use the estimate to perform wavelet domain wiener filtering.

% Calculate the variances of the noise at different levels
[temp_w,temp_s] = mrdwt_cycle2D(temp_filt,Wavelet_Filt_type2,Decomp_levels);
for lev = 1:Decomp_levels
  for sub = 1:3
    noise_varw(lev,sub) = matrix_norm(temp_w(:,:,lev,sub),2)*sigma;
  end
end
noise_vars = matrix_norm(temp_s,2)*sigma;
clear temp_w temp_s temp_filt

[x_fordw, x_fords] = mrdwt_cycle2D(x_ford,Wavelet_Filt_type2,Decomp_levels);
[x_refw, x_refs] = mrdwt_cycle2D(x_ref,Wavelet_Filt_type2,Decomp_levels);

% Do the wavelet domain wiener filtering

for lev = 1:Decomp_levels
  for sub = 1:3
    temp_w(:,:,lev,sub)=x_fordw(:,:,lev,sub) .*...
	((x_refw(:,:,lev,sub).^2)./...
	(x_refw(:,:,lev,sub).^2 + noise_varw(lev,sub)^2)); 
  end
end
temp_s =x_fords .* ((x_refs.^2)./(x_refs.^2 + noise_vars^2)); 
clear x_fordw x_fords x_refw x_refs
x_ward = mirdwt_cycle2D(temp_w,temp_s,Wavelet_Filt_type2,Decomp_levels);
clear temp_w temp_s






