function [x_best, psi_best, out] = CSDF(mapp, x0, options)
% CSDF is a derivative-free algorithm for solving systems of nonlinear
% equations :math:`f(x) = 0`, x in :math:`R^m`
% using the nonlinear unconstrained minimization :math:`\textrm{min}\ \psi(x) = 1/2 ||f(x)||^2` s.t. `x` in :math:`R^m`.
%
% USAGE:
%
%    [x_best, psi_best, out] = CSDF(mapp, x0, options)
%
% INPUTS:
%    mapp:        function handle provides `f(x)` and gradient `f(x)`
%    x0:          initial point
%    options:     structure including the parameteres of scheme
%
%                   * .MaxNumIter - maximum number of iterations
%                   * .MaxNumMapEval - maximum number of function evaluations
%                   * .TimeLimit - maximum running time
%                   * .epsilon - accuracy parameter
%                   * .x_opt - optimizer
%                   * .psi_opt - optimum
%                   * .sigma - strong duplomonotone parameter
%                   * .l - Lipschitz continuity constant of `f`
%                   * .tauBar - a constant for determining the step-size
%                   * .flag_x_error - 1: saves :math:`x_{error}`, 0: do not saves :math:`x_{error}` (default)
%                   * .flag_psi_error - 1:saves :math:`\psi_{error}`, 0: do not saves :math:`\psi_{error}` (default)
%                   * .flag_time - 1: saves :math:`\psi_{error}`, 0: do not saves :math:`\psi_{error}` (default)
%                   * .Stopping_Crit - stopping criterion:
%
%                     1. stop if :math:`||nfxk|| \leq \epsilon`
%                     2. stop if `MaxNumIter` is reached
%                     3. stop if `MaxNumMapEval` is reached
%                     4. stop if `TimeLimit` is reached
%                     5. stop if (default) :math:`||hxk|| \leq \epsilon` or `MaxNumIter` is reached
%
% OUTPUTS:
%    x_best:      the best approximation of the optimizer
%    psi_best:    the best approximation of the optimum
%    out:         structure including more output information
%
%                   * .T - running time
%                   * .Niter - total number of iterations
%                   * .Nmap - total number of mapping evaluations
%                   * .merit_func - array including all merit function values
%                   * .x_error - relative error :math:`norm(x_k(:)-x_{opt}(:))/norm(x_{opt})`
%                   * .psi_error - relative error :math:`(\psi_k-\psi_{opt})/(\psi_0-\psi_{opt}))`
%                   * .Status - reason of termination
%
% .. REFERENCE:
% .. Algorithm 2 of [1]: F.J. Aragon Artacho, R.M.T. Fleming, Globally convergent algorithms for finding zeros of duplomonotone mappings, Optimization Letter, 9, 569-584 (2015)
% .. Author: - Masoud Ahookhosh, System Biochemistry Group, Luxembourg Center for System Biomedicine, University of Luxembourg, Luxembourg
%            - Update July 2017 - M. Ahookhosh

format longG ;

% ================ Error messages for input and output =================
if nargin > 3
    error('The number of input arguments is more than what is needed');
elseif nargin < 3
    error('The number of input arguments is not enough');
end;

if isempty(mapp)
    error('the function handle mapp has to be defined');
elseif ~isa(mapp,'function_handle')
    error('mapp should be a function handle');
end

if isempty(x0)
    error('The starting point x0 has to be defined');
elseif ~isa(x0,'numeric')
    error('x0 should be a numeric vector');
end

% =================== initializing the parameters ======================
% ===== user has requested viewing the default values of "options" =====
[MaxNumIter,MaxNumMapEval,TimeLimit,epsilon,alpha, ...
         beta,sigma,l,tauBar,lambda_min,lambda_max,flag_x_error, ...
         flag_psi_error,flag_time,Stopping_Crit] = InitialDuplo(options);

if isfield(options,'x_opt')
    x_opt=options.x_opt;
elseif flag_x_error==1
    error('x_error requires to x_opt be specified');
end

if flag_x_error == 1
    Nxopt      = sqrt(sum(x_opt(:).^2));
    x_error(1) = sqrt(sum((x0(:)-x_opt(:)).^2))/Nxopt;
end

if flag_psi_error == 1
    psi_error(1) = 1;
end

if flag_time == 1
    Time(1) = 0;
end

xk         = x0;
Xk         = xk;
Niter      = 1;
fx0        = mapp(x0);
Nmap       = 1;
nfx0       = norm(fx0);
fxk        = fx0;
nfxk       = nfx0;
merit_func = 0.5*nfxk^2;
lambda     = min(sigma/l^2,tauBar);
StopFlag   = 0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%% Main body of CSDF.m %%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
T0 = tic;

% ======================= start of the main loop =======================
while ~StopFlag

    xk    = xk-lambda*fxk;
    Niter = Niter+1
    fxk   = mapp(xk);
    Nmap  = Nmap+1;
    nfxk2 = norm(fxk)^2;

    % ================= Gathering output information ===================
    psik              = 0.5*nfxk2;
    merit_func(Niter) = psik;
    if flag_time == 1
        Time(Niter+1) = toc(T0);
    end

    if flag_x_error == 1
        Nx_opt = norm(x_opt);
        x_error(Niter+1) = sqrt(sum((xk(:)-x_opt(:)).^2))/Nx_opt;
    end

    if flag_psi_error == 1
        psi_error(Niter+1) = (psik-psi_opt)/(psi0-psi_opt);
    end

    % ================== checking stopping criteria ====================
    T = toc(T0);

    [StopFlag,Status] = StopCritDuplo(nfxk,Niter,Nmap,T, ...
              MaxNumIter,MaxNumMapEval,TimeLimit,epsilon,Stopping_Crit);

end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Outputs %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Status
x_best         = xk;
psi_best       = psik;
out.Xk         = Xk;
out.T          = T;
out.nhx        = nfxk;
out.merit_func = merit_func';
out.Niter      = Niter;
out.Nmap       = Nmap;
out.Status     = Status;

if flag_x_error == 1
    out.x_error = x_error;
end
if flag_psi_error == 1
    out.psi_error = psi_error;
end
if flag_time == 1
    out.Time = Time;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%% End of CSDF.m %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


function [xi, em, gx, gy] = surface_interpolation(x, PARAM, INTERP, res, KLIM, nlev)
% surface_interpolation  Interpolate a surface from a scattered set of points
%
% [XI, EM, GX, GY] = surface_interpolation(X)
%
%   X is a 3-row matrix. Each column has the coordinates of a point that
%   belongs to the surface we want to interpolate.
%
%   XI is a 3-row matrix with the coordinates of the interpolated points.
%
%   EM is a 2-row matrix with the coordinates of the X points projected
%   onto the interpolation domain. Note that the interpolation domain may
%   change between methods, so don't expect results to be aligned if you do
%   e.g. plot3(gx(:), gy(:), y(:, 3), 'o').
%
%   GX, GY are the grid for the box that contains EM.
%
% ... = surface_interpolation(X, PARAM, INTERP, RES, KLIM, NLEV)
%
%   PARAM is a string with the method used to parametrize the surface and
%   X:
%
%     'xy' (default): No change, the X coordinates are kept the same.
%
%     'pca': X points are rotated according to their eigenvectors to make
%     the dominant plane of the points X as horizontal as possible before
%     interpolating.
%
%     'isomap': Use the Isomap method by [1] to "unfold" the curved surface
%     defined by X before interpolating. (This option requires function
%     IsomapII).
%
%   INTERP is a string with the interpolation method:
%
%      'tps' (default): Thin-plate spline. Global support.
%
%      'tsi': Matlab's TriScatteredInterp() function. Local support,
%      limited to the convex hull of the scattered points 2D projection on
%      the interpolation domain.
%
%      'gridfit': John D'Errico's gridfit() function [3] (note:
%      approximation, rather than interpolation). Local support with
%      extrapolation outside the convex hull.
%
%      'mba': Multilevel B-Spline Approximation Library by SINTEF ICT [4].
%      Local support, limited to a rectangle that tighly contains the
%      scattered points 2D projection on the interpolation domain.
%
%      'mbae': Like the 'mba' method, but first a thin-plate spline is used
%      to extrapolate values on the interpolation domain boundary. Then MBA
%      is used for the local support interpolation of X and the boundary
%      values set by the thin-plate spline.
%
%   RES is a 2-vector with the grid spacing in the x- and y-directions. By
%   default, RES=[1 1].
%
%   KLIM is a scalar factor for the extension of the interpolation domain.
%   By default, KLIM=1 and the interpolation domain is a rectangle that
%   tightly contains X. Sections of the interpolated surface that protude
%   from the image volume are removed.
%
%   NLEV is the number of levels in the hierarchical construction of 'mba'
%   and 'mbae'. For other INTERP options, it will be ignored.  By default,
%   NLEV = 7.
%
%
% [1] J.B. Tenenbaum, V. de Silva and J.C. Langford, "A Global Geometric
% Framework for Nonlinear Dimensionality Reduction", Science 290(5500):
% 2319-2323, 2000.
%
% [2] Isomap Homepage, http://isomap.stanford.edu/
%
% [3] Surface Fitting using gridfit by John D'Errico 11 Nov 2005 (Updated
% 29 Jul 2010). Code covered by the BSD License
% http://www.mathworks.com/matlabcentral/fileexchange/8998-surface-fitting-using-gridfit
%
% [4] MBA - Multilevel B-Spline Approximation Library
% http://www.sintef.no/Projectweb/Geometry-Toolkits/MBA/

% Author: Ramon Casero <rcasero@gmail.com>
% Copyright © 2010-2011 University of Oxford
% Version: 0.2.0
% $Rev: 782 $
% $Date: 2012-06-02 22:55:03 +0100 (Sat, 02 Jun 2012) $
%
% University of Oxford means the Chancellor, Masters and Scholars of
% the University of Oxford, having an administrative office at
% Wellington Square, Oxford OX1 2JD, UK.
%
% This file is part of Gerardus.
%
% 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. The offer of this
% program under the terms of the License is subject to the License
% being interpreted in accordance with English Law and subject to any
% action against the University of Oxford being under the jurisdiction
% of the English Courts.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

% check arguments
error(nargchk(2, 6, nargin, 'struct'));
error(nargoutchk(0, 4, nargout, 'struct'));

% defaults
if (nargin < 2 || isempty(PARAM))
    PARAM = 'xy';
end
if (nargin < 3 || isempty(INTERP))
    INTERP = 'tps';
end
if (nargin < 4 || isempty(res))
    res = [1 1];
end
if (nargin < 5 || isempty(KLIM))
    KLIM = 1;
end
if (nargin > 5 && ~isempty(nlev) ...
        && ~(strcmp(INTERP, 'mba') || strcmp(INTERP, 'mbae')))
    warning('NLEV input argument ignored for this INTERP option')
end
if (nargin < 6 || isempty(nlev))
    nlev = 7;
end

%% map the 3D points (x,y,z) to a 2D domain (u,v)

% this is analogous to computing the knot vector for a curve interpolation.
% The idea is that (x,y)->z is not necessarily a function, due to the valve
% folding over. However, we hope that (u,v)->(x,y,z) is a function
switch PARAM

    case 'xy'

        % (u,v) is simply (x,y)
        em = x(1:2, :);

    case 'pca'

        % rotate valve points to make the valve surface as horizontal as
        % possible
        m = mean(x, 2);
        em = x - m(:, ones(1, size(x, 2)));
        eigv = pts_pca(em);
        if any(isnan(eigv(:)))
            error('Cannot interpolate surface');
        end
        em = eigv' * em;
        em = em(1:2, :);

    case 'isomap'

        % compute distance matrix
        d = dmatrix(x, x, 'euclidean');

        % compute 2-d projection of the 3-d data
        options.dims = 2;
        options.display = 0;
        options.overlay = 0;
        options.verbose = 0;
        em = IsomapII(d, 'k', round(size(x, 2)/3), options);
        em = em.coords{1};

    otherwise
        error('Parametrization method not implemented')
end

%% compute interpolation domain

% find box that contains embedded coordinates
emmin = min(em, [], 2);
emmax = max(em, [], 2);

% box size and centroid
delta = emmax - emmin;
boxm = mean([emmax emmin], 2);

% extend the box
emmin = boxm - delta/2*KLIM;
emmax = boxm + delta/2*KLIM;

% generate grid for the embedding box
[gy, gx] = ndgrid(emmin(2):res(1):emmax(2), emmin(1):res(2):emmax(1));

%% compute interpolating surface

% source and target points that will define the warp
%s = em; % don't duplicate data in memory
%t = x; % don't duplicate data in memory

