%FILTER_DESIGN_ZP_CON 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. CON
%   stands for constrained optimization.
%
%   h = filter_design_zp_unc(Hd,Nfilt_row,Nfilt_col,filename)
%   Hd: the ideal frequency response
%   Nfilt_row: vertical order of the output filter
%   Nfilt_col: horizontal order of the output filter
%   filename: the full path to the text file containing the constraint
%   information. The first 2 columns of this text file contains the
%   locations (horizontal and vertical respectively) of the constraints in
%   the frequency domain and the third column contains their respected
%   values.
%   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
%   with the following constraints given in the text file "constraint.txt"
%
%   0.0 0.0 1.0
%   0.5 0.5 0.0
%
%   h=filter_design_zp_con(Hd,15,15,'constraint.txt');
%
%   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_con(Hd,Nfilt_row,Nfilt_col,filename)

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);

%Define constraint conditions Aeq and beq such that Aeq.ha_vec=beq
%For vectors u and v, run a for loop, each iteration adds a new row to
%matrix Aeq
tem=textread(filename);
u=tem(:,1);
v=tem(:,2);
beq=tem(:,3);
for i=1:length(u)
    n1=1:1:(Nfilt_col-1)/2;
    n2=1:1:(Nfilt_row-1)/2;
    n1_tem=2*cos(2*pi*u(i)*n1);%n2=0; n1=1,...,N1
    n1_row=[fliplr(n1_tem) 1 n1_tem];%n2=0; n1=-N1,...,N1
    n1=-(Nfilt_col-1)/2:1:(Nfilt_col-1)/2;
    [n1,n2]=meshgrid(n1,n2);
    n12_array=2*cos(2*pi*(u(i)*n1+v(i)*n2));%n1=-N1,...N1; n2=1,...,N2
    %Generate the half-matrix
    aeq= [n1_row ; n12_array];
    %Convert the half-matrix to a row vector
    Aeq(i,:)=[reshape(aeq(2:size(aeq,1),:),size(aeq,1)*size(aeq,2)-size(aeq,2),1);aeq(1,(size(aeq,2)-1)/2+1:size(aeq,2))']';
end;

p=2;
exitcond=0;
sol = fmincon(@(ha_vec) p_error(ha_vec,Hd_half,Npt,Nfilt_row,Nfilt_col,2),ha_vec0,[],[],Aeq,beq);
error_minimax=minmax_error(sol,Hd_half,Npt,Nfilt_row,Nfilt_col);
while not(exitcond)
    p=p+2;
    sol_tem = fmincon(@(ha_vec) p_error(ha_vec,Hd_half,Npt,Nfilt_row,Nfilt_col,p),ha_vec0,[],[],Aeq,beq);
    err_tem=minmax_error(sol_tem,Hd_half,Npt,Nfilt_row,Nfilt_col);
    if (err_tem>=error_minimax)
        exitcond=1;
        p=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)));