%FILTER_DESIGN_ZP_UNC Design a 2-D zero phase filter by using
%   the minimax error between the ideal frequency response and the
%   approximation given by the window method as the design criteria. UNC
%   stands for unconstrained optimization.
%
%   h = filter_design_zp_unc(Hd,Nfilt_row,Nfilt_col)
%   Hd: the ideal frequency response
%   Nfilt_row: vertical order of the output filter
%   Nfilt_col: horizontal order of the output filter
%   h: output filter
%
%   This function will also print out the minimax error between the output
%   filter and the ideal filter.
%
%   Example
%   --------
%   Design a 15x15 lowpass filter based on the ideal frequency response Hd
%
%   h=filter_design_zp_unc(Hd,15,15);
%
%   NOTE: Hd can be designed using the function ideal_response_zp. Please
%   refer to the help documents of ideal_response_zp for more information.
function h = filter_design_nq_unc(Hd,Nfilt_row,Nfilt_col)

Npt=size(Hd,1);
Hd_half = Hd((size(Hd,1)*0.5+1):size(Hd,1),:);

%Generate desired window
win=ones(Nfilt_row,Nfilt_col);
%Generate actual filter response using window method
ha = fwind2(Hd,win);
a = ha(((size(ha,1)-1)*0.5+1):size(ha,1) ,:);

%Extract vector of ind. coefficients
ha_vec0 = [reshape(a(2:size(a,1),:),size(a,1)*size(a,2)-size(a,2),1);a(1,(size(a,2)-1)/2+1:size(a,2))'];

% %Set optimizing options
% options = optimset('MaxFunEvals',5000,'TolFun',1e-10,'TolCon',1e-10,'TolX',1e-5);

%Optimize solution
p=2;
exitcond=0;
sol = fminunc(@(ha_vec) p_error(ha_vec,Hd_half,Npt,Nfilt_row,Nfilt_col,2),ha_vec0);
error_minimax=minmax_error(sol,Hd_half,Npt,Nfilt_row,Nfilt_col);
while not(exitcond)
    p=p+2;
    sol_tem = fminunc(@(ha_vec) p_error(ha_vec,Hd_half,Npt,Nfilt_row,Nfilt_col,p),ha_vec0);
    err_tem=minmax_error(sol_tem,Hd_half,Npt,Nfilt_row,Nfilt_col);
    if (err_tem>=error_minimax)
        exitcond=1;
        p-2
    else
        sol=sol_tem;
        error_minimax=err_tem;
    end
end

% %Calculate minimax error
% error_minmax = minmax_error(sol,Hd_half,Npt,Nfilt_row,Nfilt_col)
error_minimax
%Generate full response from vector of solution
tem=[fliplr(sol(Nfilt_col*(Nfilt_row-1)/2+2:length(sol))') sol(Nfilt_col*(Nfilt_row-1)/2+1:length(sol))'; 
    reshape(sol(1:Nfilt_col*(Nfilt_row-1)/2),(Nfilt_row-1)/2,Nfilt_col)];
h = [flipud(fliplr(tem(2:size(tem,1),:)));tem];


%%%%%%%%%%%%%Subfunctions to be used above%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%Subfunction1:
function err = p_error(ha_vec,Hd_half,Npt,Nfilt_row,Nfilt_col,p)

%Create the corner quadrants
%Convert vector of ind. coefficients to array
ha_mat=[fliplr(ha_vec(Nfilt_col*(Nfilt_row-1)/2+2:length(ha_vec))') ha_vec(Nfilt_col*(Nfilt_row-1)/2+1:length(ha_vec))'; 
    reshape(ha_vec(1:Nfilt_col*(Nfilt_row-1)/2),(Nfilt_row-1)/2,Nfilt_col)];
%Create the quadrants of the full response
dleft = ha_mat(:,1:(Nfilt_col-1)/2);
uright = fliplr(flipud(ha_mat(2:size(ha_mat,1),1:((Nfilt_col-1)*.5+1))));
dright = ha_mat(:,((Nfilt_col-1)*.5 + 1):Nfilt_col);
uleft = fliplr(flipud(ha_mat(2:size(ha_mat,1),((Nfilt_col-1)*.5+2):Nfilt_col)));
%Pad the actual response to take fft2
ha_pad = [dright zeros(size(dright,1),Npt-Nfilt_col) dleft;zeros(Npt-Nfilt_row,Npt);
    uright zeros(size(uright,1),Npt-Nfilt_col) uleft];
tem = fft2(ha_pad);
Ha_half = [tem(1:Npt/2,Npt/2+1:Npt) tem(1:Npt/2,1:Npt/2)];

%Calculate the p-error
err = sum(sum((Ha_half - Hd_half).^p));


%Subfunction 2:
function err = minmax_error(ha_vec,Hd_half,Npt,Nfilt_row,Nfilt_col)

%Generate full impulse response and frequency response from one quadrant.
%Convert vector of ind. coefficients into half of the full response
ha_mat=[fliplr(ha_vec(Nfilt_col*(Nfilt_row-1)/2+2:length(ha_vec))') ha_vec(Nfilt_col*(Nfilt_row-1)/2+1:length(ha_vec))'; 
    reshape(ha_vec(1:Nfilt_col*(Nfilt_row-1)/2),(Nfilt_row-1)/2,Nfilt_col)];
%Create the four quadrants of the response
dleft = ha_mat(:,1:(Nfilt_col-1)/2);
uright = fliplr(flipud(ha_mat(2:size(ha_mat,1),1:((Nfilt_col-1)*.5+1))));
dright = ha_mat(:,((Nfilt_col-1)*.5 + 1):Nfilt_col);
uleft = fliplr(flipud(ha_mat(2:size(ha_mat,1),((Nfilt_col-1)*.5+2):Nfilt_col)));
%Pad the actual response to take fft2
ha_pad = [dright zeros(size(dright,1),Npt-Nfilt_col) dleft;zeros(Npt-Nfilt_row,Npt);
    uright zeros(size(uright,1),Npt-Nfilt_col) uleft];
tem = fft2(ha_pad);
Ha_half = [tem(1:Npt/2,Npt/2+1:Npt) tem(1:Npt/2,1:Npt/2)];

%Calculate the minimax error
err = max(max(abs(Ha_half - Hd_half)));