function w = saSIDomp(y, sid, s, SAmet, tms, verbose)
% saSIDomp        OMP Sparse Approximation with Shift Invariant Dictionary 
%                 and Orthogonal Matching Pursiuit (OMP) is used.
% The large SID dictionary is divided into blocks and OMP is used on
% smaller blocks of the signal. For this task sparseapprox is used.
%
% use:   w = saSIDomp(y, sid, s);
%        w = saSIDomp(y, sid, w0);
%        w = saSIDomp(y, sid, s, SAmet, tms, verbose);  
% -------------------------------------------------------------------------
%   w     The returned sparse coefficients, size Kx1, or KxL
%   y     The (training) signal, size Nx1, or NxL
%   sid   The Shift Invariant Dictionary, as in initSID
%   s     Number of non-zeros to use in each column of y
%   w0    Initial coefficients, may be given instead of s, size KxL
%   SAmet Sparse Approximatio method as used in sparseapprox.m
%         can be 'OMP' (default) 'PSnn', where nn is 10, 20, 50 or 100
%   tms   targetMaxSize, i.e. number of elements for segment dictionary 
%         defalut is 4e5, larger may give better approximation but will use
%         more time, smaller will be faster. 
%         Note large SID also need large D (tms)
%         If tms <= 1000 it is used as Nseg (ver 1.2)
%   verbose    0: only warnings (default), 1: some info,  2: more,  3: much
% -------------------------------------------------------------------------

% -------------------------------------------------------------------------
% Made by Karl Skretting, University of Stavanger (UiS), 
% Department of Electrical Engineering and Computer Science (IDE)
% Mail:  karl.skretting@uis.no   Homepage:  http://www.ux.uis.no/~karlsk/
% 
% HISTORY:  dd.mm.yyyy
% Ver. 1.0  19.11.2015  KS: made file, saved as _v10
% Ver. 1.1  03.05.2016  KS: tre is not used now, w0 may be given,
%                       SAmet can be 'OMP' (default) or 'PSnn'
% Ver. 1.2  23.12.2016  KS: Nseg instead of tms,  if  <= 1000
% -------------------------------------------------------------------------

w =[];
if (~isstruct(sid) || ~isfield(sid,'Type') || ~strcmp(sid.Type, 'SID') || ...
    ~isfield(sid,'M') || ~isfield(sid,'Qm') || ~isfield(sid,'Pm')  )
    disp([mfilename,': Second argument not a SID structure.']);
    return
end
if (nargin < 3)
    disp([mfilename,': There should be at least three input arguments.']);
    return
end
if (nargin < 4) || isempty(SAmet)
    SAmet = 'OMP';
end
% target max number of elements in D, 
%   120 000 --> ex size of D: 300x400, 200x600, 150x800 
%   250 000 --> ex size of D: 500x500, 400x625, 250x1000, 200x1250
% 1 000 000 --> ex size of D: 1000x1000, 500x2000, 400x2500
if (nargin < 5) || isempty(tms)
    targetMaxSize = 4e5;   
else
    targetMaxSize = tms; 
    % if targetMaxSize <= 1000 it is Nseg (from ver 1.2)
end
if nargin < 6
    verbose = 0;
end
[N,L] = size(y);
M = sid.M;    %  number of atoms in dictionary
if verbose >= 1
    fprintf('\n%s: %s on y (%i x %i) using sid of %i atoms.\n', mfilename, SAmet, N, L, M);
    fprintf(['SID atom length: Qm = [%3i',char(kron(ones(1,M-1),',%3i')),']\n'], sid.Qm);
    fprintf(['SID atom shift:  Pm = [%3i',char(kron(ones(1,M-1),',%3i')),']\n'], sid.Pm);
end

% check input
Qm = sid.Qm;  %  length of each atom, size Mx1
Pm = sid.Pm;  %  shift for each atom, size Mx1
Km = ceil( (N+1-Qm)./Pm ); 
K = sum(Km);
k2 = cumsum(Km);
k1 = [1, reshape(k2(1:(M-1)),1,M-1)+1];
% check normalization for SID
wasNormalized = true;       % if input sid was normalized or not
scaleFactor = ones(M,1);
for m = 1:M       % in this loop: c = D'*r 
    atom = sid.freeVar(sid.i1(m):sid.i2(m));
    noa = atom'*atom;   % norm of atom
    if (abs(noa - 1) > 1e-8) 
        wasNormalized = false;
        scaleFactor(m) = 1/sqrt(noa);
        sid.freeVar(sid.i1(m):sid.i2(m)) = atom*scaleFactor(m);
        if verbose >= 1
            fprintf(' atom %3i has length %5i and norm %6.2f. (normalize it) \n', m, numel(atom), sqrt(noa));
        end
    end
