function [s_hat,sigma2_hat,A_index,Count]=ExCoVls(H,Ht,y,varargin)
%large scale ExCoV routine
%(function handle implemetation of sensing matrix and conjugate gradient
%solution of inverse system)
%
%Coded by Kun Qiu (kqiu@iastate.edu)
%updated Dec 29, 2009

%Function usage
%=======================================================
%INPUT(compulsory):
%H:                      the function handle of sensing matrix
%Ht:                     the function handle of H transpose
%y:                       the measurement column vector
%
%INPUT(optional):
%'FIR_len':         the size of moving average window
%                         (default=100)
%'maxiter':          maximum number of conjugate gradient iterations in inverting a linear system
%                         (default=300)
%'Visibility':         Option to see the reconstrution process (valued in {0,1})
%                         0: work silently
%                         1: work openly
%                         (default=1)
%========================================================
%OUTPUT:
%s_hat:              the signal esitmate
%sigma2_hat:    the noise variance estimate
%A_index:         the estimated set of high level signal components
%Count:             Count of the number of EM iterations
%========================================================


if (nargin-length(varargin))~=3
    error('Missing required inputs!');
end

N=length(y);
m=length(Ht(y));
scale=sqrt(N/(y'*y));
y=scale*y;

%Setting default values for the optional inputs
FIR_len=100;
maxiter=300;
Visibility=1;


%Read the optional inputs
if (rem(length(varargin),2)==1)
    error('Optional inputs must go by pairs!');
else
    for i=1:2:(length(varargin)-1)
        switch upper(varargin{i})
            case upper('FIR_len')
                FIR_len=varargin{i+1};
            case upper('maxiter')
                maxiter=varargin{i+1};
            case upper('Visibility')
                Visibility=varargin{i+1};     
            otherwise
                error(['Unrecognized optional input: ''' varargin{i} '''']);
        end
    end
end


%Initialization
ma=1;
mb=m-ma;
u=maskfcn2(Ht,y,1:m);
A_index=find(abs(u)==max(abs(u)),1,'first');
B_index=1:m;
B_index(A_index)=[];
HA_ii=maskfcn1(H,m,1,A_index(1));
HAtHAdiag=HA_ii'*HA_ii;
s_A_init=u(A_index);
sigma2=(y-HA_ii*s_A_init)'*(y-HA_ii*s_A_init)/N;
delta2_a=10*sigma2/HAtHAdiag;

expand=1;
shrink=0;
exit_flag=0;
Count=0;
Cycle=1;
p=0;
A_index_record_pre=[];
GML_star=-inf;
GML_record=-inf;
state_str='Expanding';

%Main iteration
while ~exit_flag
    Count=Count+1;
    p=p+1;
    
    %One EM step
    HAty=maskfcn2(Ht,y,A_index);
    OmegaInv=@(x,transpose) (x./delta2_a*sigma2+maskfcn2(Ht,maskfcn1(H,m,x,A_index),A_index));
    [s_A,flag_lqsr]=lsqr(OmegaInv,HAty,[],maxiter);
    HAsA=maskfcn1(H,m,s_A,A_index);
    sigma2=norm(y-HAsA)^2/N;
    delta2_a=max(s_A.^2,sigma2/10./HAtHAdiag);

    GML(p)=real(0.5*(-N*log(2*pi)-log(0.5*(N-ma))-(N-ma-2)*log(sigma2)-y'*(y-HAsA)/sigma2-sum(log((HAtHAdiag.^2)/2./(sigma2+HAtHAdiag.*(delta2_a))))));

    if p>=FIR_len
        MvAvgGML(p)=mean(GML(p-FIR_len+1:p));
    else
        MvAvgGML(p)=-inf;
    end

    if GML(p)>GML_star
        GML_star=GML(p);
        A_index_star=A_index;
        B_index_star=B_index;
        s_A_star=s_A;
        delta2_a_star=delta2_a;
        sigma2_star=sigma2;
        ma_star=ma;
        mb_star=mb;
    end

    GML_plot=GML(p);

    clc
    if Visibility
        display(['Now ',state_str,'...']);
        display(['GML=',num2str(GML_plot)]);
        display(['Cycle=',num2str(Cycle),' Count=',num2str(Count),' ma=',num2str(ma)]);
    end

    %Try to do expansion or compression
    if expand
        state_str='Expanding';
        if (p<=FIR_len|MvAvgGML(p)>=MvAvgGML(p-1)|GML(p)>=MvAvgGML(p-1))&ma<N-1
            %Expansion
            Res_B=maskfcn2(Ht,y-HAsA,B_index);
            k=find(abs(Res_B)==max(abs(Res_B)),1,'first');
            A_index=[A_index,B_index(k)];
            B_index(k)=[];
            h_expand=maskfcn1(H,m,1,A_index(end));     
            HAtHAdiag=[HAtHAdiag;h_expand'*h_expand];
            delta2_a=[delta2_a;sigma2/HAtHAdiag(end)];
            ma=ma+1;
            mb=mb-1;
        else
            expand=0;
            clear GML
            clear MvAvgGML
            p=0;
            shrink=1;
            expand=0;
        end
    end
    
    if shrink
        state_str='Shrinking';
        if (p<=FIR_len|MvAvgGML(p)>=MvAvgGML(p-1)|GML(p)>=MvAvgGML(p-1))&ma>2            
            %Shrink
            k=find(delta2_a==min(delta2_a),1,'first');
            B_index=[B_index,A_index(k)];
            A_index(k)=[];
            delta2_a(k)=[];
            HAtHAdiag(k)=[];
            ma=ma-1;
            mb=mb+1;
        else
            shrink=0;
            A_index=A_index_star;
            B_index=B_index_star;
            s_A=s_A_star;
            ma=ma_star;
            mb=mb_star;            
            delta2_a=delta2_a_star;
            sigma2=sigma2_star;
            clear GML
            clear MvAvgGML
            GML_plot=GML_star;
        end
    end%if shrink
    
    %Reset step
    if ~(expand|shrink)
        if GML_record<GML_star
            GML_record=GML_star;
            s_A_record=s_A_star;
            ma_record=ma_star;
            A_index_record=A_index_star;
            B_index_record=B_index_star;
            delta2_a_record=delta2_a_star;
            sigma2_record=sigma2_star;
            GML_star=-inf;
            
            s_init=zeros(m,1);
            s_init(A_index_record)=s_A_record;
            u=s_init+Ht(y-H(s_init));
            u_sort=sort(abs(u),'descend');
            A_index=find(abs(u)>=u_sort(ma_record))';
            B_index=1:m;
            B_index(A_index)=[];
            
            HA_ii=maskfcn1(H,m,1,A_index(1));
            HAtHAdiag=HA_ii'*HA_ii;
            for ii=2:ma
                h_expand=maskfcn1(H,m,1,A_index(ii));   
                HAtHAdiag=[HAtHAdiag;h_expand'*h_expand];
            end
            
            s_A_init=u(A_index);
            HAs_A_init=maskfcn1(H,m,s_A_init,A_index);
            sigma2=(y-HAs_A_init)'*(y-HAs_A_init)/N;
            delta2_a=10*sigma2./HAtHAdiag;
            
            state_str='Expanding';
            expand=1;
            shrink=0;
            p=0;
            GML_record_history(Cycle)=GML_record;
            Cycle=Cycle+1;
        end
        if length(union(A_index_record_pre,A_index_record))==length(intersect(A_index_record_pre,A_index_record))
            exit_flag=1;
        end
        A_index_record_pre=A_index_record;       
    end
end

A_index=A_index_record;
s_hat=zeros(m,1);
s_hat(A_index_record)=s_A_record;
sigma2_hat=sigma2_record;

s_hat=s_hat/scale;
sigma2_hat=sigma2_hat/(scale^2);
return;

%%%%%%%%%%%Sub functions%%%%%%%%%%%%%%%%%
function vectout=maskfcn1(H,m,vectin,index_set)
    vecttemp=zeros(m,1);
    vecttemp(index_set)=vectin;
    vectout=H(vecttemp);
return;

function vectout=maskfcn2(Ht,vectin,index_set)
    vectout=Ht(vectin);
    vectout=vectout(index_set);
return;

