function [m,v,w,g,f,pp,gg]=gaussmix(x,c,l,m0,v0,w0,wx)
%GAUSSMIX fits a gaussian mixture pdf to a set of data observations [m,v,w,g,f]=(x,c,l,m0,v0,w0,wx)
%
% Usage:
%    (1) [m,v,w]=gaussmix(x,[],[],k);    % create GMM with k mixtures and diagonal covariances
%    (2) [m,v,w]=gaussmix(x,[],[],k,'v);    % create GMM with k mixtures and full covariances
%
% Inputs: n data values, k mixtures, p parameters, l loops
%
%     X(n,p)   Input data vectors, one per row.
%     c(1)     Minimum variance of normalized data (Use [] to take default value of 1/n^2)
%     L        The integer portion of l gives a maximum loop count. The fractional portion gives
%              an optional stopping threshold. Iteration will cease if the increase in
%              log likelihood density per data point is less than this value. Thus l=10.001 will
%              stop after 10 iterations or when the increase in log likelihood falls below
%              0.001.
%              As a special case, if L=0, then the first three outputs are omitted.
%              Use [] to take default value of 100.0001
%     M0       Number of mixtures required (or initial mixture means - see below)
%     V0       Initialization mode:
%                'f'    Initialize with K randomly selected data points [default]
%                'p'    Initialize with centroids and variances of random partitions
%                'k'    k-means algorithm ('kf' and 'kp' determine initialization)
%                'h'    k-harmonic means algorithm ('hf' and 'hp' determine initialization) [default]
%                's'    do not scale data during initialization to have equal variances
%                'm'    M0 contains the initial centres
%                'v'    full covariance matrices
%              Mode 'hf' [the default] generally gives the best results but 'f' is faster and often OK
%     W0(n,1)  Data point weights
%
%     Alternatively, initial values for M0, V0 and W0 can be given  explicitly:
%
%     M0(k,p)  Initial mixture means, one row per mixture.
%     V0(k,p)  Initial mixture variances, one row per mixture.
%      or V0(p,p,k)  one full-covariance matrix per mixture
%     W0(k,1)  Initial mixture weights, one per mixture. The weights should sum to unity.
%     WX(n,1)  Data point weights
%
% Outputs: (Note that M, V and W are omitted if L==0)
%
%     M(k,p)   Mixture means, one row per mixture. (omitted if L==0)
%     V(k,p)   Mixture variances, one row per mixture. (omitted if L==0)
%       or V(p,p,k) if full covariance matrices in use (i.e. either 'v' option or V0(p,p,k) specified)
%     W(k,1)   Mixture weights, one per mixture. The weights will sum to unity. (omitted if L==0)
%     G       Average log probability of the input data points.
%     F        Fisher's Discriminant measures how well the data divides into classes.
%              It is the ratio of the between-mixture variance to the average mixture variance: a
%              high value means the classes (mixtures) are well separated.
%     PP(n,1)  Log probability of each data point
%     GG(l+1,1) Average log probabilities at the beginning of each iteration and at the end
%
% The fitting procedure uses one of several initialization methods to create an initial guess
% for the mixture centres and then uses the EM (expectation-maximization) algorithm to refine
% the guess. Although the EM algorithm is deterministic, the initialization procedures use
% random numbers and so the routine will not give identical answers if you call it multiple
% times with the same input data.

%  Bugs/Suggestions
%     (1) Allow processing in chunks by outputting/reinputting an array of sufficient statistics
%     (2) Other initialization options:
%              'l'    LBG algorithm
%              'm'    Move-means (dog-rabbit) algorithm
%     (3) Allow updating of weights-only, not means/variances

%      Copyright (C) Mike Brookes 2000-2009
%      Version: $Id: gaussmix.m 7784 2016-04-15 11:09:50Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[n,p]=size(x);
wn=ones(n,1);
mx0=sum(x,1)/n;         % calculate mean and variance of input data in each dimension
vx0=sum(x.^2,1)/n-mx0.^2;
sx0=sqrt(vx0);
sx0(sx0==0)=1;      % do not divide by zero when scaling
scaled=0;           % data is not yet scaled
memsize=voicebox('memsize');    % set memory size to use
if isempty(c)
    c=1/n^2;
else
    c=c(1);         % just to prevent legacy code failing
end
fulliv=0;           % initial variance is not full
if isempty(l)
    l=100+1e-4;         % max loop count + stopping threshold
end
if nargin<5 || isempty(v0) || ischar(v0)             % no initial values specified for m0, v0, w0
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %  No initialvalues given, so we must use k-means or equivalent
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    if nargin<6
        if nargin<5 || isempty(v0)
            v0='hf';                 % default initialization mode: hf
        end
        wx=wn;                      % no data point weights
    else
        wx=w0(:);                   % data point weights
    end
    if any(v0=='m')
        k=size(m0,1);
    else
        k=m0;
    end
    fv=any(v0=='v');                % full covariance matrices requested
    if n<=k                         % each data point can have its own mixture
        xs=(x-mx0(wn,:))./sx0(wn,:);          % scale the data
        m=xs(mod((1:k)-1,n)+1,:);   % just include all points several times
        v=zeros(k,p);               % will be set to floor later
        w=zeros(k,1);
        w(1:n)=1/n;
        if l>0
            l=0.1;                  % no point in iterating
        end
    else                            % more points than mixtures
        if any(v0=='s')
            xs=x;                   % do not scale data during initialization
        else
            xs=(x-mx0(wn,:))./sx0(wn,:);  % else scale now
            if any(v0=='m')
                m=(m0-mx0(ones(k,1),:))./sx0(ones(k,1),:);  % scale specified means as well
            end
        end
        w=repmat(1/k,k,1);                      % all mixtures equally likely
        if any(v0=='k')                         % k-means initialization
            if any(v0=='m')
                [m,e,j]=v_kmeans(xs,k,m);
            elseif any(v0=='p')
                [m,e,j]=v_kmeans(xs,k,'p');
            else
                [m,e,j]=v_kmeans(xs,k,'f');
            end
        elseif any(v0=='h')                     % k-harmonic means initialization
            if any(v0=='m')
                [m,e,j]=kmeanhar(xs,k,[],4,m);
            else
                if any(v0=='p')
                    [m,e,j]=kmeanhar(xs,k,[],4,'p');
                else
                    [m,e,j]=kmeanhar(xs,k,[],4,'f');
                end
            end
        elseif any(v0=='p')                     % Initialize using a random partition
            j=ceil(rand(n,1)*k);                % allocate to random clusters
            j(rnsubset(k,n))=1:k;               % but force at least one point per cluster
            for i=1:k
                m(i,:)=mean(xs(j==i,:),1);
            end
        else
            if any(v0=='m')
                m=m0;                           % use specified centres
            else
                m=xs(rnsubset(k,n),:);          % Forgy initialization: sample k centres without replacement [default]
            end
            [e,j]=v_kmeans(xs,k,m,0);             % find out the cluster allocation
        end
        if any(v0=='s')
            xs=(x-mx0(wn,:))./sx0(wn,:);      % scale data now if not done previously
        end
        v=zeros(k,p);                   % diagonal covariances
        w=zeros(k,1);
        for i=1:k
            ni=sum(j==i);               % number assigned to this centre
            w(i)=(ni+1)/(n+k);          % weight of this mixture
            if ni
                v(i,:)=sum((xs(j==i,:)-repmat(m(i,:),ni,1)).^2,1)/ni;
            else
                v(i,:)=zeros(1,p);
            end
        end
    end
else
    %%%%%%%%%%%%%%%%%%%%%%%%
    % use initial values given as input parameters
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    [k,p]=size(m0);
    xs=(x-mx0(wn,:))./sx0(wn,:);          % scale the data
    m=(m0-mx0(ones(k,1),:))./sx0(ones(k,1),:);          % and the means
    v=v0;
    w=w0;
    fv=ndims(v)>2 || size(v,1)>k;                       % full covariance matrix is supplied
    if fv
        mk=eye(p)==0;                                    % off-diagonal elements
        fulliv=any(v(repmat(mk,[1 1 k]))~=0);            % check if any are non-zero
        if ~fulliv
            v=reshape(v(repmat(~mk,[1 1 k])),p,k)'./repmat(sx0.^2,k,1);   % just pick out and scale the diagonal elements for now
        else
            v=v./repmat(sx0'*sx0,[1 1 k]);              % scale the full covariance matrix
        end
    end
    if nargin<7
        wx=wn;              % no data point weights
    end
end
if length(wx)~=n
    error('%d datapoints but %d weights',n,length(wx));
end
lsx=sum(log(sx0));
xsw=xs.*repmat(wx,1,p); % weighted data points
nwt=sum(wx);        % number of data points counting duplicates
if ~fulliv          % initializing with diagonal covariance
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Diagonal Covariance matrices  %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    v=max(v,c);         % apply the lower bound
    xs2=xs.^2.*repmat(wx,1,p);          % square and weight the data for variance calculations

    % If data size is large then do calculations in chunks

    nb=min(n,max(1,floor(memsize/(8*p*k))));    % chunk size for testing data points
    nl=ceil(n/nb);                  % number of chunks
    jx0=n-(nl-1)*nb;                % size of first chunk

    im=repmat(1:k,1,nb); im=im(:);
    th=(l-floor(l))*n;
    sd=(nargout > 3*(l~=0)); % = 1 if we are outputting log likelihood values
    lp=floor(l)+sd;   % extra loop needed to calculate final G value

    lpx=zeros(1,n);             % log probability of each data point
    wk=ones(k,1);
    wp=ones(1,p);
    wnb=ones(1,nb);
    wnj=ones(1,jx0);

    % EM loop

    g=0;                           % dummy initial value for comparison
    gg=zeros(lp+1,1);
    ss=sd;                       % initialize stopping count (0 or 1)
    for j=1:lp
        g1=g;                    % save previous log likelihood (2*pi factor omitted)
        m1=m;                       % save previous means, variances and weights
        v1=v;
        w1=w;
        vi=-0.5*v.^(-1);                % data-independent scale factor in exponent
        lvm=log(w)-0.5*sum(log(v),2);   % log of external scale factor (excluding -0.5*p*log(2pi) term)

        % first do partial chunk (of length jx0)

        jx=jx0;
        ii=1:jx;                        % indices of data points in this chunk
        kk=repmat(ii,k,1);              % kk(jx,k): one row per data point, one column per mixture
        km=repmat(1:k,1,jx);            % km(jx,k): one row per data point, one column per mixture
        py=reshape(sum((xs(kk(:),:)-m(km(:),:)).^2.*vi(km(:),:),2),k,jx)+lvm(:,wnj); % py(k,jx) pdf of each point with each mixture
        mx=max(py,[],1);                % mx(1,jx) find normalizing factor for each data point to prevent underflow when using exp()
        px=exp(py-mx(wk,:));            % find normalized probability of each mixture for each datapoint
        ps=sum(px,1);                   % total normalized likelihood of each data point
        px=px./ps(wk,:);                % relative mixture probabilities for each data point (columns sum to 1)
        lpx(ii)=log(ps)+mx;
        pk=px*wx(ii);                       % pk(k,1) effective number of data points for each mixture (could be zero due to underflow)
        sx=px*xsw(ii,:);
        sx2=px*xs2(ii,:);
        for il=2:nl                     % process the data points in chunks
            ix=jx+1;
            jx=jx+nb;                   % increment upper limit
            ii=ix:jx;                   % indices of data points in this chunk
            kk=repmat(ii,k,1);
            py=reshape(sum((xs(kk(:),:)-m(im,:)).^2.*vi(im,:),2),k,nb)+lvm(:,wnb);
            mx=max(py,[],1);            % find normalizing factor for each data point to prevent underflow when using exp()
            px=exp(py-mx(wk,:));        % find normalized probability of each mixture for each datapoint
            ps=sum(px,1);               % total normalized likelihood of each data point
            px=px./ps(wk,:);            % relative mixture probabilities for each data point (columns sum to 1)
            lpx(ii)=log(ps)+mx;
            pk=pk+px*wx(ii);                % pk(k,1) effective number of data points for each mixture (could be zero due to underflow)
            sx=sx+px*xsw(ii,:);
            sx2=sx2+px*xs2(ii,:);
        end
        g=lpx*wx;                       % total log probability summed over all data points
        gg(j)=g;                        % save log prob at each iteration
        w=pk/nwt;                       % normalize to get the weights
        if pk                           % if all elements of pk are non-zero
            m=sx./pk(:,wp);             % calculate mixture means
            v=sx2./pk(:,wp);            % and variances
        else
            wm=pk==0;                   % mask indicating mixtures with zero weights
            nz=sum(wm);              	% number of zero-weight mixtures
            [vv,mk]=sort(lpx);          % find the lowest probability data points
            m=zeros(k,p);               % initialize means and variances to zero (variances are floored later)
            v=m;
            m(wm,:)=xs(mk(1:nz),:); 	% set zero-weight mixture means to worst-fitted data points
            w(wm)=1/n;               	% set these weights non-zero
            w=w*n/(n+nz);            	% normalize so the weights sum to unity
            wm=~wm;                 	% mask for non-zero weights
            m(wm,:)=sx(wm,:)./pk(wm,wp);  % recalculate means and variances for mixtures with a non-zero weight
            v(wm,:)=sx2(wm,:)./pk(wm,wp);
        end
        v=max(v-m.^2,c);                % apply floor to variances
        if g-g1<=th && j>1
            if ~ss, break; end  %  stop
            ss=ss-1;       % stop next time
        end

    end
    if sd && ~fv  % we need to calculate the final probabilities
        pp=lpx'-0.5*p*log(2*pi)-lsx;   % log of total probability of each data point
        gg=gg(1:j)/n-0.5*p*log(2*pi)-lsx;    % average log prob at each iteration
        g=gg(end);
        %     gg' % *** DEBUG ***
        m=m1;       % back up to previous iteration
        v=v1;
        w=w1;
        mm=sum(m,1)/k;
        f=(m(:)'*m(:)-k*mm(:)'*mm(:))/sum(v(:));
    end
    if ~fv
        m=m.*sx0(ones(k,1),:)+mx0(ones(k,1),:);	% unscale means
        v=v.*repmat(sx0.^2,k,1);                % and variances
    else
        v1=v;
        v=zeros(p,p,k);
        mk=eye(p)==1;                           % mask for diagonal elements
        v(repmat(mk,[1 1 k]))=v1';              % set from v1
    end
end
if fv              % check if full covariance matrices were requested
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Full Covariance matrices  %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    pl=p*(p+1)/2;
    lix=1:p^2;
    cix=repmat(1:p,p,1);
    rix=cix';
    lix(cix>rix)=[];                                        % index of lower triangular elements
    cix=cix(lix);                                           % index of lower triangular columns
    rix=rix(lix);                                           % index of lower triangular rows
    dix=find(rix==cix);
    lixi=zeros(p,p);
    lixi(lix)=1:pl;
    lixi=lixi';
    lixi(lix)=1:pl;                                        % reverse index to build full matrices
    v=reshape(v,p^2,k);
    v=v(lix,:)';                                            % lower triangular in rows

    % If data size is large then do calculations in chunks

    nb=min(n,max(1,floor(memsize/(24*p*k))));    % chunk size for testing data points
    nl=ceil(n/nb);                  % number of chunks
    jx0=n-(nl-1)*nb;                % size of first chunk
    %
    th=(l-floor(l))*n;
    sd=(nargout > 3*(l~=0)); % = 1 if we are outputting log likelihood values
    lp=floor(l)+sd;   % extra loop needed to calculate final G value
    %
    lpx=zeros(1,n);             % log probability of each data point
    wk=ones(k,1);
    wp=ones(1,p);
    wpl=ones(1,pl);             % 1 index for lower triangular matrix
    wnb=ones(1,nb);
    wnj=ones(1,jx0);

    % EM loop

    g=0;                        % dummy initial value for comparison
    gg=zeros(lp+1,1);
    ss=sd;                      % initialize stopping count (0 or 1)
    vi=zeros(p*k,p);            % stack of k inverse cov matrices each size p*p
    vim=zeros(p*k,1);       	% stack of k vectors of the form inv(v)*m
    mtk=vim;                  	% stack of k vectors of the form m
    lvm=zeros(k,1);
    wpk=repmat((1:p)',k,1);
    for j=1:lp
        g1=g;               	% save previous log likelihood (2*pi factor omitted)
        m1=m;                	% save previous means, variances and weights
        v1=v;
        w1=w;
        for ik=1:k

            % these lines added for debugging only
            %             vk=reshape(v(k,lixi),p,p);
            %             condk(ik)=cond(vk);
            %%%%%%%%%%%%%%%%%%%%
            [uvk,dvk]=eig(reshape(v(ik,lixi),p,p));	% convert lower triangular to full and find eigenvalues
            dvk=max(diag(dvk),c);                	% apply variance floor to eigenvalues
            vik=-0.5*uvk*diag(dvk.^(-1))*uvk';      % calculate inverse
            vi((ik-1)*p+(1:p),:)=vik;               % vi contains all mixture inverses stacked on top of each other
            vim((ik-1)*p+(1:p))=vik*m(ik,:)';       % vim contains vi*m for all mixtures stacked on top of each other
            mtk((ik-1)*p+(1:p))=m(ik,:)';           % mtk contains all mixture means stacked on top of each other
            lvm(ik)=log(w(ik))-0.5*sum(log(dvk));       % vm contains the weighted sqrt of det(vi) for each mixture
        end
        %
        %         % first do partial chunk
        %
        jx=jx0;
        ii=1:jx;
        xii=xs(ii,:).';
        py=reshape(sum(reshape((vi*xii-vim(:,wnj)).*(xii(wpk,:)-mtk(:,wnj)),p,jx*k),1),k,jx)+lvm(:,wnj);
        mx=max(py,[],1);                % find normalizing factor for each data point to prevent underflow when using exp()
        px=exp(py-mx(wk,:));            % find normalized probability of each mixture for each datapoint
        ps=sum(px,1);                   % total normalized likelihood of each data point
        px=px./ps(wk,:);                % relative mixture probabilities for each data point (columns sum to 1)
        lpx(ii)=log(ps)+mx;
        pk=px*wx(ii);                       % effective number of data points for each mixture (could be zero due to underflow)
        sx=px*xsw(ii,:);
        sx2=px*(xsw(ii,rix).*xs(ii,cix));	% accumulator for variance calculation (lower tri cov matrix as a row)
        for il=2:nl
            ix=jx+1;
            jx=jx+nb;        % increment upper limit
            ii=ix:jx;
            xii=xs(ii,:).';
            py=reshape(sum(reshape((vi*xii-vim(:,wnb)).*(xii(wpk,:)-mtk(:,wnb)),p,nb*k),1),k,nb)+lvm(:,wnb);
            mx=max(py,[],1);                % find normalizing factor for each data point to prevent underflow when using exp()
            px=exp(py-mx(wk,:));            % find normalized probability of each mixture for each datapoint
            ps=sum(px,1);                   % total normalized likelihood of each data point
            px=px./ps(wk,:);                % relative mixture probabilities for each data point (columns sum to 1)
            lpx(ii)=log(ps)+mx;
            pk=pk+px*wx(ii);                    % effective number of data points for each mixture (could be zero due to underflow)
            sx=sx+px*xsw(ii,:);             % accumulator for mean calculation
            sx2=sx2+px*(xsw(ii,rix).*xs(ii,cix));	% accumulator for variance calculation
        end
        g=lpx*wx;                   % total log probability summed over all data points
        gg(j)=g;                    % save convergence history
        w=pk/nwt;               	% w(k,1) normalize to get the column of weights
        if pk                       % if all elements of pk are non-zero
            m=sx./pk(:,wp);         % find mean and mean square
            v=sx2./pk(:,wpl);
        else
            wm=pk==0;                       % mask indicating mixtures with zero weights
            nz=sum(wm);                  % number of zero-weight mixtures
            [vv,mk]=sort(lpx);             % find the lowest probability data points
            m=zeros(k,p);                   % initialize means and variances to zero (variances are floored later)
            v=zeros(k,pl);
            m(wm,:)=xs(mk(1:nz),:);                % set zero-weight mixture means to worst-fitted data points
            w(wm)=1/n;                      % set these weights non-zero
            w=w*n/(n+nz);                   % normalize so the weights sum to unity
            wm=~wm;                         % mask for non-zero weights
            m(wm,:)=sx(wm,:)./pk(wm,wp);  % recalculate means and variances for mixtures with a non-zero weight
            v(wm,:)=sx2(wm,:)./pk(wm,wpl);
        end
        v=v-m(:,cix).*m(:,rix);                 % subtract off mean squared
        if g-g1<=th && j>1
            if ~ss, break; end  %  stop
            ss=ss-1;       % stop next time
        end
    end
    if sd  % we need to calculate the final probabilities
        pp=lpx'-0.5*p*log(2*pi)-lsx;   % log of total probability of each data point
        gg=gg(1:j)/nwt-0.5*p*log(2*pi)-lsx;    % average log prob at each iteration
        g=gg(end);
        %             gg' % *** DEBUG ONLY ***
        m=m1;                                           % back up to previous iteration
        v=zeros(p,p,k);                                 % reserve spave for k full covariance matrices
        trv=0;                                          % sum of variance matrix traces
        for ik=1:k                                      % loop for each mixture to apply variance floor
            [uvk,dvk]=eig(reshape(v1(ik,lixi),p,p));	% convert lower triangular to full and find eigenvectors
            dvk=max(diag(dvk),c);                       % apply variance floor to eigenvalues
            v(:,:,ik)=uvk*diag(dvk)*uvk';               % reconstitute full matrix
            trv=trv+sum(dvk);                           % add trace to the sum
        end
        w=w1;
        mm=sum(m,1)/k;
        f=(m(:)'*m(:)-k*mm(:)'*mm(:))/trv;
    else
        v1=v;                                           % lower triangular form
        v=zeros(p,p,k);                                 % reserve spave for k full covariance matrices
        for ik=1:k                                      % loop for each mixture to apply variance floor
            [uvk,dvk,]=eig(reshape(v1(ik,lixi),p,p));	% convert lower triangular to full and find eigenvectors
            dvk=max(diag(dvk),c);                       % apply variance floor
            v(:,:,ik)=uvk*diag(dvk)*uvk';               % reconstitute full matrix
        end
    end
    m=m.*sx0(ones(k,1),:)+mx0(ones(k,1),:);  % unscale means
    v=v.*repmat(sx0'*sx0,[1 1 k]);
end
if l==0         % suppress the first three output arguments if l==0
    m=g;
    v=f;
    w=pp;
end


function Xcq = cqt(x, B, fs, fmin, fmax, varargin)
%CQT  Constant-Q/Variable-Q transform
%   Usage:  Xcq = cqt(x, B, fs, fmin, fmax, varargin)
%
%   Input parameters:
%         x         : input signal
%         B         : number of bins per octave
%         fs        : sampling frequency
%         fmin      : lowest frequency to be analyzed
%         fmax      : highest frequency to be analyzed
%         varargin  : Optional input pairs (see table below)
%
%   Output parameters:
%         Xcq       : Struct consisting of
%           .c           : CQT coefficients
%           .cDC         : transform coefficients for f = 0
%           .cNyq        : transform coefficients for fs/2
%           .g           : cell array of analysis filters
%           .shift       : center frequencies of analysis filters
%           .M           : bandwidth of analysis filters
%           .xlen        : length of input signal
%           .phasemode   : 'local'  -> zero-centered filtered used
%                        : 'global' -> mapping function used
%           .rast        : time-frequency plane sampling scheme (full,
%                          piecewise, none)
%           .fmin
%           .fmax
%           .B
%           .format      : eighter 'cell' or 'matrix' (only applies for
%                          piecewise rasterization)
%
%   Optional input arguments arguments can be supplied like this:
%
%       Xcq = cqt(x, B, fs, fmin, fmax, 'rasterize', 'piecewise')
%
%   The arguments must be character strings followed by an
%   argument:
%
%     'rasterize':  can be set to (default is 'full');
%           - 'none':      Hop sizes are distinct for each frequency
%                          channel. Transform coefficients will be
%                          presented in a cell array.
%           - 'full':      The hop sizes for all freqency channels are
%                          set to the smallest hop size in the representa-
%                          tion. Transform coefficients will be presented
%                          in matrix format.
%           - 'piecewise': Hop sizes will be rounded down to be a power-of-
%                          two integer multiple of the smallest hop size in
%                          the representation. Coefficients will be
%                          presented either in a sparse matrix or as cell
%                          arrays (see 'format' option)
%
%     'phasemode':  can be set to (default is 'global')
%           - 'local':     Zero-centered filtered used
%           - 'global':    Mapping function used (see reference)
%
%     'format':     applies only for piecewise rasterization
%           - 'sparse':   Coefficients will be presented in a sparse matrix
%           - 'cell':     Coefficients will be presented in a cell array
%
%     'gamma':      the bandwidth of each filter is given by
%                            Bk = 1/Q * fk + gamma,
%                   where fk is the filters center frequency, Q is fully
%                   determined by the number of bins per octave and gamma
%                   is a bandwidth offset. If gamma = 0 the obtained
%                   filterbank is constant-Q. Setting gamma > 0 time
%                   resolution towards lower frequencies can be improved
%                   compared to the constant-Q case (e.g. ERB proportional
%                   bandwidths). See reference for more information.
%     'normalize':  coefficient normalization
%          - 'sine':    Filters are scaled such that a sinusoid with
%                       amplitude A in time domain will exhibit the same
%                       amplitude in the time-frequency representation.
%          - 'impulse': Filters are scaled such that an impulse in time
%                       domain will exhibit a flat response in the
%                       time-frequency representation (in the frame that
%                       centers the impulse)
%          - 'none':      ...
%     'winfun':        defines the window function that is used for filter
%                   design. See winfuns for more information.
%
%   See also:  nsgtf_real, winfuns
%
%   References:
%     C. Sch�rkhuber, A. Klapuri, N. Holighaus, and M. D�rfler. A Matlab
%     Toolbox for Efficient Perfect Reconstruction log-f Time-Frequecy
%     Transforms.
%
%     G. A. Velasco, N. Holighaus, M. D�rfler, and T. Grill. Constructing an
%     invertible constant-Q transform with non-stationary Gabor frames.
%     Proceedings of DAFX11, Paris, 2011.
%
%     N. Holighaus, M. D�rfler, G. Velasco, and T. Grill. A framework for
%     invertible, real-time constant-q transforms. Audio, Speech, and
%     Language Processing, IEEE Transactions on, 21(4):775-785, April 2013.
%
%
%
% Copyright (C) 2013 Christian Sch�rkhuber.
%
% This work is licensed under the Creative Commons
% Attribution-NonCommercial-ShareAlike 3.0 Unported
% License. To view a copy of this license, visit
% http://creativecommons.org/licenses/by-nc-sa/3.0/
% or send a letter to
% Creative Commons, 444 Castro Street, Suite 900,
% Mountain View, California, 94041, USA.

% Authors: Christian Sch�rkhuber
% Date: 20.09.13

%% check input arguments

%defaults
rasterize = 'full'; %fully rasterized
phasemode = 'global';
outputFormat = 'sparse'; %only applies if rasterize == 'octave'
normalize = 'sine';
windowFct = 'hann';
gamma = 0;

if nargin >= 6
    Larg = length(varargin);
    for ii=1:2:Larg
       switch varargin{ii}
           case {'rasterize'}
               rasterize = varargin{ii+1};
           case {'phasemode'}
               phasemode = varargin{ii+1};
           case {'format'}
               outputFormat = varargin{ii+1};
           case {'gamma'}
               gamma = varargin{ii+1};
           case {'normalize'}
               normalize = varargin{ii+1};
           case {'win'}
               windowFct = varargin{ii+1};
       end
    end
end

%% window design
[g,shift,M] = nsgcqwin(fmin,fmax,B,fs, length(x), ...
        'winfun', windowFct, 'gamma', gamma, 'fractional', 0);
 fbas = fs*cumsum(shift(2:end))./ length(x);
 fbas = fbas(1:size(M,1)/2-1);

%% compute coefficients
bins = size(M,1)/2 - 1;
switch rasterize
    case 'full'
        M(2:bins+1) = M(bins+1);
        M(bins+3:end) = M(bins+1:-1:2);

    case 'piecewise'
        temp = M(bins+1);
        octs = ceil(log2(fmax/fmin));
        %make sure that the number of coefficients in the highest octave is
        %dividable by 2 at least octs-times
        temp = ceil(temp/2^octs)*2^octs;
        mtemp = temp./ M;
        mtemp = 2.^( ceil(log2(mtemp)) -1);
        mtemp = temp./ mtemp;
        mtemp(bins+2) = M(bins+2); %don't rasterize Nyquist bin
        mtemp(1) = M(1); %don't rasterize DC bin
        M = mtemp;

    otherwise
end

switch normalize
    case {'sine','Sine','SINE','sin'}
        normFacVec = 2*M(1:bins+2)./length(x);
    case {'impulse','Impulse', 'IMPULSE','imp'}
        normFacVec = 2*M(1:bins+2)/cellfun(@length,g);
    case {'none','None','NONE','no'}
        normFacVec = ones(bins+2,1);
    otherwise
        error('Unkown normalization method!');
end

normFacVec = [normFacVec; normFacVec(end-1:-1:2)];
g = arrayfun(@(k) (g{k}*normFacVec(k)),1:(2*bins+2),'UniformOutput',0).';

c = nsgtf_real(x,g,shift,M,phasemode);

switch rasterize
    case 'full'
        cDC = cell2mat(c(1)).';
        cNyq = cell2mat(c(bins+2)).';
        c = cell2mat(c(2:bins+1).');
    case 'piecewise'
        cDC = cell2mat(c(1));
        cNyq = cell2mat(c(bins+2));
        if strcmp(outputFormat,'sparse')
            c = cqtCell2Sparse(c,M).';
        else
            c = c(2:end-1);
        end

    otherwise
        cDC = cell2mat(c(1));
        cNyq = cell2mat(c(end));
        c = c(2:end-1);
end

%% output
Xcq = struct('c', {c.'}, 'g', {g}, 'shift', shift, 'M', {M}, ...
    'xlen', length(x), 'phasemode', phasemode, 'rast', rasterize, ...
    'fmin', fmin, 'fmax', fmax, 'B', B, 'cDC', cDC, 'cNyq', cNyq, ...
    'format', outputFormat, 'fbas', fbas);


function [Jul1,Jul2]=UTC2TT(Jul1,Jul2)
%%UTC2TT Convert from universal coordinated time (UTC) given as a two-part
%        pseudo-Julian date to terrestrial time (TT), represented as a two-
%        part Julian date.
%
%INPUTS: Jul1, Jul2 Matrices of two parts of a pseudo-Julian date given in
%                   UTC. The units of the date are days. The full date is
%                   the sum of both terms. The date is broken into two
%                   parts to provide more bits of precision. It does not
%                   matter how the date is split. Corresponding elements in
%                   each matrix are times that are converted.
%
%OUTPUTS: Jul1, Jul2 The time as a Julian date in TT with the same
%                    dimensionalities as the input sets of dates.
%
%The UTC date is only pseudo-Julian, because there is not a fixed number
%of seconds in a Julian day. The convention used in the IAU standard is
%that the Julian day matches the UTC day regardless of whether the UTC day
%is 86399, 86400 or 86401 SI seconds (depending on the presence of leap
%seconds).
%
%UTC began at 1960 January 1.0 (JD 2436934.5) and this function should not
%be called with an earlier date.
%
%This just calls a number of intermediate conversion functions out of the
%International Astronomical Union's (IAU) Standard's of Fundamental
%Astronomy library.
%
%Many temporal coordinate systems standards are compared in [1].
%
%REFERENCES:
%[1] D. F. Crouse, "An Overview of Major Terrestrial, Celestial, and
%    Temporal Coordinate Systems for Target Tracking," Formal Report, Naval
%    Research Laboratory, no. NRL/FR/5344--16-10,279, 10 Aug. 2016, 173
%    pages.
%
%October 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

[Jul1,Jul2]=UTC2TAI(Jul1,Jul2);
[Jul1,Jul2]=TAI2TT(Jul1,Jul2);
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function [H,Hnorm,inv_Hnorm] = compute_homography(m,M);

%compute_homography
%
%[H,Hnorm,inv_Hnorm] = compute_homography(m,M)
%
%Computes the planar homography between the point coordinates on the plane (M) and the image
%point coordinates (m).
%
%INPUT: m: homogeneous coordinates in the image plane (3xN matrix)
%       M: homogeneous coordinates in the plane in 3D (3xN matrix)
%
%OUTPUT: H: Homography matrix (3x3 homogeneous matrix)
%        Hnorm: Normalization matrix used on the points before homography computation
%               (useful for numerical stability is points in pixel coordinates)
%        inv_Hnorm: The inverse of Hnorm
%
%Definition: m ~ H*M where "~" means equal up to a non zero scalar factor.
%
%Method: First computes an initial guess for the homography through quasi-linear method.
%        Then, if the total number of points is larger than 4, optimize the solution by minimizing
%        the reprojection error (in the least squares sense).
%
%
%Important functions called within that program:
%
%comp_distortion_oulu: Undistorts pixel coordinates.
%
%compute_homography.m: Computes the planar homography between points on the grid in 3D, and the image plane.
%
%project_points.m: Computes the 2D image projections of a set of 3D points, and also returns te Jacobian
%                  matrix (derivative with respect to the intrinsic and extrinsic parameters).
%                  This function is called within the minimization loop.

Np = size(m,2);

if size(m,1)<3,
   m = [m;ones(1,Np)];
end;

if size(M,1)<3,
   M = [M;ones(1,Np)];
end;

m = m ./ (ones(3,1)*m(3,:));
M = M ./ (ones(3,1)*M(3,:));

% Prenormalization of point coordinates (very important):
% (Affine normalization)

ax = m(1,:);
ay = m(2,:);

mxx = mean(ax);
myy = mean(ay);
ax = ax - mxx;
ay = ay - myy;

scxx = mean(abs(ax));
scyy = mean(abs(ay));

Hnorm = [1/scxx 0 -mxx/scxx;0 1/scyy -myy/scyy;0 0 1];
inv_Hnorm = [scxx 0 mxx ; 0 scyy myy; 0 0 1];

mn = Hnorm*m;

% Compute the homography between m and mn:

% Build the matrix:

L = zeros(2*Np,9);

L(1:2:2*Np,1:3) = M';
L(2:2:2*Np,4:6) = M';
L(1:2:2*Np,7:9) = -((ones(3,1)*mn(1,:)).* M)';
L(2:2:2*Np,7:9) = -((ones(3,1)*mn(2,:)).* M)';

if Np > 4,
	L = L'*L;
end;

[U,S,V] = svd(L);

hh = V(:,9);
hh = hh/hh(9);

Hrem = reshape(hh,3,3)';
%Hrem = Hrem / Hrem(3,3);

% Final homography:

H = inv_Hnorm*Hrem;

if 0,
   m2 = H*M;
   m2 = [m2(1,:)./m2(3,:) ; m2(2,:)./m2(3,:)];
   merr = m(1:2,:) - m2;
end;

%keyboard;

%%% Homography refinement if there are more than 4 points:

if Np > 4,

   % Final refinement:
   hhv = reshape(H',9,1);
   hhv = hhv(1:8);

   for iter=1:10,

		mrep = H * M;

		J = zeros(2*Np,8);

		MMM = (M ./ (ones(3,1)*mrep(3,:)));

		J(1:2:2*Np,1:3) = -MMM';
		J(2:2:2*Np,4:6) = -MMM';

		mrep = mrep ./ (ones(3,1)*mrep(3,:));

		m_err = m(1:2,:) - mrep(1:2,:);
		m_err = m_err(:);

		MMM2 = (ones(3,1)*mrep(1,:)) .* MMM;
		MMM3 = (ones(3,1)*mrep(2,:)) .* MMM;

		J(1:2:2*Np,7:8) = MMM2(1:2,:)';
		J(2:2:2*Np,7:8) = MMM3(1:2,:)';

		MMM = (M ./ (ones(3,1)*mrep(3,:)))';

		hh_innov  = inv(J'*J)*J'*m_err;

		hhv_up = hhv - hh_innov;

		H_up = reshape([hhv_up;1],3,3)';

		%norm(m_err)
		%norm(hh_innov)

		hhv = hhv_up;
      H = H_up;

   end;

end;

if 0,
   m2 = H*M;
   m2 = [m2(1,:)./m2(3,:) ; m2(2,:)./m2(3,:)];
   merr = m(1:2,:) - m2;
end;

return;

%test of Jacobian

mrep = H*M;
mrep = mrep ./ (ones(3,1)*mrep(3,:));

m_err = mrep(1:2,:) - m(1:2,:);
figure(8);
plot(m_err(1,:),m_err(2,:),'r+');
std(m_err')


function gcm = fmri_scm2gcm(X,Nnnc,TR,tPreStim,delta,tau)
%
% gcm = fmri_scm2gcm(X,Nnnc,TR,tPreStim,delta,tau)
%
% Produces a Gamma Convolution Matrix from a stimulus
% convolution matrix (X) and parameters of the gamma
% function (delta, tau).  The gamma functions
% are interpreted as basis vectors.
%
% $Id: fmri_scm2gcm.m,v 1.3 2005/06/01 01:04:52 sayres Exp $

[Ntp Nch Nr] = size(X);
Nh = Nch/Nnnc;
Ng = length(delta);

t = TR*[0:Nh-1] - tPreStim;
h = fmri_hemodyn(t,delta,tau);
h = h./(repmat(max(h),[Nh 1]));

h_all = zeros(Nch,Nnnc*Ng);
h0 = zeros(Nh,Nnnc*Ng);
h0(1:Nh,1:Ng) = h;
for c = 1:Nnnc,
    r1 = Nh*(c-1)+1;
    r2 = r1 + Nh - 1;
    h_all(r1:r2,:) = fmri_shiftcol(h0,Ng*(c-1));
end

gcm = zeros(Ntp,Nnnc*Ng,Nr);

for r = 1:Nr,
    gcm(:,:,r) = X(:,:,r)*h_all;
end

return;


function [H0,H1,H2] = spm_bilinear(A,B,C,D,x0,N,dt)
% Return global Volterra kernels for a MIMO Bilinear system
% FORMAT [H0,H1,H2] = spm_bilinear(A,B,C,D,x0,N,dt)
% A     - (n x n)     df(x(0),0)/dx                    - n states
% B     - (n x n x m) d2f(x(0),0)/dxdu                 - m inputs
% C     - (n x m)     df(x(0),0)/du - d2f(x(0),0)/dxdu*x(0)
% D     - (n x 1)     f(x(0).0) - df(x(0),0)/dx*x(0)
% x0    - (n x 1)     x(0)
% N     - kernel depth       {intervals}
% dt    - interval           {seconds}
%
% Volterra kernels:
%
% H0    - (n)                 = h0(t)         = y(t)
% H1    - (N x n x m)         = h1i(t,s1)     = dy(t)/dui(t - s1)
% H2    - (N x N x n x m x m) = h2ij(t,s1,s2) = d2y(t)/dui(t - s1)duj(t - s2)
%
% where n = p if modes are specified
%
%--------------------------------------------------------------------------
% Returns Volterra kernels for bilinear systems of the form
%
% dx/dt = f(x,u) = A*x + B1*x*u1 + ... Bm*x*um + C1u1 + ... Cmum + D
%  y(t) = x(t)
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_bilinear.m 5219 2013-01-29 17:07:07Z spm $

% Volterra kernels for bilinear systems
%==========================================================================

% parameters
%--------------------------------------------------------------------------
n     = size(A,1);                  % state variables
m     = size(C,2);                  % inputs
A     = full(A);
B     = full(B);
C     = full(C);
D     = full(D);

% eignvector solution {to reduce M0 to leading diagonal form}
%-------------------------------------------------------------------------
M0    = [0 zeros(1,n); D A];
[U,J] = eig(M0);
V     = pinv(U);

% Lie operator {M0}
%--------------------------------------------------------------------------
M0    = sparse(J);
X0    = V*[1; x0];

% 0th order kernel
%--------------------------------------------------------------------------
H0    = ex(N*dt*M0)*X0;

% 1st order kernel
%--------------------------------------------------------------------------
if nargout > 1

    % Lie operator {M1}
    %----------------------------------------------------------------------
    for i = 1:m
        M1(:,:,i)  = V*[0 zeros(1,n); C(:,i) B(:,:,i)]*U;
    end

    % 1st order kernel
    %----------------------------------------------------------------------
    H1    = zeros(N,n + 1,m);
    for p = 1:m
    for i = 1:N
        u1         = N - i + 1;
        H1(u1,:,p) = ex(u1*dt*M0)*M1(:,:,p)*ex(-u1*dt*M0)*H0;
    end
    end
end

% 2nd order kernels
%--------------------------------------------------------------------------
if nargout > 2
    H2    = zeros(N,N,n + 1,m,m);
    for p = 1:m
    for q = 1:m
    for j = 1:N
        u2         = N - j + 1;
        u1         = N - [1:j] + 1;
        H          = ex(u2*dt*M0)*M1(:,:,q)*ex(-u2*dt*M0)*H1(u1,:,p)';
        H2(u2,u1,:,q,p) = H';
        H2(u1,u2,:,p,q) = H';
    end
    end
    end
end

% project to state space and remove kernels associated with the constant
%--------------------------------------------------------------------------
if nargout > 0
    H0    = real(U*H0);
    H0    = H0([1:n] + 1);
end
if nargout > 1
    for p = 1:m
        H1(:,:,p) = real(H1(:,:,p)*U.');
    end
    H1    = H1(:,[1:n] + 1,:);
end
if nargout > 1
    for p = 1:m
    for q = 1:m
    for j = 1:N
        H2(j,:,:,p,q) = real(squeeze(H2(j,:,:,p,q))*U.');
    end
    end
    end
    H2    = H2(:,:,[1:n] + 1,:,:);
end

return

% matrix exponential function (for diagonal matrices)
%==========================================================================
function y = ex(x)
n          = length(x);
y          = spdiags(exp(diag(x)),0,n,n);
return


function [M, RHS, Mx, My, Mz, RHSx, RHSy, RHSz] = convectionTvdTerm(u, phi, FL)
% This function uses the TVD scheme to discretize a
% convection term in the form $\grad (u \phi)$ where u is a face vactor
% It also returns the x, y, x parts of the matrix of coefficient.
%
% SYNOPSIS:
%   [M, RHS, Mx, My, Mz, RHSx, RHSy, RHSz] = convectionTvdTerm(u, phi, FL)
%
% PARAMETERS:
%   u  - velocity vector, FaceVariable
%   phi  - value of phi from the previous time step or iteration, CellVariable
%   FL  - Flux Limiter function
%
% RETURNS:
%
%
% EXAMPLE:
%
% SEE ALSO:
%

Mz=[];
d = u.domain.dimension;
switch d
    case 1
        [M, RHS] = convectionTvdTerm1D(u, phi, FL);
    case 1.5
        [M, RHS] = convectionTvdTermCylindrical1D(u, phi, FL);
    case 1.8
        [M, RHS] = convectionTvdTermSpherical1D(u, phi, FL);
    case 2
        [M, RHS, Mx, My, RHSx, RHSy] = convectionTvdTerm2D(u, phi, FL);
    case 2.5
        [M, RHS, Mx, My, RHSx, RHSy] = convectionTvdTermCylindrical2D(u, phi, FL);
    case 2.8
        [M, RHS, Mx, My, RHSx, RHSy] = ...
            convectionTvdTermRadial2D(u, phi, FL);
    case 3
        [M, RHS, Mx, My, Mz, RHSx, RHSy, RHSz] = convectionTvdTerm3D(u, phi, FL);
    case 3.2
        [M, RHS, Mx, My, Mz, RHSx, RHSy, RHSz] = ...
            convectionTvdTermCylindrical3D(u, phi, FL);
end