end
% now sid is normalized

% is initial coeffisients given or is s (target number of non-zeros) given
if (numel(s)  < L)
    if (numel(s) > 1)
        fprintf('Use only first element of s (length(s) < L). s = %i.\n', s(1));
    end
    w = saSIDbmp_mex( y, sid.freeVar, sid.Qm, sid.Pm, s(1) );
elseif numel(s) == L
    if (sum(s == s(1)) == L)
        % all are equal
        w = saSIDbmp_mex( y, sid.freeVar, sid.Qm, sid.Pm, s(1) );
    else
        w = zeros(K,L);
        for i = 1:L
            w(:,i) = saSIDbmp_mex( y(:,i), sid.freeVar, sid.Qm, sid.Pm, s(i) );
        end
    end
elseif numel(s) == (K*L)
    w = reshape(full(s), K, L);
else
    fprintf('Size of third argument (s or w) is %i x % i, which is strange, see help.\n', size(s) );
    return;
end
if verbose >= 1
    r = y - multSID(sid, w, N);  %
    if numel(s) == (K*L)
        fprintf('Initial coefficients w (from input)        : nnz(w) = %6i, SNR = %6.3f\n',...
            nnz(w), 10*log10( sum(sum(y.^2)) / sum(sum(r.^2)) ) );
    else
        fprintf('Initial coefficients w (from saSIDbmp_mex) : nnz(w) = %6i, SNR = %6.3f\n',...
            nnz(w), 10*log10( sum(sum(y.^2)) / sum(sum(r.^2)) ) );
    end
end
% Initial coefficients w is full and should be ok!

if ~isempty(SAmet) && ((SAmet(1) == 'P') || (SAmet(1) == 'p'))
    if length(SAmet) > 2
        nComb = strread(SAmet(3:end));  %#ok<DSTRRD>
    else
        nComb = 20;
    end
    if verbose >= 1
        fprintf('Use %s (nComb = %i) to improve coefficients.\n', SAmet, nComb);
    end
elseif ~isempty(SAmet) && ((SAmet(1) == 'O') || (SAmet(1) == 'm'))
    if verbose >= 1
        fprintf('Use %s (--> OMP) to improve coefficients.\n', SAmet);
    end
    SAmet = 'OMP';
else
    fprintf('Not ready for SAmet = %s, use OMP to improve coefficients.\n',SAmet);
    SAmet = 'OMP';
end
% now SAmet (and nComb should be ok)

