function [map, dist] = imPointsInfluenceZones(varargin)
%IMPOINTSINFLUENCEZONES Maps influence zones of a set of 2D/3D points
%
%   MAP = imPointsInfluenceZones(DIM, POINTS)
%   DIM is a 1-by-2 or 1-by-3 row vector containing dimensions of ouput
%   label map. POINTS is a N-by-2 or N-by-3 array of coordinates.
%
%   MAP = imPointsInfluenceZones(LX, LY, POINTS)
%   MAP = imPointsInfluenceZones(LX, LY, LZ, POINTS)
%   LX and LY and optionnaly LZ are row vectors containing the values of
%   XData, YData, and ZData. The size of MAP if given by:
%   * length(LY)-by-length(LX) in case of 2D points
%   * length(LY)-by-length(LX)-by-length(LZ) in case of 3D points
%
%   [MAP, DIST] = imPointsInfluenceZones(...)
%   Also returns the distance map, containing for each pixel or voxel the
%   distance to the closest point in the input point set.
%
%   Example
%     % Planar example
%     points = rand(20, 2) * 200;
%     map = imPointsInfluenceZones([200 200], points);
%     rgb = label2rgb(map, jet(20), 'w', 'shuffle');
%     figure; imshow(rgb);
%
%     % 3D example
%     dim = [100 100 100];
%     np = 200;
%     points = rand(np, 3) * dim(1);
%     tic; [map, dist] = imPointsInfluenceZones(dim, points); toc
%     rgb = label2rgb3d(map, jet(np+1), 'w', 'shuffle');
%     figure; imshow(rgb(:,:,:,50));
%
%   See also
%     imDistance, imvoronoi2d, imvoronoi3d

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2019-05-10,    using Matlab 9.6.0.1072779 (R2019a)
% Copyright 2019 INRA - Cepia Software Platform.

%% Extract input arguments

% checkup on input argument number
if nargin < 2
    error('Requires at least two input arguments');
end

% extraction of image dimensions
var1 = varargin{1};
if size(var1, 1) == 1 && (size(var1, 2) == 2 || size(var1, 2) == 3)
    % first argument contains the size of the output image
    lx = 1:var1(2);
    ly = 1:var1(1);
    if length(var1) == 3
        lz = 1:var1(3);
    end
    varargin(1) = [];

elseif size(var1, 1) == 1 && size(varargin{2}, 1) == 1
    % first and second arguments contain vector for each coordinate
    % respectively
    lx = var1;
    ly = varargin{2};
    if size(varargin{3}, 1) == 1
        lz = varargin{3};
        varargin(1:3) = [];
    else
        varargin(1:2) = [];
    end

else
    error(['wrong input arguments in ' mfilename]);
end

% extraction of points
points = varargin{1};

%% Initialisations

% number of points
nPoints = size(points, 1);
nd = size(points, 2);

% size of output image
Nx = length(lx);
Ny = length(ly);

%% Generation of distance function

if nd == 2
    % allocate memory for label map
    map = zeros([Ny Nx]);
    % initialize distance map with arbitrarily large distance
    dist = inf * ones([Ny Nx]);

    % pixels coordinates
    [x, y] = meshgrid(lx, ly);

    % update distance for each point
    for i = 1:nPoints
        % squared distance from each pixel to current point
        di = (x - points(i,1)).^2 + (y - points(i,2)).^2;

        % update arrays for current influence zone
        inds = di < dist;
        map(inds) = i;
        dist(inds) = di(inds);
    end

    % convert squared distance to Euclidean distance
    dist = sqrt(dist);

elseif nd == 3
    % size in third dimension
    Nz = length(lz);

    % allocate memory for label map
    map = zeros([Ny Nx Nz]);

    % initialize distance map with arbitrarily large distance
    dist = inf * ones([Ny Nx Nz]);

    % pixels coordinates
    [x, y, z] = meshgrid(lx, ly, lz);

    % update distance for each point
    for i = 1:nPoints
        % distance from each pixel to current point
        di = (x - points(i,1)).^2 + (y - points(i,2)).^2 + (z - points(i,3)).^2;

        % update arrays for current influence zone
        inds = di < dist;
        map(inds) = i;
        dist(inds) = di(inds);
    end

    % convert squared distance to Euclidean distance
    dist = sqrt(dist);
end


%% ------------------------solveRWF.m--------------------------------------

% Solver for Reweighted Wirtinger Flow as given in Algorithm 1
% of the Reweightde Wirtinger Flow (TAF) paper. Refer to the userguide for
% a detailed usage of the package.

% PAPER TITLE:
%              Phase Retrieval via Reweighted Wirtinger Flow
% ARXIV LINK:
%              https://doi.org/10.1364/AO.56.002418

% INPUTS:
%         A:   Function handle/numerical matrix for data matrix A. The rows
%              of this matrix are the measurement vectors that produce
%              amplitude measurements '\psi'.
%         At:  Function handle/numerical matrix for A transpose.
%         b0:  Observed data vector consisting of amplitude measurements
%              generated from b0 = |A*x|. We assign it to 'psi' to be
%              consistent with the notation in the paper.
%         x0:  The initial vector to be used by any solver.
%        opts: struct consists of the options for the algorithm. For
%              details,see header in solvePhaseRetrieval.m or the User
%              Guide.

% OUPTUT :
%         sol: n x 1 vector. It is the estimated signal.
%        outs: A struct consists of the convergence info. For details,
%              see header in solvePhaseRetrieval.m or the User Guide.

% Note:        When a function handle is used, the value of 'n' (the length
%              of the unknown signal) and 'At' (a function handle for the
%              adjoint of 'A') must be supplied. When 'A' is numeric, the
%              values of 'At' and 'n' are ignored and inferred from the
%              arguments

% DESCRIPTION:
%             The spectral method proposed by Candes et al. in their
%             seminal Wirtinger Flow paper suffers from fat tails that can
%             distort the true solution. A possible way to overcome this is
%             to truncate the outliers. In this paper, the authors propose
%             a weighted truncated iterative descent method that removes
%             those measurement vector a_m that are not correlated to the
%             intial guess. This truncation and weighting step is done at
%             each iteration of the gradient descent method.

% METHOD:
%         1) Our implementation uses FASTA, a fast gradient solver.
%
%         2) Set the objectve f = @(z) 1/2 * sum(weights .*
%             (abs(z) - b0).^2). The weights are computed according to
%             equation (14) in algorithm 1 of the proposing paper
%
%         3) The gradient is grad = @(z) weights .* (z - b0 .* sign(z)).
%
% PhasePack by Rohan Chandra, Ziyuan Zhong, Justin Hontz, Val McCulloch,
% Christoph Studer, & Tom Goldstein
% Copyright (c) University of Maryland, 2017

%% -----------------------------START-----------------------------------

function [sol, outs] = solveRWF(A, At, b0, x0, opts)
%     addpath('solvers/linesearch');

    m = length(b0);

    innerOpts = struct;
    innerOpts.maxIters = opts.maxIters;
    innerOpts.maxTime = opts.maxTime;
    innerOpts.tol = opts.tol;
    innerOpts.verbose = opts.verbose;
    innerOpts.recordTimes = opts.recordTimes;
    innerOpts.recordResiduals = opts.recordResiduals;
    innerOpts.recordMeasurementErrors = opts.recordMeasurementErrors;
    innerOpts.recordReconErrors = opts.recordReconErrors;
    innerOpts.xt = opts.xt;

    innerOpts.updateObjectivePeriod = opts.reweightPeriod;
    innerOpts.searchMethod = opts.searchMethod;
    innerOpts.betaChoice = opts.betaChoice;

    [sol, outs] = gradientDescentSolver(A, At, x0, b0, @updateObjective, innerOpts);

    function [f, gradf] = updateObjective(~, Ax)
        weights = 1 ./ (abs(abs(Ax).^2 - b0.^2) + opts.eta*ones(m, 1));
        s = sum(weights);
        f = @(z) 0.5/s * sum(weights .* (abs(z).^2 - b0.^2).^2);
        gradf = @(z) (1.0/s)*weights .* (abs(z).^2 - b0.^2) .* z;
    end
end


function [m,v,w,g,f,pp,gg]=gaussmix(x,c,l,m0,v0,w0)
%GAUSSMIX fits a gaussian mixture pdf to a set of data observations [m,v,w,g,f]=(x,c,l,m0,v0,w0)
%
% 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(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.
%
%     Alternatively, if initial values for M0, V0 and W0 are not given explicitly:
%
%     M0       Number of mixtures required
%     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 of kmeans)
%                'h'    k-harmonic means algorithm ('hf' and 'hp' determine initialization of kmeans)
%                '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
%
% 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

%  Bugs/Suggestions
%     (2) Allow processing in chunks by outputting/reinputting an array of sufficient statistics
%     (6) Other initialization options:
%              'l'    LBG algorithm
%              'm'    Move-means (dog-rabbit) algorithm

%      Copyright (C) Mike Brookes 2000-2009
%      Version: $Id: gaussmix.m,v 1.22 2009/09/21 14:03:13 dmb Exp $
%
%   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);
mx0=sum(x,1)/n;         % calculate mean and variance of input data
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<6             % no initial values specified for m0, v0, w0
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %  No initialvalues given, so we must use k-means or equivalent
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    if nargin<5
        v0='hf';                 % default initialization mode: hf
    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(ones(n,1),:))./sx0(ones(n,1),:);          % 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(ones(n,1),:))./sx0(ones(n,1),:);  % 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]=kmeans(xs,k,m);
            elseif any(v0=='p')
                [m,e,j]=kmeans(xs,k,'p');
            else
                [m,e,j]=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]=kmeans(xs,k,m,0);             % find out the cluster allocation
        end
        if any(v0=='s')
            xs=(x-mx0(ones(n,1),:))./sx0(ones(n,1),:);      % 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(ones(n,1),:))./sx0(ones(n,1),:);          % 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