% interpolate
switch INTERP
    case 'tps' % thin-plate spline
        xi = pts_tps_map(em', x', [gx(:) gy(:)]);
    case 'tsi' % Matlab's TriScatteredInterp
        fx = TriScatteredInterp(em(1, :)', em(2, :)', x(1, :)', 'natural');
        fy = TriScatteredInterp(em(1, :)', em(2, :)', x(2, :)', 'natural');
        fz = TriScatteredInterp(em(1, :)', em(2, :)', x(3, :)', 'natural');
        fx = fx(gx, gy);
        fy = fy(gx, gy);
        fz = fz(gx, gy);
        xi = [fx(:) fy(:) fz(:)];
    case 'gridfit'
        fx = gridfit(em(1, :)', em(2, :)', x(1, :)', ...
            emmin(1):res(2):emmax(1), emmin(2):res(1):emmax(2), ...
            'tilesize', 150);
        fy = gridfit(em(1, :)', em(2, :)', x(2, :)', ...
            emmin(1):res(2):emmax(1), emmin(2):res(1):emmax(2), ...
            'tilesize', 150);
        fz = gridfit(em(1, :)', em(2, :)', x(3, :)', ...
            emmin(1):res(2):emmax(1), emmin(2):res(1):emmax(2), ...
            'tilesize', 150);
        xi = [fx(:) fy(:) fz(:)];
    case 'mba' % Multilevel B-Spline Approximation Library
        xi = [...
            mba_surface_interpolation(em(1, :)', em(2, :)', x(1, :)', gx(:), gy(:), nlev) ...
            mba_surface_interpolation(em(1, :)', em(2, :)', x(2, :)', gx(:), gy(:), nlev) ...
            mba_surface_interpolation(em(1, :)', em(2, :)', x(3, :)', gx(:), gy(:), nlev)];
    case 'mbae' % MBA extrapolated using a TPS
        % use TPS to interpolate the points, but we are only interested in
        % the edges and corners of the interpolation, where the TPS is
        % extrapolating linearly; also, we decimate the points in the edges
        em2 = [gx(1, 1:end)' gy(1, 1:end)' ; ... % top edge
            gx(2:end, end) gy(2:end, end) ; ... % right edge
            gx(end, 1:end-1)' gy(end, 1:end-1)' ; ... % bottom edge
            gx(2:end-1, 1) gy(2:end-1, 1) ; ... % left edge
            ]';
        xi = pts_tps_map(em', x', em2')';
        x = [x xi(:, 1:10:end)];
        % local support interpolation with boundary conditions provided by
        % the TPS
        em = [em em2(:, 1:10:end)];
        xi = [...
            mba_surface_interpolation(em(1, :)', em(2, :)', x(1, :)', gx(:), gy(:), nlev) ...
            mba_surface_interpolation(em(1, :)', em(2, :)', x(2, :)', gx(:), gy(:), nlev) ...
            mba_surface_interpolation(em(1, :)', em(2, :)', x(3, :)', gx(:), gy(:), nlev)];
    otherwise
        error('Interpolation method not implemented')
end


function sol = SolveBP(A, y, N, maxIters, lambda, OptTol)
% SolveBP: Solves a Basis Pursuit problem
% Usage
%	sol = SolveBP(A, y, N, maxIters, lambda, OptTol)
% Input
%	A           Either an explicit nxN matrix, with rank(A) = min(N,n)
%               by assumption, or a string containing the name of a
%               function implementing an implicit matrix (see below for
%               details on the format of the function).
%	y           vector of length n.
%   N           length of solution vector.
%	maxIters    maximum number of PDCO iterations to perform, default 20.
%   lambda      If 0 or omitted, Basis Pursuit is applied to the data,
%               otherwise, Basis Pursuit Denoising is applied with
%               parameter lambda (default 0).
%	OptTol      Error tolerance, default 1e-3
% Outputs
%	 sol        solution of BP
% Description
%   SolveBP solves the basis pursuit problem
%      min ||x||_1 s.t. A*x = y
%   by reducing it to a linear program, and calling PDCO, a primal-dual
%   log-barrier algorithm. Alternatively, if lambda ~= 0, it solves the
%   Basis Pursuit Denoising (BPDN) problem
%      min lambda*||x||_1 + 1/2||y - A*x||_2^2
%   by transforming it to an SOCP, and calling PDCO.
%   The matrix A can be either an explicit matrix, or an implicit operator
%   implemented as a function. If using the implicit form, the user should
%   provide the name of a function of the following format:
%     y = OperatorName(mode, m, n, x, I, dim)
%   This function gets as input a vector x and an index set I, and returns
%   y = A(:,I)*x if mode = 1, or y = A(:,I)'*x if mode = 2.
%   A is the m by dim implicit matrix implemented by the function. I is a
%   subset of the columns of A, i.e. a subset of 1:dim of length n. x is a
%   vector of length n is mode = 1, or a vector of length m is mode = 2.
% See Also
%   SolveLasso, SolveOMP, SolveITSP
%

if nargin < 6,
	OptTol = 1e-3;
end
if nargin < 5,
	lambda = 0;
end
if nargin < 4,
    maxIters = 20;
end

n = length(y);

n_pdco = 2*N;    % Input size
m_pdco = n;      % Output size

% upper and lower bounds
bl = zeros(n_pdco,1);
bu = Inf .* ones(n_pdco,1);

% generate the vector c
if (lambda ~= 0)
    c = lambda .* ones(n_pdco,1);
else
    c = ones(n_pdco,1);
end

% Generate an initial guess
x0 = ones(n_pdco,1)/n_pdco;       % Initial x
y0 = zeros(m_pdco,1);             % Initial y
z0 = ones(n_pdco,1)/n_pdco;       % Initial z

d1 = 1e-4;                 % Regularization parameters
if (lambda ~= 0) % BPDN
    d2 = 1;
else
    d2 = 1e-4;
end

xsize = 1;                 % Estimate of norm(x,inf) at solution
zsize = 1;                 % Estimate of norm(z,inf) at solution

options = pdcoSet;         % Option set for the function pdco
options = pdcoSet( options, ...
                     'MaxIter    ', maxIters  , ...
                     'FeaTol     ', OptTol    , ...
                     'OptTol     ', OptTol    , ...
                     'StepTol    ', 0.99      , ...
                     'StepSame   ', 0         , ...
                     'x0min      ', 0.1       , ...
                     'z0min      ', 1.0       , ...
                     'mu0        ', 0.01      , ...
                     'method     ', 1         , ...
                     'LSQRMaxIter', 20        , ...
                     'LSQRatol1  ', 1e-3      , ...
                     'LSQRatol2  ', 1e-15     , ...
                     'wait       ', 0    );

if (ischar(A) || isa(A, 'function_handle'))
    [xx,yy,zz,inform,PDitns,CGitns,time] = ...
        pdco(c, @pdcoMat, y, bl, bu, d1, d2, options, x0, y0, z0, xsize, zsize);
else
    Phi = [A -A];
    [xx,yy,zz,inform,PDitns,CGitns,time] = ...
        pdco(c, Phi, y, bl, bu, d1, d2, options, x0, y0, z0, xsize, zsize);
end

% Extract the solution from the output vector x
sol = xx(1:N) - xx((N+1):(2*N));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    function y = pdcoMat(mode,m,n,x)
        if (mode == 1) % Direct operator
            % Decompose input
            n2 = n/2;
            u = x(1:n2);
            v = x(n2+1:n);

            % Apply matrix A
            Au = feval(A,1,m,n2,u,1:n2,n2);
            Av = feval(A,1,m,n2,v,1:n2,n2);

            y = Au-Av;
        else % Adjoint operator
            n2 = n/2;
            Atx = feval(A,2,m,n2,x,1:n2,n2);
            y = [Atx; -Atx];
        end
    end

end

%
% Copyright (c) 2006. Yaakov Tsaig
%

%
% Part of SparseLab Version:100
% Created Tuesday March 28, 2006
% This is Copyrighted Material
% For Copying permissions see COPYING.m
% Comments? e-mail sparselab@stanford.edu
%


h=[4,-4;-4,8];
f=[-6;-3];
a=[1,1;4,1];
b=[3;9];
[x,value]=quadprog(h,f,a,b,[],[],zeros(2,1))


function ttf = friedman_test(n_problem, r, n)
%FRIEDMAN regression

fun = @(i, j, x) x^(j-1);

if n_problem == 1
    %Friedman 1
    d = 10;
    N = 20000;
    x = rand(d,N);
    y = 10*sin(pi*x(1,:).*x(2,:)) + 20*(x(3,:) - 0.5*ones(1,N)).^2 + 10*x(4,:) + 5*x(5,:);

    tt_rank = [1 ; r*ones(d-1,1) ; 1];
    n_basis = n*ones(d,1);
    basis_ps = cumsum([1 ; n_basis*N]);
    basis_cr = zeros(basis_ps(d+1) - basis_ps(1), 1);
    for dim = 1: d
        t = zeros(n, N);
        for i = 1: n
            t(i,:) = x(dim,:).^(i-1);
        end
        basis_cr(basis_ps(dim):basis_ps(dim+1)-1) = reshape(t, [n*N 1]);
    end
elseif n_problem == 2
    %Friedman 2
    d = 4;
    N = 20000;
    x = rand(d,N);
    x1 = x(1,:)*100;
    x2 = x(2,:)*520*pi + 40*pi*ones(1,N);
    x3 = x(3,:);
    x4 = x(4,:)*10 + ones(1,N);
    y = sqrt(x1.^2 + (x2.*x3 - 1./(x2.*x4)).^2);

    tt_rank = [1 ; r*ones(d-1,1) ; 1];
    n_basis = n*ones(d,1);
    basis_ps = cumsum([1 ; n_basis*N]);
    basis_cr = zeros(basis_ps(d+1) - basis_ps(1), 1);
    for dim = 1: d
        t = zeros(n, N);
        for i = 1: n
            t(i,:) = x(dim,:).^(i-1);
        end
        basis_cr(basis_ps(dim):basis_ps(dim+1)-1) = reshape(t, [n*N 1]);
    end
else
    %Friedman 3
    d = 4;
    N = 20000;
    x = rand(d,N);
    x1 = x(1,:)*100;
    x2 = x(2,:)*520*pi + 40*pi*ones(1,N);
    x3 = x(3,:);
    x4 = x(4,:)*10 + ones(1,N);
    y = atan((x2.*x3 - 1./(x2.*x4))./x1);

    tt_rank = [1 ; r*ones(d-1,1) ; 1];
    n_basis = n*ones(d,1);
    basis_ps = cumsum([1 ; n_basis*N]);
    basis_cr = zeros(basis_ps(d+1) - basis_ps(1), 1);
    for dim = 1: d
        t = zeros(n, N);
        for i = 1: n
            t(i,:) = x(dim,:).^(i-1);
        end
        basis_cr(basis_ps(dim):basis_ps(dim+1)-1) = reshape(t, [n*N 1]);
    end
end

coeff = reg_als(basis_cr, y, tt_rank, n_basis);
ttf = tt_function(fun, coeff);

end


%converts frequency to mel
%>
%> @param fMel: frequency
%> @param cModel: 'Fant','Shaughnessy', or 'Umesh'
%>
%> @retval Hertz value
% ======================================================================
function [fInHz] = ToolMel2Freq(fMel, cModel)

    if (nargin < 2)
        cModel  = 'Fant';
    end

    % set function handle
    hPitchFunc  = str2func (['aca' cModel '_I']);

    fInHz       = hPitchFunc(fMel);
end

function [f] = acaFant_I(m)
    %mel         = 1000 * log2(1 + f/1000);
    f   = 1000 * (2.^(m/1000)-1);
end

function [f] = acaShaughnessy_I(m)
    %mel         = 2595 * log10(1 + f/700);
    f   = 700 * (10.^(m/2595)-1);
end

function [f] = acaUmesh_I(m)
    %mel         = f./(2.4e-4*f + 0.741);
    f   = m*.741 ./ (1 - m * 2.4e-4);
end


function [modeshape,k_term] = TangentialMode(n1,n2,l1,l2,rA,rB,r0A,r0B,k)
%-----------------------------------------------------
% Compute the Tangential Mode in a room.
% Syntax: [modeshape,k_term] =
%          TangentialMode(n1,n2,l1,l2,rA,rB,r0A,r0B,k)
%-----------------------------------------------------

parameter; % Define parameters
tau_m = (3*V)/(5*c*S*bta); % time constant of mth mode
An = sqrt(4); %normalized constant

% Initialisation of variables
modeshape = [];
k_m       = [];
k_term    = [];

% Calculate tangential modes shapes and wavenumber of modes
for n = 1:length(n2)
    modeshape(n,:) = An.*cos(n1.*pi*rA/l1).*cos(n*pi*rB/l2).*...
                     An.*cos(n1.*pi*r0A/l1).*cos(n*pi*r0B/l2);
    k_m(n,:) = sqrt((n1.*pi/l1).^2 + (n*pi/l2).^2);
end;

% Denominator term for Green function
k_term = k^2 - k_m.^2 - i*k/(tau_m*c);


function [nQb, pos, vel, q] = ins(w_b, f_b, nQb, pos, vel, gravity, dt)
%% 捷联更新
rotate_vector = w_b*dt;
rotate_vector_norm = norm(rotate_vector);
if(rotate_vector_norm <1e-10) % fix nan issue
    q = [1 0 0 0];
else
    q = [cos(rotate_vector_norm/2); rotate_vector/rotate_vector_norm*sin(rotate_vector_norm/2)]';
end

% 姿态更新
nQb = ch_qmul(nQb, q); %四元数更新（秦永元《惯性导航（第二版）》P260公式9.3.3）
nQb = ch_qnormlz(nQb); %单位化四元数

% 速度更新
f_n = ch_qmulv(nQb, f_b);
dv = (f_n + [0; 0; -gravity]); %比力方程
vel = vel + dv*dt;

% 位置更新
pos = pos + vel*dt;


function [K] = R2K(R)
% Convert temperature from Rankine to Kelvin.
K = R*5/9;


function [z_Tji_Ground_m,r_I_IRji_m,r_I_ITji_m] = Rotation_Position_ToTireFixed(z_I_Ground_m,A_K_I,A_F_I,r_I_IAji_m,h_ji_m,re_ji_m)
%% Rotation ground displacement from inertial fixed coordinate system into tire fixed coordinate systems
% Input parameters:
% A_K_I          [---]    Rotation Matrix from Inertial Frame to VehicleFixed Frame
% A_F_I          [---]    Rotation Matrix from Inertial Frame to RearAxleFixed Frame
% h_ji_m         [m]      Suspension Height [SuspHFL SuspHFR SuspHRL SuspHRR]
% re_ji_m        [m]      Effective Wheel Radii [ReFL ReFR ReRL ReRR]
% r_I_IAji       [m]      Absolut Position of Axles in Inertial Reference Frame [r_I_IAFL r_I_IAFR r_I_IARL r_I_IARR] [3x4]
% z_Ground_I_m   [---]    z-Coordinate of Road Displacement in Inertial Reference Frame [1x4]
% Output parameters:
% z_Ground_Tji_m [---]    Transformed Displacement Vector [r_TFL_ITFL r_TFR_ITFR r_TRL_ITRL r_TRR_ITRR] [1x4]

%% Wheel center points in inertial reference frame

% Quarter Vehicle Model along VehicleFixed z-Axis: Rotation from VehicleFixed Axis System into Inertial Frame
% r_I_IRji_m = [(r_I_IAji_m(:,1) + transpose(A_K_I)*[0;0;1]*(h_ji_m(1))) (r_I_IAji_m(:,2) + transpose(A_K_I)*[0;0;1]*(h_ji_m(2))) (r_I_IAji_m(:,3) + transpose(A_K_I)*[0;0;1]*(h_ji_m(3))) (r_I_IAji_m(:,4) + transpose(A_K_I)*[0;0;1]*(h_ji_m(4)))];

% Quarter Vehicle Model along RearAxleFixed z-Axis: Rotation from RearAxleFixed Axis System into Inertial Frame
r_I_IRji_m = [(r_I_IAji_m(:,1) + transpose(A_F_I)*[0;0;1]*(h_ji_m(1))) (r_I_IAji_m(:,2) + transpose(A_F_I)*[0;0;1]*(h_ji_m(2))) (r_I_IAji_m(:,3) + transpose(A_F_I)*[0;0;1]*(h_ji_m(3))) (r_I_IAji_m(:,4) + transpose(A_F_I)*[0;0;1]*(h_ji_m(4)))];

%% Contact points of road-tire-interface in inertial reference frame

% Quarter Vehicle Model along VehicleFixed z-Axis: Rotation from VehicleFixed Axis System into Inertial Frame
% r_I_ITji_m = [(r_I_IAji_m(:,1) + transpose(A_K_I)*[0;0;1]*(re_ji_m(1)+h_ji_m(1))) (r_I_IAji_m(:,2) + transpose(A_K_I)*[0;0;1]*(re_ji_m(2)+h_ji_m(2))) (r_I_IAji_m(:,3) + transpose(A_K_I)*[0;0;1]*(re_ji_m(3)+h_ji_m(3))) (r_I_IAji_m(:,4) + transpose(A_K_I)*[0;0;1]*(re_ji_m(4)+h_ji_m(4)))];

% Quarter Vehicle Model along RearAxleFixed z-Axis: Rotation from RearAxleFixed Axis System into Inertial Frame
r_I_ITji_m = [(r_I_IAji_m(:,1) + transpose(A_F_I)*[0;0;1]*(re_ji_m(1)+h_ji_m(1))) (r_I_IAji_m(:,2) + transpose(A_F_I)*[0;0;1]*(re_ji_m(2)+h_ji_m(2))) (r_I_IAji_m(:,3) + transpose(A_F_I)*[0;0;1]*(re_ji_m(3)+h_ji_m(3))) (r_I_IAji_m(:,4) + transpose(A_F_I)*[0;0;1]*(re_ji_m(4)+h_ji_m(4)))];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %% Elaborate Computation of Vertical Displacement
% % The computation of the position vector of the road-tire interface
% % requires information about the rx_I_ITji_m and ry_I_ITji_m coordinates of the
% % road-tire-interface relative to the inertial frame and the vertical
% % displacement z_Ground_I_m.
%
% %% Position Vector of Road-Tire-Interface around Orientation of the Quarter Vehicle Model
%
% % Extracting (x,y)-Coordinates of r_I_ITji_m
% % rx_I_ITji_m = r_I_ITji_m(1,:);
% % ry_I_ITji_m = r_I_ITji_m(2,:);
% %
% % Quarter Vehicle Model along VehicleFixed z-Axis: Rotation from Inertial Frame into VehicleFixed Axis System
% r_Ground_m = A_K_I*vertcat(rx_I_ITji_m,ry_I_ITji_m,+z_Ground_I_m);
%
% % Quarter Vehicle Model along RearAxleFixed z-Axis: Rotation from Inertial Frame into RearAxleFixed Axis System
% r_Ground_m = A_F_I*vertcat(rx_I_ITji_m,ry_I_ITji_m,+z_Ground_I_m);
%
% %% Orientation Change from z-down-Orientation to z-up-Orientation
% z_Ground_Tji_m = -r_Ground_m(3,:);
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Robust Computation of Vertical Displacement
% A more robust computation of the Vertical Displacement is possible with
% the simplified z-displacement vector [0;0;1]*z_Ground_I_m under
% neglection of the rx_I_ITji_m and ry_I_ITji_m coordinates of the
% road-tire-interface relative to the inertial frame.

%% Vertical z-Displacement of Road-Tire-Interface around Orientation of the Quarter Vehicle Model

% Quarter Vehicle Model along VehicleFixed z-Axis: Rotation from Inertial Frame into VehicleFixed Axis System
% r_Ground_m = A_K_I*[0;0;1]*z_Ground_I_m;

% Quarter Vehicle Model along RearAxleFixed z-Axis: Rotation from Inertial Frame into RearAxleFixed Axis System
r_Ground_m = A_F_I*[0;0;1]*z_I_Ground_m;

%% Orientation Change from z-down-Orientation to z-up-Orientation
z_Tji_Ground_m = -r_Ground_m(3,:);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end


function drawCamera()

close all

addpath ../icosahedron2sphere

points = icosahedron2sphere(1);

points = points(points(:,3)>=0,:);

%{
plot3(points(:,1),points(:,2),points(:,3),'.')
title(sprintf('Level %d with %d points',1,size(points,1)))
axis equal
axis tight
xlabel('x');
ylabel('y');
zlabel('z');
%}

aspect_ratio = 4/3;
focal_length = 0.12;
h_fov = 54.4/180*pi;

w = tan(h_fov/2)*focal_length;
h = w/aspect_ratio;

camera=[...
0 -w           +w            +w            -w
0 -h           -h            +h            +h
0 focal_length focal_length  focal_length  focal_length];

%plotCamera(camera);

for i=1:size(points,1)
    center_ray = -points(i,:);

    projLen = sqrt(center_ray(1)^2+center_ray(2)^2);

    height = -projLen^2 / center_ray(3);

    upVector = [center_ray(1) center_ray(2) height];

    upVector = upVector /norm(upVector);

    %sinTheta = center_ray(3);

    leftVector = cross(center_ray,upVector);
    leftVector = leftVector/norm(leftVector);

    R = [leftVector' upVector' center_ray'];

    currentCamera = R * camera + repmat(points(i,:)',1,5);

    plotCamera(currentCamera);
end

axis equal
axis([-1 1 -1 1 -1 1]);
xlabel('x');
ylabel('y');
zlabel('z');

function plotCamera(camera)

%mid_ray = 1;
%side_rays = 1.2;

% frame
plot3(camera(1,[2 3 4 5 2]),camera(2,[2 3 4 5 2]),camera(3,[2 3 4 5 2]),'-k'); hold on;

% side rays
for i=2:5
    plot3(camera(1,[1 i]),camera(2,[1 i]),camera(3,[1 i]),'-k'); hold on;
end


%*****************************************************************************************************
%NAME: esemilogy.m
%AUTHOR: Andri M. Gretarsson
%DATE: 01/21/98
%
%SYNTAX:	esemilogy(X,Y, 'colour')
%
%Note that 'colour' is NOT an optional argument.
%
%This function acts just like the built-in function 'semilogy' but plots error-bars. The error-bars
%are plotted in the colour given by 'colour'.  'X' and 'Y' are Nx2 matrixes, the first column
%representing the values of the coordinate (x or y), the second column representing the uncertainty
%('error') of those values.  'colour' is a one-letter string which must be one of the letters allowed
%in the built-in matlab function 'plot', to specify the plot colour.  Note that the function exits
%with "hold" set to "off".
%
%This function does not print points in addition to the error bars.  Where the error bars cross, is
%the coordinate point.  This means that if both error bars are exceedingly small complared to the
%coordinate values, the mark will be correspondingly small.  In such situations, it may be better to
%use 'semilogy' directly and specify that the error is smaller than the size of the mark.
%
%EXAMPLE:
%
%X=[1.0	0.2
%   2.0	0.2]
%Y=[1.0 	0.25
%   2.0	0.25]
%esemilogy(X,Y,'g')
%
%plots a green cross of width 0.2 and height 0.25 at coordinate (1.0,1.0), and a cross of width 0.2
%and height 0.25 at coordinate( (2.0,2.0), on a linear x, log y scale
%
%LAST MODIFIED:  01/21/98
%*****************************************************************************************************

function esemilogx=semilogxplot(x,y,colourstring)

xvalue=x(:,1);												%For clarity
xerror=x(:,2);
yvalue=y(:,1);
yerror=y(:,2);

semilogy(xvalue-xerror,yvalue-yerror,'w-',xvalue+xerror,yvalue+yerror,'w-'); hold on;
%Sets appropriate axes but otherwise invisible on a white background.

for i=1:length(xvalue)
   semilogy([xvalue(i)-xerror(i) xvalue(i)+xerror(i)], [yvalue(i) yvalue(i)], colourstring);
   semilogy([xvalue(i) xvalue(i)], [yvalue(i)-yerror(i) yvalue(i)+yerror(i)], colourstring);
end
hold off;


function [W, U, mu] = get_svds(dWU, Nrank)

[Wall, Sv, Uall] = svd(gather_try(dWU), 0);
[~, imax] = max(abs(Wall(:,1)));
Uall(:,1) = -Uall(:,1) * sign(Wall(imax,1));
Wall(:,1) = -Wall(:,1) * sign(Wall(imax,1));

%     [~, imin] = min(diff(Wall(:,1), 1));
%     [~, imin] = min(Wall(:,1));
%     dmax(k) = - (imin- 20);

%     if dmax(k)>0
%         dWU((dmax(k) + 1):nt0, :,k) = dWU(1:nt0-dmax(k),:, k);
%         Wall((dmax(k) + 1):nt0, :)  = Wall(1:nt0-dmax(k),:);
%     else
%         dWU(1:nt0+dmax(k),:, k) = dWU((1-dmax(k)):nt0,:, k);
%         Wall(1:nt0+dmax(k),:) = Wall((1-dmax(k)):nt0,:);
%     end

Wall = Wall * Sv;

Sv = diag(Sv);
mu = sum(Sv(1:Nrank).^2).^.5;
Wall = Wall/mu;

W = Wall(:,1:Nrank);
U = Uall(:,1:Nrank);


function  [F, G] = F_update(upd, DCMbn, imu)
% F_update: updates F and G matrices before the execution of Kalman filter.
%
% INPUT
%   upd, 1x8 vector with data from the INS.
%   DCMbn, DCM body-to-nav.
%   imu, IMU data structure.
%
% OUTPUT
%   F,  15x15 state transition matrix.
%   G,  15x12 control-input matrix.
%
%   Copyright (C) 2014, Rodrigo Gonzalez, all rights reserved.
%
%   This file is part of NaveGo, an open-source MATLAB toolbox for
%   simulation of integrated navigation systems.
%
%   NaveGo is free software: you can redistribute it and/or modify
%   it under the terms of the GNU Lesser General Public License (LGPL)
%   version 3 as published by the Free Software Foundation.
%
%   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 Lesser General Public License for more details.
%
%   You should have received a copy of the GNU Lesser General Public
%   License along with this program. If not, see
%   <http://www.gnu.org/licenses/>.
%
% References:
%
%   Groves, P.D. (2013), Principles of GNSS, Inertial, and
% Multisensor Integrated Navigation Systems (2nd Ed.). Artech House.
% Matrix F from Eq. 14.63.
%
% 	Farrell, J. (2008). Aided Navigation: GPS With High Rate
% Sensors. McGraw-Hill Professional, USA. Matrix G from Eq. 11.108.
%
% Version: 010
% Date:    2022/03/06
% Author:  Rodrigo Gonzalez <rodralez@frm.utn.edu.ar>
% URL:     https://github.com/rodralez/navego

Vn =  upd(1);
Ve =  upd(2);
Vd =  upd(3);
lat = upd(4);
h  =  upd(5);

fn =  upd(6:8);
% wn =  upd(9:11);

Om = 7.292115e-5;
I = eye(3);
Z = zeros(3);

[RM,RN] = radius(lat);

RO = sqrt(RN*RM) + h;

%% ATTITUDE MATRICES

a11 = 0;
a12 = -( (Om * sin(lat)) + (Ve / RO * tan(lat)) );
a13 = Vn / RO;
a21 = (Om * sin(lat)) + (Ve / RO * tan(lat));
a22 = 0 ;
a23 = (Om * cos(lat)) + (Ve / RO) ;
a31 = -Vn / RO;
a32 = -Om * cos(lat) - (Ve / RO);
a33 = 0;
F11 = [a11 a12 a13; a21 a22 a23; a31 a32 a33;];

% Groves, 14.64
% F11 = skewm(wn);

a11 = 0;
a12 = 1 / RO;
a13 = 0;
a21 = -1 / RO;
a22 = 0;
a23 = 0;
a31 = 0;
a32 = -tan(lat) / RO;
a33 = 0;
F12 = [a11 a12 a13; a21 a22 a23; a31 a32 a33;];

a11 = -Om * sin(lat);
a12 = 0;
a13 = -Ve / (RO^2);
a21 = 0 ;
a22 = 0 ;
a23 = Vn / (RO^2);
a31 =  -Om * cos(lat) - (Ve / ((RO) * (cos(lat))^2));
a32 = 0 ;
a33 = (Ve * tan(lat)) / (RO^2) ;
F13 = [a11 a12 a13; a21 a22 a23; a31 a32 a33;];

%% VELOCITY MATRICES

F21 = skewm(fn);

a11 = Vd / RO;
a12 = -2 * ((Om * sin(lat)) + ((Ve / RO) * tan(lat))) ;
a13 = Vn / RO ;
a21 = (2 * Om * sin(lat)) + ( (Ve / RO) * tan(lat) );
a22 = (1 / RO) * ((Vn * tan(lat)) + Vd) ;
a23 = 2 * Om * cos(lat) + (Ve / RO);
a31 = (-2 * Vn) / RO;
a32 = -2 * (Om * cos(lat) +  (Ve / RO)) ;
a33 = 0;
F22 = [a11 a12 a13; a21 a22 a23; a31 a32 a33;];

e = 0.0818191908425;        % WGS84 eccentricity
res = RN * sqrt( cos(lat)^2 + (1-e^2)^2 * sin(lat)^2);
g = gravity(lat,h);
g0 = g(3);

a11 = -Ve * ((2 * Om * cos(lat)) + (Ve / (RO * (cos(lat))^2)));
a12 = 0 ;
a13 = (1 / RO^2) * ( (Ve^2 * tan(lat)) - (Vn * Vd) );
a21 = 2 * Om * ( (Vn * cos(lat)) - (Vd * sin(lat)) ) + ( (Vn * Ve) / (RO * (cos(lat))^2) ) ;
a22 = 0 ;
a23 = -(Ve / RO^2) * (Vn * tan(lat) + Vd);
a31 = 2 * Om * Ve * sin(lat);
a32 = 0;
% a33 = (1 / RO^2) * (Vn^2 + Ve^2);
a33 = Ve^2 / (RN+h)^2 + Vn^2 / (RM+h)^2 - 2 * g0 / res;
F23 = [a11 a12 a13; a21 a22 a23; a31 a32 a33;];

%% POSITIONING MATRICES

F31 = zeros(3);

a11 = 1 / RO;
a12 = 0;
a13 = 0;
a21 = 0;
a22 = 1 / (RO * cos(lat));
a23 = 0;
a31 = 0;
a32 = 0;
a33 = -1;
F32 = [a11 a12 a13; a21 a22 a23; a31 a32 a33;];

a11 = 0;
a12 = 0;
a13 = -Vn / RO^2;
a21 = (Ve * tan(lat)) / (RO * cos(lat));
a22 = 0;
a23 = -Ve / (RO^2 * cos(lat));
a31 = 0;
a32 = 0;
a33 = 0;
F33 = [a11 a12 a13; a21 a22 a23; a31 a32 a33;];

Fbg = I;
Fba = I;

if (isinf(imu.gb_corr))
    Fgg = Z;
else
    Fgg = -diag( 1./ imu.gb_corr);
    %     Fbg = -diag(sqrt (2 ./ imu.gb_corr .* imu.gb_dyn.^2));
end

if (isinf(imu.ab_corr))
    Faa = Z;
else
    Faa = -diag(1 ./ imu.ab_corr);
    %     Fba = -diag(sqrt (2 ./ imu.ab_corr .* imu.ab_dyn.^2));
end

% Eq. 14.63 from Groves
F = [F11 F12 F13 DCMbn Z  ;
    F21  F22 F23 Z     DCMbn  ;
    F31  F32 F33 Z     Z      ;
    Z    Z   Z   Fgg   Z      ;
    Z    Z   Z   Z     Faa    ;
    ];

% Eq. 11.108 from Farrell
G = [DCMbn Z     Z   Z ;
    Z      DCMbn Z   Z ;
    Z      Z     Z   Z ;
    Z      Z     Fbg Z ;
    Z      Z     Z   Fba ;
    ];

end


function I2= iir(filename,f,varargin)
%IIR Increases the resolution of an image by interpolation
% B= IIR(inputfile,f) returns the image stored in file 'inputfile' with
% resolution increased by factor f in both dimensions. 'filename' must be a
% valid graphic file (jpg, gif, tiff, etc.). It can be grayscale or color.
% Parameter 'f' is the size increase ratio, so to increase by 50%
% use f= 1.5, to double size (in each dimension) use f= 2.
%
% Additional parameters:
% B= IIR(A,f,'Display','off') eliminates display of both images, the original
% and the modified. Deafult 'on'
%
% B= IIR(A,f,'Method',method) Allows to choose between five methods of
% interpolation: linear, spline, pchip, cubic or v5cubic. 'method' must
% be a string character. Default 'linear'
%
% Example:
% B= iir('myimage.jpg',2,'Method','cubic');
%
% Last modified: Sep. 2010

% Defaults
method= 'linear';
displ= 'on';
f= max(f,1);
npass= 1;

% Extract optional arguments
for j= 1:2:length(varargin)
	switch varargin{j}
		case 'method'
			method= varargin{j+1};
		case 'display'
			displ= varargin{j+1};
		otherwise
			error('Unknown parameter name');
	end
end

% Read image file
I= imread(filename);

% -------------------------------------------
% Do the math
% Sizes and new image array
nrow= ceil(size(I,1)+ size(I,1)*(f-1)/npass);
ncol= ceil(size(I,2)+ size(I,2)*(f-1)/npass);
I2= uint8(zeros(nrow,ncol,size(I,3)));

% Loop through rows & cols
for j= 1:size(I,1)
	for c= 1:size(I,3)
		I2(j,:,c)= expand(double(I(j,:,c)),ncol,method);
	end
end
for j= 1:size(I2,2)
	for c= 1:size(I,3)
		I2(:,j,c)= expand(double(I2(1:size(I,1),j,c)),nrow,method);
	end
end

% Plot final images
if strcmp(displ,'on')
	if size(I,3) == 1
		figure(1), imagesc(I); colormap(gray); axis image, axis off
		figure(2), imagesc(I2);colormap(gray); axis image, axis off
	else
		figure(1), image(I); axis image, axis off
		figure(2), image(I2);axis image, axis off
	end
end

% ##################################################
% Support function
% ##################################################

function yy= expand(y,ndot,method)

x = 1:length(y);
xx = linspace(1,length(y),ndot);
switch method
	case 'linear'
		yy = uint8(interp1(y,xx,'linear'));
	case 'spline'
		yy = uint8(interp1(y,xx,'spline'));
	case 'pchip'
		yy = uint8(interp1(y,xx,'pchip'));
	case 'cubic'
		yy = uint8(interp1(y,xx,'cubic'));
	case 'v5cubic'
		yy = uint8(interp1(y,xx,'v5cubic'));
end


%ANGVEC2TR Convert angle and vector orientation to a homogeneous transform
%
% T = ANGVEC2TR(THETA, V) is a homogeneous transform matrix equivalent to a
% rotation of THETA about the vector V.
%
% Note::
% - The translational part is zero.
%
% See also EUL2TR, RPY2TR, ANGVEC2R.

% Copyright (C) 1993-2011, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for Matlab (RTB).
%
% RTB is free software: 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.
%
% RTB 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.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.

function T = angvec2tr(theta, k)

    if nargin < 2
        error('RTB:angvec2tr:badarg', 'bad arguments');
    end

    T = r2t( angvec2r(theta, k) );


function [node,face,elem]=meshcylinders(c0, v, len, varargin)
%
% [node,face]=meshcylinders(c0, v, len, r,tsize,maxvol,ndiv)
%    or
% [node,face,elem]=meshacylinder(c0, v, len, r, tsize,maxvol,ndiv)
% [nplc,fplc]=meshacylinder(c0, v, len,r,0,0,ndiv);
%
% create the surface and (optionally) tetrahedral mesh of a 3D cylinder
%
% author: Qianqian Fang, <q.fang at neu.edu>
%
% input:
%   c0, cylinder list axis's starting point
%   v: directional vector of the cylinder
%   len: a scalar or a vector denoting the length of each
%        cylinder segment along the direction of v
%   tsize, maxvol, ndiv: please see the help for meshacylinder for details
%
% output:
%   node, face, elem: please see the help for meshacylinder for details
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

len=cumsum(len);
[ncyl,fcyl]=meshacylinder(c0,c0+v*len(1),varargin{:});

for i=2:length(len)
   [ncyl1,fcyl1]=meshacylinder(c0+v*len(i-1),c0+v*len(i),varargin{:});
   fcyl1=cellfun(@(x) {x{1}+size(ncyl,1),x{2}}, fcyl1, 'UniformOutput', false);
   ncyl=[ncyl; ncyl1];
   if(i==1)
       fcyl1=fcyl1(1:end-1);
   else
       fcyl1={fcyl1{1:end-2},fcyl1{end}};
   end
   fcyl={fcyl{:}, fcyl1{:}};
end

[ncyl,I,J]=unique(round(ncyl*1e10),'rows');
ncyl=ncyl*1e-10;
fcyl=cellfun(@(x) {J(x{1})',x{2}}, fcyl, 'UniformOutput', false);

tsize=varargin{2};
maxvol=varargin{3};

if(nargout==2 && tsize==0.0 && maxvol==0.0)
    node=ncyl;
    face=fcyl;
    return;
end
if(nargin==3)
    tsize=len/10;
end
if(nargin<5)
    maxvol=tsize*tsize*tsize;
end

centroid=cumsum([0 len(1:end-1)])+len/2;   % define the centroids of each cylinder segment
seeds=repmat(c0(:)',length(len),1)+repmat(v(:)',length(len),1).*repmat(centroid(:),1,3);
[node,elem,face]=surf2mesh(no,fc,min(no),max(no),1,maxvol,seeds,[],0);


function [ bool, in,out ] = gsp_check_connectivity( G,param )
%GSP_CHECK_CONNECTIVITY Check if the graph G is aperiodic strongly connected
%   Usage: bool=gsp_check_connectivity( G );
%          bool=gsp_check_connectivity( L );
%          bool=gsp_check_connectivity( W );
%          [bool,in,out]=gsp_check_connectivity( ... );
%
%   Input parameters:
%       G,W,L: Graph, Laplacian matrix or Weight martrix
%       param: Optional parameters
%
%   Output parameters
%       bool: Boolean
%       in  : Nodes without any in connections
%       out : Nodes without any out connections
%
%   Test if each node have at least one in connection and one out
%   connection. If this simple test give good results, the function compute
%   the perron vector of G and test it. It might take some time.
%
%   *param* is an optional structure that contains the following field
%
%   * *param.verbose*: display parameter - 0 no log - 1 display the errors
%

% Date: 6 june 2013
% Author: Nathanael Perraudin

%TODO: Use a clever method

% Handle Input parameters
if nargin<1, error('Not enought inputs parameters!'); end

if nargin<2, param=struct; end

if ~isfield(param, 'verbose'), param.verbose = 0; end

if isstruct(G)
    % If the graph is undirected, use the other function...
    if G.directed == 0
       [bool, in] = gsp_check_connectivity_undirected(G,param);
       out = in;
       return;
    end
    A=G.W;
else
    A=G;
end

if ~gsp_isdirected(A)
     [bool, in] = gsp_check_connectivity_undirected(G,param);
      out = in;
     return;
end

warning('This code is really bad!')

% Number of vertex
N=length(A);

% Remove the diagonal
A=A-diag(diag(A));

% Check the connecivity -- simple
    bool=~boolean((sum(1.-(sum(A,1)>0))+sum(1.-(sum(A,2)>0))));
    in=find(1.-(sum(A,1)>0));
    out=(find(1.-(sum(A,2)>0)))';
if param.verbose
    fprintf('   ---   Test if the graph is strongly connected   ---\n');
end

if bool
% Check the connectivity -- harder

    % Compute the Probablility matrix
    P=A./repmat(sum(A,2),1,N);

    % Compute the perron vector of P
    [phi,max_eig_P] = eigs(P',2);
    % test if max_eig_P==1
    if abs(max_eig_P(1,1)-1)>10e3*eps;
        bool=0;
        if param.verbose
                fprintf('    The maximum eigenvalue of P is not 1. \n');
        end
    else
        if param.verbose
                fprintf('    The maximum eigenvalue of P is 1. \n');
        end
    end
    phi=phi(:,1);
    % Test if the perron vector is positive
    if sum( phi)<0;
        phi=-phi;
    end

    if sum(phi<=10e3*eps)
        bool=0;
        if param.verbose
                fprintf('    Null of negative entry in the perron vector. \n');
        end
    else
        if param.verbose
                fprintf('    Stricly positive perron vector. \n');
        end
    end

    % see code from undirected for comments about this line
    max_eig_P = eigs(speye(size(P))+P,2);
    if abs(max_eig_P(2,2)-2)<10e3*eps;
        bool=0;
        if param.verbose
                fprintf('    Second eigenvalue of P is 1. \n');
        end
    else
        if param.verbose
                fprintf('    Only one unit eigenvalue of P.\n');
        end
    end

elseif param.verbose;
    fprintf('     Not every node has an input connection and an output connection! \n');
    fprintf('         No in connections for nodes: %i\n',in );
    fprintf('         No out connections for nodes: %i\n', out);
end

if param.verbose
    fprintf('   ---   End of tests   ---\n');
    if bool
        fprintf('    The graph is aperiodic strongly connected.\n')
    else
        fprintf('    The graph is not aperiodic stronly connected.\n');
    end
end

end


function regressor=task_regressor(TR,T,taskfile)

%taskfile: If using the Custom 3 column format, prepare a plain text file (avoiding editors such as MS Word)
%containing one line for each stimulus associated with the condition to be modelled by this EV.
%Each line must contain three numbers: the onset time (in seconds); the duration (in seconds);
%the relative magnitude of each stimulus (usually set to 1)
% Example:
% taskfile='fear.txt';
% 32.08   18   1
% 74.223  18   1
% 116.365  18  1

%TR: in seconds
%T: number of frames

%Convolve with HRF
Convolve=1; %0=no 1=yes

hrf=spm_hrf(TR);

%Regressor
regressor=zeros(T,1);
t=dlmread(taskfile);
for i=1:size(t,1)
    start=t(i,1);
    finish=start+t(i,2);

    %Convert from seconds to TRs
    start=round(start/TR)+1;
    finish=round(finish/TR)+1;

    regressor(start:finish)=1;
end
if Convolve; regressor=conv(hrf,regressor); end %Convolve with HRF
regressor=regressor(1:T);


% another simple convection example using method of lines, still a bit
% messy, wait for a blog post
% Using Matlab ODE solvers to solve a convective flux that
% moves a plume in diagonal direction
% see how terribly diffusive the solution is!
% Written by Ali A. Eftekhari
% Last checked: June 2021
clc
% define a 2D mesh
H = 1;
W = 1;
Nx = 100;
Ny = 100;
mesh1 = createMesh2D(Nx, Ny, W, H);
% velocity field
u = createCellVariable(mesh1, 0.001*ones(Nx,Ny));
uf = arithmeticMean(u);
% uf.yvalue(:,:)=0;
% diffusion field
D = 1e-2*createCellVariable(mesh1, 1e-2);
Df = arithmeticMean(D);
% transient term coefficient
alfa = createCellVariable(mesh1, 1);
% define the boundaries
% dirichlet on all the boundaries
BC = createBC(mesh1); % all Neumann
BC.bottom.a(:) = 0; BC.bottom.b(:) = 1; BC.bottom.c(:) = 0;
BC.left.a(:) = 0; BC.left.b(:) = 1; BC.left.c(:) = 0;
% BC.right.a(:) = 0; BC.right.b(:) = 1; BC.right.c(:) = 0;
% BC.top.a(:) = 0; BC.top.b(:) = 1; BC.top.c(:) = 0;
% Initial values
phi_old = createCellVariable(mesh1, 0, BC);
phi_old.value(5:10, 5:10) = 1;
% define the convection term
Mconv = convectionUpwindTerm(uf);
Mdif = diffusionTerm(Df);
% define the BC term
[Mbc, RHSbc] = boundaryCondition(BC);
% solver
final_t = 500;
% define the transient term
[M, RHS] = combineBC2D(BC, Mconv, ...
    zeros((Nx+2)*(Ny+2),1));
% eq: dcdt = D d2c/dx2
dcdt = @(t,c)(-M*c-RHS);
[t_temp, c_temp] = ode45(dcdt, [0 final_t], internalCells(phi_old));
for i =1:length(t_temp)
    phi.value = reshape(c_temp(i,:), Nx, Ny);
    figure(1);pcolor(phi.value');title(["t = ", num2str(t_temp(i))]); colorbar;drawnow;
end


function [y,e] = powspec(x, sr, wintime, steptime, dither)
%[y,e] = powspec(x, sr, wintime, steptime, sumlin, dither)
%
% compute the powerspectrum and frame energy of the input signal.
% basically outputs a power spectrogram
%
% each column represents a power spectrum for a given frame
% each row represents a frequency
%
% default values:
% sr = 8000Hz
% wintime = 25ms (200 samps)
% steptime = 10ms (80 samps)
% which means use 256 point fft
% hamming window
%
% $Header: /Users/dpwe/matlab/rastamat/RCS/powspec.m,v 1.3 2012/09/03 14:02:01 dpwe Exp dpwe $

% for sr = 8000
%NFFT = 256;
%NOVERLAP = 120;
%SAMPRATE = 8000;
%WINDOW = hamming(200);

if nargin < 2
  sr = 8000;
end
if nargin < 3
  wintime = 0.025;
end
if nargin < 4
  steptime = 0.010;
end
if nargin < 5
  dither = 1;
end

winpts = round(wintime*sr);
steppts = round(steptime*sr);

NFFT = 2^(ceil(log(winpts)/log(2)));
%WINDOW = hamming(winpts);
%WINDOW = [0,hanning(winpts)'];
WINDOW = [hanning(winpts)'];
% hanning gives much less noisy sidelobes
NOVERLAP = winpts - steppts;
SAMPRATE = sr;

% Values coming out of rasta treat samples as integers,
% not range -1..1, hence scale up here to match (approx)
y = abs(specgram(x*32768,NFFT,SAMPRATE,WINDOW,NOVERLAP)).^2;

% imagine we had random dither that had a variance of 1 sample
% step and a white spectrum.  That's like (in expectation, anyway)
% adding a constant value to every bin (to avoid digital zero)
if (dither)
  y = y + winpts;
end
% ignoring the hamming window, total power would be = #pts
% I think this doesn't quite make sense, but it's what rasta/powspec.c does

% that's all she wrote

% 2012-09-03 Calculate log energy - after windowing, by parseval
e = log(sum(y));


%%
% test for linear programming and interpolation of discrete meaures.

addpath('../toolbox/');
addpath('../toolbox/mexEMD/');

test = 'weighted';
test = 'empirical';

rep = ['results/' test '/'];
[~,~] = mkdir(rep);

% helpers
SetAR = @(ar)set(gca, 'PlotBoxAspectRatio', [1 ar 1]);
myplot = @(x,y,ms,col)plot(x,y, 'o', 'MarkerSize', ms, 'MarkerEdgeColor', col, 'MarkerFaceColor', col, 'LineWidth', 2);
myplot = @(x,y,ms,col)plot(x,y, 'o', 'MarkerSize', ms, 'MarkerEdgeColor', 'k', 'MarkerFaceColor', col, 'LineWidth', 1);

% Dimensions  of the clouds.
n0 = 4000;
n1 = n0;

% Compute a first point cloud \(X_0\) that is Gaussian.
% and a second point cloud \(X_1\) that is Gaussian mixture.
randn('state', 666);
gauss = @(q,a,c)a*randn(2,q)+repmat(c(:), [1 q]);
X0 = randn(2,n0)*.3;
X1 = [gauss(n1/2,.5, [0 1.6]) gauss(n1/4,.3, [-1 -1]) gauss(n1/4,.3, [1 -1])];
% weights
normalize = @(a)a/sum(a(:));
switch test
    case 'weighted'
        p0 = normalize(rand(n0,1));
        p1 = normalize(rand(n1,1));
    case 'empirical'
        p0 = 2*ones(n0,1)/n0;
        p1 = 2*ones(n1,1)/n1;
end

%%
% Display the point clouds.
% The size of each dot is proportional to its probability density weight.

clf; hold on;
for i=1:length(p0)
    myplot(X0(1,i), X0(2,i), p0(i)*length(p0)*10, 'b');
end
for i=1:length(p1)
    myplot(X1(1,i), X1(2,i), p1(i)*length(p1)*10, 'r');
end
axis([min(X1(1,:)) max(X1(1,:)) min(X1(2,:)) max(X1(2,:))]); axis off;

%%
% Compute the cost matrix

C = repmat( sum(X0.^2)', [1 n1] ) + ...
    repmat( sum(X1.^2), [n0 1] ) - 2*X0'*X1;

%%
% Solve the linprog of OT

[cost,gamma] = mexEMD(p0,p1,C);

%%
% Compute displacement interpolation.

[I,J,gammaij] = find(gamma);
tlist = linspace(0,1,6);
clf;
for k=1:length(tlist)
    t=tlist(k);
    Xt = (1-t)*X0(:,I) + t*X1(:,J);
    % subplot(2,3,i);
    clf;
    hold on;
    for i=1:length(gammaij)
        myplot(Xt(1,i), Xt(2,i), gammaij(i)*length(gammaij)*6, [t 0 1-t]);
    end
    % title(['t=' num2str(t,2)]);
    axis([min(X1(1,:)) max(X1(1,:)) min(X1(2,:)) max(X1(2,:))]);
    % dummy points
    plot([min(X1(1,:)) max(X1(1,:))], [min(X1(2,:)) max(X1(2,:))], '.', 'MarkerFaceColor', 'w', 'MarkerEdgeColor', 'w');
    axis equal; axis square;
    axis off;
    drawnow;
    saveas(gcf, [rep 'interp-' num2str(k) '.eps'], 'epsc');
end


% MESH_BOOLEAN Compute boolean csg operations on "solid", consistently oriented
% meshes.
%
% [W,H] = mesh_boolean(V,F,U,G,operation)
% [W,H] = mesh_boolean(V,F,U,G,operation,'ParameterName',paramter_value, ...)
% [W,H] = mesh_boolean({V1,V2,..,Vn},{F1,F2,...,Fn}, ...
%    operation,'ParameterName',paramter_value, ...)
%
% Inputs:
%   V  #V by 3 list of vertex positions of first mesh
%   F  #F by 3 list of triangle indices into V
%   U  #U by 3 list of vertex positions of second mesh
%   G  #G by 3 list of triangle indices into U
%   operation  followed by operation to perform as a string, one of: 'union',
%     'intersect', 'minus', 'xor', or 'resolve'
%     Optional:
%       'BooleanLib' followed by boolean library back-end to use, one of:
%         {'libigl'}  uses CGAL's exact arithmetic kernel and is believed to be
%                     correct.
%         'cork'  is faster but may give incorrect results.
%         'libigl-try-cork-resolve'  libigl boolean extraction but tries to use
%                                    cork's fast resolve, if intersections
%                                    persist, then resolves remaining with
%                                    libigl's resolve. This adds a "layer of
%                                    robustness" on top of cork, but since it's
%                                    not understood _how_ cork is failing, it
%                                    is unknown whether this will lead to
%                                    correct results.
% Outputs:
%   W  #W by 3 list of vertex positions of boolean result mesh
%   H  #H by 3 list of triangle indices into W
%   J  #H list of indices into [FA;FB] of facet birth parents
%
% See also: self_intersect
%


function p = lbfgsbprod(H,g)
%lbfgshprod  computes products with the L-BFGS matrix B.
%
%   p = lbfgsbprod(H,g)  returns  p = B*g.
%
%   The product is computed using the factoriation
%   [(9.15),p.231] described in Nocedal and Wright, 1999.

%   See also lbfgsadd, lbfgsdel, lbfgsupdate, lbfgsinit.

% $Id$

% ----------------------------------------------------------------------
% Explicit matrix formulation
% ----------------------------------------------------------------------

%jMax = H.jMax;
%jNew = H.jNew;
%jOld = H.jOld;

% |---|---|---|---|---|
%       ^   ^
%       |   |
%      old new
%   4   5   1   2   3    1=newest, 2=2nd newest,... 5=oldest

%if false
%   Sk = H.S(:,[jNew-1:-1:1,jMax:-1:jNew]);
%   Yk = H.Y(:,[jNew-1:-1:1,jMax:-1:jNew]);
%   Lk = (Sk' * Yk) .* (repmat((1:jMax)',1,jMax) > repmat((1:jMax),jMax,1)) ;
%   Dk = diag(sum(Sk.*Yk));

%   deltak = H.delta;

%   M = [deltak*Sk'*Sk, Lk; Lk', -Dk];

%   p = [deltak*Sk'; Yk'] * g;
%   p = M \ p;
%   p = [deltak*Sk, Yk] * p;
%   p = deltak * g - p;
%end

% ----------------------------------------------------------------------
% Formulation directly based on arrays in permuted form :-)
% ----------------------------------------------------------------------

if ~isempty(H.ML)

   % This code works much faster for larger problems
   v1 = H.delta * (g' * H.S)';
   v2 = (g' * H.Y)';
   p  = [v1(H.valid); v2(H.valid)];

%  v1 = H.delta * (g' * H.S(:,H.valid))';
%  v2 = (g' * H.Y(:,H.valid))';
%  q = [v1; v2];

   p = H.MU \ (H.ML \ p); % H.M \ p (optionally: use linsolve)

%  v1 = H.S(:,H.valid) * p(1:H.rank) * H.delta;
%  v2 = H.Y(:,H.valid) * p(H.rank+1:2*H.rank);
%  q = v1 + v2;

   % This code works much faster for larger problems
   pe = zeros(size(H.S,2),1); pe(H.valid) = p(1:H.rank);
   v1 = H.S * (pe * H.delta);
   pe = zeros(size(H.Y,2),1); pe(H.valid) = p(H.rank+1:2*H.rank);
   v2 = H.Y * pe;
   p  = v1 + v2;
else
   p = 0;
end

p = H.delta * g - p;


function zz=lpcss2zz(ss)
%LPCSS2ZZ Convert s-place poles to z-plane poles ZZ=(SS)
%the s-plane is in units of Normalized Hz and so the imaginary part
% of each ss() value is in the range +-0.5
%
% If you multiply ss by the sample frequency, a formant with
% frequency f and bandwidth b will give an s-plane pole-pair
% of approximately -b/2 +- j f

%      Copyright (C) Mike Brookes 1997
%      Version: $Id: lpcss2zz.m,v 1.4 2007/05/04 07:01:39 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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

zz=exp(2*pi*ss);


function [Q, R, E] = qr(f, varargin)
%QR   QR factorisation of an array-valued BNDFUN.
%   [Q, R] = QR(F) returns a QR factorisation of F such that F = Q*R, where the
%   BNDFUN Q is orthogonal (with respect to the continuous L^2 norm on the
%   domain of F) and of the same size as F and R is an m x m upper-triangular
%   matrix when F has m columns.
%
%   [Q, R, E] = QR(F) produces unitary Q, upper-triangular R, and a permutation
%   matrix E so that F*E = Q*R. The column permutation E is chosen to reduce
%   fill-in in R.
%
%   [Q, R, E] = QR(F, 'vector') returns the permutation information as a vector
%   instead of a matrix.  That is, E is a row vector such that F(:,E) = Q*R.
%   Similarly, [Q, R, E] = QR(F, 'matrix') returns a permutation matrix E. This
%   is the default behavior.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% Deal with empty case:
if ( isempty(f) )
    Q = [];
    R = [];
    E = [];
    return
end

% Initialise Q to be a BNDFUN:
Q = f;

% Rescaling factor, (b - a)/2.
rescaleFactor = .5*diff(f.domain);

% Call QR on the ONEFUN of f:
if ( nargout == 3 )
    [Q.onefun, R, E] = qr(f.onefun, varargin{:});
else
    [Q.onefun, R] = qr(f.onefun, varargin{:});
end

% Rescale so that columns of Q will be orthonormal (rather than orthogonal):
Q = Q/sqrt(rescaleFactor);

% Rescale R so that f = QR:
R = R*sqrt(rescaleFactor);

end


%{
load('dataset/trafficdb/traffic_patches.mat');
[M,m,n,p] = convert_video3d_to_2d(im2double(imgdb{100}));
out = run_algorithm('MC', 'LMaFit', M, [])
show_results(M.*out.Omega,out.L,out.S,out.O,p,m,n);
%}

MIdx = M(Idx);
rank = 10;
[X,Y] = lmafit_mc_adp(size(M,1),size(M,2),rank,Idx,MIdx,[]);
L = X*Y; % low-rank
S = (M - L); % sparse


function [Xc_opt,R_opt,T_opt,err_opt,iter]=optimize_betas_gauss_newton(Km,Cw,Beta0,Alph,Xw,U,A)

% COMPUTE_BETAS_GAUSS_NEWTON
%
%       Km: vector of the kernel
%       Cw: position of the control point in world coordinates
%       Beta0: initial guess of the betas
%
%
% Copyright (C) <2007>  <Francesc Moreno-Noguer, Vincent Lepetit, Pascal Fua>
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the version 3 of the GNU General Public License
% as published by the Free Software Foundation.
%
% 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/>.
%
% Francesc Moreno-Noguer, CVLab-EPFL, October 2007.
% fmorenoguer@gmail.com, http://cvlab.epfl.ch/~fmoreno/

n=size(Beta0,2);

[Beta_opt,err,iter]=gauss_newton(Km,Cw,Beta0);

%Extract control point camera coordinates from Betas and Kernel
X=zeros(12,1);
for i=1:n
   X=X+Beta_opt(i)*Km(:,i);
end

Cc=zeros(4,3);
for i=1:4
    Cc(i,:)=X(3*i-2:3*i);
end

%check sign of the determinant (keep orienation of the control points)
s_Cw=sign_determinant(Cw);
s_Cc=sign_determinant(Cc);
Cc=Cc*(s_Cw/s_Cc);

%Reconstruct and compute error=
Xc_opt=Alph*Cc; %reconstruction: points in camera coordinate system
[R_opt,T_opt]=getrotT(Xw,Xc_opt);  %solve exterior orientation

[err_opt,Urep_opt]=reprojection_error_usingRT(Xw,U,R_opt,T_opt,A);


%
% LibQPEP: A Library for Globally Optimal Solving Quadratic Pose Estimation Problems (QPEPs),
%          It also gives highly accurate uncertainty description of the solutions.
%
%
% Article:
%      Wu, J., Zheng, Y., Gao, Z., Jiang, Y., Hu, X., Zhu, Y., Jiao, J., Liu, M. (2020)
%           Quadratic Pose Estimation Problems: Globally Optimal Solutions,
%           Solvability/Observability Analysis and Uncertainty Description.
%           IEEE Transactions on Robotics.
%           https://doi.org/10.1109/TRO.2022.3155880
%
%
% Authors:      Jin Wu and Ming Liu
% Affiliation:  Hong Kong University of Science and Technology (HKUST)
% Emails:       jin_wu_uestc@hotmail.com; eelium@ust.hk
% Websites:     https://zarathustr.github.io
%               https://ram-lab.com
%
%
% test_rel_att.m: The QPEP illustration of range-based
%                 relative attitude estimtion

clear all
close all
clc

if(verLessThan('matlab', '8.0.0'))
   error('The MATLAB version is too old to be supported.');
end

addpath('func_files');
addpath('solvers');
addpath('utils');
addpath('homotopy');

p00 = randn(3, 1);
d00 = sqrt(p00.' * p00);
C0 = orthonormalize(randn(3, 3));

len = 10;
p1 = zeros(3, len);
p2 = zeros(3, len);
epsilon = zeros(len, 1);
noise = 1e-2; % Noise level
for i = 1 : len
    p1(:, i) = randn(3, 1);
    p2(:, i) = randn(3, 1);
    epsilon(i) = (p1(:, i) - p00).' * C0 * p2(:, i) + p1(:, i).' * p00 + noise * randn(1, 1);
end

syms q0 q1 q2 q3
q = [q0; q1; q2; q3];
syms t1 t2 t3;
t = [t1; t2; t3];
R = q2R(q);
rr = R.' * t;
syms r1 r2 r3
r = [r1; r2; r3];
eqs = sym(zeros(len + 2, 1));
for i = 1 : len
    eqs(i) = p1(:, i).' * R * p2(:, i) + p1(:, i).' * t - r.' * p2(:, i) - epsilon(i);
end
eqs(len + 1) = q.' * q - 1;
eqs(len + 2) = r.' * r - t.' * t;
eqs = expand(eqs);
x = [q; t; r];
H = expand(jacobian(eqs, x).' * eqs);
assumeAlso(q.' * q == 1);
assumeAlso(r.' * r == t.' * t);
eq = vpa(expand(simplify(H)), 32);
ss = vpasolve(eq(5 : 10), [t; r]);
eq_ = eq(1 : 4);
eq_ = subs(eq_, t1, ss.t1);
eq_ = subs(eq_, t2, ss.t2);
eq_ = subs(eq_, t3, ss.t3);
t_func = matlabFunction([ss.t1; ss.t2; ss.t3], 'Vars', {q});
eq_ = subs(eq_, r1, ss.r1);
eq_ = subs(eq_, r2, ss.r2);
eq_ = subs(eq_, r3, ss.r3);
eq_ = vpa(expand(eval(eq_)), 32);
syms lambda
eqs = [
    eq_;
    q.' * q - 1;
    ]

str = '';
for i = 1 : length(eqs)
    str = strcat(str, sprintf(' PP{%d} = char(vpa(%%s, 32));', i));
end

str_ = sprintf(str, char(eqs(1)), ...
                    char(eqs(2)), ...
                    char(eqs(3)), ...
                    char(eqs(4)), ...
                    char(eqs(5)));
eval(str_);
[S, vars] = psolve(PP);
S = S.';
SS = S;
for i = 1 : length(vars)
    if(strcmp(vars{i}, 'q0'))
        SS(:, 1) = S(:, i);
    elseif(strcmp(vars{i}, 'q1'))
        SS(:, 2) = S(:, i);
    elseif(strcmp(vars{i}, 'q2'))
        SS(:, 3) = S(:, i);
    elseif(strcmp(vars{i}, 'q3'))
        SS(:, 4) = S(:, i);
    elseif(strcmp(vars{i}, 'lambda'))
        SS(:, 5) = S(:, i);
    end
end
S = real(SS);
xs_ = S;
sols = SS.';

num = size(sols, 2);
sol = zeros(4, num);
ts = zeros(3, num);
Ls = zeros(len, 1);
for i = 1 : num
    sol(:, i) = real(sols(1 : 4, i));
    sol(:, i) = sol(:, i) ./ norm(sol(:, i));
    C = q2R(sol(:, i));
    t = t_func(sol(:, i));
    ts(:, i) = t;
    loss = 0;
    for j = 1 : len
        loss = loss + (epsilon(j) - (p1(:, j) - t).' * C * p2(:, j) - p1(:, j).' * t)^2;
    end
    Ls(i) = loss;
end
[~, idx] = sort(Ls);

q_ = positive_quat(sol(:, idx(1)).')
q_true = positive_quat(dcm2quat(C0))

t_ = ts(:, idx(1)).'
t_true = p00.'


function [Model, Info] = linear_map_sparse_cov_pinv(X,Y,Model,parm)
%  Estimate linear weight matrix for input-output mapping
%     Automatic Relevance Prior for each input dimension
%     is imposed to get sparse weight matrix
%
%   [Model, Info] = linear_map_sparse_cov_pinv(X,Y,Model,parm)
%
% --- Input
%  X  : Input data  ( M x T )
%  Y  : Output data ( N x T )
%  N  =  # of output
%  M  =  # of input
%  T  =  # of data
%
%  Model : Structure for estimated model
%  Model.SY0 :  Output data variance                 ( 1 x 1 )
%  Model.A0  :  (Output data var)/(Input data var)   ( 1 x 1 )
%
%  parm  : Structure for learning parameter
%  parm.Ntrain :  # of training
%  parm.Nskip  :  skip # for print
%  parm.a_min  :  Min value for pruning small variance component
%  parm.Prune  :  = 1 : Prune small variance & irrelevant input dimension
%
% --- Output
%  Model : Structure for estimated model
%  Model.SY  :  Noise variance         ( 1 x 1 )
%  Model.SW  :  Weight variance        ( M x M )
%  Model.W   :  Weight matrix          ( N x M )
%  Model.A   :  Prior weight variance  ( N x M ) ARD hyper parameter
%
%  Info  : Structure for learning process history
%  Info.FE  = LP + H : Free energy
%  Info.LP  = - (Log error)
%  Info.H   = - (# of effective weight parameters)
%
% 2007/1/26 Made by M. Sato

% Constants
MINVAL  = 1.0e-15;
MinCond = 1.0e-10;

% # of total training iteration
Ntrain = parm.Ntrain;

Nskip  = 100;   % skip steps for display info
a_min  = 1e-10; % Minimum value for weight pruning
Fdiff  = 1e-10; % Threshold for convergence
Ncheck = 100;   % Minimum number of training iteration
Fstep  = 5;     % Free energy convergence check step
Prune  = 1;     % Prune mode

if isfield(parm,'Nskip'), Nskip  = parm.Nskip; end;
if isfield(parm,'Fdiff'), Fdiff   = parm.Fdiff; end;
if isfield(parm,'a_min'), a_min   = parm.a_min ; end;
if isfield(parm,'Prune'), Prune = parm.Prune; end;
if isfield(parm,'Ncheck'), Ncheck = parm.Ncheck; end
if isfield(parm,'Fstep'),Fstep  = parm.Fstep ; end

% # of embedding dimension
if isfield(parm,'Dtau')
	D    = parm.Dtau;
	tau  = parm.Tau;
else
	D    = 1;
	tau  = 1;
end

% Dimension
[M ,Tx ,Nx ]= size(X); % input dim
[N ,Ty ,Ny ]= size(Y); % output dim

if Nx~=Ny, error('Trial number is different for input & output'); end
if Tx~=Ty, error('Time sample is different for input & output'); end

% Reshape into 2D matrix
T = Tx*Nx; % # of data
X = reshape(X, [M T]);
Y = reshape(Y, [N T]);

% # of stable VB-update in initial training
if isfield(parm,'Npre_train')
	Npre_train = parm.Npre_train;
else
%	Npre_train = Ntrain;
%	Npre_train = 0;
	if T >= 2*M*D
		Npre_train = 0;
	elseif T >= M*D
		Npre_train = fix(Ntrain/2);
	else
		Npre_train = Ntrain;
	end
end
if Npre_train > Ntrain, Npre_train = Ntrain; end;

fprintf('linear map sparse covariance_pinv start\n')
fprintf('--- Output Dimension  = %d\n',N)
fprintf('--- Input  Dimension  = %d\n',M)
fprintf('--- Embedding  Dimension  = %d\n',D)
fprintf('--- Number of trials  = %d\n',Nx)
fprintf('--- Number of training sample = %d\n',Tx)
fprintf('--- Total update iteration    = %d (%d)\n',Ntrain,Npre_train)

%
% --- Initialization
%  A  : Initial variable to use 1st update
%     : 1 x M

% Input/Output variance
sx = mean(repadd(X, - mean(X,2)).^2, 2);
sy = mean(repadd(Y, - mean(Y,2)).^2, 2);

A0  = 1./mean(sx);
SY0 = mean(sy);

if isempty(Model)
	A   = repmat(A0, [1,M]);
	W   = zeros(N,M);
	SY  = SY0;
else
	A   = Model.A ;	 % 1 x M
	W   = Model.W ;  % N x M
	SY  = mean(Model.SY);  % 1 x 1
end

if isfield(parm,'Ta0') && parm.Ta0 > 0,
	Ta0 = parm.Ta0;
	a0  = parm.a0 * A0;
else
	Ta0 = 0;
	a0  = 1;
end

%  --- Initialization by other method ---
% Model.mode = 'scalar': ARD term = alpha * W^2
% Model.mode = 'cov'   : ARD term = alpha * W^2 * SY(^-1)
%
if isfield(Model, 'mode') &&  strcmp(Model.mode,'scalar')==1,
	fprintf('Old result is used as initial value\n')
	fprintf('Old method = %s\n', Model.method)
	A  = sum(A,1)./(sum(SY));
end

A = max(A,MINVAL);

% Original input dimension
M_ALL = M;

if isfield(Model,'ix_act')
	% Active index
	IX_act = Model.ix_act;

	X = X(IX_act,:); 	% M x T
	M = length(IX_act);
else
	IX_act = 1:M;
end

% Initial active index
M_all  = M;
A_all  = A/max(A) ;
ix_act_old = 1:M;

ix_act = find( A_all > a_min );   % effective indices
Mnew   = length(ix_act);  		% # of effective input

if Mnew < M,
    % convert to relative index
    jx_act = trans_index(ix_act,ix_act_old,M_all);

    M   = Mnew;
    A   = A(jx_act) ;  			% 1 x M
    W   = W(:,jx_act) ;  		% N x M
	X 	= X(jx_act,:);			% M x T
end

% Input variance
%XX  = (X * X')/T;   	% M x M
%YX  = (Y * X')/T;       % N x M
%YY  = sum(Y.^2,2)/T;    % N x 1
% Covariance matrix (not normalised)
YX  = (Y * X');       % N x M
YY  = sum(Y.^2,2);    % N x 1

fprintf('a_min = %g\n', a_min)
fprintf('SY0   = %g\n', SY0)
fprintf('SY    = %g\n', SY)

% Working variable
if T <= M
	XX  = [];
	Cinv= zeros(T,T);
	SW  = zeros(T,T);
else
	XX  = (X * X');   	  % M x M
	Cinv= zeros(M,M);
	SW  = zeros(M,M);
end

G_A = zeros(1,M);       % 1 x M
log_a = 0;
A_old = A;

% Free energy histry
FE  = zeros(Ntrain,1);
LP  = zeros(Ntrain,1);
H   = zeros(Ntrain,1);
MM  = zeros(Ntrain,1);
Err = zeros(Ntrain,1);

% ARD hyper param. history
if isfield(parm,'Debug') & ~isempty(parm.Debug) & parm.Debug > 0
	Debug = 1;
	A_tmp = zeros(M_all, ceil(Ntrain/Nskip));
else
	Debug = 0;
end

k_save  = 0;

%%%%%% Learning Loop %%%%%%
for k=1:Ntrain
	% ARD hyper variance parameter
	% A = 1/alpha

	if T < M
	    % Weight variance
	    % inv(X*X' + 1./A) = A - A * X * inv(X'*A*X + 1) * X' * A
		%  C = ( X' *A* X + eye(T) );
		XA   = repmultiply(X' , A); % T x M
	    Cinv = XA * X + eye(T);  % T x T

    	if rcond(Cinv) > MinCond,
			Cinv  = inv( Cinv );  % M x M
		else
			Cinv  = pinv( Cinv );
		end

		% Weight update
	    % inv(X*X' + 1./A) = A - A * X * Cinv * X' * A
		% W0 = YX .* A;
		% W  = W0 - (((W0 * X) * Cinv) * X') .* A;
		% W  = W0 - ((W0 * X) * Cinv) * (X'.* A);
		W  = repmultiply(YX , A);
		W  = W - ((W * X) * Cinv) * XA;
%		W  = W - ((W * X) / C ) * XA;
%		W  = W - repmultiply( ((W * X) * Cinv) * X' , A);

		%  C = ( X' *A* X + eye(T) );
		%  G_A = diag( X * inv(C) * X' *A )
		% G_A  = A .* sum(X .* (X / C), 2)';
		G_A  = A .* sum(X .* (X * Cinv), 2)';

		% Log variance
		log_sw  = log_det(Cinv) ;
		if mod(k, Nskip)==0, fprintf('- '); end
	else
		if isempty(XX)
			% covariance matrix in reduced space
			XX = X * X';
			% save original index
			IX_act = IX_act(ix_act);
			% new active index in reduced space
			ix_act = 1:M;
			M_all  = M;
			A_all  = A;
		end
    	SW  = XX + diag(1./A);

    	if rcond(SW) > MinCond,
			SW  = inv( SW );  % M x M
		else
			SW  = pinv( SW );
		end

		% Weight update
		W  = YX * SW;

		% SW  = X*X' + diag(1./A)
		% G_A = diag(inv(SW) * X*X')
		%     = 1 - diag(inv(SW)) ./A
		% G_A = diag( XX /SW );
		G_A = sum( SW .* XX ,1);
		%G_A = 1 - diag(SW)' ./A;

		log_sw  = log_det(SW) - sum(log(A));
		if mod(k, Nskip)==0, fprintf('+ '); end
	end

	WW = sum(W.^2, 1);
    % Noise variance update
%    SY = (sum(YY) - sum(sum(W.*YX)))/(N*T);
%
%    if (SY/SY0) <= MINVAL,
	    % Error
	    dY  = Y - W * X;        % N x T
	    dYY = sum(dY.^2, 2);  	% N x 1

	    SY  = (sum(dYY) + sum( WW./A ))/(N*T);
	    % Prevent zero variance
	    SY  = max( SY, MINVAL);
%	    fprintf('*')
%	end

	% Log variance
    log_sy  = N * log(SY) ;
    if Ta0 > 0,
	    log_a   = Ta0 * sum( - log(A./a0) - a0./A + 1);
	end

    % Free energy
    H(k)   =   0.5*N * (log_sw - M);
    LP(k)  = - 0.5*(T * log_sy) ;
    FE(k)  = LP(k) + H(k);
    Err(k) = (SY)./(SY0);
    MM(k)  = M;

    % Hyper parameter for weight variance (ARD)
    G_A = max((G_A), MINVAL);

	if k <= Npre_train,
		% VB update rule
		%  	N * A  = (W.^2)./SY + N * (A - A.*G_A)  ;
   		%A  = WW./SY + N * (A - A.*G_A)  ;
		if T < M
			A  = (WW./SY + N * A.*(1 - G_A) + 2*Ta0*a0)./( N + 2*Ta0 );
		else
			A  = (WW./SY + N * diag(SW)' + 2*Ta0*a0)./( N + 2*Ta0 );
		end
		%	A^2  = A .* (1./SY)' * (W.^2) ./ (G_A * N);
	else
	    % Accelerated update rule
		%	A  = (1./SY)' * (W.^2) ./ (G_A * N);
	    A  = sqrt(A.*(WW./SY)./(G_A * N));
	    %A  = ((WW./SY) + 2*Ta0*a0)./(G_A * N + 2*Ta0);
	end

    % Prune small variance
    if Prune == 1
	    ix_act_old = ix_act;

	    % Recover all component
	    switch	Prune
	    case	1
		    A_all(ix_act) = WW/max(WW);    % Prune by Weight
	    case	2
		    A_all(ix_act) = A /max(A);	   % Prune by Alpha
	    case	3
		    A_all(ix_act) = A * (1/SX);    % Prune by Alpha
	    end

	    % Find active input dimension (absolute index)
	    ix_act = find( A_all > a_min ); % effective indices
	    Mnew   = length(ix_act);  		% # of effective input

	    if Mnew < M,
		    % convert to relative index
		    jx_act = trans_index(ix_act,ix_act_old,M_all);

		    M   = Mnew;
		    A   = A(jx_act) ;  			% 1 x M
		    W   = W(:,jx_act) ;  		% N x M
			X 	= X(jx_act,:);			% M x T
			YX  = YX(:,jx_act);  	 	% N x M
			if ~isempty(XX)
				XX	= XX(jx_act,jx_act);	% M x M
			end
		end
    end

	A = max(A,MINVAL);

    if mod(k, Nskip)==0
        % Save history
		if Debug == 1
        	k_save = k_save + 1;
        	A_tmp(:,k_save) = A_all(:);
		end

        fprintf('Iter = %4d, M = %4d, err = %g, F = %g, H = %g\n', ...
               k, M, Err(k), FE(k), - H(k));
    end

	if k > Ncheck && M == MM(k-1)
		Adif = max(abs(A - A_old));
	else
		Adif = 1;
	end
	if Adif < Fdiff,
		fprintf('Converged : Alpha change = %g\n',Adif)
		break;
	end;

	A_old = A;

%		Fdif = (FE(k) - FE(k-Fstep))/abs(FE(k));
%	else
%		Fdif = Fdiff + 1;
%	end
end

% convert to relative index
ix_act = IX_act(ix_act);

% Active index
Model.ix_act = ix_act;
Model.M_all  = M_ALL ;

% Save trained variable
%  W & A is sufficient for cov-method initialization
Model.A  = A ;
Model.W  = W ;
Model.SY = SY;

Model.method = 'linear_map_sparse_cov';
Model.mode   = 'cov';
Model.sparse = 'sparse';

% Save history
Info.FE  = FE(1:k);
Info.LP  = LP(1:k);
Info.H   = H(1:k) ;
Info.Err = Err(1:k);
Info.M   = MM(1:k);

if exist('A_tmp','var')
	Info.A   = A_tmp(:,1:k_save) ;
end

%%% ---- Index transformation from old active_index to current active_index
function	jx = trans_index(ix,ix_old,M)

N = length(ix_old);
Itrans = zeros(M,1);
Itrans(ix_old) = 1:N;

jx = Itrans(ix);


function [mask] = estimate_mask(s, filter_params, iterations)
% Creates binary mask based on SNR estimate of the signal
%   [mask] = estimate_mask(s, filter_params, iterations)
%     s              Source signal
%     filter_params  Filter parameters for smoothing variance estimate
%     iterations     Number of iterations for estimating noise variance

% Copyright (C) 2004, 2005 DSS MATLAB package team (dss@cis.hut.fi).
% Distributed by Laboratory of Computer and Information Science,
% Helsinki University of Technology. http://www.cis.hut.fi/projects/dss/.
% $Id$

filter_h = @denoise_filter;

if nargin<2;
    % default filtering is lowpass DCT
    T = length(s);
    t = 1:T;
    filter_params.filter_dct = exp(-0.5*(t-1).^2 / (T/64).^2 );
end
if nargin<3; iterations = 8; end

% noise variance estimate for gaussian noise
[p, var_smooth] = feval(filter_h, filter_params, randn(1, length(s)).^2, []);
noise_var = est_noise_var(var_smooth, 0);
normalization_c = 1 / noise_var;

% smoothed variance
[p, var_totsm] = feval(filter_h, filter_params, s.^2, []);

% iterate signal noise variance estimate
var_noise = 1;
for i = 1 : iterations
  var_noise = est_noise_var(var_totsm, var_noise)*normalization_c;
end

% create binary mask
mask = var_totsm>var_noise;

%DEBUG
%fprintf('Noise: %d\n', var_noise);
%clf
%subplot(3, 1, 1);
%plot(s);
%subplot(3, 1, 2);
%plot(var_totsm);
%subplot(3, 1, 3);
%plot(mask);
%axis([0 length(s) -0.5 1.5])

% --------
function noise_var = est_noise_var(var_tot, var_noise)
% Estimates noise variance based on total variance estimate and
% previous noise variance estimate.
noise_var = exp(mean(log(var_tot+repmat(var_noise,1,size(var_tot,2))),2))-var_noise;


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
%
% function [y,dy] = affine3D(w,x,varargin)
%
% computes y = Q*w and the derivative wrt. w.
% x = reshape(x,[],3);
% Q = [x(:,1),x(:,2),x(:,3),1, 0      0        0      0  0      0        0
%      0      0      0,     0, x(:,1),x(:,2,1),x(:,3),1, 0      0        0
%      0      0      0,     0, 0      0        0      0, x(:,1),x(:,2,1),x(:,3),1]
% dy = Q:
% if no arguments are given, the parameters for the identity map are returned.
%
% see also transformations/contents.m, trafo.m
%==============================================================================

function [y,dy] = affine3D(w,x,varargin)

% the persitent variable stores the matrix
% Q(x) = kron( I_2 , [x(:,1),x(:,2),1] );
persistent Q

if nargin==0
    help(mfilename)
    runMinimalExample;
    return;
else
  y = mfilename('fullfile');
  dy = reshape(eye(4,3),[],1);         % parameterization of identity
  if ischar(w),  Q  = []; w = []; end; % reset Q
  if isempty(w), return;          end;
end;

if isempty(w) || (size(Q,1) ~= numel(x)),
  n = length(x)/3; x = reshape(x,n,3);
  Q = sparse(kron(speye(3),[x,ones(n,1)]));
  if nargout == 0, return; end;
end;
y  = Q*w;
dy = Q;

%------------------------------------------------------------------------------
function runMinimalExample
fprintf('%s: minimal example\n',mfilename)

omega = [0,10,0,8,0,6]; m = [8,7,6];
w = 22/pi;c = (omega(2:2:end)-omega(1:2:end))'/2;
R = [ cos(w),-sin(w),0;sin(w),cos(w),0;0,0,1];
g = (eye(3)-R)*reshape(c,[],1);
w = reshape([R,g]',[],1)
x = getNodalGrid(omega,m);
z = feval(mfilename,w,x);
FAIRfigure(1); clf;
plotGrid(x,omega,m,'color','r'); axis image; hold on;
plotGrid(z,omega,m,'color','b'); axis image; hold off;

fctn = @(w) feval(mfilename,w,x);
w = w + randn(size(w));
checkDerivative(fctn,w,'fig',2);

%==============================================================================


function [y,deriv] = cllr_obj(w,T,weights,logit_prior)
% This is an MV2DF. See MV2DF_API_DEFINITION.readme.
%
% Weighted binary classifier cross-entropy objective, based on logarithmic
% cost function.
%
%  Differentiable inputs:
%   w: is vector of N detection scores (in log-likelihood-ratio format)
%
%  Fixed parameters:
%   T: is vector of N labels: 1 for target and -1 for non-target.
%   weights: is N-vector of objective function weights, one per trial.
%   logit_prior: is logit(prior), this controls the region of interest

if nargin==0
    test_this();
    return;
end

if isempty(w)
    y = @(w)cllr_obj(w,T,weights,logit_prior);
    return;
end

if isa(w,'function_handle')
    outer = cllr_obj([],T,weights,logit_prior);
    y = compose_mv(outer,w,[]);
    return;
end

w = w(:);
scores = w.';
arg = bsxfun(@plus,scores,logit_prior).*T;
neglogp1 = neglogsigmoid(arg); % 1*N           p1 = p(tar)
y = neglogp1*weights(:);

if nargout>1
    neglogp2 = neglogsigmoid(-arg); % 1*N      p2 = 1-p1 = p(non)
    deriv = @(dy) deriv_this(dy,-neglogp1(:),-neglogp2(:),T(:),weights(:));
end

function [g,hess,linear] = deriv_this(dy,logp1,logp2,T,weights)
g0 = -exp(logp2).*weights.*T;
g = dy*g0;
linear = false;
hess = @(d) hessianprod(d,dy,g0,logp1,logp2,weights);

function [h,Jv] = hessianprod(d,dy,g0,logp1,logp2,weights)

h = dy*(exp(logp1+logp2).*weights(:).*d(:));

if nargout>1
    Jv = d.'*g0;
end

function test_this()
N = 30;
T = [ones(1,N/3),-ones(1,N/3),zeros(1,N/3)];
W = randn(1,N);
weights = [rand(1,2*N/3),zeros(1,N/3)];
f = @(w) cllr_obj(w,T,weights,-2.23);
test_MV2DF(f,W(:));


function value = legendre_integral ( d )

%*****************************************************************************80
%
%% LEGENDRE_INTEGRAL returns the Legendre integral of a test function.
%
%  Discussion:
%
%    The same function, integrated over [-1,+1]^D, has an integral that
%    is a factor of 2^D times as large as this result.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 May 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer D, the spatial dimension.
%
%    Output, real VALUE, the value of the integral.
%
  value = ( 0.5 * erf ( 0.5 / sqrt ( 2.0 ) ) ) .^ d;

  return
end


function dz = singlePendulumRhs(~,z,g,l)
% This function is used inside of ode45 for running a simulation of a
% single pendulum. The first argument (time) is not used.

th = z(1,:);
w = z(2,:);

dth = w;
dw = singlePendulumDynamics(th,g,l);

dz = [dth;dw];

end


function [L,U,P] = lu_rightp (A)
%LU_RIGHTP right-looking LU factorization, with partial pivoting.
%
% Example:
%   [L,U,P] = lu_rightp (A)
% See also: cs_demo

%   Copyright 2006-2007, Timothy A. Davis.
%   http://www.cise.ufl.edu/research/sparse

n = size (A,1) ;
P = eye (n) ;
for k = 1:n
    [x,i] = max (abs (A (k:n,k))) ;                           % partial pivoting
    i = i+k-1 ;
    P ([k i],:) = P ([i k], :) ;
    A ([k i],:) = A ([i k], :) ;                              % (6.10), (6.11)
    A (k+1:n,k) = A (k+1:n,k) / A (k,k) ;                               % (6.12)
    A (k+1:n,k+1:n) = A (k+1:n,k+1:n) - A (k+1:n,k) * A (k,k+1:n) ;     % (6.9)
end
L = tril (A,-1) + eye (n) ;
U = triu (A) ;


function [ f, rank ] = rgf_successor ( m, f, rank )

%*****************************************************************************80
%
%% RGF_SUCCESSOR generates the next restricted growth function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 January 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Donald Kreher, Douglas Simpson,
%    Combinatorial Algorithms,
%    CRC Press, 1998,
%    ISBN: 0-8493-3988-X,
%    LC: QA164.K73.
%
%  Parameters:
%
%    Input, integer M, the domain of the RGF is the integers
%    from 1 to M.  M must be positive.
%
%    Input/output, integer F(M), the restricted growth function.
%
%    Input/output, integer RANK, the rank.
%    If RANK = -1 on input, then the routine understands that this is
%    the first call, and that the user wishes the routine to supply
%    the first element in the ordering, which has RANK = 0.
%    In general, the input value of RANK is increased by 1 for output,
%    unless the very last element of the ordering was input, in which
%    case the output value of RANK is 0.
%

%
%  Return the first element.
%
  if ( rank == -1 )
    f(1:m) = 1;
    rank = 0;
    return
  end
%
%  Check.
%
  ierror = rgf_check ( m, f );

  if ( ierror ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'RGF_SUCCESSOR - Fatal error!\n' );
    fprintf ( 1, '  The input array is illegal!\n' );
    fprintf ( 1, '  IERROR = %d\n', ierror );
    error ( 'RGF_SUCCESSOR - Fatal error!\n' );
  end
%
%  Find the first position from the right which can be incremented.
%
  for i = m : -1 : 2

    fmax = 1;
    for j = 2 : i - 1
      fmax = max ( fmax, f(j) );
    end
%
%  Increment the function at this position, and set later entries to 1.
%
    if ( f(i) ~= fmax + 1 )
      f(i) = f(i) + 1;
      f(i+1:m) = 1;
      rank = rank + 1;
      return
    end

  end
%
%  The final element was input.
%  Return the first element.
%
  f(1:m) = 1;
  rank = 0;

  return
end


function fem1d_bvp_linear_test03 ( )

%*****************************************************************************80
%
%% FEM1D_BVP_LINEAR_TEST03 carries out test case #3.
%
%  Location:
%
%    http://people.sc.fsu.edu/~jburkardt/m_src/fem1d_bvp_linear/fem1d_bvp_linear_test03.m
%
%  Discussion:
%
%    Use A3, C3, F3, EXACT3, EXACT_UX3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Dianne O'Leary,
%    Scientific Computing with Case Studies,
%    SIAM, 2008,
%    ISBN13: 978-0-898716-66-5,
%    LC: QA401.O44.
%
  n = 11;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'FEM1D_BVP_LINEAR_TEST03\n' );
  fprintf ( 1, '  Solve -( A(x) U''(x) )'' + C(x) U(x) = F(x)\n' );
  fprintf ( 1, '  for 0 < x < 1, with U(0) = U(1) = 0.\n' );
  fprintf ( 1, '  A3(X)  = 1.0\n' );
  fprintf ( 1, '  C3(X)  = 2.0 * X\n' );
  fprintf ( 1, '  F3(X)  = - X * ( 2 * X * X - 3 * X - 3 ) * exp ( X )\n' );
  fprintf ( 1, '  U3(X)  = X * ( 1 - X ) * exp ( X )\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of nodes = %d\n', n );
%
%  Geometry definitions.
%
  x_first = 0.0;
  x_last = 1.0;
  x = linspace ( x_first, x_last, n );
  x = x(:);

  u = fem1d_bvp_linear ( n, @a3, @c3, @f3, x );

  uexact = exact3 ( x );

  fprintf ( 1, '\n' );
  fprintf ( 1, '     I    X         U         Uexact    Error\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    fprintf ( 1, '  %4d  %8f  %8f  %8f  %8e\n', ...
      i, x(i), u(i), uexact(i), abs ( u(i) - uexact(i) ) );
  end

  e1 = l1_error ( n, x, u, @exact3 );
  e2 = l2_error_linear ( n, x, u, @exact3 );
  h1s = h1s_error_linear ( n, x, u, @exact_ux3 );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  l1 norm of error  = %g\n', e1 );
  fprintf ( 1, '  L2 norm of error  = %g\n', e2 );
  fprintf ( 1, '  Seminorm of error = %g\n', h1s  );

  return
end
function value = a3 ( x )

%*****************************************************************************80
%
%% A3 evaluates A function #3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the value of A(X).
%
  value = 1.0;

  return
end
function value = c3 ( x )

%*****************************************************************************80
%
%% C3 evaluates C function #3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 March 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the value of C(X).
%
  value = 2.0 * x;

  return
end
function value = exact3 ( x )

%*****************************************************************************80
%
%% EXACT3 evaluates exact solution #3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 March 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the value of U(X).
%
  value = x .* ( 1.0 - x ) .* exp ( x );

  return
end
function value = exact_ux3 ( x )

%*****************************************************************************80
%
%% EXACT_UX3 evaluates the derivative of exact solution #3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 February 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the value of dUdX(X).
%
  value = ( 1.0 - x - x .* x ) .* exp ( x );

  return
end
function value = f3 ( x )

%*****************************************************************************80
%
%% F3 evaluates right hand side function #3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 March 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the value of F(X).
%
  value = - x .* ( 2.0 * x .* x - 3.0 * x - 3.0 ) .* exp ( x );

  return
end


% pop_kaiserbeta() - Estimate Kaiser window beta
%
% Usage:
%   >> [beta, dev] = pop_kaiserbeta; % pop-up window mode
%   >> beta = pop_kaiserbeta(dev);
%
% Inputs:
%   dev       - scalar maximum passband deviation/ripple
%
% Output:
%   beta      - scalar Kaiser window beta
%   dev       - scalar maximum passband deviation/ripple
%
% References:
%   [1] Proakis, J. G., & Manolakis, D. G. (1996). Digital Signal
%       Processing: Principles, Algorithms, and Applications (3rd ed.).
%       Englewood Cliffs, NJ: Prentice-Hall
%
% Author: Andreas Widmann, University of Leipzig, 2005
%
% See also:
%   pop_firws, firws, pop_firwsord, windows

%123456789012345678901234567890123456789012345678901234567890123456789012

% Copyright (C) 2005 Andreas Widmann, University of Leipzig, widmann@uni-leipzig.de
%
% 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 should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

function [beta, dev] = pop_kaiserbeta(dev)

    beta = [];

    if nargin < 1 || isempty(dev)
        drawnow;
        uigeom = {[1 1]};
        uilist = {{'style' 'text' 'string' 'Max passband deviation/ripple:'} ...
                  {'style' 'edit' 'string' ''}};
        result = inputgui(uigeom, uilist, 'pophelp(''pop_kaiserbeta'')', 'Estimate Kaiser window beta -- pop_kaiserbeta()');
        if length(result) == 0, return, end
        if ~isempty(result{1})
            dev = str2num(result{1});
        else
            error('Not enough input arguments.');
        end
    end

    devdb = -20 * log10(dev);
    if devdb > 50
        beta = 0.1102 * (devdb - 8.7);
    elseif devdb >= 21
        beta = 0.5842 * (devdb - 21)^0.4 + 0.07886 * (devdb - 21);
    else
        beta = 0;
    end

end