% find size of segment dictionary, D
if ((N*K) > targetMaxSize)
    % must divide into blocks
    if M == 1
        Ns = Pm(1);
    else
        Ns = lcm(Pm(1), Pm(2));
        for m = 3:M
            Ns = lcm(Ns, Pm(m));
        end
    end
    Ks = Ns*sum(1./sid.Pm);
    N0 = ceil(max(sid.Qm)/Ns)*Ns - Ns;    % = N1-Ns, N0 is a multiple of Ns
    K0 = sum(1+floor((N0+Ns-sid.Qm)./sid.Pm)) - Ks;
    if verbose == 3
        fprintf('Smallest segment dictionary is D1 (N1xK1)=(%i x %i) not D0 (N0xK0)=(%i x %i)!\n', ...
            N0+Ns, K0+Ks, N0, K0);
        fprintf('Ns = %i,  Ks = %i, N0 = %i, K0 = %i  and Ks/Ns = %8.4f \n', Ns, Ks, N0, K0, Ks/Ns);
        fprintf('Large SID has N = %8i, K = %8i.  For the blocks of D columns have: \n', N, K);
        fprintf(['  number Km = [%8i',char(kron(ones(1,M-1),',%8i')),']\n'], Km);
        fprintf(['  start  k1 = [%8i',char(kron(ones(1,M-1),',%8i')),']\n'], k1);
        fprintf(['  end    k2 = [%8i',char(kron(ones(1,M-1),',%8i')),']\n'], k2);
    end
    if targetMaxSize > 1000    % ver 1.2
        a = Ns*Ks;
        b = Ns*K0 + Ks*N0;
        c = N0*K0 - targetMaxSize;
        nofMiniB = floor( (-b + sqrt(b^2 - 4*a*c))/(2*a) ); % number of miniblocks in each segment
    else
        nofMiniB = ceil( (targetMaxSize-N0)/Ns );  % Nseg = targetMaxSize <= 1000
    end
    totMiniB = floor((N-N0)/Ns);                   % total number of miniblocks in SID
    D = full(makeSIDmatrix(sid, N0 + nofMiniB*Ns));
    [Nseg,Kseg] = size(D);
    iD = ceil(N0/Ns) + nofMiniB;   % number of 'MiniBlocks' between each segment
    if verbose >= 1
        fprintf('Segment SID has N = %8i, K = %8i,   nofMiniB = %5i,  iD = %5i.\n', ...
            Nseg, Kseg, nofMiniB, iD);
    end
    %
    [~, kk0] = idxSIDmap(sid, nofMiniB, totMiniB, 0, 0);
    [~, kk1] = idxSIDmap(sid, nofMiniB, totMiniB, 1, 0);
    kkdiff = kk1 - kk0;
else
    D = full(makeSIDmatrix(sid, N));
    [Nseg,Kseg] = size(D);         % (Nseg == N)
    if verbose >= 2
        fprintf('Segm. SID has N = %8i, K = %8i.\n', Nseg, Kseg);
    end
end