end
lsx=sum(log(sx0));
if ~fulliv          % initializing with diagonal covariance
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Diagonal Covariance matrices  %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    v=max(v,c);         % apply the lower bound
    xs2=xs.^2;          % square 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

        jx=jx0;
        ii=1:jx;
        kk=repmat(ii,k,1);
        km=repmat(1:k,1,jx);
        py=reshape(sum((xs(kk(:),:)-m(km(:),:)).^2.*vi(km(:),:),2),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=sum(px,2);                   % effective number of data points for each mixture (could be zero due to underflow)
        sx=px*xs(ii,:);
        sx2=px*xs2(ii,:);

        for il=2:nl
            ix=jx+1;
            jx=jx+nb;                    % increment upper limit
            ii=ix:jx;
            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+sum(px,2);                   % effective number of data points for each mixture (could be zero due to underflow)
            sx=sx+px*xs(ii,:);
            sx2=sx2+px*xs2(ii,:);
        end
        g=sum(lpx);                    % total log probability summed over all data points
        gg(j)=g;
        w=pk/n;                         % normalize to get the weights
        if pk                       % if all elements of pk are non-zero
            m=sx./pk(:,wp);
            v=sx2./pk(:,wp);
        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=sum(px,2);                   % effective number of data points for each mixture (could be zero due to underflow)
        sx=px*xs(ii,:);
        sx2=px*(xs(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+sum(px,2);                % effective number of data points for each mixture (could be zero due to underflow)
            sx=sx+px*xs(ii,:);               % accumulator for mean calculation
            sx2=sx2+px*(xs(ii,rix).*xs(ii,cix));            % accumulator for variance calculation
        end
        g=sum(lpx);                    % total log probability summed over all data points
        gg(j)=g;                        % save convergence history
        w=pk/n;                         % 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)/n-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);
        trv=0;      % sum of variance matrix traces
        for ik=1:k
            [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
            trv=trv+sum(dvk);
        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);
        for ik=1:k
            [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 [A,B,flag] = hmxRSVD(varargin)
%+========================================================================+
%|                                                                        |
%|         OPENHMX - LIBRARY FOR H-MATRIX COMPRESSION AND ALGEBRA         |
%|           openHmx is part of the GYPSILAB toolbox for Matlab           |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2018.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : hmxRSVD.m                                     |
%|    #    |   VERSION    : 0.52                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 01.01.2019                                    |
%| ( === ) |   SYNOPSIS   : RSVD from Halko et al. 'finding structure with|
%|  `---'  |                randomness'. Adapted from Antoine Liutkus.    |
%+========================================================================+

% Input analysis
if (numel(varargin{2}) == 1)
    % Input
    M   = varargin{1};
    tol = varargin{2};
    rk  = varargin{3};

    % Dimensions
    [m,n] = size(M);

    % Matrix vectot product
    MV = @(V) M * V;
    VM = @(V) V * M;

    % Type
    typ = class(M);

else
    % Input
    A   = varargin{1};
    B   = varargin{2};
    tol = varargin{3};

    % Dimensions
    [m,rk] = size(A);
    n      = size(B,2);

    % Type
    typ = class(A);

    % Matrix vectot product
    MV    = @(V) A * (B * V);
    VM    = @(V) (V * A) * B;
end

% Randomized
p = min(2*rk,n);
X = randn(n,p,typ);
Y = MV(X);
try
    W1 = orth(Y);
catch
    A    = [];
    B    = [];
    flag = 0;
    return
end
B = VM(W1');

% Truncated SVD
try
    [W2,S,V] = svd(B,'econ');
catch
    A    = [];
    B    = [];
    flag = 0;
    return
end

% Product
U = W1*W2;

% Rank with fixed accuracy
if (numel(S) ~= 0)
    I = find(abs(diag(S)/S(1)) >= tol);
else
    I = [];
end

% No values
if isempty(I)
   A    = zeros(m,0);
   B    = zeros(0,n);
   flag = 1;

% Accuracy not reached
elseif (length(I) >= rk)
    A    = [];
    B    = [];
    flag = 0;

% Low-rank representation
else
    A    = U(:,I);
    B    = S(I,I) * V(:,I)';
    flag = 1;
end
end


%IMM_UPDATE  Interacting Multiple Model (IMM) Filter update step
%
% Syntax:
%   [X_i,P_i,MU,X,P] = IMM_UPDATE(X_p,P_p,c_j,ind,dims,Y,H,R)
%
% In:
%   X_p  - Cell array containing N^j x 1 mean state estimate vector for
%          each model j after prediction step
%   P_p  - Cell array containing N^j x N^j state covariance matrix for
%          each model j after prediction step
%   c_j  - Normalizing factors for mixing probabilities
%   ind  - Indices of state components for each model as a cell array
%   dims - Total number of different state components in the combined system
%   Y    - Dx1 measurement vector.
%   H    - Measurement matrices for each model as a cell array.
%   R    - Measurement noise covariances for each model as a cell array.
%
% Out:
%   X_i  - Updated state mean estimate for each model as a cell array
%   P_i  - Updated state covariance estimate for each model as a cell array
%   MU   - Estimated probabilities of each model
%   X    - Combined state mean estimate
%   P    - Combined state covariance estimate
%
% Description:
%   IMM filter measurement update step.
%
% See also:
%   IMM_PREDICT, IMM_SMOOTH, IMM_FILTER

% History:
%   01.11.2007 JH The first official version.
%
% Copyright (C) 2007 Jouni Hartikainen
%
% $Id: imm_update.m 111 2007-11-01 12:09:23Z jmjharti $
%
% This software is distributed under the GNU General Public
% Licence (version 2 or later); please refer to the file
% Licence.txt, included with the software, for details.

function [X_i,P_i,MU,X,P] = imm_update(X_p,P_p,c_j,ind,dims,Y,H,R)
    % Number of models
    m = length(X_p);

    % Space for update state mean, covariance and likelihood of measurements
    X_i = cell(1,m);
    P_i = cell(1,m);
    lambda = zeros(1,m);

    % Update for each model
    for i = 1:m
        % Update the state estimates
        [X_i{i}, P_i{i}, K, IM, IS, lambda(i)] = kf_update(X_p{i},P_p{i},Y,H{i},R{i});
    end

    % Calculate the model probabilities
    MU = zeros(1,m);
    c = sum(lambda.*c_j);
    MU = c_j.*lambda/c;

    % Output the combined updated state mean and covariance, if wanted.
    if nargout > 3
        % Space for estimates
        X = zeros(dims,1);
        P = zeros(dims,dims);
        % Updated state mean
        for i = 1:m
            X(ind{i}) = X(ind{i}) + MU(i)*X_i{i};
        end
        % Updated state covariance
        for i = 1:m
            P(ind{i},ind{i}) = P(ind{i},ind{i}) + MU(i)*(P_i{i} + (X_i{i}-X(ind{i}))*(X_i{i}-X(ind{i}))');
        end
    end


% ASTROTIK by Francesco Santilli
% R2BP (Restricted Two Bodies Problem)
% Compute the Minimum Orbit Intersection Distance (MOID)
%
% Usage: [d,f] = moid(orbitA,orbitB)
%
% where: orbit = [p e i o w] = 3d orbit elements
%        orbit = [p e w] = 2d orbit elements
%           p = semi-latus rectum [L] (p>0)
%           e = eccentricity [-] (0<=e<1)
%           i = inclinaion [rad]
%           o = raan [rad]
%           w = argument of perifocus [rad]
%        d = MOID [L]
%        f = [fA fB] = true anomalies [rad]

function [d,f] = moid(orbitA,orbitB)

    if ~(nargin == 2)
        error('Wrong number of input arguments.')
    end

    DA = check(orbitA,1);
    DB = check(orbitB,1);

    if DA ~= DB
        error('Wrong size of input arguments.')
    end

    if ~(DA==3 || DA==5)
        error('Wrong size of input arguments.')
    end
    d3 = (DA==5);

    % rotation matrix
    if d3
        [pA,eA,iA,oA,wA] = take(orbitA);
        [pB,eB,iB,oB,wB] = take(orbitB);
        RA = rotation([oA iA wA]);
        RB = rotation([oB iB wB]);
        RA = RA(1:2,:);
        RB = RB(1:2,:);
    else
        [pA,eA,wA] = take(orbitA);
        [pB,eB,wB] = take(orbitB);
        cwA = cos(wA);
        swA = sin(wA);
        cwB = cos(wB);
        swB = sin(wB);
        RA = [ cwA swA
             -swA cwA];
        RB = [ cwB swB
              -swB cwB];
    end

    if pA<=0 || pB<=0
        error('p must be a stricly positive value.')
    end

    if eA<0 || eB<0 || eA>=1 || eB>=1
        error('e must be in the range [0,1).')
    end

    L2 = pA^2+pB^2;
    f0 = [+1 +1 -1 -1
          +1 -1 +1 -1]*pi/2;

    f = zeros(2,4);
    d = zeros(1,4);
    ef = zeros(1,4);

    opt = optimset('LargeScale','on','Display','off','GradObj','on',...
                   'TolX',eps,'TolFun',0,... % 'Hessian','on',
                   'MaxFunEvals',Inf,'MaxIter',Inf);
    for k = 1:4
        try
            [f(:,k),d(k),ef(k)] = fminunc(@fun,f0(:,k),opt);
        catch
            f(:,k) = [NaN NaN]';
            d(k) = NaN;
            ef(k) = 0;
        end
    end

    bad = (ef==0);
    f(:,bad) = [];
    d(:,bad) = [];
    if isempty(f)
        error('No solution found.')
    end

    d = sqrt(d*L2);
    f = mod(f+pi,2*pi)-pi;
    [d,k] = min(d);
    f = f(:,k(1))';

    function [F,FF,FFF] = fun(f)

        fA = f(1);
        fB = f(2);

        cA = cos(fA);
        cB = cos(fB);
        sA = sin(fA);
        sB = sin(fB);

        rA = pA/(1+eA*cA);
        rB = pB/(1+eB*cB);

        A = rA*[cA sA]*RA;
        B = rB*[cB sB]*RB;

        D = A-B;
        F = D*D' / L2;

        if nargout > 1

            kA = rA^2/pA;
            kB = rB^2/pB;

            GA = [-sA eA+cA]*RA;
            GB = [-sB eB+cB]*RB;

            FA =  2*kA*D*GA';
            FB = -2*kB*D*GB';
            FF = [FA; FB] / L2;

            if nargout > 2

                kAA = 2*kA^2/rA*eA*sA;
                kBB = 2*kB^2/rB*eB*sB;

                GAA = -[cA sA]*RA;
                GBB = -[cB sB]*RB;

                FAA =  2*(kAA*D*GA' + kA*D*GAA' + kA^2*GA*GA');
                FBB = -2*(kBB*D*GB' + kB*D*GBB' + kB^2*GB*GB');
                FAB = -2*kA*kB*GA*GB';

                FFF = [FAA FAB
                       FAB FBB] / L2;

            end
        end

        %fprintf('%.20f %.20f -> %.20f\n',f',F);

    end

end


% Copyright Andrew Binning 2013
% Please feel free to use and modify this code as you see if fit. If you
% use this code in any academic work, please cite
% Andrew Binning, 2013.
% "Underidentified SVAR models: A framework for combining short and long-run restrictions with sign-restrictions,"
% Working Paper 2013/14, Norges Bank.
function C = generateDraw(C,k)
%==========================================================================
% Generates a draw that is consistent with the shock variance/covariance
% matrix. Based on Juan F. Rubio-Ramirez & Daniel F. Waggoner & Tao Zha, 2010.
% "Structural Vector Autoregressions: Theory of Identification and
% Algorithms for Inference," Review of Economic Studies, Oxford University Press, vol. 77(2), pages 665-696.
%
% inputs:
% C = initial impact matrix, usually from the cholesky decomposition of the
% forecast error variance decomposition
% k = number of dependent variables
%
% outputs:
% C = new draw of the short run impact matrix
%==========================================================================

newmatrix = randn(k,k);

[Q,R] = qr(newmatrix);

for ii = 1:k
    if R(ii,ii)<0
        Q(:,ii) = -Q(:,ii);
    end
end

C = C*Q;


% Log-harmonic scale
%
% Inputs
%  Hb    : Below this harmonic limit, the scale is linear
%          (similar to the mel scale which is linear below 1000Hz)
%          (e.g. 12)
%          Based on observation of the LF model, the asymptotic behavior of the
%          spectrum starts around the 12th harmonic (for the most tense voice)
%  Hmax  : The maxmimum number of harmonic considered during synthesis
%          (e.g. 256)
%  order : The reduced number of phase coefficients (e.g. 24)
%
% Outputs
%  hsl   : The log-harmonic scale
%
% Copyright (c) 2013 University of Crete - Computer Science Department(UOC-CSD)/
%                    Foundation for Research and Technology-Hellas - Institute
%                    of Computer Science (FORTH-ICS)
%
% License
%  This file is under the LGPL license,  you can
%  redistribute it and/or modify it under the terms of the GNU Lesser General
%  Public License as published by the Free Software Foundation, either version 3
%  of the License, or (at your option) any later version. This file 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 Lesser General Public License for more
%  details.
%
% This function is part of the Covarep project: http://covarep.github.io/covarep
%
% Author
%  Gilles Degottex <degottex@csd.uoc.gr>
%  Jonas Ballani for the bezier curve (see nested function)
%

function hsl = hlin2hlog(Hb, Hmax, order)

    %   The last compressed coef (i.e. order) corresponds to a given Hmax

    % Use linear scale below Hb (higher coefs will be overwritten)
    hsl = 1:Hmax;

    % Build a Bezier curve to start with a linear scale and finish smoothly
    % at (Hmax,order)
    p(1,:) = [Hb, Hb];
    p(2,:) = [order, order];
    p(3,:) = [Hmax, order];
    t = 0:0.01:1;
    [X,Y,p_bez] = CASTELJAU(0,1,p,t);
    hsl(Hb+1:end) = interp1(p_bez(:,1), p_bez(:,2), (Hb+1):Hmax);

    if 0
        hs = 1:Hmax;
        plot(hs, hsl, 'k');

        hold on;
        keyboard
    end

return

function [X,Y,val] = CASTELJAU(a,b,p,y)

    % function val = CASTELJAU(a,b,p,y)
    %
    % INPUT:  a   Linke Intervallgrenze
    %         b   Rechte Intervallgrenze
    %         p   Stützstellen (nx2-Matrix)
    %         y   Auswertungspunkte (Spaltenvektor)
    %
    % OUTPUT: val   Werte des Bezierpolynoms an y (mx2-Matrix)
    %
    % Date:   2007-11-05
    % Author: Jonas Ballani

    % Notes from degottex@csd.uoc.gr:
    %  From bezier.zip
    %  From http://m2matlabdb.ma.tum.de/download.jsp?MC_ID=7&SC_ID=8&MP_ID=480
    %  No license or copyright specified.

    n = size(p,1);
    m = length(y);
    T = zeros(n,n);
    val = zeros(m,2);
    X(:,1) = p(:,1);
    Y(:,1) = p(:,2);

    for j = 1:m
        for i = 2:n
            X(i:n,i) = (b-y(j))/(b-a)*X(i-1:n-1,i-1) + (y(j)-a)/(b-a)*X(i:n,i-1);
            Y(i:n,i) = (b-y(j))/(b-a)*Y(i-1:n-1,i-1) + (y(j)-a)/(b-a)*Y(i:n,i-1);
        end
        val(j,1) = X(n,n);
        val(j,2) = Y(n,n);
    end

return


function [ output ] = FW_T( par)

% this function implements Frank-Wolfe-thresholding method
% 0.5*norm(Omega.*(L+S-M), 'fro')^2 + lambda_1 ||L||_* + lambda_2 ||S||_1
%
%
%%  Cun Mu and John Wright, Mar '14

M = par.M; % data matrix
[m,n] = size(M);

epsilon = 10^-3; compare = 0; % by default (if no specification)
if isfield(par, 'epsilon') epsilon = par.epsilon; end
lambda_1 = par.lambda_1;
lambda_2 = par.lambda_2;
iter = par.iter;
display = par.display;
method = par.method; % power or exact
Omega = par.Omega;
rho = sum(sum(Omega))/m/n;
%% FW-T method

% initialization
L = zeros(m,n); S = zeros(m,n);
t_1 = 0; t_2 = 0;

U_1 = 0.5*norm(M,'fro')^2/lambda_1; % initial rough guess
U_2 = 0.5*norm(M,'fro')^2/lambda_2; % initial rough guess

history = 0; % to store the values of each iteration
tic
%% full observation scenario
%if ~isfield(par, 'Omega')
if rho == 1
    fprintf('full observation. \n');
    history(1) = norm(M,'fro')^2/2;
    temp = L + S - M;  % gradient
    count = 0;
    for k = 1: iter

        if k>=2
            if abs(history(k)-history(k-1))/history(k-1)<epsilon
                count = count+1;
            else
                count=0;
            end
            if count==5
                break;
                fprintf('--------------------------------\n')
                fprintf('total # of iter. used is %d. \n', k);
            end
        end

        fprintf('the current function value is %d \n', history(k));

        if mod(k,1) == 0
            fprintf('--------------------------------\n')
            fprintf('this is the %d th iter. \n', k);
        end

        %------------------linearization subproblem-----------------------%

        if  strcmp(method,'power') % approximate
            fprintf('power method is leveraged \n')
            [U ev V] =  power_method(temp, 5);
        else % exact
            [U,ev,V] = lansvd(temp,1,'l'); % top eigen
        end

        D_L = -U*V';

        if lambda_1 >= ev
            V_L = 0; V_t_1 = 0;
        else
            V_L = U_1*D_L; V_t_1 = U_1;
        end

        [mag ind] = max(vec(abs(temp)));
        j = floor((ind-1)/m)+1; i = mod(ind-1,m)+1;
        sign_ = sign(temp(i,j));
        D_S = zeros(m,n);
        D_S(i,j) = -sign_;

        if lambda_2 >= mag
            V_S = 0; V_t_2 = 0;
        else
            V_S = U_2*D_S; V_t_2 = U_2;
        end

        H = zeros(2,2);
        temp_1 = V_L-L; temp_2 = V_S-S; % temp = L + S - M;
        H(1,1) = norm(temp_1, 'fro')^2;
        H(2,2) = norm(temp_2, 'fro')^2;
        H(1,2) = sum(sum(temp_1.*temp_2));
        H(2,1) = H(1,2);
        f = zeros(1,2);
        f(1) = sum(sum(temp_1.*temp));
        f(2) = sum(sum(temp_2.*temp));
        f = f + [lambda_1*(V_t_1-t_1), lambda_2*(V_t_2-t_2)];

        % using QP solvers
        lb = zeros(2,1);
        ub = [1;1];
        options.Display = 'off';
        options.TolFun = 10^-5;
        x = quadprog(H,f,[],[],[],[],lb,ub,[],options);
        x = quadprog(H,f,[],[],[],[],lb,ub,[],options);

        %----------------------- update L and S --------------------------%
        alpha = x(1);
        beta = x(2);
        L = (1-alpha)*L + alpha*V_L; t_1 = t_1 + alpha*(V_t_1-t_1);
        S = (1-beta)*S + beta*V_S; t_2 = t_2 + beta*(V_t_2-t_2);

        %------------------------ thresholding----------------------------%

        temp_3 = M-L;

        S = max(temp_3 - lambda_2, 0);
        S = S + min(temp_3 + lambda_2, 0);

        t_2 = sum(sum(abs(S)));

        %----------- update U_1 and U_2 to a better esitmate -------------%
        temp = L + S - M;
        history(k+1) = norm(temp,'fro')^2/2+lambda_1*t_1+lambda_2*t_2;
        U_1 = min(history(k+1)/lambda_1,U_1);
        U_2 = min(history(k+1)/lambda_2,U_2);

    end

end

%% partial observation scenario
if rho<1
    fprintf('partial observation. \n');
    temp = Omega.*(L + S - M);  % gradient
    history(1) = norm(temp,'fro')^2/2+lambda_1*t_1+lambda_2*t_2;
    count = 0;

    for k = 1: iter

        if k>=2
            if abs(history(k)-history(k-1))*2/history(1)<epsilon
                count = count+1;
            else
                count=0;
            end
            if count==5
                break;
                fprintf('--------------------------------\n')
                fprintf('total # of iter. used is %d. \n', k);
            end
        end

        % current function value
        fprintf('the current function value is %d \n', history(k));
        fprintf('--------------------------------\n')
        fprintf('this is the %d th iter. \n', k);

        %------------------linearization subproblem-----------------------%

        if  strcmp(method,'power') % approximate
            [U ev V] =  power_method(temp, 5);
        else % exact
            [U,ev,V] = lansvd(temp,1,'l'); % top eigen
        end

        D_L = -U*V';

        if lambda_1 >= ev
            V_L = 0; V_t_1 = 0;
        else
            V_L = U_1*D_L; V_t_1 = U_1;
        end

        [mag ind] = max(vec(abs(temp)));
        j = floor((ind-1)/m)+1; i = mod(ind-1,m)+1;
        sign_ = sign(temp(i,j));
        D_S = zeros(m,n);
        D_S(i,j) = -sign_;

        if lambda_2 >= mag
            V_S = 0; V_t_2 = 0;
        else
            V_S = U_2*D_S; V_t_2 = U_2;
        end

        %--------------------- use QP (exact search) ---------------------%
        H = zeros(2,2);
        temp_1 = Omega.*(V_L-L); temp_2 = Omega.*(V_S-S); % temp = L + S - M;
        H(1,1) = norm(temp_1, 'fro')^2;
        H(2,2) = norm(temp_2, 'fro')^2;
        H(1,2) = sum(sum(temp_1.*temp_2));
        H(2,1) = H(1,2);
        f = zeros(1,2);
        f(1) = sum(sum(temp_1.*temp));
        f(2) = sum(sum(temp_2.*temp));
        f = f + [lambda_1*(V_t_1-t_1), lambda_2*(V_t_2-t_2)];

        % using QP solvers
        lb = zeros(2,1);
        ub = [1;1];
        options.Display = 'off';
        options.TolFun = 10^-5;
        x = quadprog(H,f,[],[],[],[],lb,ub,[],options);
        x = quadprog(H,f,[],[],[],[],lb,ub,[],options);

        %----------------------- update L and S --------------------------%
        alpha = x(1);
        beta = x(2);
        L = (1-alpha)*L + alpha*V_L; t_1 = t_1 + alpha*(V_t_1-t_1);
        S = (1-beta)*S + beta*V_S; t_2 = t_2 + beta*(V_t_2-t_2);

        %------------------------ thresholding----------------------------%
        temp_3 = S-Omega.*(L+S-M);;
        S = max(temp_3 - lambda_2, 0);
        S = S + min(temp_3 + lambda_2, 0);
        t_2 = norm(vec(S),1);

        %----------- update U_1 and U_2 to a better esitmate -------------%
        temp = Omega.*(L + S - M);  % gradient
        history(k+1) = norm(temp,'fro')^2/2+lambda_1*t_1+lambda_2*t_2;
        U_1 = min(history(k)/lambda_1,U_1);
        U_2 = min(history(k)/lambda_2,U_2);

    end

end
toc
%% output
output.L = L;
output.S = S;
output.hist = history;
output.iter = k;
end


function [T]=affineTransformationMatrixDirect(V1,V2)

%%
%Force input to 3D
if size(V1,2)==2
    V1(:,3)=0;
end

if size(V2,2)==2
    V2(:,3)=0;
end

%Expand to nx4
V1_M=V1;
V1_M(:,4)=1;
V2_M=V2;
V2_M(:,4)=1;

%Get transformation using left devide
T=(V1_M\V2_M)';

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% 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 3 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 should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


% Calculate the value of lambda so that if lambda >= lambdamax, the TVD
% functional solved by l1pwc is minimized by the trivial constant
% solution x = mean(y). This can then be used to determine a useful range
% of values of lambda, for example.
%
% Usage:
% lambdamax = l1pwclmax(y)
%
% Input arguments:
% - y          Original signal to denoise, size N x 1.
%
% Output arguments:
% - lambdamax  Value of at which x = mean(y) is the output of the l1pwc
%              function.
%
% (c) Max Little, 2010. If you use this code for your research, please
% cite:
% M.A. Little, Nick S. Jones (2010)
% "Sparse Bayesian Step-Filtering for High-Throughput Analysis of Molecular
% Machine Dynamics", in 2010 IEEE International Conference on Acoustics,
% Speech and Signal Processing, 2010, ICASSP 2010 Proceedings.
%

function lambdamax = ML_l1pwclmax(y)

narginchk(1,1);
y = y(:);
N = length(y);
M = N - 1;

% Construct sparse operator matrices
I1 = speye(M,M);
O1 = spalloc(M,1,M);
D = [I1 O1]-[O1 I1];

DDT = D*D';
Dy  = D*y;

lambdamax = max(abs(DDT\Dy));

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  MATLAB Code for                                                  %
%                                                                   %
%  Multi-Objective Particle Swarm Optimization (MOPSO)              %
%  Version 1.0 - Feb. 2011                                          %
%                                                                   %
%  According to:                                                    %
%  Carlos A. Coello Coello et al.,                                  %
%  "Handling Multiple Objectives with Particle Swarm Optimization," %
%  IEEE Transactions on Evolutionary Computation, Vol. 8, No. 3,    %
%  pp. 256-279, June 2004.                                          %
%                                                                   %
%  Developed Using MATLAB R2009b (Version 7.9)                      %
%                                                                   %
%  Programmed By: S. Mostapha Kalami Heris                          %
%                                                                   %
%         e-Mail: sm.kalami@gmail.com                               %
%                 kalami@ee.kntu.ac.ir                              %
%                                                                   %
%       Homepage: http://www.kalami.ir                              %
%                                                                   %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function G=CreateHypercubes(costs,ngrid,alpha)

    nobj=size(costs,1);

    empty_grid.Lower=[];
    empty_grid.Upper=[];
    G=repmat(empty_grid,nobj,1);

    for j=1:nobj

        min_cj=min(costs(j,:));
        max_cj=max(costs(j,:));

        dcj=alpha*(max_cj-min_cj);

        min_cj=min_cj-dcj;
        max_cj=max_cj+dcj;

        gx=linspace(min_cj,max_cj,ngrid-1);

        G(j).Lower=[-inf gx];
        G(j).Upper=[gx inf];

    end

end


function imgOut = ConvertXYZtoYxy(img, inverse)
%
%       imgOut = ConvertxXYZtoYxy(img, inverse)
%
%
%        Input:
%           -img: image to convert from XYZ to Yxy or from Yxy to XYZ.
%           -inverse: takes as values 0 or 1. If it is set to 0 the
%                     transformation from XYZ to Yxy is applied, otherwise
%                     the transformation from Yxy to XYZ.
%
%        Output:
%           -imgOut: converted image in Yxy or XYZ.
%
%     Copyright (C) 2013  Francesco Banterle
%
%     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 3 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 should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%

check3Color(img);
[r, c, col] = size(img);
imgOut = zeros(r, c, col);

if(inverse == 0)%forward transform
    norm = zeros(r, c);
    for i=1:3
        norm = norm + img(:,:,i);
    end

    imgOut(:,:,1) = img(:,:,2);

    imgOut(:,:,2) = img(:,:,1) ./ (norm);
    imgOut(:,:,3) = img(:,:,2) ./ (norm);
end

if(inverse == 1)%inverse transform
    Y_over_y = img(:,:,1) ./ img(:,:,3);
    imgOut(:,:,1) = Y_over_y .* img(:,:,2);
    imgOut(:,:,2) = img(:,:,1);
    imgOut(:,:,3) = Y_over_y .* (1.0 - img(:,:,2) - img(:,:,3));
end

imgOut = RemoveSpecials(imgOut);

end


function [Va, success] = dcpf(B, Pbus, Va0, ref, pv, pq)
%DCPF  Solves a DC power flow.
%   [VA, SUCCESS] = DCPF(B, PBUS, VA0, REF, PV, PQ) solves for the bus
%   voltage angles at all but the reference bus, given the full system
%   B matrix and the vector of bus real power injections, the initial
%   vector of bus voltage angles (in radians), and column vectors with
%   the lists of bus indices for the swing bus, PV buses, and PQ buses,
%   respectively. Returns a vector of bus voltage angles in radians.
%
%   See also RUNDCPF, RUNPF.

%   MATPOWER
%   Copyright (c) 1996-2016, Power Systems Engineering Research Center (PSERC)
%   by Carlos E. Murillo-Sanchez, PSERC Cornell & Universidad Nacional de Colombia
%   and Ray Zimmerman, PSERC Cornell
%
%   This file is part of MATPOWER.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://matpower.org for more info.

%% constant
Va_threshold = 1e5;     %% arbitrary threshold on |Va| for declaring failure

%% initialize result vector
Va = Va0;
success = 1;    %% successful by default

%% set up to trap non-singular matrix warnings
[lastmsg, lastid] = lastwarn;
lastwarn('');

%% update angles for non-reference buses
Va([pv; pq]) = B([pv; pq], [pv; pq]) \ ...
                (Pbus([pv; pq]) - B([pv; pq], ref) * Va0(ref));

[msg, id] = lastwarn;
%% Octave is not consistent in assigning proper warning id, so we'll just
%% check for presence of *any* warning
if ~isempty(msg) || max(abs(Va)) > Va_threshold
    success = 0;
end

%% restore warning state
lastwarn(lastmsg, lastid);


function [Detect] = cfar_ca1D_square(Xcube,noiseWin,guardLen,Pfa,wrapMode,ord_stat)
N = noiseWin*2;
alpha = N*(Pfa^(-1/N)-1);
alpha_oneside = noiseWin*(Pfa^(-1/noiseWin)-1);
Xcube = Xcube.^2;
Xlength = length(Xcube);
Detect = [];
numOfDet = 0;
% ord_stat = 0.7;
% if not CAOS-CFAR, set ord_stat = 1

if wrapMode == 0    %%% disabled warpped mode
    for i = 1:Xlength
        if i < noiseWin+guardLen+1  %%% one-sided comparision for left section
            Xcube_select = sort(Xcube(i+guardLen+1:i+guardLen+noiseWin), 'descend');
            num_filter = round(length(Xcube_select) * (1-ord_stat));
            noiseWin_len = noiseWin - num_filter;
            if num_filter > 0
                Xcube_select(1:num_filter) = 0;
            end
            noise_estimate = sum(Xcube_select)/noiseWin_len;
            if Xcube(i) > alpha_oneside*noise_estimate
                numOfDet = numOfDet + 1;
                Detect(1,numOfDet) = i; %%% index
                Detect(2,numOfDet) = Xcube(i);  %%% object power
                Detect(3,numOfDet) = noise_estimate;  %%% estimated noise
            end
        elseif i < Xlength-noiseWin-guardLen+1  %%% two-sided comparison for middle section
            Xcube_select = sort(Xcube(i+guardLen+1:i+guardLen+noiseWin), 'descend');
            Xcube_select2 = sort(Xcube(i-guardLen-noiseWin:i-guardLen-1), 'descend');
            num_filter = round(length(Xcube_select) * (1-ord_stat));
            noiseWin_len = noiseWin - num_filter;
            if num_filter > 0
                Xcube_select(1:num_filter) = 0;
                Xcube_select2(1:num_filter) = 0;
            end
            noise_estimate = (sum(Xcube_select) + sum(Xcube_select2))/(2*noiseWin_len);
            if Xcube(i) > alpha*noise_estimate
                numOfDet = numOfDet + 1;
                Detect(1,numOfDet) = i; %%% index
                Detect(2,numOfDet) = Xcube(i);  %%% object power
                Detect(3,numOfDet) = noise_estimate;  %%% estimated noise
            end
        else     %%%  one-sided comparision for right section
            Xcube_select = sort(Xcube(i-guardLen-noiseWin:i-guardLen-1), 'descend');
            num_filter = round(length(Xcube_select) * (1-ord_stat));
            noiseWin_len = noiseWin - num_filter;
            if num_filter > 0
                Xcube_select(1:num_filter) = 0;
            end
            noise_estimate = sum(Xcube_select)/noiseWin_len;
            if Xcube(i) > alpha_oneside*noise_estimate
                numOfDet = numOfDet + 1;
                Detect(1,numOfDet) = i; %%% index
                Detect(2,numOfDet) = Xcube(i);  %%% object power
                Detect(3,numOfDet) = noise_estimate;  %%% estimated noise
            end
        end
    end
else       %%% enabled wrapped mode
    for i = 1:Xlength
        if i < noiseWin+guardLen+1  %%% two-sided comparision for left section with wrap
            %%% discuss the wrap scenario
            if i <= guardLen
                noise_estimate = (sum(Xcube(i+guardLen+1:i+guardLen+noiseWin))...
                    + sum(Xcube(Xlength+i-guardLen-noiseWin:Xlength+i-guardLen-1)))/N;
            else
                noise_estimate = (sum(Xcube(i+guardLen+1:i+guardLen+noiseWin))...
                    + sum(Xcube(Xlength+i-guardLen-noiseWin:Xlength))+sum(Xcube(1:i-1-guardLen)))/N;
            end

            if Xcube(i) > alpha*noise_estimate
                numOfDet = numOfDet + 1;
                Detect(1,numOfDet) = i; %%% index
                Detect(2,numOfDet) = Xcube(i);  %%% object power
                Detect(3,numOfDet) = noise_estimate;  %%% estimated noise
            end

        elseif i < Xlength-noiseWin-guardLen+1  %%% two-sided comparison for middle section
            noise_estimate = (sum(Xcube(i+guardLen+1:i+guardLen+noiseWin))...
                + sum(Xcube(i-guardLen-noiseWin:i-guardLen-1)))/N;
            if Xcube(i) > alpha*noise_estimate
                numOfDet = numOfDet + 1;
                Detect(1,numOfDet) = i; %%% index
                Detect(2,numOfDet) = Xcube(i);  %%% object power
                Detect(3,numOfDet) = noise_estimate;  %%% estimated noise
            end

        else     %%%  two-sided comparision for right section with wrap
            if i >= Xlength-guardLen+1
                noise_estimate = (sum(Xcube(i-guardLen-noiseWin:i-guardLen-1))...
                    + sum(Xcube(guardLen+i-Xlength+1:guardLen+i-Xlength+noiseWin)))/N;
            else
                noise_estimate = (sum(Xcube(i-guardLen-noiseWin:i-guardLen-1))...
                    + sum(Xcube(guardLen+i+1:Xlength))+sum(Xcube(1:noiseWin-Xlength+i+guardLen)))/N;
            end

            if Xcube(i) > alpha*noise_estimate
                numOfDet = numOfDet + 1;
                Detect(1,numOfDet) = i; %%% index
                Detect(2,numOfDet) = Xcube(i);  %%% object power
                Detect(3,numOfDet) = noise_estimate;  %%% estimated noise
            end
        end
    end
end
end


%This file will generate the library we prepared for the MMK example.
% Last Updated: 2019/04/22
% Coded By: K

function [Data,Sym_Struct]=SINDyLib(X,dX,u,Highest_Poly_Order,Highest_Trig_Order,Highest_U_Order,Highest_dPoly_Order)
%% First get the size of the X matrix, determin the data length and the number of variables we have.
[Data_Length,Variable_Number]=size(X);
[~,Variable_Number_dX]=size(dX);
[~,Variable_Number_u]=size(u);
%Also create the symbolic variable
Symbol=sym('z',[Variable_Number,1]);
Symbol_dX=sym('dz',[Variable_Number,1]);
Symbol_u=sym('u',[Variable_Number_u,1]);

%Now according the Highest Polynomial Order entered, we will calculate the data matrix.
Data=[];
Index=1;

%% First calculate the polynomial term
%Order zero:
Index=1;
Data(:,Index)=ones(Data_Length,1);
Sym_Struct{1,Index}=1;

%Order One:
if Highest_Poly_Order>=1
    for i=1:Variable_Number
        Index=Index+1;
        Data(:,Index)=X(:,i);
        Sym_Struct{1,Index}=Symbol(i,1);
    end
end

%Order Two:
if Highest_Poly_Order>=2
    for i=1:Variable_Number
        for j=i:Variable_Number
            Index=Index+1;
            Data(:,Index)=X(:,i).*X(:,j);
            Sym_Struct{1,Index}=Symbol(i,1)*Symbol(j,1);
        end
    end
end

%Order Three:
if Highest_Poly_Order>=3
    for i=1:Variable_Number
        for j=i:Variable_Number
            for k=j:Variable_Number
                Index=Index+1;
                Data(:,Index)=X(:,i).*X(:,j).*X(:,k);
                Sym_Struct{1,Index}=Symbol(i,1)*Symbol(j,1)*Symbol(k,1);
            end
        end
    end
end

%Order Four:
if Highest_Poly_Order>=4
    for i=1:Variable_Number
        for j=i:Variable_Number
            for k=j:Variable_Number
                for pi=k:Variable_Number
                    Index=Index+1;
                    Data(:,Index)=X(:,i).*X(:,j).*X(:,k).*X(:,pi);
                    Sym_Struct{1,Index}=Symbol(i,1)*Symbol(j,1)*Symbol(k,1)*Symbol(pi,1);
                end
            end
        end
    end
end

%% Then add the Trigonometric Function in the output data:

%Order One:
if Highest_Trig_Order>=1
    for i=1:Variable_Number
        Index=Index+1;
        Data(:,Index)=sin(X(:,i));
        Sym_Struct{1,Index}=sin(Symbol(i,1));
    end
    for i=1:Variable_Number
        Index=Index+1;
        Data(:,Index)=cos(X(:,i));
        Sym_Struct{1,Index}=cos(Symbol(i,1));
    end
end

%Order Two:
if Highest_Trig_Order>=2
    for i=1:Variable_Number
        for j=1:Variable_Number
            Index=Index+1;
            Data(:,Index)=sin(X(:,i)+X(:,j));
            Sym_Struct{1,Index}=sin(Symbol(i,1)+Symbol(j,1));
        end
    end
    for i=1:Variable_Number
        for j=1:Variable_Number
            Index=Index+1;
            Data(:,Index)=cos(X(:,i)+X(:,j));
            Sym_Struct{1,Index}=cos(Symbol(i,1)+Symbol(j,1));
        end
    end
    %
    for i=1:Variable_Number
        for j=i+1:Variable_Number
            Index=Index+1;
            Data(:,Index)=sin(X(:,i)-X(:,j));
            Sym_Struct{1,Index}=sin(Symbol(i,1)-Symbol(j,1));
        end
    end
    for i=1:Variable_Number
        for j=i+1:Variable_Number
            Index=Index+1;
            Data(:,Index)=cos(X(:,i)-X(:,j));
            Sym_Struct{1,Index}=cos(Symbol(i,1)-Symbol(j,1));
        end
    end
    %
    for i=1:Variable_Number
        for j=1:Variable_Number
            Index=Index+1;
            Data(:,Index)=sin(X(:,i)-2*X(:,j));
            Sym_Struct{1,Index}=sin(Symbol(i,1)-2*Symbol(j,1));
        end
    end
    %
    for i=1:Variable_Number
        for j=1:Variable_Number
            Index=Index+1;
            Data(:,Index)=cos(X(:,i)-2*X(:,j));
            Sym_Struct{1,Index}=cos(Symbol(i,1)-2*Symbol(j,1));
        end
    end
end

%Order Three:
if Highest_Trig_Order>=3
    %
    for i=1:Variable_Number
        for j=i+1:Variable_Number
            Index=Index+1;
            Data(:,Index)=cos(X(:,i)-X(:,j)).^2;
            Sym_Struct{1,Index}=cos(Symbol(i,1)-Symbol(j,1))^2;
        end
    end
    %
    for i=1:Variable_Number
        for j=i+1:Variable_Number
            Index=Index+1;
            Data(:,Index)=sin(X(:,i)-X(:,j)).^2;
            Sym_Struct{1,Index}=sin(Symbol(i,1)-Symbol(j,1))^2;
        end
    end
end

%Order Four
if Highest_Trig_Order>=4
    %
    for i=1:Variable_Number
        for j=i:Variable_Number
            Index=Index+1;
            Data(:,Index)=sin(2*X(:,i)-2*X(:,j)).*X(:,i).^2;
            Sym_Struct{1,Index}=sin(2*Symbol(i,1)-2*Symbol(j,1))*Symbol(i,1)^2;
        end
    end
    %
    for i=1:Variable_Number
        for j=i:Variable_Number
            Index=Index+1;
            Data(:,Index)=cos(2*X(:,i)-2*X(:,j)).*X(:,i).^2;
            Sym_Struct{1,Index}=cos(2*Symbol(i,1)-2*Symbol(j,1))*Symbol(i,1)^2;
        end
    end
    %
    for i=1:Variable_Number
        for j=i+1:Variable_Number
            Index=Index+1;
            Data(:,Index)=sin(X(:,i)-X(:,j)).*X(:,i).^2;
            Sym_Struct{1,Index}=sin(Symbol(i,1)-Symbol(j,1))*Symbol(i,1)^2;
        end
    end
    %
    for i=1:Variable_Number
        for j=i+1:Variable_Number
            Index=Index+1;
            Data(:,Index)=cos(X(:,i)-X(:,j)).*X(:,i).^2;
            Sym_Struct{1,Index}=cos(Symbol(i,1)-Symbol(j,1))*Symbol(i,1)^2;
        end
    end
    %
    for i=1:Variable_Number
        j=2;
        Index=Index+1;
        Data(:,Index)=cos(X(:,i)-X(:,j)).*sin(X(:,i));
        Sym_Struct{1,Index}=cos(Symbol(i,1)-Symbol(j,1))*sin(Symbol(i,1));
    end
end

pin=Index;

%% From here, we add the dX*Theta elements in our data.
if Highest_dPoly_Order>=1
    for j=1:Variable_Number_dX
    for k=1:pin
        Index=Index+1;
        Data(:,Index)=dX(:,j).*Data(:,k);
        Sym_Struct{1,Index}=Symbol_dX(j,1)*(Sym_Struct{1,k});
    end
    end
end

%% Frome here, we add the u*Theta elements in our data
if Highest_U_Order>=1
    for j=1:Variable_Number_u
        for k=1:pin
            Index=Index+1;
            Data(:,Index)=u(:,j).*Data(:,k);
            Sym_Struct{1,Index}=Symbol_u(j,1)*(Sym_Struct{1,k});
        end
    end
end


function x = english_word_length_cdf_inv ( cdf )

%*****************************************************************************80
%
%% ENGLISH_WORD_LENGTH_CDF_INV inverts the English Word Length CDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 August 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Henry Kucera, Winthrop Francis,
%    Computational Analysis of Present-Day American English,
%    Brown University Press, 1967.
%
%  Parameters:
%
%    Input, real CDF, the value of the CDF.
%    0.0 <= CDF <= 1.0.
%
%    Output, integer X, the corresponding word length for which
%    CDF(X-1) < CDF <= CDF(X)
%
  word_length_max = 27;

  pdf_vec = [ ...
    0.03160, ...
    0.16975, ...
    0.21192, ...
    0.15678, ...
    0.10852, ...
    0.08524, ...
    0.07724, ...
    0.05623, ...
    0.04032, ...
    0.02766, ...
    0.01582, ...
    0.00917, ...
    0.00483, ...
    0.00262, ...
    0.00099, ...
    0.00050, ...
    0.00027, ...
    0.00022, ...
    0.00011, ...
    0.00006, ...
    0.00005, ...
    0.00002, ...
    0.00001, ...
    0.00001, ...
    0.00001, ...
    0.00001, ...
    0.00001 ];
  pdf_sum = 0.99997;

  if ( cdf < 0.0 || 1.0 < cdf )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'ENGLISH_WORD_LENGTH_CDF_INV - Fatal error!\n' );
    fprintf ( 1, '  CDF < 0 or 1 < CDF.\n' );
    error ( 'ENGLISH_WORD_LENGTH_CDF_INV - Fatal error!' );
  end

  cum = 0.0;

  for j = 1 : word_length_max

    cum = cum + pdf_vec(j);

    if ( cdf <= cum / pdf_sum )
      x = j;
      return
    end

  end

  x = word_length_max;

  return
end


function [f] = fact(n)
%FACT  Vectorized Factorial function
%
%usage: f = fact(n)
%
%tested under version 5.3.1
%
%     This function computes the factorial of
%     the elements of N.
%     N can be any size but must contain
%     Real, Non-Negative, Integers.
%
%     This routine is much more robust than
%     the built in FACTORIAL function.
%
%see also: Gamma, Prod, Factorial, Binomial

%Paul Godfrey
%pgodfrey@conexant.com
%8-23-00

[row,col]=size(n);
n=n(:);

f=NaN*n;

pp=find(imag(n)==0 & real(n)>=0 & round(n)==n);
%find integer values
nn=n(pp);
ff=zeros(length(pp),1);

s=1;
p=[];
if ~isempty(nn)
   p=find(nn==0);
end
if ~isempty(p)
   ff(p)=s;
end

%upper limit here depends upon realmax
if ~isempty(nn)
for k=1:170
    s=s*k;
%empty=scalar warning
    p=find(nn==k);
    if ~isempty(p)
        ff(p)=s;
    end
end
end

p=find(nn>170);
if ~isempty(p)
    ff(p)=Inf;
end

f(pp)=ff;
f=reshape(f,row,col);

return


% Test script for solving the 2D advection
Globals2D;

% Generate simple mesh
filename = 'Maxwell025.neu';

[Nv, VX, VY, K, EToV] = MeshReaderGambit2D(filename);
Nfaces = 3;
[EToE,EToF] = tiConnect2D(EToV);
BCType = Wall*(EToE==((1:K)'*ones(1,Nfaces)));

% Build mesh
Norder = ceil(10*rand(K,1));

% Set up arbitrary order elements mesh
[pinfo] = BuildPNonCon2D(Norder, K, VX, VY, EToV, BCType);

% Set initial conditions
mmode = 1; nmode = 1;

x = []; y = [];
for N1=1:max(Norder)
  pinf = pinfo(N1);
  x(pinf.ids) = pinf.x;
  y(pinf.ids) = pinf.y;
end
x = x'; y = y';
Ez = sin(mmode*pi*x).*sin(nmode*pi*y);
Hx = zeros(size(x)); Hy = zeros(size(x));

% Solve Problem for exactly one period
FinalTime = 1;
[Hx,Hy,Ez,time] = MaxwellPNonCon2D(pinfo, Hx,Hy,Ez,FinalTime);


% ALS Completion
% as described in
%
%   Michael Steinlechner, Riemannian optimization for high-dimensional tensor completion,
%   Technical report, March 2015, revised December 2015.
%   To appear in SIAM J. Sci. Comput.
%

%   TTeMPS Toolbox.
%   Michael Steinlechner, 2013-2016
%   Questions and contact: michael.steinlechner@epfl.ch
%   BSD 2-clause license, see LICENSE.txt
function [X,cost,test,stats] = completion_als( A_Omega, Omega, A_Gamma, Gamma, X, opts )

    if ~isfield( opts, 'maxiter');  opts.maxiter = 100;     end
    if ~isfield( opts, 'tol');      opts.tol = 1e-6;        end
    if ~isfield( opts, 'reltol');   opts.reltol = 1e-6;     end

	n = X.size;
	r = X.rank;
    d = X.order;

	cost = zeros(2*opts.maxiter,1);
	test = zeros(2*opts.maxiter,1);

    norm_A_Omega = norm( A_Omega );
    norm_A_Gamma = norm( A_Gamma );

    X = orthogonalize( X, 1 );

    t = tic;
    stats.time = [0];
    stats.conv = false;

	for i = 1:opts.maxiter

        % ===================
        % FORWARD SWEEP:
        % ===================
        fprintf(1,'Currently optimizing core: ')
        for mu = 1:d-1
            fprintf(1,'%i ', mu)
            X.U{mu} = solve_least_squares( A_Omega, Omega, X, mu );
            X = orth_at( X, mu, 'left' );
        end
		cost(2*i-1) = sqrt(2*func(A_Omega, X, Omega)) / norm_A_Omega;

        if cost(2*i-1) < opts.tol
            disp(sprintf('CONVERGED AFTER %i HALF-SWEEPS. Rel. residual smaller than %0.3g', ...
                          2*i-1, opts.tol))
            stats.conv = true;
            cost = cost(1:2*i-1,1);
            stats.time = [stats.time stats.time(end)+toc(t)];
            test(2*i-1) = sqrt(2*func(A_Gamma, X, Gamma)) / norm_A_Gamma;
            test = test(1:2*i-1,1);
            break
        end

        if i > 1
            reltol = abs(cost(2*i-1) - cost(2*i-2)) / cost(2*i-1);
            if reltol < opts.reltol
                disp(sprintf('No more progress in gradient change, but not converged after %i half-sweeps. ABORTING!. \nRelative change is smaller than %0.3g', ...
                              i, opts.reltol))
                stats.conv = false;
                cost = cost(1:2*i-1,1);
                stats.time = [stats.time stats.time(end)+toc(t)];
                test(2*i-1) = sqrt(2*func(A_Gamma, X, Gamma)) / norm_A_Gamma;
                test = test(1:2*i-1,1);
                break
            end
        end

        stats.time = [stats.time stats.time(end)+toc(t)];
        test(2*i-1) = sqrt(2*func(A_Gamma, X, Gamma)) / norm_A_Gamma;
        t = tic;

        fprintf(1,'\nFinished forward sweep.\n    Cost: %e\n    Test: %e\n', cost(2*i-1), test(2*i-1) );
        % ===================
        % BACKWARD SWEEP:
        % ===================
        fprintf(1,'Currently optimizing core: ')
        for mu = d:-1:2
            fprintf(1,'%i ', mu)
            X.U{mu} = solve_least_squares( A_Omega, Omega, X, mu );
            X = orth_at( X, mu, 'right' );
        end

		cost(2*i) = sqrt(2*func(A_Omega, X, Omega)) / norm_A_Omega;

        if cost(2*i) < opts.tol
            disp(sprintf('CONVERGED AFTER %i HALF-SWEEPS. Rel. residual smaller than %0.3g', ...
                          2*i, opts.tol))
            stats.conv = true;
            cost = cost(1:2*i,1);
            stats.time = [stats.time stats.time(end)+toc(t)];
            test(2*i) = sqrt(2*func(A_Gamma, X, Gamma)) / norm_A_Gamma;
            test = test(1:2*i,1);
            break
        end

        if i > 1
            reltol = abs(cost(2*i) - cost(2*i-1)) / cost(2*i);
            if reltol < opts.reltol
                disp(sprintf('No more progress in gradient change, but not converged after %i half-sweeps. ABORTING!. \nRelative change is smaller than %0.3g', ...
                              2*i, opts.reltol))
                stats.conv = false;
                cost = cost(1:2*i,1);
                stats.time = [stats.time stats.time(end)+toc(t)];
                test(2*i) = sqrt(2*func(A_Gamma, X, Gamma)) / norm_A_Gamma;
                test = test(1:2*i,1);
                break
            end
        end

        stats.time = [stats.time stats.time(end)+toc(t)];
        test(2*i) = sqrt(2*func(A_Gamma, X, Gamma)) / norm_A_Gamma;
        t = tic;
        fprintf(1,'\nFinished backward sweep.\n    Cost: %e\n    Test: %e\n', cost(2*i), test(2*i) );

        disp('_______________________________________________________________')
    end

    % This is to match original shape of stats.time, since we artificially start w/ [0]
    % for consistency in how we count time
    stats.time = stats.time(2:end);

end

function res = func(A_Omega, X, Omega)
	res = 0.5*norm( A_Omega - X(Omega) )^2;
end

function res = solve_least_squares( A_Omega, Omega, X, mu )

    n = X.size;
    d = X.order;
    r = X.rank;

    [jmu,idx] = sort(Omega(:,mu),'ascend');
    Omega = Omega(idx,:);
    A_Omega = A_Omega(idx);

    C = cell(1,d);
    for i=1:d
        C{i} = permute( X.U{i}, [1 3 2]);
    end
    res = zeros( size(C{mu}) );

    %B = zeros(size(Omega,1), r(mu)*r(mu+1));

    %imu = 1;
    %for sample = 1:size(Omega,1)

    %    L = 1;
    %    for i = 1:mu-1
    %        L = L * C{i}(:,:,Omega(sample,i));
    %    end

    %    R = 1;
    %    for i = d:-1:mu+1
    %        R = C{i}(:,:,Omega(sample,i)) * R;
    %    end
    %
    %    %B(sample,:) = kron(R',L);
    %    B(sample,:) = reshape( L'*R', 1, r(mu)*r(mu+1) );
    %end

    B = als_solve_mex( n, r, C, Omega', mu)';

    for i = 1:X.size(mu)
        idx = find(jmu == i);

        if isempty(idx)
            error('No samples for this slice!')
        end
        res(:,:,i) = reshape(B(idx,:)\A_Omega(idx), r(mu), r(mu+1));
    end

    res = permute( res, [1 3 2] );

end


function C = exp(X,p)
%EXP          Long exponential function
%
%  C = exp(X,p)
%
%for long number X. Input parameter p is optional; if specified,
%approximate accuracy of C approximately p decimals, otherwise that of X.
%Input X must be less than beta.
%

% written  12/30/98     S.M. Rump
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
% modified 08/26/12     S.M. Rump  global variables removed
%

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  INTLAB_LONG_BETA = getappdata(0,'INTLAB_LONG_BETA');
  INTLAB_LONG_LOGBETA = getappdata(0,'INTLAB_LONG_LOGBETA');

  % convert decimal precision in beta-digits
  if nargin==2
    longprecision(p);
    p = ceil( p/log10(INTLAB_LONG_BETA) );
  else
    p = size(X.mantissa,2) + 1;
  end

  if any( X.exponent>1 )
    error('exponent too big for long exponential')
  end

  index = ( X.exponent>0 );
  if any( index )
    large = 1;
    E = zeros(size(X.sign));
    while any(index)
      E(index) = E(index) + 1;
      X(index) = X(index)/2;
      index = ( X.exponent>0 );
    end
  else
    large = 0;
  end

  C = 1 + X;
  T = X;
  i = 1;
  while 1
    i = i+1;
    T = T * X / i;
    if all( T.exponent<-p )
      if isequal( longinit('ErrorTerm',0) , 'WithErrorTerm' )
        C.error = errorupdate( 1 , C.error , 0 , 1 , 1 , -p );
      end
      break
    end
    C = C + T;
  end

  if large
    index = ( E~=0 );
    while any(index)
      C(index) = C(index)*C(index);
      E(index) = E(index) - 1;
      index = ( E~=0 );
    end
  end

  if rndold
    setround(rndold)
  end


function redoxsteady
% Steady state redox zones
%    using MATLAB ode
%
%   $Ekkehard Holzbecher  $Date: 2006/03/31 $
%--------------------------------------------------------------------------
L = 100;                        % length [m]
v = 1;                          % velocity [m/s]
D = 0.2;                        % diffusivity [m*m/s]
lambda = 0.01;                  % organic carbon degradation parameter [1/m]
k1 = [0.1; 1; 0.9];             % 1. Michaelis-Menten parameter
k2 = [0.035; 1; 1];             % 2. Michaelis-Menten parameter [kg/m*m*m]
k3 = [3.5e-3; 1];               % inhibition coefficient [kg/m*m*m]
corg = 1;                       % organic carbon concentration at interface
cin = [4; 3; 0.001];            % interface concentrations [kg/m*m*m]
N = 100;                        % number of nodes

%----------------------execution-------------------------------------------

x = linspace(0,L,N);
solinit = bvpinit (x,[cin; zeros(3,1)]);
sol = bvp4c(@redox,@bcs,solinit,odeset,D,v,lambda,k1,k2,k3,corg,cin);

%---------------------- graphical output ----------------------------------

plot (x,corg*exp(-lambda*x),sol.x,sol.y(1:3,:));
legend ('C_{org}','O_2','NO_2','Mn'); grid;

%----------------------functions------------------------------
function dydx = redox(x,y,D,v,lambda,k1,k2,k3,corg,cin)

c0 = corg*exp(-lambda*x);
monod = k1.*(y(1:3)>0).*y(1:3)./(k2+y(1:3));
monod(3) = k1(3);
inhib = k3./(k3+y(1:2));
dydx = zeros (6,1);
dydx(1) =  y(4);
dydx(4) =  (v*y(4)+c0*monod(1))/D;
dydx(2) =  y(5);
dydx(5) =  (v*y(5)+c0*monod(2)*inhib(1))/D;
dydx(3) =  y(6);
dydx(6) =  (v*y(6)-c0*monod(3)*inhib(1)*inhib(2))/D;

function res = bcs (ya,yb,D,v,lambda,k1,k2,k3,corg,cin)
res = [ya(1:3)-cin; yb(4:6)-zeros(3,1)];


% nufft_table_test.m

%
% 3D test
%
if 1
	if ~isvar('s.t'), disp 'precompute structure'
%		ktype = 'minmax:kb';
		ktype = 'kaiser';
		Jd = [3 4 3];
		Nd = [10 8 9];
		Ld = [2^8 2^7 2^7];
		Kd = 2 * Nd;
%		n_shift = zeros(size(Nd)); disp 'easy: 0 shift'
		n_shift = [3 7 2]; % stress it

		gam = 2*pi ./ Kd;
		k1 = linspace(-2*Kd(1), 2*Kd(1), 21)';
		k2 = linspace(-2*Kd(2), 2*Kd(2), 18)';
		k3 = linspace(-2*Kd(3), 2*Kd(3), 23)';
		[kk1 kk2 kk3] = ndgrid(k1, k2, k3);
		om = [gam(1)*kk1(:) gam(2)*kk2(:) gam(3)*kk3(:)];

		tic
		s.p = nufft_init(om, Nd, Jd, Kd, n_shift, ktype);
		printf('pre time init %g', toc)

		tic
		s.t = nufft_init(om, Nd, Jd, Kd, n_shift, 'table', Ld, ktype);
		printf('tab time init %g', toc)
	end

	% 3D forward direction
	if 1, disp '3D forward'
%		x = [1:Nd(1)]'*triang(Nd(2))';
		rand('state', 0);
		x = rand(Nd);
		x = zeros(Nd);
		x(4, 2, 3) = 1;

		tic
		Y.d = dtft(x, om, n_shift); % exact
		printf('dtft3 time %g', toc)

		tic
		Y.p = nufft(x, s.p);
		printf('pre time nufft %g', toc)
		printf('pre max%%diff = %g', max_percent_diff(Y.d, Y.p))

		tic
		Y.t = nufft(x, s.t);
		printf('tab time nufft %g', toc)
		printf('tab max%%diff = %g', max_percent_diff(Y.d, Y.t))
		printf('tab vs pre max%%diff = %g', max_percent_diff(Y.p, Y.t))

		if 1
			clf
			im(231, k1, k2, abs(Y.p - Y.d), 'pre'), cbar
			im(232, k1, k2, abs(Y.t - Y.d), 'tab'), cbar
			im(233, k1, k2, abs(Y.t - Y.p), 'tab-pre'), cbar
		end
	end

	% 3D adjoint
	if 1, disp '3D adjoint'
		X = [1:s.t.M]' + 1i * ones(s.t.M,1);

		tic
		c.d = dtft_adj(X, om, Nd, n_shift); % exact
		printf('dtft time adj %g', toc)
		tic
		c.p = nufft_adj(X, s.p);
		printf('pre time nufft adj %g', toc)
		tic
		c.t = nufft_adj(X, s.t);
		printf('tab time nufft adj %g', toc)

		printf('pre max%%diff = %g', max_percent_diff(c.d, c.p))
		printf('tab max%%diff = %g', max_percent_diff(c.d, c.t))
		printf('tab vs pre max%%diff = %g', max_percent_diff(c.p, c.t))

		if 1
			im(234, abs(c.p - c.d), 'pre adj err'), cbar
			im(235, abs(c.t - c.d), 'tab adj err'), cbar
			im(236, abs(c.t - c.p), 'tab-pre adj'), cbar
		end
	end
return
end

%
% 2D test
%
if 1
	if ~isvar('s.t'), disp 'precompute structure'
%		ktype = 'minmax:kb';
		ktype = 'kaiser';
		Jd = [4 5];
		Nd = [20 16];
		Ld = [2^12 2^13];
		Kd = 2 * Nd;
%		n_shift = zeros(size(Nd));
		n_shift = [3 7]; % stress it

		gam = 2*pi ./ Kd;
		k1 = linspace(-2*Kd(1), 2*Kd(1), 51)';
		k2 = linspace(-2*Kd(2), 2*Kd(2), 81)';
		[kk1 kk2] = ndgrid(k1, k2);
		om = [gam(1)*kk1(:) gam(2)*kk2(:)];

		tic
		s.p = nufft_init(om, Nd, Jd, Kd, n_shift, ktype);
		printf('pre time init %g', toc)

		tic
		s.t = nufft_init(om, Nd, Jd, Kd, n_shift, 'table', Ld, ktype);
		printf('tab time init %g', toc)
	end

	% forward direction
	if 1, disp 'test forward'
		x = [1:Nd(1)]'*triang(Nd(2))';

		tic
		Y.d = dtft2(x, om, n_shift); % exact
		printf('dtft2 time %g', toc)

		tic
		Y.p = nufft(x, s.p);
		printf('pre time nufft %g', toc)
		printf('pre max%%diff = %g', max_percent_diff(Y.d, Y.p))

		tic
		Y.t = nufft(x, s.t);
		printf('tab time nufft %g', toc)
		printf('tab max%%diff = %g', max_percent_diff(Y.d, Y.t))
		printf('tab vs pre max%%diff = %g', max_percent_diff(Y.p, Y.t))

		clf
		im(231, k1, k2, abs(Y.p - Y.d), 'pre'), cbar
		im(232, k1, k2, abs(Y.t - Y.d), 'tab'), cbar
		im(233, k1, k2, abs(Y.t - Y.p), 'tab-pre'), cbar
	end

	% 2D adjoint
	if 1, disp '2D adjoint'
		X = [1:s.t.M]' + 1i * ones(s.t.M,1);

		tic
		c.d = dtft2_adj(X, om, Nd(1), Nd(2), n_shift); % exact
		printf('dtft2 time adj %g', toc)
		tic
		c.p = nufft_adj(X, s.p);
		printf('pre time nufft adj %g', toc)
		tic
		c.t = nufft_adj(X, s.t);
		printf('tab time nufft adj %g', toc)

		printf('pre max%%diff = %g', max_percent_diff(c.d, c.p))
		printf('tab max%%diff = %g', max_percent_diff(c.d, c.t))
		printf('tab vs pre max%%diff = %g', max_percent_diff(c.p, c.t))

		im(234, abs(c.p - c.d), 'pre adj err'), cbar
		im(235, abs(c.t - c.d), 'tab adj err'), cbar
		im(236, abs(c.t - c.p), 'tab-pre adj'), cbar
	end
return
end

%
% 1D test
%
if ~isvar('s.t')
%	ktype = 'minmax:kb';
	ktype = 'kaiser';
	Jd = 4;
	Nd = 32;
	Ld = 2^14; % table over-sampling
	Kd = 2 * Nd;
	n_shift = zeros(size(Nd));
	% n_shift = Nd/2;

	gam = 2*pi ./ Kd;
	kv = 2*Kd;
	kv = linspace(-kv, kv, 1001)';
	om = gam * kv;

	tic
	s.p = nufft_init(om, Nd, Jd, Kd, n_shift, ktype);
	printf('pre time init %g', toc)

	tic
	s.t = nufft_init(om, Nd, Jd, Kd, n_shift, 'table', Ld, ktype);
	printf('tab time init %g', toc)
end

if 0
	ramp = [1:Nd]';

	Y.dr = dtft(ramp, om, n_shift);
	Y.de = dtft(eye(Nd), om, n_shift);
	printf('dtft max%%diff = %g', max_percent_diff(Y.dr,Y.de*ramp))

	Y.pr = nufft(ramp, s.p);
	Y.pe = nufft(eye(Nd), s.p);
	printf('pre max%%diff = %g', max_percent_diff(Y.pr,Y.pe*ramp))

	printf('max%%diff = %g', max_percent_diff(Y.dr,Y.pr))
	printf('max%%diff = %g', max_percent_diff(Y.de,Y.pe))

	e = Y.pr - Y.dr;
	clf, plot(kv, abs(e), 'c-')
%	clf, plot(kv, sum(abs(e),2), 'c-')
return
end

% forward direction
if 1
	x = [1:Nd]';
	% x = unitv(Nd, ii);

	Y.d = dtft(x, om, n_shift); % exact
	tic
	Y.p = nufft(x, s.p);
	printf('pre time nufft %g', toc)
	printf('pre max%%diff = %g', max_percent_diff(Y.d,Y.p))

	tic
	Y.t = nufft(x, s.t);
	printf('tab time nufft %g', toc)
	printf('tab max%%diff = %g', max_percent_diff(Y.d, Y.t))

	clf
	subplot(211)
	plot(kv, abs(Y.p - Y.d), 'c-'), title 'pre'
	subplot(212)
	plot(kv, abs(Y.t - Y.d), 'y-'), title 'tab'
prompt
end

% adjoint
if 1
	X = [1:s.t.M]' + 1i * ones(size(om));

	c.d = dtft2_adj(X, [om 0*om], Nd(1), 1, [n_shift 0]); % exact
	tic
	c.p = nufft_adj(X, s.p);
	printf('pre time nufft adj %g', toc)
	tic
	c.t = nufft_adj(X, s.t);
	printf('tab time nufft adj %g', toc)

	printf('pre nufft adj vs dtft adj max%%diff = %g', max_percent_diff(c.d,c.p))
	printf('tab nufft adj vs dtft adj max%%diff = %g', max_percent_diff(c.d,c.t))

	clf
	nn = 0:Nd(1)-1;
	subplot(211)
	plot(nn, abs(c.p - c.d) / mean(abs(c.d)), 'c-'), title 'pre'
	subplot(212)
	plot(nn, abs(c.t - c.d) / mean(abs(c.d)), 'y-'), title 'tab'
end


%% data association for every kalman filter (every object from previous frame)
function [k, ocn, szn] = asc(k, ocn, szn)
for i             = 1 : size(k, 2)                      % for every kalman filter
[k(i).s, idx]     = assoc(k(i).s, ocn);                 % apply data association
k(i).sz           = szn(:, idx);
szn(:, idx)       = [];
ocn(:, idx)       = [];                                 % eliminated checked objects from the new frame and go for
end                                                     % associating remained objects to the next kalman filter
end
%% data association: 1. no object, 2. one object, 3. multiple objects
function [s, idx] = assoc(s, ocn)
can    = ocn(1 : 2, :);                                 % candidates
pe     = [s.x(1); s.x(3)];                              % previous estimate (to do: replace with last prediction!)
gate   = 4;                                             % gating: 7
idx    = ((sum((can - repmat(pe, 1, size(can, 2)))...   % indexes of objects inside the gate
         .^2)) .^0.5) < gate;
if     sum(idx) == 0                                    % 1. no object
s.z    = pe;                                            % previous estimate/prediction!
elseif sum(idx) == 1                                    % 2. one object
s.z    = can(:, idx);                                   % associated object
elseif sum(idx) > 1                                     % 3. multiple objects (take nearest object)
idx    = ((sum((can - repmat(pe, 1, size(can, 2)))...   % index of the nearest object
         .^2)) .^0.5) == min((sum((can - repmat(pe...
         , 1, size(can, 2))) .^2)) .^0.5);
s.z    = can(:, idx);                                   % associated object
end
end


function [ n_data, x, fx ] = bessel_y1_values ( n_data )

%*****************************************************************************80
%
%% BESSEL_Y1_VALUES returns some values of the Y1 Bessel function.
%
%  Discussion:
%
%    In Mathematica, the function can be evaluated by:
%
%      BesselY[1,x]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 August 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real X, the argument of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 16;

  fx_vec = [ ...
     -0.6458951094702027E+01, ...
     -0.7812128213002887E+00, ...
     -0.1070324315409375E+00, ...
      0.3246744247918000E+00, ...
      0.3979257105571000E+00, ...
      0.1478631433912268E+00, ...
     -0.1750103443003983E+00, ...
     -0.3026672370241849E+00, ...
     -0.1580604617312475E+00, ...
      0.1043145751967159E+00, ...
      0.2490154242069539E+00, ...
      0.1637055374149429E+00, ...
     -0.5709921826089652E-01, ...
     -0.2100814084206935E+00, ...
     -0.1666448418561723E+00, ...
      0.2107362803687351E-01 ];

  x_vec = [ ...
      0.1E+00, ...
      1.0E+00, ...
      2.0E+00, ...
      3.0E+00, ...
      4.0E+00, ...
      5.0E+00, ...
      6.0E+00, ...
      7.0E+00, ...
      8.0E+00, ...
      9.0E+00, ...
     10.0E+00, ...
     11.0E+00, ...
     12.0E+00, ...
     13.0E+00, ...
     14.0E+00, ...
     15.0E+00  ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    x = 0.0;
    fx = 0.0;
  else
    x = x_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


function linpack_d_test18 ( )

%*****************************************************************************80
%
%% TEST18 tests DPOFA and DPOSL.
%
%  Discussion:
%
%    DPOFA factors a positive definite symmetric matrix,
%    and DPOSL can solve a factored linear system.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 June 2009
%
%  Author:
%
%    John Burkardt
%
  n = 20;
  lda = n;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST18\n' );
  fprintf ( 1, '  For a positive definite symmetric matrix,\n' );
  fprintf ( 1, '  DPOFA computes the LU factors.\n' );
  fprintf ( 1, '  DPOSL solves a factored linear system.\n' );
  fprintf ( 1, '  The matrix size is N = %d\n', n );
%
%  Set the matrix A.
%
  a(1:n,1:n) = 0.0;

  for i = 1 : n
    a(i,i) = 2.0;
    if ( 1 < i )
      a(i,i-1) = -1.0;
    end
    if ( i < n )
      a(i,i+1) = -1.0;
    end
  end
%
%  Set the right hand side.
%
  x = zeros ( n, 1 );

  for i = 1 : n
    x(i) = i;
  end

  b(1:n) = a(1:n,1:n) * x(1:n);
%
%  Factor the matrix.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Factor the matrix.\n' );

  [ a, info ] = dpofa ( a, lda, n );

  if ( info ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Error, DPOFA returns INFO = %d\n', info );
    return
  end
%
%  Solve the linear system.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Solve the linear system.\n' );

  b = dposl ( a, lda, n, b );
%
%  Print the result.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The first and last five entries of the solution:\n' );
  fprintf ( 1, '  (Should be 1,2,3,4,5,...,n-1,n.)\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    if ( i <= 5 | n-5 < i )
      fprintf ( 1, '  %6d  %14f\n', i, b(i) );
    end
    if ( i == 5 )
      fprintf ( 1, '  ......  ..............\n' );
    end
  end

  return
end


function th_img = mmse7(inp,inp_map,s,n)
[row col]=size(inp);
th_img=zeros(row,col);
th_img(1:(row/2^n),1:(col/2^n))=inp(1:(row/2^n),1:(col/2^n));
for i=n:-1:1
    a=inp((row/2^i)+1:(row/2^(i-1)),(col/2^i)+1:(col/2^(i-1))); %diagonal coefficients
    b=inp_map((row/2^i)+1:(row/2^(i-1)),(col/2^i)+1:(col/2^(i-1)));
    th_img((row/2^i)+1:(row/2^(i-1)),(col/2^i)+1:(col/2^(i-1)))=th(a,b,s);
    a=inp(1:(row/2^(i)),(col/2^i)+1:(col/2^(i-1)));   %horizontal coefficients
    b=inp(1:(row/2^(i)),(col/2^i)+1:(col/2^(i-1)));
    th_img(1:(row/2^(i)),(col/2^i)+1:(col/2^(i-1)))=th(a,b,s);
    a=inp((row/2^i)+1:(row/2^(i-1)),1:(col/2^(i)));     %vertical coefficients
    b=inp((row/2^i)+1:(row/2^(i-1)),1:(col/2^(i)));
    th_img((row/2^i)+1:(row/2^(i-1)),1:(col/2^(i)))=th(a,b,s);
end
end
function y=th(inp,inp1,s)
[row col]=size(inp);
lambda=1/var(inp1(:));
test1=zeros(row+6,col+6);
test2=zeros(size(test1));
test1(4:end-3,4:end-3)=inp;
[row col]=size(test1);
for i=4:row-3
    for j=4:col-3
        b=test1(i-3:i+3,j-3:j+3);
%         sigsq=max(0,((1/numel(b))*sum(sum(b.^2))-s));
%         lambda=1/sqrt(sigsq);
        A=numel(b)*(-1+sqrt(1+(8*lambda/numel(b)^2)*sum(sum(b.^2))));
        sig_cap_sq=max(0,(A/(4*lambda)-s));
        %test2(i,j)=(sqrt(2)*s)./sqrt(sigsq);
        test2(i,j)=(sig_cap_sq.*test1(i,j))./(sig_cap_sq+s);
    end
end
y=test2(4:row-3,4:col-3);
% % c=(abs(inp)-test2(4:row-3,4:col-3));
% % c(c<0)=0;
% % y=sign(inp).*c;
% [row col]=size(inp);
% y(1:row/2^n,1:col/2^n)=inp(1:row/2^n,1:col/2^n);
end