% now try to improve the coefficients using SAmet  (OMP)
if (exist('sparseapprox','file') == 2)
    if Nseg < N
        % iFirst = repmat([1,floor(iD/2)+1],1,floor(verbose/2)); % this was not so good ???
        iFirst = ceil([iD/5, iD/2, 4*iD/5, 1, 1+iD/2, iD]);
        % iFirst = ceil(iD*[0.25, 0.75, 0.001, 0.5, 1]);
        if verbose >= 4   % this means that the number in iFirst may vary
            % 'verbose' is assumed to be number of iterations in ver. 1.2
            % iFirst = randi(iD, 1, verbose);  % random values may be used
            % or better
            iFirst = ones(1, verbose);   
            for i = 2:(numel(iFirst)-1)
                iFirst(i) = rem(iFirst(i-1) + randi(ceil(iD/2)) + ceil(iD/5), iD) + 1;
            end
        end
        if verbose >= 3
            fprintf([' Nseg = %5i  iFirst = [%5i',char(kron(ones(1,numel(iFirst)-1),',%5i')),']\n'],Nseg,iFirst);
        end
        for i = iFirst
            iTab = i:iD:(totMiniB - nofMiniB + 1);
            if verbose == 3
                if (numel(iTab) < 15)
                    fprintf([' i =%4i: iTab = [%5i',char(kron(ones(1,numel(iTab)-1),',%5i')),']\n'],i,iTab);
                else
                    fprintf([' i =%4i: iTab = [%5i',char(kron(ones(1,9),',%5i')),', ...], numel(iTab) = %i \n'],...
                        i, iTab(1:10), numel(iTab));
                end
            end
            S = zeros(1,L*numel(iTab));   % the size may vary!
            W0 = zeros(Kseg,L*numel(iTab));
            i2 = 1:L;
            for i1 = iTab   
                kk = kk0 + (i1-1)*kkdiff;
                S(i2) = sum(w(kk,:) ~= 0); 
                W0(:,i2) = w(kk,:);
                i2 = i2 + L;
                w(kk,:) = 0;
            end
            if verbose == 3
                if (numel(iTab) < 15)
                    fprintf([' i =%4i:    S = [%5i',char(kron(ones(1,numel(iTab)-1),',%5i')),']\n'],i,S);
                else
                    fprintf([' i =%4i:    S = [%5i',char(kron(ones(1,9),',%5i')),', ...]\n'],i,S(1:10));
                end
                fprintf(' D is size %5i x %5i,  R will be size %5i x %5i \n', size(D), Nseg, L*numel(iTab) );
            end
            r = y - multSID(sid, w, N);       
            R = zeros(Nseg, L*numel(iTab));    
            i2 = 1:L;
            for i1 = iTab   
                n1 = 1 + (i1-1)*Ns;  
                n2 = Nseg + (i1-1)*Ns;
                R(:,i2) = r(n1:n2,:);
                i2 = i2 + L;    
            end
            E0 = sum((R-D*W0).^2);
            if verbose == 3
                % Note r has some more elements than R, beginning or end of
                % r is not included in R.
                fprintf('Proccess iteration where line start at n1 = %5i,  (i = %3i).\n', 1 + (iTab(1)-1)*Ns, iTab(1) );
                fprintf('Signal after keeping %6i non-zero coefficients has ssr = %12.3f, \n',...
                    nnz(w), sum(sum(r.^2)) );
                fprintf('        will now use %6i coeffs for approx. of R,  ssR = %12.3f. \n',...
                    sum(S), sum(sum(R.^2)) );
                fprintf('  If approx. use the %6i non-zeros in W0,   ss(R-D*W0) = %12.3f. \n',...
                    nnz(W0), sum(E0) );
            end
            %
            if (SAmet(1) == 'O') || (SAmet(1) == 'm')
                W = sparseapprox(R, D, 'mexOMP', struct('targetNonZeros',S, 'verbose',0) );
            elseif SAmet(1) == 'P'
                W = sparseapprox(R, D, 'javaPS', struct('targetNonZeros',S, 'verbose',0, 'nComb',nComb) );
            end
            E = sum((R-D*W).^2);
            if verbose == 3
                fprintf('Do %s. S has %i elements, min(S)=%i, mean(S)=%6.2f, max(S)=%i, std(S)=%6.2f.\n', ...
                    SAmet, numel(S), min(S), mean(S), max(S), std(S) );
                fprintf('  If approx. use the %6i non-zeros in W,     ss(R-D*W) = %12.3f. \n',...
                    nnz(W), sum(E) );
            end
            % select best
            idx = (E0 < E);
            if nnz(idx)   % should keep some old coefficients
                W(:,idx) = W0(:,idx);
                E = sum((R-D*W).^2);
                if verbose == 3
                    fprintf('Keep coefficents for %i out of %i columns in R.\n', nnz(idx), numel(idx));
                    fprintf('  If approx. use the %6i non-zeros in W,     ss(R-D*W) = %12.3f. \n',...
                        nnz(W), sum(E) );
                end
            end
            %
            i2 = 1:L;
            for i1 = iTab   
                kk = kk0 + (i1-1)*kkdiff;
                w(kk,:) = W(:,i2);
                i2 = i2 + L;
            end
            if verbose >= 1
                r = y - multSID(sid, w, N);  %
                fprintf(' Loop start n1 = %5i, (i=%3i) : (keep %6i of %6i col)  %s -> nnz(w) = %6i, SNR = %6.2f\n',...
                    1 + (iTab(1)-1)*Ns, i,  nnz(idx), numel(idx), SAmet, nnz(w), 10*log10( sum(sum(y.^2)) / sum(sum(r.^2)) ) );
                if (verbose >= 2); fprintf('\n'); end;
            end
        end
        
    else
        % try to use the full dictionary here
        if numel(w) == (K*L)
            w0 = w;
        end
        if (SAmet(1) == 'O') || (SAmet(1) == 'm')
            w = sparseapprox(y, D, 'mexOMP', struct('targetNonZeros',s, 'verbose',0) );
        elseif SAmet(1) == 'P'
            w = sparseapprox(y, D, 'javaPS', struct('targetNonZeros',s, 'verbose',0, 'nComb',nComb) );
        end
        idx = (sum((y-D*w0).^2) < sum((y-D*w).^2));
        if nnz(idx) > 0
            % some old coefficients were better than the new ones
            if numel(idx) == 1
                w = w0;
            else
                w(:,idx) = w0(:,idx);
            end
        end
    end
else
    fprintf('%s: sparseapprox is not available. \n', mfilename);
end

if ~wasNormalized    % sid is not normalized
    for m = 1:M
        w(k1(m):k2(m),:) = w(k1(m):k2(m),:)*scaleFactor(m); 
    end
end

end




