function [x_best, psi_best, out] = LevMar(mapp, lin_sym_solver, x0, options)
% LevMar is a Levenberg-Marquardt trust-region algorithm for solving
% systems of nonlinear equations :math:`h(x) = 0`, `x` in :math:`R^m`
% using the nonlinear unconstrained minimization :math:`\textrm{min}\ \psi(x) = 1/2 ||h(x)||^2`
% s.t. `x` in :math:`R^m`.
%
% USAGE:
%
%    [x_best, psi_best, out] = LevMar(mapp, lin_sym_solver, x0, options)
%
% INPUTS:
%    mapp:              function handle provides `h(x)` and gradient `h(x)`
%    lin_sym_solver:    function handle for solving the linear system
%    x0:                initial point
%    options:           structure including the parameteres of scheme
%
%                         * .eta - parameter of the scheme
%                         * .MaxNumIter - maximum number of iterations
%                         * .MaxNumMapEval - maximum number of function evaluations
%                         * .MaxNumGmapEval - maximum number of subgradient evaluations
%                         * .TimeLimit - maximum running time
%                         * .epsilon - accuracy parameter
%                         * .x_opt - optimizer
%                         * .psi_opt - optimum
%                         * .adaptive - update lambda adaptively
%                         * .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:`||grad|| \leq \epsilon`
%                           2. stop if :math:`||nhxk|| \leq \epsilon`
%                           3. stop if `MaxNumIter` is reached
%                           4. stop if `MaxNumMapEval` is reached
%                           5. stop if `MaxNumGmapEval` is reached
%                           6. stop if `TimeLimit` is reached
%                           7. stop if :math:`||grad|| \leq \textrm{max}(\epsilon, \epsilon^2 * ngradx0)`
%                           8. stop if :math:`||nhxk|| \leq \textrm{max}(\epsilon, \epsilon^2 * nhx0)`
%                           9. stop if (default) :math:`||hxk|| \leq \epsilon` or `MaxNumIter` is reached
%
% OUTPUT:
%    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
%                         * .Ngmap - total number of mapping gradient evaluations
%                         * .merit_func - array including all merit function values
%                         * .x_error - relative error :math:`\textrm{norm}(x_k(:)-x_{opt}(:))/\textrm{norm}(x_{opt})`
%                         * .psi_error - relative error :math:`(\psi_k-\psi_{opt})/(\psi_0-\psi_{opt}))`
%                         * .Status - reason of termination
%
% .. REFERENCE:
% .. 1. I.C.F. Ipsen, C.T. Kelley, S.R. Pope, Rank-deficient nonlinear least squares problems and subset selection, SIAM Journal on Numerical Analysis, 49(3), 1244-1266 (2011
% .. 2. C.T. Kelley, Iterative Methods for Optimization. SIAM Press, Philadelphia, 1999.
% .. 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 > 4
    error('The number of input arguments is more than what is needed');
elseif nargin < 4
    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(lin_sym_solver)
    error('the function handle lin_sym_solver has to be defined');
elseif ~isa(lin_sym_solver,'function_handle')
    error('lin_sym_solver 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" =====
[eta,epsilon,MaxNumIter,MaxNumMapEval,MaxNumGmapEval,adaptive, ...
    TimeLimit,flag_x_error,flag_psi_error,flag_time,Stopping_Crit] ...
    = Initialization(options);

if ~isa(eta,'numeric') || (eta <= 0)
    error('eta should be numeric and eta in (0,4*delta)');
end

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

mu0        = 1e-4;
mulow      = 0.25;
muhigh     = 0.75;
wdown      = 0.5;
wup        = 2;
nu0        = 1e-3;

xk         = x0;
Niter      = 1;
[hxk,ghxk] = mapp(x0);
Nmap       = 1;
Ngmap      = 1;
grad       = ghxk*hxk;
ngradx0    = norm(grad);
nhx0       = norm(hxk);
nhxk       = nhx0;
I          = eye(length(xk));
psik       = 0.5*nhxk^2;
merit_func = psik;
nuk        = ngradx0;
StopFlag   = 0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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

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

    Gk    = ghxk*ghxk';
    Hk    = Gk+nuk*I;
    dk    = lin_sym_solver(Hk,grad);
    xk1   = xk+dk;
    hxk1  = mapp(xk1);

    Nmap  = Nmap+1;

    flag   = 0;
    initer = 0;
    while ~flag
        nhxk1  = norm(hxk1);
        psik1 = 0.5*nhxk1^2;
        ared  = psik-psik1;
        pred  = -0.5*(grad'*dk);
        rk    = ared/pred;
        if rk < mu0
            nuk    = max(nuk*wup,nu0);
		    Hk     = Gk+nuk*I;
            dk     = lin_sym_solver(Hk,grad);
            xk1    = xk+dk;
            hxk1   = mapp(xk1);
            Nmap   = Nmap+1;
            initer = initer+1;
            if initer>30
                flag = 1;
            end
	    elseif rk < mulow
	    	xk    = xk1;
            Niter = Niter+1;
            nuk   = max(nuk*wup,nu0);
            flag  = 1;
        else
		    xk    = xk1;
            Niter = Niter+1;
            if rk > muhigh
		        nuk = wdown*nuk;
                %if nuk < 10^(-3)
                if nuk < 10^(-3)
                    nuk =1e-8;
                end
            end
            flag = 1;
        end
    end

    [hxk,ghxk] = mapp(xk);
    Nmap       = Nmap+1;
    grad       = ghxk*hxk;
    nhxk       = norm(hxk);

    % ================= Gathering output information ===================
    psik              = 0.5*nhxk^2;
    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] = StopCriterion(grad,nhxk,Niter,Nmap, ...
    Ngmap,MaxNumIter,MaxNumMapEval,MaxNumGmapEval,T,TimeLimit, ...
    epsilon,nhx0,ngradx0,Stopping_Crit);

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Outputs %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Status
x_best         = xk;
psi_best       = psik;
out.T          = T;
out.nhx        = nhxk;
out.merit_func = merit_func';
out.Niter      = Niter;
out.Nmap       = Nmap;
out.Ngmap      = Ngmap;
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 LevMar.m %%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


function [w, W_f, W_v] = fromFrameVec(F,v)

% FROMFRAMEVEC  From frame function for vectors.
%   FROMFRAMEVEC(F,V) transforms the 3d vector V from frame F to the global
%   frame.
%
%   [w, W_f, W_v] = FROMFRAMEVEC(...) returns the Jacobians wrt F and V.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

w = F.R*v;

if nargout > 1   % Jacobians.

    if size(v,2) == 1

        s = 2*q2Pi(F.q)*v;

        W_q = [...
            s(2) -s(1)  s(4) -s(3)
            s(3) -s(4) -s(1)  s(2)
            s(4)  s(3) -s(2) -s(1)];
        W_v = F.R;
        W_f = [zeros(3) W_q];

    else % multiple points
        error('??? Can''t give Jacobians for multiple points');
    end
end

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function [At,b,Z] = getequation(symexpr,vartable,decvartable,varmat)
%function [At,b,Z] = getequation(symexpr,vartable,decvartable,decvartablename)
%
% GETEQUATION --- Convert a symbolic expression to At, b, and Z
%         used in an SOS program.

% This file is part of SOSTOOLS - Sum of Squares Toolbox ver 3.00.
%
% Copyright (C)2002, 2004, 2013  A. Papachristodoulou (1), J. Anderson (1),
%                                G. Valmorbida (1), S. Prajna (2),
%                                P. Seiler (3), P. A. Parrilo (4)
% (1) Department of Engineering Science, University of Oxford, Oxford, U.K.
% (2) Control and Dynamical Systems - California Institute of Technology,
%     Pasadena, CA 91125, USA.
% (3) Aerospace and Engineering Mechanics Department, University of
%     Minnesota, Minneapolis, MN 55455-0153, USA.
% (4) Laboratory for Information and Decision Systems, M.I.T.,
%     Massachusetts, MA 02139-4307
%
% Send bug reports and feedback to: sostools@cds.caltech.edu
%
% 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, see <http://www.gnu.org/licenses/>.
%

% 03/01/02 - SP
% 03/10/02 - SP -- Use Maple
% 07/10/13 - JA&GV -- Use the matlab's symbolic engine
% 09/11/13 - PJS -- Addition for multipoly objects

if isa(symexpr,'polynomial')
    % PJS: Handle Polynomial Objects including the Matrix Case
    decvarnum = numel(decvartable);
    vartable = [vartable; varmat];
    cvartable = char(vartable);
    dimp = size(symexpr,2);

    % Collect symexpr = g(c)*h(x) where x are the vars in vartable,
    % c are the decision variables, and h(x) is a vector of monoms.
    % in the actual polynomial vars.  Use this to find unique
    % monomials in the polynomial variables.
    [g0,g,h] = collect(symexpr(:),setdiff(symexpr.varname,cvartable));
    g = g(:);
    if ~isequal(g0,0)
        g = [g0;g];
        h = [1; h];
    end
    [nmon,nvar] = size(h.degmat);

    % Reorder the monomials matrix with variables in the order
    % listed in cvartable and sorted monomials
    Z = zeros(nmon,nvar);
    [~,idx]=ismember(h.varname,cvartable);
    Z(:,idx) = full(h.degmat);
    Z = sortNoRepeat([],Z);

    % Initialize coefficient variables
    [nmon,nvar] = size(Z);
    coeffnts = sparse(nmon*dimp,dimp);
    if decvarnum>0
        coeffnts_decvar = polynomial(sparse(nmon*dimp,dimp));
    end

    % Define coefficients
    for i = 1:dimp
        for j = i:dimp
            exprij = symexpr(i,j);
            [g0,g,h] = collect(exprij,setdiff(exprij.varname,cvartable));
            g = g(:);
            if ~isequal(g0,0)
                g = [g0;g];
                h = [1; h];
            end
            coefmatr = double(subs(g,decvartable,zeros(decvarnum,1)));
            monmatr = zeros(length(h),nvar);
            [~,idx]=ismember(h.varname,cvartable);
            monmatr(:,idx) = h.coef'*h.degmat;

            for k = 1:length(h);
                s_ijk = coefmatr(k,1);
                s_ijk_decvar = g(k);
                mon_k= monmatr(k,:);

                [val,ind_k] = max(sum((Z == kron(ones(nmon,1),mon_k)),2));
                coeffnts((ind_k-1)*dimp+i,j) = s_ijk;
                coeffnts((ind_k-1)*dimp+j,i) = s_ijk;

                coeffnts_decvar((ind_k-1)*dimp+i,j) = s_ijk_decvar;
                coeffnts_decvar((ind_k-1)*dimp+j,i) = s_ijk_decvar;
            end
        end
    end

    % Constructing At and b matrices
    At = sparse(decvarnum,size(Z,1)*dimp^2);
    if decvarnum>0
        for i=1:nmon
            Mivec = reshape(coeffnts_decvar((i-1)*dimp+1:i*dimp,:),dimp^2,1);
            At(:,(i-1)*dimp^2+1:i*dimp^2) = sparse(-double(jacobian(Mivec,decvartable))');
        end
    end
    b = sparse(coeffnts);

else
    % PJS: Original Code to Handle Symbolic Objects

    if strcmp(decvartable,'[]');
        decvarnum = 0;
    else
        decvarnum = length(find(decvartable == ','))+1;
    end;
    expr = evalin(symengine,symexpr);
    %vartable = evalin(symengine,vartable);
    if  length(varmat)==2
        vartable = evalin(symengine,vartable);
    else
        vartable = evalin(symengine,[vartable(1:end-1),',',varmat(2:end)]);  %JA&GV 07/06/13
    end

    decvartable = evalin(symengine,decvartable);
    charvartable = converttochar([vartable]);

    FPexpr = feval(symengine,'expand',expr);

    %JA&GV 07/13 commented the below to implement the matrix case
    %FPexpr = feval(symengine,'collect',FPexpr,charvartable);

    dimp = size(FPexpr,2);%JA&GV jul/04/2013 sets the dimension of the matrix of polynomials

    for i = 1:size(FPexpr,1)
        for j = 1:dimp
            FPexpr(i,j) = feval(symengine,'collect',FPexpr(i,j),charvartable);
        end
    end

    if isempty(decvartable)==0%JA&GV 07/13 the code below  addresses the matrix case

        Zfull = [];
        for i = 1:dimp
            for j = i:dimp
                coefmon = feval(symengine,'poly2list',FPexpr(i,j),charvartable);
                coefmonmatr = subs(coefmon.',decvartable,zeros(1,length(decvartable)));
                Z = double(coefmonmatr(:,2:end));
                Zfull = sortNoRepeat(Zfull,Z);
            end
        end
        Z = Zfull;
        [nmon,nvar] = size(Z);
        coeffnts = sparse(nmon*dimp,dimp);
        coeffnts_decvar = sym(sparse(nmon*dimp,dimp));

        for i = 1:dimp
            for j = i:dimp
                coefmon = feval(symengine,'poly2list',FPexpr(i,j),charvartable);
                coefmonmatr = subs(coefmon.',decvartable,zeros(1,length(decvartable)));
                for k = 1:size(coefmonmatr,1)
                    s_ijk = coefmonmatr(k,1);

                    dummy_decvar = reshape(coefmon(k),2,1);
                    s_ijk_decvar = dummy_decvar;

                    mon_k= double(coefmonmatr(k,2:end));
                    [val,ind_k] = max(sum((Zfull == kron(ones(nmon,1),mon_k)),2));
                    coeffnts((ind_k-1)*dimp+i,j) = s_ijk;
                    coeffnts((ind_k-1)*dimp+j,i) = s_ijk;

                    coeffnts_decvar((ind_k-1)*dimp+i,j) = s_ijk_decvar;
                    coeffnts_decvar((ind_k-1)*dimp+j,i) = s_ijk_decvar;

                end

            end
        end

    else%JA&GV 07/2013 the code below  addresses the matrix case

        Zfull = [];
        for i = 1:dimp
            for j = i:dimp
                coefmon = feval(symengine,'poly2list',FPexpr(i,j),charvartable);
                coefmon = coefmon.';
                for k = 1:length(coefmon)
                    dummyvar = reshape(coefmon(k),2,1);
                    Z(k,:) = double(dummyvar(2));
                end
                Zfull = sortNoRepeat(Zfull,Z);
                Z = [];
            end
        end
        Z = sparse(Zfull);
        [nmon,nvar] = size(Z);
        coeffnts = sparse(nmon*dimp,dimp);

        for i = 1:dimp
            for j = i:dimp
                coefmon = feval(symengine,'poly2list',FPexpr(i,j),charvartable);
                coefmon = coefmon.';
                for k = 1:length(coefmon)
                    dummyvar = reshape(coefmon(k),2,1);
                    s_ijk = double(dummyvar(1));
                    mon_k= double(dummyvar(2));
                    [val,ind_k] = max(sum((Zfull == kron(ones(nmon,1),mon_k))'));
                    coeffnts((ind_k-1)*dimp+i,j) = s_ijk;
                    coeffnts((ind_k-1)*dimp+j,i) = s_ijk;
                end
            end
        end

    end

    % Constructing At and b matrices
    At = sparse(decvarnum,size(Z,1)*dimp^2);

    %JA&GV may/2013 uses the jacobian function to derive the expression
    if isempty(decvartable)==0
        for i = 1:size(Z,1)
            Mivec = reshape(coeffnts_decvar((i-1)*dimp+1:i*dimp,:),dimp^2,1);
            At(:,(i-1)*dimp^2+1:i*dimp^2) = sparse(-double(jacobian(Mivec,decvartable))');
        end
    end

    b = sparse(coeffnts);
end


%% FUNCTION Logistic_Lasso
%   Sparse Structure-Regularized Learning with Logistic Loss.
%
%% OBJECTIVE
%   argmin_{W,C} { sum_i^t (- sum(log (1./ (1+ exp(-X{i}*W(:, i) - Y{i} .* C(i)))))/length(Y{i}))
%            + rho1 * \|W\|_1 + opts.rho_L2 * \|W\|_F^2}
%
%% INPUT
%   X: {n * d} * t - input matrix
%   Y: {n * 1} * t - output matrix
%   rho1: sprasity controlling parameter
%   opts.rho_L2: L2-norm regularization parameter
%
%% OUTPUT
%   W: model: d * t
%   C: model: 1 * t
%   funcVal: function value vector.
%
%% LICENSE
%   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/>.
%
%   Copyright (C) 2011 - 2012 Jiayu Zhou and Jieping Ye
%
%   You are suggested to first read the Manual.
%   For any problem, please contact with Jiayu Zhou via jiayu.zhou@asu.edu
%
%   Last modified on June 3, 2012.
%
%% Related papers
%
%   [1] Tibshirani, J. Regression shrinkage and selection via
%   the Lasso, Journal of the Royal Statistical Society. Series B 1996
%
%% Related functions
%   Least_Lasso, init_opts

%% Code starts here
function [W, C, funcVal] = Logistic_Lasso(X, Y, rho1, opts)

if nargin <3
    error('\n Inputs: X, Y, and rho1 should be specified!\n');
end
X = multi_transpose(X);

if nargin <4
    opts = [];
end

% initialize options.
opts=init_opts(opts);

if isfield(opts, 'rho_L2')
    rho_L2 = opts.rho_L2;
else
    rho_L2 = 0;
end

task_num  = length (X);
dimension = size(X{1}, 1);
funcVal = [];

%initialize a starting point
C0_prep = zeros(1, task_num);
for t_idx = 1: task_num
    m1 = nnz(Y{t_idx} == 1);
    m2 = nnz(Y{t_idx} == -1);
    if ( m1==0 || m2==0 )
        C0_prep(t_idx) = 0;
    else
        C0_prep(t_idx) = log(m1/m2);
    end
end

if opts.init==2
    W0 = zeros(dimension, task_num);
    %C0 = zeros(1, task_num);
    C0 = C0_prep;
elseif opts.init== 0
    W0 = randn(dimension, task_num);
    C0 = C0_prep;
else
    if isfield(opts,'W0')
        W0=opts.W0;
        if (nnz(size(W0)-[dimension, task_num]))
            error('\n Check the input .W0');
        end
    else
        W0 = zeros(dimension, task_num);
    end
    if isfield(opts,'C0')
        C0=opts.C0;
    else
        C0=C0_prep;
    end
end

bFlag=0; % this flag tests whether the gradient step only changes a little

Wz= W0;
Cz= C0;
Wz_old = W0;
Cz_old = C0;

t = 1;
t_old = 0;
iter = 0;
gamma = 1;
gamma_inc = 2;

while iter < opts.maxIter
    alpha = (t_old - 1) /t;

    Ws = (1 + alpha) * Wz - alpha * Wz_old;
    Cs = (1 + alpha) * Cz - alpha * Cz_old;

    %Ws = Wz;
    %Cs = Cz;

    % compute function value and gradients of the search point
    [gWs, gCs, Fs ]  = gradVal_eval(Ws, Cs);

    % the Armijo Goldstein line search scheme
    while true
        [Wzp l1c_wzp] = l1_projection(Ws - gWs/gamma, 2 * rho1 / gamma);
        Czp = Cs - gCs/gamma;
        Fzp = funVal_eval  (Wzp, Czp);

        delta_Wzp = Wzp - Ws;
        delta_Czp = Czp - Cs;
        nrm_delta_Wzp = norm(delta_Wzp, 'fro')^2;
        nrm_delta_Czp = norm(delta_Czp, 'fro')^2;
        r_sum = (nrm_delta_Wzp+nrm_delta_Czp)/2;

        %         Fzp_gamma = Fs + trace(delta_Wzp' * gWs)...
        %             + trace(delta_Czp' * gCs)...
        %             + gamma/2 * nrm_delta_Wzp ...
        %             + gamma/2 * nrm_delta_Czp;

        Fzp_gamma = Fs + sum(sum(delta_Wzp .* gWs))...
            + sum(sum(delta_Czp .* gCs))...
            + gamma/2 * nrm_delta_Wzp ...
            + gamma/2 * nrm_delta_Czp;

        if (r_sum <=1e-20)
            bFlag=1; % this shows that, the gradient step makes little improvement
            break;
        end

        if (Fzp <= Fzp_gamma)
            break;
        else
            gamma = gamma * gamma_inc;
        end
    end

    Wz_old = Wz;
    Cz_old = Cz;
    Wz = Wzp;
    Cz = Czp;

    funcVal = cat(1, funcVal, Fzp + rho1 * l1c_wzp);

    if (bFlag)
        % fprintf('\n The program terminates as the gradient step changes the solution very small.');
        break;
    end

    % test stop condition.
    switch(opts.tFlag)
        case 0
            if iter>=2
                if (abs( funcVal(end) - funcVal(end-1) ) <= opts.tol)
                    break;
                end
            end
        case 1
            if iter>=2
                if (abs( funcVal(end) - funcVal(end-1) ) <=...
                        opts.tol* funcVal(end-1))
                    break;
                end
            end
        case 2
            if ( funcVal(end)<= opts.tol)
                break;
            end
        case 3
            if iter>=opts.maxIter
                break;
            end
    end

    iter = iter + 1;
    t_old = t;
    t = 0.5 * (1 + (1+ 4 * t^2)^0.5);

end

W = Wzp;
C = Czp;

% private functions

    function [z, l1_comp_val] = l1_projection (v, beta)
        % this projection calculates
        % argmin_z = \|z-v\|_2^2 + beta \|z\|_1
        % z: solution
        % l1_comp_val: value of l1 component (\|z\|_1)

        z = sign(v).*max(0,abs(v)- beta/2);

        l1_comp_val = sum(sum(abs(z)));
    end

    function [grad_W, grad_C, funcVal] = gradVal_eval(W, C)
        grad_W = zeros(dimension, task_num);
        grad_C = zeros(1, task_num);
        lossValVect = zeros (1 , task_num);

        for i = 1:task_num
            [ grad_W(:, i), grad_C(:, i), lossValVect(:, i)] = unit_grad_eval( W(:, i), C(i), X{i}, Y{i});
        end

        grad_W = grad_W + rho_L2 * 2 * W;
        % here when computing function value we do not include
        % l1 norm.
        funcVal = sum(lossValVect) + rho_L2 * norm(W, 'fro')^2;
    end

    function [funcVal] = funVal_eval (W, C)
        funcVal = 0;
        for i = 1: task_num
            funcVal = funcVal + unit_funcVal_eval( W(:, i), C(i), X{i}, Y{i});
        end

        % here when computing function value we do not include
        % l1 norm.
        funcVal = funcVal + rho_L2 * norm(W, 'fro')^2;
    end

end

function [ grad_w, grad_c, funcVal ] = unit_grad_eval( w, c, x, y)
%gradient and logistic evaluation for each task
m = length(y);
weight = ones(m, 1)/m;
weighty = weight.* y;
aa = -y.*(x'*w + c);
bb = max( aa, 0);
%funcVal = weight' * log(exp(-bb) + exp(aa-bb) + bb); %bug!!!
funcVal = weight'* ( log( exp(-bb) +  exp(aa-bb) ) + bb );
pp = 1./ (1+exp(aa));
b = -weighty.*(1-pp);
grad_c = sum(b);
grad_w = x * b;
end

function [ funcVal ] = unit_funcVal_eval( w, c, x, y)
%function value evaluation for each task
m = length(y);
weight = ones(m, 1)/m;
aa = -y.*(x'*w + c);
bb = max( aa, 0);
%funcVal = weight' * log(exp(-bb) + exp(aa-bb) + bb); % BUG!!!!
funcVal = weight'* ( log( exp(-bb) +  exp(aa-bb) ) + bb );
end


function [eV] = Nm2eV(Nm)
% Convert energy or work from newton-meters to electron volts.
% Chad A. Greene 2012
eV = Nm*6241506480000000000;


function sparse_grid_mixed_write ( dim_num, rule, alpha, beta, point_num, ...
  sparse_weight, sparse_point, file_name )

%*****************************************************************************80
%
%% SPARSE_GRID_MIXED_WRITE writes a sparse grid rule to five files.
%
%  Discussion:
%
%    The files are:
%    * the "A" file stores the ALPHA values, as a DIM_NUM x 1 list.
%    * the "B" file stores the BETA values, as a DIM_NUM x 1 list.
%    * the "R" file stores the region, as a DIM_NUM x 2 list.
%    * the "W" file stores the weights as a POINT_NUM list;
%    * the "X" file stores the abscissas as a DIM_NUM x POINT_NUM list;
%
%    The entries in the "R" file are the two corners of the DIM_NUM dimensional
%    rectangle that constitutes the integration region.  Coordinates that
%    should be infinite are set to 1.0E+30.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 March 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Fabio Nobile, Raul Tempone, Clayton Webster,
%    A Sparse Grid Stochastic Collocation Method for Partial Differential
%    Equations with Random Input Data,
%    SIAM Journal on Numerical Analysis,
%    Volume 46, Number 5, 2008, pages 2309-2345.
%
%  Parameters:
%
%    Input, int DIM_NUM, the spatial dimension.
%
%    Input, integer RULE(DIM_NUM), the rule in each dimension.
%     1, "CC",  Clenshaw Curtis, Closed Fully Nested rule.
%     2, "F2",  Fejer Type 2, Open Fully Nested rule.
%     3, "GP",  Gauss Patterson, Open Fully Nested rule.
%     4, "GL",  Gauss Legendre, Open Weakly Nested rule.
%     5, "GH",  Gauss Hermite, Open Weakly Nested rule.
%     6, "GGH", Generalized Gauss Hermite, Open Weakly Nested rule.
%     7, "LG",  Gauss Laguerre, Open Non Nested rule.
%     8, "GLG", Generalized Gauss Laguerre, Open Non Nested rule.
%     9, "GJ",  Gauss Jacobi, Open Non Nested rule.
%    10, "GW",  Golub Welsch, (presumed) Open Non Nested rule.
%    11, "CC_SE", Clenshaw Curtis Slow Exponential, Closed Fully Nested rule.
%    12, "F2_SE", Fejer Type 2 Slow Exponential, Closed Fully Nested rule.
%    13, "GP_SE", Gauss Patterson Slow Exponential, Closed Fully Nested rule.
%    14, "CC_ME", Clenshaw Curtis Moderate Exponential, Closed Fully Nested rule.
%    15, "F2_ME", Fejer Type 2 Moderate Exponential, Closed Fully Nested rule.
%    16, "GP_ME", Gauss Patterson Moderate Exponential, Closed Fully Nested rule.
%    17, "CCN", Clenshaw Curtis Nested, Linear, Closed Fully Nested rule.
%
%    Input, real ALPHA(DIM_NUM), BETA(DIM_NUM), parameters used for
%    Generalized Hermite, Generalized Laguerre, and Jacobi rules.
%
%    Input, int POINT_NUM, the number of points in the grid.
%
%    Input, real SPARSE_WEIGHT(POINT_NUM), the weights.
%
%    Input, real SPARSE_POINT(DIM_NUM,POINT_NUM), the points.
%
%    Input, string FILE_NAME, the main part of the file name.
%
  for dim = 1 : dim_num
    if ( rule(dim) == 1 )
      sparse_region(dim,1) = -1.0;
      sparse_region(dim,2) = +1.0;
    elseif ( rule(dim) == 2 )
      sparse_region(dim,1) = -1.0;
      sparse_region(dim,2) = +1.0;
    elseif ( rule(dim) == 3 )
      sparse_region(dim,1) = -1.0;
      sparse_region(dim,2) = +1.0;
    elseif ( rule(dim) == 4 )
      sparse_region(dim,1) = -1.0;
      sparse_region(dim,2) = +1.0;
    elseif ( rule(dim) == 5 )
      sparse_region(dim,1) = - r8_huge ( );
      sparse_region(dim,2) = + r8_huge ( );
    elseif ( rule(dim) == 6 )
      sparse_region(dim,1) = - r8_huge ( );
      sparse_region(dim,2) = + r8_huge ( );
    elseif ( rule(dim) == 7 )
      sparse_region(dim,1) = 0.0;
      sparse_region(dim,2) = r8_huge ( );
    elseif ( rule(dim) == 8 )
      sparse_region(dim,1) = 0.0;
      sparse_region(dim,2) = r8_huge ( );
    elseif ( rule(dim) == 9 )
      sparse_region(dim,1) = -1.0;
      sparse_region(dim,2) = +1.0;
    elseif ( rule(dim) == 10 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'SPARSE_GRID_MIXED_WRITE - Fatal error!\n' );
      fprintf ( 1, '  Do not know how to specify region for rules of type 10.\n');
      error ( 'SPARSE_GRID_MIXED_WRITE - Fatal error!' );
    elseif ( rule(dim) == 11 )
      sparse_region(dim,1) = -1.0;
      sparse_region(dim,2) = +1.0;
    elseif ( rule(dim) == 12 )
      sparse_region(dim,1) = -1.0;
      sparse_region(dim,2) = +1.0;
    elseif ( rule(dim) == 13 )
      sparse_region(dim,1) = -1.0;
      sparse_region(dim,2) = +1.0;
    elseif ( rule(dim) == 14 )
      sparse_region(dim,1) = -1.0;
      sparse_region(dim,2) = +1.0;
    elseif ( rule(dim) == 15 )
      sparse_region(dim,1) = -1.0;
      sparse_region(dim,2) = +1.0;
    elseif ( rule(dim) == 16 )
      sparse_region(dim,1) = -1.0;
      sparse_region(dim,2) = +1.0;
    elseif ( rule(dim) == 17 )
      sparse_region(dim,1) = -1.0;
      sparse_region(dim,2) = +1.0;
    else
      fprintf ( 1, '\n' );
      fprintf ( 1, 'SPARSE_GRID_MIXED_WRITE - Fatal error!\n' );
      fprintf ( 1, '  Unexpected value of RULE = %d\n', rule(dim) );
      error ( 'SPARSE_GRID_MIXED_WRITE - Fatal error!' );
    end
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, 'SPARSE_GRID_MIXED_WRITE:\n' );

  file_name_a = strcat ( file_name, '_a.txt' );
  r8mat_write ( file_name_a, dim_num, 1, alpha );
  fprintf ( 1, '  Wrote the A file = "%s".\n', file_name_a );

  file_name_b = strcat ( file_name, '_b.txt' );
  r8mat_write ( file_name_b, dim_num, 1, beta );
  fprintf ( 1, '  Wrote the B file = "%s".\n', file_name_b );

  file_name_r = strcat ( file_name, '_r.txt' );
  r8mat_write ( file_name_r, dim_num, 2, sparse_region );
  fprintf ( 1, '  Wrote the R file = "%s".\n', file_name_r );

  file_name_w = strcat ( file_name, '_w.txt' );
  r8mat_write ( file_name_w, 1, point_num, sparse_weight );
  fprintf ( 1, '  Wrote the W file = "%s".\n', file_name_w );

  file_name_x = strcat ( file_name, '_x.txt' );
  r8mat_write ( file_name_x, dim_num, point_num, sparse_point );
  fprintf ( 1, '  Wrote the X file = "%s".\n', file_name_x );

  return
end


%%*****************************************************************************
%% HSDsqlp: solve an semidefinite-quadratic-linear program
%%   by infeasible path-following method on the homogeneous self-dual model.
%%
%%  [obj,X,y,Z,info,runhist] =
%%      HSDsqlp(blk,At,C,b,OPTIONS,X0,y0,Z0);
%%
%%  Input: blk: a cell array describing the block diagonal structure of SQL data.
%%          At: a cell array with At{p} = [svec(Ap1) ... svec(Apm)]
%%         b,C: data for the SQL instance.
%%     OPTIONS: a structure that specifies parameters required in HSDsqlp.m,
%%              (if it is not given, the default in sqlparameters.m is used).
%%
%%  (X0,y0,Z0): an initial iterate (if it is not given, the default is used).
%%
%%  Output: obj  = [<C,X> <b,y>].
%%          (X,y,Z): an approximately optimal solution or a primal or dual
%%                   infeasibility certificate.
%%          info.termcode = termination-code
%%          info.iter     = number of iterations
%%          info.obj      = [primal-obj, dual-obj]
%%          info.cputime  = total-time
%%          info.gap      = gap
%%          info.pinfeas  = primal_infeas
%%          info.dinfeas  = dual_infeas
%%          runhist.pobj    = history of primal objective value.
%%          runhist.dobj    = history of dual   objective value.
%%          runhist.gap     = history of <X,Z>.
%%          runhist.pinfeas = history of primal infeasibility.
%%          runhist.dinfeas = history of dual   infeasibility.
%%          runhist.cputime = history of cputime spent.
%%----------------------------------------------------------------------------
%%  The OPTIONS structure specifies the required parameters:
%%      vers  gam  predcorr  expon  gaptol  inftol  steptol
%%      maxit  printlevel  ...
%%      (all have default values set in sqlparameters.m).
%%
%%*************************************************************************
%% SDPT3: version 3.1
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*************************************************************************

function [obj,X,y,Z,info,runhist] = ...
    HSDsqlp(blk,At,C,b,OPTIONS,X0,y0,Z0,kap0,tau0,theta0)

if (nargin < 5); OPTIONS = []; end

isemptyAtb = 0;
if isempty(At) && isempty(b);
    %% Add redundant constraint: <-I,X> <= 0
    b = 0;
    At = ops(ops(blk,'identity'),'*',-1);
    numblk = size(blk,1);
    blk{numblk+1,1} = 'l'; blk{numblk+1,2} = 1;
    At{numblk+1,1} = 1; C{numblk+1,1} = 0;
    isemptyAtb = 1;
end
%%
%%-----------------------------------------
%% get parameters from the OPTIONS structure.
%%-----------------------------------------
%%
% warning off;

matlabversion = sscanf(version,'%f');
if strcmp(computer,'PCWIN64') || strcmp(computer,'GLNXA64')
    par.computer = 64;
else
    par.computer = 32;
end
par.matlabversion = matlabversion(1);
par.vers        = 0;
par.predcorr    = 1;
par.gam         = 0;
par.expon       = 1;
par.gaptol      = 1e-8;
par.inftol      = 1e-8;
par.steptol     = 1e-6;
par.maxit       = 100;
par.printlevel  = 3;
par.stoplevel   = 1;
par.scale_data  = 0;
par.spdensity   = 0.4;
par.rmdepconstr = 0;
par.smallblkdim = 50;
par.schurfun     = cell(size(blk,1),1);
par.schurfun_par = cell(size(blk,1),1);
%%
parbarrier  = cell(size(blk,1),1);
for p = 1:size(blk,1)
    pblk = blk(p,:);
    if strcmp(pblk{1},'s') || strcmp(pblk{1},'q')
        parbarrier{p} = zeros(1,length(pblk{2}));
    elseif strcmp(pblk{1},'l') || strcmp(pblk{1},'u' )
        parbarrier{p} = zeros(1,sum(pblk{2}));
    end
end
parbarrier_0 = parbarrier;
%%
if nargin > 4,
    if isfield(OPTIONS,'vers');        par.vers     = OPTIONS.vers; end
    if isfield(OPTIONS,'predcorr');    par.predcorr = OPTIONS.predcorr; end
    if isfield(OPTIONS,'gam');         par.gam      = OPTIONS.gam; end
    if isfield(OPTIONS,'expon');       par.expon    = OPTIONS.expon; end
    if isfield(OPTIONS,'gaptol');      par.gaptol   = OPTIONS.gaptol; end
    if isfield(OPTIONS,'inftol');      par.inftol   = OPTIONS.inftol; end
    if isfield(OPTIONS,'steptol');     par.steptol  = OPTIONS.steptol; end
    if isfield(OPTIONS,'maxit');       par.maxit    = OPTIONS.maxit; end
    if isfield(OPTIONS,'printlevel');  par.printlevel  = OPTIONS.printlevel; end
    if isfield(OPTIONS,'stoplevel');   par.stoplevel   = OPTIONS.stoplevel; end
    if isfield(OPTIONS,'scale_data');  par.scale_data  = OPTIONS.scale_data; end
    if isfield(OPTIONS,'spdensity');   par.spdensity   = OPTIONS.spdensity; end
    if isfield(OPTIONS,'rmdepconstr'); par.rmdepconstr = OPTIONS.rmdepconstr; end
    if isfield(OPTIONS,'smallblkdim'); par.smallblkdim = OPTIONS.smallblkdim; end
    if isfield(OPTIONS,'parbarrier');
        parbarrier = OPTIONS.parbarrier;
        if isempty(parbarrier); parbarrier = parbarrier_0; end
        if ~iscell(parbarrier);
            tmp = parbarrier; clear parbarrier; parbarrier{1} = tmp;
        end
        if (length(parbarrier) < size(blk,1))
            len = length(parbarrier);
            parbarrier(len+1:size(blk,1)) = parbarrier_0(len+1:size(blk,1));
        end
    end
    if isfield(OPTIONS,'schurfun');
        par.schurfun = OPTIONS.schurfun;
        if ~isempty(par.schurfun); par.scale_data = 0; end
    end
    if isfield(OPTIONS,'schurfun_par'); par.schurfun_par = OPTIONS.schurfun_par; end
    if isempty(par.schurfun);     par.schurfun = cell(size(blk,1),1); end
    if isempty(par.schurfun_par); par.schurfun_par = cell(size(blk,1),1); end
end
if (size(blk,2) > 2); par.smallblkdim = 0; end
%%
%%-----------------------------------------
%% convert matrices to cell arrays.
%%-----------------------------------------
%%
if ~iscell(At); At = {At}; end;
if ~iscell(C);  C = {C}; end;
if all(size(At) == [size(blk,1), length(b)]);
    convertyes = zeros(size(blk,1),1);
    for p = 1:size(blk,1)
        if strcmp(blk{p,1},'s') && all(size(At{p,1}) == sum(blk{p,2}))
            convertyes(p) = 1;
        end
    end
    if any(convertyes)
        if (par.printlevel);
            fprintf('\n sqlp: converting At into required format');
        end
        At = svec(blk,At,ones(size(blk,1),1));
    end
end
%%
%%-----------------------------------------
%% validate SQLP data.
%%-----------------------------------------
%%
% tstart = cputime;
[blk,At,C,b,blkdim,numblk] = validate(blk,At,C,b,par);
[blk,At,C,b,iscmp] = convertcmpsdp(blk,At,C,b);
if (iscmp) && (par.printlevel>=2)
    fprintf('\n SQLP has complex data');
end
if (nargin <= 5) || (isempty(X0) || isempty(y0) || isempty(Z0));
    if (max([ops(At,'norm'),ops(C,'norm'),norm(b)]) > 1e2)
        [X0,y0,Z0] = infeaspt(blk,At,C,b,1);
    else
        [X0,y0,Z0] = infeaspt(blk,At,C,b,2,1);
    end
end
if ~iscell(X0);  X0 = {X0}; end;
if ~iscell(Z0);  Z0 = {Z0}; end;
if (length(y0) ~= length(b))
    error('HSDsqlp: length of b and y0 not compatible');
end
[X0,Z0] = validate_startpoint(blk,X0,Z0,par.spdensity);
%%
if (nargin <= 8) || (isempty(kap0) || isempty(tau0) || isempty(theta0))
    if (max([ops(At,'norm'),ops(C,'norm'),norm(b)]) > 1e6)
        kap0 = 10*blktrace(blk,X0,Z0);
    else
        kap0 = blktrace(blk,X0,Z0);
    end
    tau0 = 1; theta0 = 1;
end
%%
if (par.printlevel>=2)
    fprintf('\n num. of constraints = %2.0d',length(b));
    if blkdim(1);
        fprintf('\n dim. of sdp    var  = %2.0d,',blkdim(1));
        fprintf('   num. of sdp  blk  = %2.0d',numblk(1));
    end
    if blkdim(2);
        fprintf('\n dim. of socp   var  = %2.0d,',blkdim(2));
        fprintf('   num. of socp blk  = %2.0d',numblk(2));
    end
    if blkdim(3); fprintf('\n dim. of linear var  = %2.0d',blkdim(3)); end
    if blkdim(4); fprintf('\n dim. of free   var  = %2.0d',blkdim(4)); end
end
%%
%%-----------------------------------------
%% detect unrestricted blocks in linear blocks
%%-----------------------------------------
%%
user_supplied_schurfun = 0;
for p = 1:size(blk,1)
    if ~isempty(par.schurfun{p}); user_supplied_schurfun = 1; end
end
if (user_supplied_schurfun == 0)
    [blk2,At2,C2,ublkinfo,parbarrier2,X02,Z02] = ...
        detect_ublk(blk,At,C,parbarrier,X0,Z0,par.printlevel);
else
    blk2 = blk; At2 = At; C2 = C;
    parbarrier2 = parbarrier; X02 = X0; Z02 = Z0;
    ublkinfo = cell(size(blk2,1),1);
end
ublksize = blkdim(4);
for p = 1:size(ublkinfo,1)
    ublksize = ublksize + length(ublkinfo{p});
end
%%
%%-----------------------------------------
%% detect diagonal blocks in semidefinite blocks
%%-----------------------------------------
%%
if (user_supplied_schurfun==0)
    [blk3,At3,C3,diagblkinfo,diagblkchange,parbarrier3,X03,Z03] = ...
        detect_lblk(blk2,At2,C2,b,parbarrier2,X02,Z02,par.printlevel); %#ok
else
    blk3 = blk2; At3 = At2; C3 = C2;
    % parbarrier3 = parbarrier2;
    X03 = X02; Z03 = Z02;
    diagblkchange = 0;
    diagblkinfo = cell(size(blk3,1),1);
end
%%
%%-----------------------------------------
%% main solver
%%-----------------------------------------
%%
%exist_analytic_term = 0;
%for p = 1:size(blk3,1);
%    idx = find(parbarrier3{p} > 0);
%    if ~isempty(idx); exist_analytic_term = 1; end
%end
%%
if (par.vers == 0);
    if blkdim(1); par.vers = 1; else par.vers = 2; end
end
par.blkdim = blkdim;
par.ublksize = ublksize;
[obj,X3,y,Z3,info,runhist] = ...
    HSDsqlpmain(blk3,At3,C3,b,par,X03,y0,Z03,kap0,tau0,theta0);
%%
%%-----------------------------------------
%% recover semidefinite blocks from linear blocks
%%-----------------------------------------
%%
if any(diagblkchange)
    X2 = cell(size(blk2,1),1); Z2 = cell(size(blk2,1),1);
    count = 0;
    for p = 1:size(blk2,1)
        pblk = blk2(p,:);
        n = sum(pblk{2});
        blkno      = diagblkinfo{p,1};
        idxdiag    = diagblkinfo{p,2};
        idxnondiag = diagblkinfo{p,3};
        if ~isempty(idxdiag)
            len = length(idxdiag);
            Xtmp = [idxdiag,idxdiag,X3{end}(count+1:count+len); n, n, 0];
            Ztmp = [idxdiag,idxdiag,Z3{end}(count+1:count+len); n, n, 0];
            if ~isempty(idxnondiag)
                [ii,jj,vv] = find(X3{blkno});
                Xtmp = [Xtmp; idxnondiag(ii),idxnondiag(jj),vv]; %#ok
                [ii,jj,vv] = find(Z3{blkno});
                Ztmp = [Ztmp; idxnondiag(ii),idxnondiag(jj),vv]; %#ok
            end
            X2{p} = spconvert(Xtmp);
            Z2{p} = spconvert(Ztmp);
            count = count + len;
        else
            X2(p) = X3(blkno); Z2(p) = Z3(blkno);
        end
    end
else
    X2 = X3; Z2 = Z3;
end
%%
%%-----------------------------------------
%% recover linear block from unrestricted block
%%-----------------------------------------
%%
numblk = size(blk,1);
numblknew = numblk;
X = cell(numblk,1); Z = cell(numblk,1);
for p = 1:numblk
    n = blk{p,2};
    if isempty(ublkinfo{p,1})
        X{p} = X2{p}; Z{p} = Z2{p};
    else
        Xtmp = zeros(n,1); Ztmp = zeros(n,1);
        Xtmp(ublkinfo{p,1}) = max(0,X2{p});
        Xtmp(ublkinfo{p,2}) = max(0,-X2{p});
        Ztmp(ublkinfo{p,1}) = max(0,Z2{p});
        Ztmp(ublkinfo{p,2}) = max(0,-Z2{p});
        if ~isempty(ublkinfo{p,3})
            numblknew = numblknew + 1;
            Xtmp(ublkinfo{p,3}) = X2{numblknew};
            Ztmp(ublkinfo{p,3}) = Z2{numblknew};
        end
        X{p} = Xtmp; Z{p} = Ztmp;
    end
end
%%
%%-----------------------------------------
%% recover complex solution
%%-----------------------------------------
%%
if (iscmp)
    for p = 1:numblk
        pblk = blk(p,:);
        n = sum(pblk{2})/2;
        if strcmp(pblk{1},'s');
            X{p} = X{p}(1:n,1:n) + sqrt(-1)*X{p}(n+1:2*n,1:n);
            Z{p} = Z{p}(1:n,1:n) + sqrt(-1)*Z{p}(n+1:2*n,1:n);
            X{p} = 0.5*(X{p}+X{p}');
            Z{p} = 0.5*(Z{p}+Z{p}');
        end
    end
end
if (isemptyAtb)
    X = X(1:end-1); Z = Z(1:end-1);
end
%%*****************************************************************************


function halham_write ( dim_num, n, step, seed, leap, base, r, file_out_name )

%*****************************************************************************80
%
%% HALHAM_WRITE writes a Halton or Hammersley dataset to a file.
%
%  Discussion:
%
%    The initial lines of the file are comments, which begin with a
%    '#' character.
%
%    Thereafter, each line of the file contains the DIM_NUM-dimensional
%    components of the next entry in the dataset.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer N, the number of elements in the subsequence.
%
%    Input, integer STEP, the index of the subsequence element.
%    0 <= STEP is required.
%
%    Input, integer SEED(DIM_NUM), the sequence index corresponding
%    to STEP = 0.
%
%    Input, integer LEAP(DIM_NUM), the successive jumps in the sequence.
%
%    Input, integer BASE(DIM_NUM), the bases.
%
%    Input, real R(DIM_NUM,N), the points.
%
%    Input, string FILE_OUT_NAME, the name of
%    the output file.
%
  file_out_unit = fopen ( file_out_name, 'w' );

  if ( file_out_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'HALHAM_WRITE - Fatal error!\n' );
    fprintf ( 1, '  Could not open the output file:\n' );
    fprintf ( 1, '  "%s".\n', file_out_name );
    error ( 'HALHAM_WRITE - Fatal error!' );
  end

  fprintf ( file_out_unit, '#  %s\n', file_out_name );
  fprintf ( file_out_unit, '#  created by HALHAM_WRITE.M\n' );
  fprintf ( file_out_unit, '#\n' );
  fprintf ( file_out_unit, '#  DIM_NUM = %d\n', dim_num );
  fprintf ( file_out_unit, '#  N =    %d\n', n );
  fprintf ( file_out_unit, '#  STEP = %d\n', step );

  for mlo = 1 : 5 : dim_num
    mhi = min ( mlo + 5 - 1, dim_num );
    if ( mlo == 1 )
      fprintf ( file_out_unit, '#  SEED = ' );
    else
      fprintf ( file_out_unit, '#         ' );
    end
    for i = mlo : mhi
      fprintf ( file_out_unit, '  %12d', seed(i) );
    end
    fprintf ( file_out_unit, '\n' );
  end
  for mlo = 1 : 5 : dim_num
    mhi = min ( mlo + 5 - 1, dim_num );
    if ( mlo == 1 )
      fprintf ( file_out_unit, '#  LEAP = ' );
    else
      fprintf ( file_out_unit, '#         ' );
    end
    for i = mlo : mhi
      fprintf ( file_out_unit, '  %12d', leap(i) );
    end
    fprintf ( file_out_unit, '\n' );
  end
  for mlo = 1 : 5 : dim_num
    mhi = min ( mlo + 5 - 1, dim_num );
    if ( mlo == 1 )
      fprintf ( file_out_unit, '#  BASE = ' );
    else
      fprintf ( file_out_unit, '#         ' );
    end
    for i = mlo : mhi
      fprintf ( file_out_unit, '  %12d', base(i) );
    end
    fprintf ( file_out_unit, '\n' );
  end
  fprintf ( file_out_unit, '#\n' );

  for j = 1 : n
    for i = 1 : dim_num
      fprintf ( file_out_unit, '  %10f', r(i,j) );
    end
    fprintf ( file_out_unit, '\n' );
  end

  fclose ( file_out_unit );

  return
end


function c=voigtUnMap(cVoigt)

if isvector(cVoigt) %assume that c is a 4th order tensor
    siz_c=[3 3];
    cVoigt(4:end)=(1/2).*cVoigt(4:end); %Undo doubling
else
    siz_c=[3 3 3 3];
end

[linearIndexVoigt,linearIndexFourthOrder]=tensor2voigtMap(zeros(siz_c));

switch class(cVoigt)
    case 'double'
        c=zeros(siz_c);
    case 'sym'
        c=sym(zeros(siz_c));
end

c(linearIndexFourthOrder)=cVoigt(linearIndexVoigt);

end

%%
% _*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/>.


function [D, Y, optinf] = cbpdndl(D0, S, lambda, opt)

% cbpdndl -- Convolutional BPDN Dictionary Learning
%
%         argmin_{x_m,d_m} (1/2) \sum_k ||\sum_m d_m * x_k,m - s_k||_2^2 +
%                           lambda \sum_k \sum_m ||x_k,m||_1
%
%         The solution is computed using Augmented Lagrangian methods
%         (see boyd-2010-distributed) with efficient solution of the
%         main linear systems (see wohlberg-2016-efficient).
%
% Usage:
%       [D, Y, optinf] = cbpdndl(D0, S, lambda, opt)
%
% Input:
%       D0          Initial dictionary
%       S           Input images
%       lambda      Regularization parameter
%       opt         Options/algorithm parameters structure (see below)
%
% Output:
%       D           Dictionary filter set (3D array)
%       X           Coefficient maps (4D array)
%       optinf      Details of optimisation
%
%
% Options structure fields:
%   Verbose          Flag determining whether iteration status is displayed.
%                    Fields are iteration number, functional value,
%                    data fidelity term, l1 regularisation term, and
%                    primal and dual residuals (see Sec. 3.3 of
%                    boyd-2010-distributed). The values of rho and sigma
%                    are also displayed if options request that they are
%                    automatically adjusted.
%   MaxMainIter      Maximum main iterations
%   AbsStopTol       Absolute convergence tolerance (see Sec. 3.3.1 of
%                    boyd-2010-distributed)
%   RelStopTol       Relative convergence tolerance (see Sec. 3.3.1 of
%                    boyd-2010-distributed)
%   L1Weight         Weight array for L1 norm
%   Y0               Initial value for Y
%   U0               Initial value for U
%   G0               Initial value for G (overrides D0 if specified)
%   H0               Initial value for H
%   rho              Augmented Lagrangian penalty parameter
%   AutoRho          Flag determining whether rho is automatically updated
%                    (see Sec. 3.4.1 of boyd-2010-distributed)
%   AutoRhoPeriod    Iteration period on which rho is updated
%   RhoRsdlRatio     Primal/dual residual ratio in rho update test
%   RhoScaling       Multiplier applied to rho when updated
%   AutoRhoScaling   Flag determining whether RhoScaling value is
%                    adaptively determined (see wohlberg-2015-adaptive). If
%                    enabled, RhoScaling specifies a maximum allowed
%                    multiplier instead of a fixed multiplier
%   sigma            Augmented Lagrangian penalty parameter
%   AutoSigma        Flag determining whether sigma is automatically
%                    updated (see Sec. 3.4.1 of boyd-2010-distributed)
%   AutoSigmaPeriod  Iteration period on which sigma is updated
%   SigmaRsdlRatio   Primal/dual residual ratio in sigma update test
%   SigmaScaling     Multiplier applied to sigma when updated
%   AutoSigmaScaling Flag determining whether SigmaScaling value is
%                    adaptively determined (see wohlberg-2015-adaptive). If
%                    enabled, SigmaScaling specifies a maximum allowed
%                    multiplier instead of a fixed multiplier.
%   StdResiduals     Flag determining whether standard residual definitions
%                    (see Sec 3.3 of boyd-2010-distributed) are used instead
%                    of normalised residuals (see wohlberg-2015-adaptive)
%   XRelaxParam      Relaxation parameter (see Sec. 3.4.3 of
%                    boyd-2010-distributed) for X update
%   DRelaxParam      Relaxation parameter (see Sec. 3.4.3 of
%                    boyd-2010-distributed) for D update
%   LinSolve         Linear solver for main problem: 'SM' or 'CG'
%   MaxCGIter        Maximum CG iterations when using CG solver
%   CGTol            CG tolerance when using CG solver
%   CGTolAuto        Flag determining use of automatic CG tolerance
%   CGTolFactor      Factor by which primal residual is divided to obtain CG
%                    tolerance, when automatic tolerance is active
%   NoBndryCross     Flag indicating whether all solution coefficients
%                    corresponding to filters crossing the image boundary
%                    should be forced to zero.
%   DictFilterSizes  Array of size 2 x M where each column specifies the
%                    filter size (rows x columns) of the corresponding
%                    dictionary filter
%   NonNegCoef       Flag indicating whether solution should be forced to
%                    be non-negative
%   ZeroMean         Force learned dictionary entries to be zero-mean
%
%
% Author: Brendt Wohlberg <brendt@lanl.gov>  Modified: 2015-12-18
%
% This file is part of the SPORCO library. Details of the copyright
% and user license can be found in the 'License' file distributed with
% the library.

if nargin < 4,
  opt = [];
end
checkopt(opt, defaultopts([]));
opt = defaultopts(opt);

% Set up status display for verbose operation
hstr = ['Itn   Fnc       DFid      l1        Cnstr     '...
        'r(X)      s(X)      r(D)      s(D) '];
sfms = '%4d %9.2e %9.2e %9.2e %9.2e %9.2e %9.2e %9.2e %9.2e';
nsep = 84;
if opt.AutoRho,
  hstr = [hstr '     rho  '];
  sfms = [sfms ' %9.2e'];
  nsep = nsep + 10;
end
if opt.AutoSigma,
  hstr = [hstr '     sigma  '];
  sfms = [sfms ' %9.2e'];
  nsep = nsep + 10;
end
if opt.Verbose && opt.MaxMainIter > 0,
  disp(hstr);
  disp(char('-' * ones(1,nsep)));
end

% Collapsing of trailing singleton dimensions greatly complicates
% handling of both SMV and MMV cases. The simplest approach would be
% if S could always be reshaped to 4d, with dimensions consisting of
% image rows, image cols, a single dimensional placeholder for number
% of filters, and number of measurements, but in the single
% measurement case the third dimension is collapsed so that the array
% is only 3d.
if size(S,3) > 1,
  xsz = [size(S,1) size(S,2) size(D0,3) size(S,3)];
  % Insert singleton 3rd dimension (for number of filters) so that
  % 4th dimension is number of images in input s volume
  S = reshape(S, [size(S,1) size(S,2) 1 size(S,3)]);
else
  xsz = [size(S,1) size(S,2) size(D0,3) 1];
end
Nx = prod(xsz);
Nd = prod(xsz(1:2))*size(D0,3);
cgt = opt.CGTol;

% Dictionary size may be specified when learning multiscale
% dictionary
if isempty(opt.DictFilterSizes),
  dsz = [size(D0,1) size(D0,2)];
else
  dsz = opt.DictFilterSizes;
end

% Mean removal and normalisation projections
Pzmn = @(x) bsxfun(@minus, x, mean(mean(x,1),2));
Pnrm = @(x) bsxfun(@rdivide, x, sqrt(sum(sum(x.^2, 1), 2)));

% Projection of filter to full image size and its transpose
% (zero-pad and crop respectively)
Pzp = @(x) zpad(x, xsz(1:2));
PzpT = @(x) bcrop(x, dsz);

% Projection of dictionary filters onto constraint set
if opt.ZeroMean,
  Pcn = @(x) Pnrm(Pzp(Pzmn(PzpT(x))));
else
  Pcn = @(x) Pnrm(Pzp(PzpT(x)));
end

% Start timer
tstart = tic;

% Project initial dictionary onto constraint set
D = Pnrm(D0);

% Compute signal in DFT domain
Sf = fft2(S);

% Set up algorithm parameters and initialise variables
rho = opt.rho;
if isempty(rho), rho = 50*lambda+1; end;
if opt.AutoRho,
  asgr = opt.RhoRsdlRatio;
  asgm = opt.RhoScaling;
end
sigma = opt.sigma;
if isempty(sigma), sigma = size(S,3); end;
if opt.AutoSigma,
  asdr = opt.SigmaRsdlRatio;
  asdm = opt.SigmaScaling;
end
optinf = struct('itstat', [], 'opt', opt);
rx = Inf;
sx = Inf;
rd = Inf;
sd = Inf;
eprix = 0;
eduax = 0;
eprid = 0;
eduad = 0;

% Initialise main working variables
X = [];
if isempty(opt.Y0),
  Y = zeros(xsz, class(S));
else
  Y = opt.Y0;
end
Yprv = Y;
if isempty(opt.U0),
  if isempty(opt.Y0),
    U = zeros(xsz, class(S));
  else
    U = (lambda/rho)*sign(Y);
  end
else
  U = opt.U0;
end
Df = [];
if isempty(opt.G0),
  G = Pzp(D);
else
  G = opt.G0;
end
Gprv = G;
if isempty(opt.H0),
  if isempty(opt.G0),
    H = zeros(size(G), class(S));
  else
    H = G;
  end
else
  H = opt.H0;
end
Gf = fft2(G, size(S,1), size(S,2));
GSf = bsxfun(@times, conj(Gf), Sf);

% Main loop
k = 1;
while k <= opt.MaxMainIter && (rx > eprix|sx > eduax|rd > eprid|sd >eduad),

  % Solve X subproblem. It would be simpler and more efficient (since the
  % DFT is already available) to solve for X using the main dictionary
  % variable D as the dictionary, but this appears to be unstable. Instead,
  % use the projected dictionary variable G
  Xf = solvedbi_sm(Gf, rho, GSf + rho*fft2(Y - U));
  X = ifft2(Xf, 'symmetric');
  clear Xf Gf GSf;

  % See pg. 21 of boyd-2010-distributed
  if opt.XRelaxParam == 1,
    Xr = X;
  else
    Xr = opt.XRelaxParam*X + (1-opt.XRelaxParam)*Y;
  end

  % Solve Y subproblem
  Y = shrink(Xr + U, (lambda/rho)*opt.L1Weight);
  if opt.NonNegCoef,
    Y(Y < 0) = 0;
  end
  if opt.NoBndryCross,
    %Y((end-max(dsz(1,:))+2):end,:,:,:) = 0;
    Y((end-size(D0,1)+2):end,:,:,:) = 0;
    %Y(:,(end-max(dsz(2,:))+2):end,:,:) = 0;
    Y(:,(end-size(D0,2)+2):end,:,:) = 0;
  end
  Yf = fft2(Y);
  YSf = sum(bsxfun(@times, conj(Yf), Sf), 4);

  % Update dual variable corresponding to X, Y
  U = U + Xr - Y;
  clear Xr;

  % Compute primal and dual residuals and stopping thresholds for X update
  nX = norm(X(:)); nY = norm(Y(:)); nU = norm(U(:));
  if opt.StdResiduals,
    % See pp. 19-20 of boyd-2010-distributed
    rx = norm(vec(X - Y));
    sx = norm(vec(rho*(Yprv - Y)));
    eprix = sqrt(Nx)*opt.AbsStopTol+max(nX,nY)*opt.RelStopTol;
    eduax = sqrt(Nx)*opt.AbsStopTol+rho*nU*opt.RelStopTol;
  else
    % See wohlberg-2015-adaptive
    rx = norm(vec(X - Y))/max(nX,nY);
    sx = norm(vec(Yprv - Y))/nU;
    eprix = sqrt(Nx)*opt.AbsStopTol/max(nX,nY)+opt.RelStopTol;
    eduax = sqrt(Nx)*opt.AbsStopTol/(rho*nU)+opt.RelStopTol;
  end
  clear X;

  % Compute l1 norm of Y
  Jl1 = sum(abs(vec(bsxfun(@times, opt.L1Weight, Y))));

  % Update record of previous step Y
  Yprv = Y;

  % Solve D subproblem. Similarly, it would be simpler and more efficient to
  % solve for D using the main coefficient variable X as the coefficients,
  % but it appears to be more stable to use the shrunk coefficient variable Y
  if strcmp(opt.LinSolve, 'SM'),
    Df = solvemdbi_ism(Yf, sigma, YSf + sigma*fft2(G - H));
  else
    [Df, cgst] = solvemdbi_cg(Yf, sigma, YSf + sigma*fft2(G - H), ...
                              cgt, opt.MaxCGIter, Df(:));
  end
  clear YSf;
  D = ifft2(Df, 'symmetric');
  if strcmp(opt.LinSolve, 'SM'), clear Df; end

  % See pg. 21 of boyd-2010-distributed
  if opt.DRelaxParam == 1,
    Dr = D;
  else
    Dr = opt.DRelaxParam*D + (1-opt.DRelaxParam)*G;
  end

  % Solve G subproblem
  G = Pcn(Dr + H);
  Gf = fft2(G);
  GSf = bsxfun(@times, conj(Gf), Sf);

  % Update dual variable corresponding to D, G
  H = H + Dr - G;
  clear Dr;

  % Compute primal and dual residuals and stopping thresholds for D update
  nD = norm(D(:)); nG = norm(G(:)); nH = norm(H(:));
  if opt.StdResiduals,
    % See pp. 19-20 of boyd-2010-distributed
    rd = norm(vec(D - G));
    sd = norm(vec(sigma*(Gprv - G)));
    eprid = sqrt(Nd)*opt.AbsStopTol+max(nD,nG)*opt.RelStopTol;
    eduad = sqrt(Nd)*opt.AbsStopTol+sigma*nH*opt.RelStopTol;
  else
    % See wohlberg-2015-adaptive
    rd = norm(vec(D - G))/max(nD,nG);
    sd = norm(vec(Gprv - G))/nH;
    eprid = sqrt(Nd)*opt.AbsStopTol/max(nD,nG)+opt.RelStopTol;
    eduad = sqrt(Nd)*opt.AbsStopTol/(sigma*nH)+opt.RelStopTol;
  end

  % Apply CG auto tolerance policy if enabled
  if opt.CGTolAuto && (rd/opt.CGTolFactor) < cgt,
    cgt = rd/opt.CGTolFactor;
  end

  % Compute measure of D constraint violation
  Jcn = norm(vec(Pcn(D) - D));
  clear D;

  % Update record of previous step G
  Gprv = G;

  % Compute data fidelity term in Fourier domain (note normalisation)
  Jdf = sum(vec(abs(sum(bsxfun(@times,Gf,Yf),3)-Sf).^2))/(2*xsz(1)*xsz(2));
  clear Yf;
  Jfn = Jdf + lambda*Jl1;

  % Record and display iteration details
  tk = toc(tstart);
  optinf.itstat = [optinf.itstat;...
       [k Jfn Jdf Jl1 rx sx rd sd eprix eduax eprid eduad rho sigma tk]];
  if opt.Verbose,
    dvc = [k, Jfn, Jdf, Jl1, Jcn, rx, sx, rd, sd];
    if opt.AutoRho,
      dvc = [dvc rho];
    end
    if opt.AutoSigma,
      dvc = [dvc sigma];
    end
    disp(sprintf(sfms, dvc));
  end

  % See wohlberg-2015-adaptive and pp. 20-21 of boyd-2010-distributed
  if opt.AutoRho,
    if k ~= 1 && mod(k, opt.AutoRhoPeriod) == 0,
      if opt.AutoRhoScaling,
        rhomlt = sqrt(rx/sx);
        if rhomlt < 1, rhomlt = 1/rhomlt; end
        if rhomlt > opt.RhoScaling, rhomlt = opt.RhoScaling; end
      else
        rhomlt = opt.RhoScaling;
      end
      rsf = 1;
      if rx > opt.RhoRsdlRatio*sx, rsf = rhomlt; end
      if sx > opt.RhoRsdlRatio*rx, rsf = 1/rhomlt; end
      rho = rsf*rho;
      U = U/rsf;
    end
  end
  if opt.AutoSigma,
    if k ~= 1 && mod(k, opt.AutoSigmaPeriod) == 0,
      if opt.AutoSigmaScaling,
        sigmlt = sqrt(rd/sd);
        if sigmlt < 1, sigmlt = 1/sigmlt; end
        if sigmlt > opt.SigmaScaling, sigmlt = opt.SigmaScaling; end
      else
        sigmlt = opt.SigmaScaling;
      end
      ssf = 1;
      if rd > opt.SigmaRsdlRatio*sd, ssf = sigmlt; end
      if sd > opt.SigmaRsdlRatio*rd, ssf = 1/sigmlt; end
      sigma = ssf*sigma;
      H = H/ssf;
    end
  end

  k = k + 1;

end

D = PzpT(G);

% Record run time and working variables
optinf.runtime = toc(tstart);
optinf.Y = Y;
optinf.U = U;
optinf.G = G;
optinf.H = H;
optinf.lambda = lambda;
optinf.rho = rho;
optinf.sigma = sigma;
optinf.cgt = cgt;
if exist('cgst'), optinf.cgst = cgst; end

if opt.Verbose && opt.MaxMainIter > 0,
  disp(char('-' * ones(1,nsep)));
end

return

function u = vec(v)

  u = v(:);

return

function u = shrink(v, lambda)

  if isscalar(lambda),
    u = sign(v).*max(0, abs(v) - lambda);
  else
    u = sign(v).*max(0, bsxfun(@minus, abs(v), lambda));
  end

return

function u = zpad(v, sz)

  u = zeros(sz(1), sz(2), size(v,3), size(v,4), class(v));
  u(1:size(v,1), 1:size(v,2),:,:) = v;

return

function u = bcrop(v, sz)

  if numel(sz) <= 2,
    if numel(sz) == 1
      cs = [sz sz];
    else
      cs = sz;
    end
    u = v(1:cs(1), 1:cs(2), :);
  else
    cs = max(sz,[],2);
    u = zeros(cs(1), cs(2), size(v,3), class(v));
    for k = 1:size(v,3),
      u(1:sz(1,k), 1:sz(2,k), k) = v(1:sz(1,k), 1:sz(2,k), k);
    end
  end

return

function opt = defaultopts(opt)

  if ~isfield(opt,'Verbose'),
    opt.Verbose = 0;
  end
  if ~isfield(opt,'MaxMainIter'),
    opt.MaxMainIter = 1000;
  end
  if ~isfield(opt,'AbsStopTol'),
    opt.AbsStopTol = 1e-6;
  end
  if ~isfield(opt,'RelStopTol'),
    opt.RelStopTol = 1e-4;
  end
  if ~isfield(opt,'L1Weight'),
    opt.L1Weight = 1;
  end
  if ~isfield(opt,'Y0'),
    opt.Y0 = [];
  end
  if ~isfield(opt,'U0'),
    opt.U0 = [];
  end
  if ~isfield(opt,'G0'),
    opt.G0 = [];
  end
  if ~isfield(opt,'H0'),
    opt.H0 = [];
  end
  if ~isfield(opt,'rho'),
    opt.rho = [];
  end
  if ~isfield(opt,'AutoRho'),
    opt.AutoRho = 0;
  end
  if ~isfield(opt,'AutoRhoPeriod'),
    opt.AutoRhoPeriod = 10;
  end
  if ~isfield(opt,'RhoRsdlRatio'),
    opt.RhoRsdlRatio = 10;
  end
  if ~isfield(opt,'RhoScaling'),
    opt.RhoScaling = 2;
  end
  if ~isfield(opt,'AutoRhoScaling'),
    opt.AutoRhoScaling = 0;
  end
  if ~isfield(opt,'sigma'),
    opt.sigma = [];
  end
  if ~isfield(opt,'AutoSigma'),
    opt.AutoSigma = 0;
  end
  if ~isfield(opt,'AutoSigmaPeriod'),
    opt.AutoSigmaPeriod = 10;
  end
  if ~isfield(opt,'SigmaRsdlRatio'),
    opt.SigmaRsdlRatio = 10;
  end
  if ~isfield(opt,'SigmaScaling'),
    opt.SigmaScaling = 2;
  end
  if ~isfield(opt,'AutoSigmaScaling'),
    opt.AutoSigmaScaling = 0;
  end
  if ~isfield(opt,'StdResiduals'),
    opt.StdResiduals = 0;
  end
  if ~isfield(opt,'XRelaxParam'),
    opt.XRelaxParam = 1;
  end
  if ~isfield(opt,'DRelaxParam'),
    opt.DRelaxParam = 1;
  end
  if ~isfield(opt,'LinSolve'),
    opt.LinSolve = 'SM';
  end
  if ~isfield(opt,'MaxCGIter'),
    opt.MaxCGIter = 1000;
  end
  if ~isfield(opt,'CGTol'),
    opt.CGTol = 1e-3;
  end
  if ~isfield(opt,'CGTolAuto'),
    opt.CGTolAuto = 0;
  end
  if ~isfield(opt,'CGTolAutoFactor'),
    opt.CGTolFactor = 50;
  end
  if ~isfield(opt,'NoBndryCross'),
    opt.NoBndryCross = 0;
  end
  if ~isfield(opt,'DictFilterSizes'),
    opt.DictFilterSizes = [];
  end
  if ~isfield(opt,'NonNegCoef'),
    opt.NonNegCoef = 0;
  end
  if ~isfield(opt,'ZeroMean'),
    opt.ZeroMean = 0;
  end

return


function [ node_num, tree, tree_data ] = r8btree_node_add ( node_num, tree, ...
  data_num, tree_data, node_data )

%*****************************************************************************80
%
%% R8BTREE_NODE_ADD adds one node to a BTREE.
%
%  Discussion:
%
%    A BTREE is a binary tree.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 November 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of nodes in the tree.
%
%    Input, integer TREE(4,NODE_NUM).
%    TREE(1,I), the index in TREE_DATA(1,*) of the coordinate of node I.
%    TREE(2,I), the left child of node I.
%    TREE(3,I), the right child of node I.
%    TREE(4,I), the parent of node I.
%
%    Input, integer DATA_NUM, the number of data items per node.
%
%    Input, real TREE_DATA(DATA_NUM,NODE_NUM), node data.
%
%    Input, real NODE_DATA(DATA_NUM), data associated with the new node.
%
%    Output, integer NODE_NUM, the updated value.
%
%    Output, integer TREE(4,NODE_NUM), the updated value.
%
%    Output, integer TREE_DATA(DATA_NUM,NODE_NUM), the updated value.
%
  if ( node_num <= 0 )
    tree(1:4,1) = [ 1, -1, -1, -1 ];
    tree_data(1:data_num,1) = node_data(1:data_num);
    node_num = 1;
    return
  end

  l = -1;
  m = 1;
  r = -1;
%
%  Divide the current interval [L,R] by the midpoint M.
%  X falls either into [L,M] or [M,R].
%
  x = node_data(1);

  while ( 1 )

    xm = tree_data(1,tree(1,m));
    m_old = m;

    if ( x < xm )
      r = m;
      m = tree(2,m);
      if ( m == - 1 )
        tree(2,m_old) = node_num + 1;
        tree(1,node_num+1) = node_num + 1;
        tree(2,node_num+1) = -1;
        tree(3,node_num+1) = -1;
        tree(4,node_num+1) = m_old;
        tree_data(1:data_num,node_num+1) = node_data(1:data_num);
        node_num = node_num + 1;
        break
      end
    elseif ( xm < x )
      l = m;
      m = tree(3,m);
      if ( m == -1 )
        tree(3,m_old) = node_num + 1;
        tree(1,node_num+1) = node_num + 1;
        tree(2,node_num+1) = -1;
        tree(3,node_num+1) = -1;
        tree(4,node_num+1) = m_old;
        tree_data(1:data_num,node_num+1) = node_data(1:data_num);
        node_num = node_num + 1;
        break
      end
    else
      r = m;
      l = m;
      break
    end

  end

  return
end


%%*********************************************************
%% convertcmpsdp: convert SDP with complex data into one
%%                with real data by converting
%%
%%  C - sum_{k=1}^m yk*Ak psd
%%  to
%%  [CR,-CI] - sum ykR*[AkR,-AkI] psd
%%  [CI, CR]           [AkI, AkR]
%%
%%  ykI = 0 for k = 1:m
%%
%% [bblk,AAt,CC,bb] = convertcmpsdp(blk,A,C,b);
%%
%%*********************************************************

function [bblk,AAt,CC,bb,iscmp] = convertcmpsdp(blk,A,C,b)

m = length(b);
pp = size(A,1);
if (pp ~= size(blk,1))
    error('blk and A not compatible');
end
numblk = size(blk,1);
iscmp = zeros(numblk,m+1);
for p = 1:size(blk,1)
    len = size(A(p),2);
    for k = 1:len
        if ~isempty(A{p,k})
            iscmp(p,k) = 1-isreal(A{p,k});
        end
    end
    iscmp(p,m+1) = 1-isreal(C{p});
end
iscmp = norm(iscmp,'fro');
%%
if (iscmp == 0)
    %% data is real
    bblk = blk; AAt = A; CC = C; bb = b;
    return;
end
%%
bb = real(b);
bblk = cell(size(blk,1),2);
for p = 1:size(blk,1)
    pblk = blk(p,:);
    if (size(pblk{2},1) > size(pblk{2},2))
        pblk{2} = pblk{2}';
    end
    if strcmp(pblk{1},'s')
        ss = [0,cumsum(pblk{2})];
        ss2 = [0,cumsum(2*pblk{2})];
        n = sum(pblk{2});
        n2 = sum(pblk{2}.*(pblk{2}+1))/2;
        AR = cell(1,m); Ctmp = sparse(2*n,2*n);
        if (size(A{p},1)==n2 && size(A{p},2)==m);
            Atype = 1;
        elseif (size(A(p),1)==1 && size(A(p),2)==1);
            Atype = 2;
        else
            error('convertcmp: At is not properly coded');
        end
        for k = 0:m
            if (k == 0)
                Ak = C{p};
            else
                if (Atype == 1)
                    Ak = smat(pblk,A{p}(:,k),1);
                elseif (Atype == 2)
                    Ak = A{p,k};
                end
            end
            Atmp = sparse(2*n,2*n);
            if (length(pblk{2}) == 1)
                tmp = [real(Ak),-imag(Ak);  imag(Ak), real(Ak)];
                if (k==0)
                    Ctmp = tmp;
                else
                    Atmp = tmp;
                end
            else
                for j = 1:length(pblk{2})
                    idx = ss(j)+1: ss(j+1);
                    Akj = Ak(idx,idx);
                    tmp = [real(Akj),-imag(Akj);  imag(Akj), real(Akj)];
                    idx2 = ss2(j)+1: ss2(j+1);
                    if (k==0)
                        Ctmp(idx2,idx2) = tmp; %#ok
                    else
                        Atmp(idx2,idx2) = tmp; %#ok
                    end
                end
            end
            if (k==0);
                CC{p,1} = Ctmp; %#ok
            else
                AR{k} = Atmp;
            end
        end
        bblk{p,1} = 's'; bblk{p,2} = 2*pblk{2};
        AAt(p,1) = svec(bblk(p,:),AR); %#ok
    elseif strcmp(pblk{1},'q');
        error('SOCP block with complex data is currently not allowed');
    elseif strcmp(pblk{1},'l');
        if isreal(A{p}) && isreal(C{p})
            bblk(p,:) = blk(p,:);
            AAt{p,1} = A{p}; CC{p,1} = C{p}; %#ok
        else
            error('data for linear block must be real');
        end
    elseif strcmp(pblk{1},'u');
        if isreal(A{p}) && isreal(C{p})
            bblk(p,:) = blk(p,:);
            AAt{p,1} = A{p}; CC{p,1} = C{p}; %#ok
        else
            error('data for unrestricted block must be real');
        end
    end
end
%%*********************************************************


function [Fp, Fi] = triangle_triangle_adjacency(F)
  % TRIANGLE_TRIANGLE_ADJACENCY Build a face adjacency data structure for a
  % **manifold** triangle mesh. From each face we can find where on its
  % neighboring faces it's incident.
  %
  % [Fp, Fi] = triangle_triangle_adjacency(F)
  %
  % Input:
  %   F  list of face indices, #faces by 3
  % Output:
  %   Fp  #faces by 3, where Fp(i,j) tells the index of the neighboring triangle
  %     to the jth edge of the ith triangle in F. -1 if the jth edge of the ith
  %     triangle in F is a border edge. (jth edge refers to the edge opposite
  %     the jth vertex: so triangle in a triangle (a,b,c), the 1st edge is
  %     b-->c, the 2nd is c-->b and the 3rd is a-->b
  %   Fi  #faces by 3, where Fi(i,j) tells the position on the neighboring
  %     triangle to the jth edge of the ith triangle in F. -1 if the jth edge
  %     if the ith triangle in F is a border edge. Uses the same indexing of
  %     positions of edges on a triangle as above.
  %
  % For example:
  %
  % F = [ 1 3 2;
  %       2 3 4];
  % [Fp, Fi] = triangle_triangle_adjacency(F);
  % Fp =
  %     2    -1    -1
  %    -1    -1     1
  % Fi =
  %     3    -1    -1
  %    -1    -1     1
  %

  if size(F,2) == 3

    % get list of edges (1st edges then 2nd edges then 3rd edges)
    E = [F(:,2) F(:,3); F(:,3) F(:,1); F(:,1) F(:,2)];

    % sparse adjacency matrix where (i,j) = k, k is the index of i-->j in edge
    % list if i-->j AND j-->i exist in edge list. This way is slightly faster
    % than building a proper adjacency list first.
    Ei = sparse(E(:,1),E(:,2),1:size(E,1));
    % adjacency list of edges as if edges represent undirected graph
    unadj = Ei>0;
    % "non-adjacency" list, (i,j) > 0 iff i-->j exists but j-->i does not exist
    nonadj = unadj-(unadj');
    adj = ((unadj + nonadj)==1).*Ei;

    % need to get mapping from sparse ordering to original order
    [ii jj si] = find(adj);
    % this is slightly faster than sorting the above by ii, which is the same
    [ii jj v] = find(adj');
    % build map from edges to their corresponding faces
    E2F = repmat(1:size(F,1),1,3)';
    % initialize adjacency map to -1
    Fp = -ones(size(E,1),1);
    Fp(si) = E2F(v);
    Fp = reshape(Fp,size(F));
    % build map from edges to edge positions
    I = reshape(repmat((1:3),size(F,1),1),1,3*size(F,1))';
    % use corresponding edges to find positions of correponding faces
    Fi = -ones(size(E,1),1);
    Fi(si) = I(v);
    Fi = reshape(Fi,size(F));
  else
    error('Not supported yet...');
  end

end


function a = ge( x, y )

%Disciplined convex programming information for GE (>=):
%   The right-hand side of a less-than constraint must be convex. The
%   left-hand side must be concave. Of course, real constant and affine
%   expressions are both convex and concave and can be used on either
%   side as well.
%
%Disciplined geometric programming information for GE (>=):
%   The right-hand side of a less-than constraint must be log-convex---
%   including positive constants, monomials, posynomials, generalized
%   posynomials, and products thereof. The left-hand side must be log-
%   concave---including positive constants, monomials, reciprocals of
%   log-convex expressions, and products thereof.
%
%Note that CVX does not distinguish between strict greater-than (>) and
%greater-than-or-equal (<=) constraints; they are treated identically.
%Feasible interior-point solvers tend to return points which satisfy
%strict inequality, but not all solvers do.

evalin( 'caller', 'cvx_verify' );
b = cvx_pushcnstr( x, y, '>=' );
if nargout, a = b; end

% Copyright 2005-2014 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function setdt (delt)

% this function allows for the specification of the delta-t value
% (delta-t = tt - ut1) to be used in the calculation of sidereal
% time and the terrestrial-to-celestial transformation.  it allows
% these calculations to be performed, correctly, using ut1 as the
% time argument for the earth rotation angle and tdb as the time
% argument for the precession and nutation components.  this
% function, if used, should be called before any function
% related to earth rotation (e.g., sidtim or tercel) for a given
% date.  the value of delta-t specified here will be used until
% explicitly changed.

%  delt = value of delta-t (tt-ut1) in seconds (in)

% note 1:  the computed value of sidereal time, and the equivalent
% earth orientation angles, are relatively insensitive to the value
% of delta-t: up to only ~3 microarcseconds per second of delta-t.
% therefore, for many applications, this function either need not
% be called at all, or can be called just once for a wide range of
% dates (e.g., a year).  if this call is not used, a default
% value of delta-t of 64 seconds is used, which is appropriate to
% 2000.0.

% note 2:  the input time arguments to sidtim and tercel (tjdh and
% tjdl) are expressed in ut1 regardless of whether this call is
% used.

% ported from NOVAS 3.1

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

global dt

dt = delt / 86400.0d0;

end


function [ indx, i, j ] = sort_heap_external ( n, indx, isgn )

%*****************************************************************************80
%
%% SORT_HEAP_EXTERNAL externally sorts a list of items into ascending order.
%
%  Discussion:
%
%    The actual list of data is not passed to the routine.  Hence this
%    routine may be used to sort integers, reals, numbers, names,
%    dates, shoe sizes, and so on.  After each call, the routine asks
%    the user to compare or interchange two items, until a special
%    return value signals that the sorting is completed.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 February 2010
%
%  Author:
%
%    Original FORTRAN77 version by Albert Nijenhuis, Herbert Wilf.
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Albert Nijenhuis, Herbert Wilf.
%    Combinatorial Algorithms,
%    Academic Press, 1978, second edition,
%    ISBN 0-12-519260-6.
%
%  Parameters:
%
%    Input, integer N, the number of items to be sorted.
%
%    Input, integer INDX, the main communication signal.
%    The user must set INDX to 0 before the first call.
%    Thereafter, the user should set the input value of INDX
%    to the output value from the previous call.
%
%    Input, integer ISGN, results of comparison of elements I and J.
%    (Used only when the previous call returned INDX less than 0).
%    ISGN <= 0 means I is less than or equal to J;
%    0 <= ISGN means I is greater than or equal to J.
%
%    Output, integer INDX, the main communication signal.
%    If INDX is
%
%      greater than 0, the user should:
%      * interchange items I and J;
%      * call again.
%
%      less than 0, the user should:
%      * compare items I and J;
%      * set ISGN = -1 if I < J, ISGN = +1 if J < I;
%      * call again.
%
%      equal to 0, the sorting is done.
%
%    Output, integer I, J, the indices of two items.
%    On return with INDX positive, elements I and J should be interchanged.
%    On return with INDX negative, elements I and J should be compared, and
%    the result reported in ISGN on the next call.
%
  persistent i_save;
  persistent j_save;
  persistent k;
  persistent k1;
  persistent n1;

  if ( isempty ( i_save ) )
    i_save = -1;
  end

  if ( isempty ( j_save ) )
    j_save = -1;
  end
%
%  INDX = 0: This is the first call.
%
  if ( indx == 0 )

    k = floor ( n / 2 );
    k1 = k;
    n1 = n;
%
%  INDX < 0: The user is returning the results of a comparison.
%
  elseif ( indx < 0 )

    if ( indx == -2 )

      if ( isgn < 0 )
        i_save = i_save + 1;
      end

      j_save = k1;
      k1 = i_save;
      indx = -1;
      i = i_save;
      j = j_save;
      return;
    end

    if ( 0 < isgn )
      indx = 2;
      i = i_save;
      j = j_save;
      return;
    end

    if ( k <= 1 )

      if ( n1 == 1 )
        i_save = 0;
        j_save = 0;
        indx = 0;
      else
        i_save = n1;
        n1 = n1 - 1;
        j_save = 1;
        indx = 1;
      end

      i = i_save;
      j = j_save;
      return;

    end

    k = k - 1;
    k1 = k;
%
%  0 < INDX, the user was asked to make an interchange.
%
  elseif ( indx == 1 )

    k1 = k;

  end

  while ( 1 )

    i_save = 2 * k1;

    if ( i_save == n1 )
      j_save = k1;
      k1 = i_save;
      indx = -1;
      i = i_save;
      j = j_save;
      return;
    elseif ( i_save <= n1 )
      j_save = i_save + 1;
      indx = -2;
      i = i_save;
      j = j_save;
      return;
    end

    if ( k <= 1 )
      break;
    end

    k = k - 1;
    k1 = k;

  end

  if ( n1 == 1 )
    i_save = 0;
    j_save = 0;
    indx = 0;
    i = i_save;
    j = j_save;
  else
    i_save = n1;
    n1 = n1 - 1;
    j_save = 1;
    indx = 1;
    i = i_save;
    j = j_save;
  end

  return
end


function u = cos(a)
%COS          Slope cosine cos(a)
%

% written  12/06/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_SLOPE = getappdata(0,'INTLAB_SLOPE');

  u = a;

  u.r = cos(a.r);
  indexc = 1:INTLAB_SLOPE.NUMVAR;
  indexr = 2:INTLAB_SLOPE.NUMVAR+1;
  Xxs = hull(a.r(:,indexc),a.r(:,indexr));
  u.s = - sin(Xxs) .* a.s;

  if rndold
    setround(rndold)
  end


function node_boundary = triangulation_order6_boundary_node ( node_num, ...
  triangle_num, triangle_node )

%*****************************************************************************80
%
%% TRIANGULATION_ORDER6_BOUNDARY_NODE indicates which nodes are on the boundary.
%
%  Discussion:
%
%    This routine is given an order 6 triangulation, an abstract list of sets
%    of six nodes.  The vertices are listed clockwise, then the
%    midside nodes.
%
%    It is assumed that each edge of the triangulation is either
%    * an INTERIOR edge, which is listed twice, once with positive
%      orientation and once with negative orientation, or;
%    * a BOUNDARY edge, which will occur only once.
%
%    This routine should work even if the region has holes - as long
%    as the boundary of the hole comprises more than 3 edges!
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 June 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, integer TRIANGLE_NUM, the number of triangles.
%
%    Input, integer TRIANGLE_NODE(6,TRIANGLE_NUM), the nodes that make up the
%    triangles.
%
%    Output, logical NODE_BOUNDARY(NODE_NUM), is TRUE if the node
%    is on a boundary edge.
%
  m = 3;
  n = 3 * triangle_num;
%
%  Set up the edge array.  The midside node is listed last, as
%  it is not needed for the sorting process.
%
  edge(1,               1:  triangle_num) = triangle_node(1,1:triangle_num);
  edge(2,               1:  triangle_num) = triangle_node(4,1:triangle_num);
  edge(3,               1:  triangle_num) = triangle_node(2,1:triangle_num);

  edge(1,  triangle_num+1:2*triangle_num) = triangle_node(2,1:triangle_num);
  edge(2,  triangle_num+1:2*triangle_num) = triangle_node(5,1:triangle_num);
  edge(3,  triangle_num+1:2*triangle_num) = triangle_node(3,1:triangle_num);

  edge(1,2*triangle_num+1:3*triangle_num) = triangle_node(3,1:triangle_num);
  edge(2,2*triangle_num+1:3*triangle_num) = triangle_node(6,1:triangle_num);
  edge(3,2*triangle_num+1:3*triangle_num) = triangle_node(1,1:triangle_num);
%
%  In each column, force the smaller of the two vertices to appear first.
%
  e1(1:n) = min ( edge(1:2:3,1:n) );
  e2(1:n) = max ( edge(1:2:3,1:n) );

  edge(1,1:n) = e1(1:n);
  edge(3,1:n) = e2(1:n);
%
%  Ascending sort the column array.
%
  edge = ( sortrows ( edge' ) )';
%
%  Records which appear twice are internal edges and can be ignored.
%
  node_boundary(1:node_num) = 0;

  i = 0;

  while ( i < 3 * triangle_num )

    i = i + 1;

    if ( i == 3 * triangle_num )
      node_boundary(edge(1:m,i)) = 1;
    elseif ( all ( edge(1:m,i) == edge(1:m,i+1) ) )
      i = i + 1;
    else
      node_boundary(edge(1:m,i)) = 1;
    end

  end

  return
end


classdef ballfunv
%BALLFUNV   BALLFUNV class for representing vector-valued functions on the unit ball.
%
%  Class for approximating smooth vector-valued functions defined on the unit ball.
%
% BALLFUNV(F, G, H) constructs a BALLFUNV object representing the vector-valued
% function [F;G;H] on the unit ball. F, G, and H may be BALLFUN objects, function
% handles or scalars. If they are function handles, then those function handles
% should be vectorized.
%
% See also BALLFUN.

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% CLASS PROPERTIES:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    properties

        comp   % The three components Vx, Vy, Vz of a BALLFUNV

    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% CLASS CONSTRUCTOR:
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    methods ( Access = public, Static = false )
        % Take the 3 components
        function F = ballfunv(varargin)

            % Return an empty BALLFUNV:
            if ( (nargin == 0) || isempty(varargin) )
                F.comp = {};
                return
            end

            % If the argument is a BALLFUNV, nothing to do:
            if ( isa(varargin{1}, 'ballfunv') )
                F = varargin{1};
                return
            end

            % BALLFUNV objects are vector-valued so complain if there
            % are less than 3 components:
            if ( numel(varargin) < 2 )
                error('BALLFUNV:ballfunv', ...
                    'Less than three components is not supported.')
            end

            % BALLFUNV objects cannot contain more than five components.
            % Complain if we have been given six or more.
            if ( numel(varargin) > 5 )
                error('BALLFUNV:ballfunv', ...
                    'More than four components is not supported.')
            end

            % Create a BALLFUNV from 3 ballfun or function handles
            if ( numel(varargin) == 3 )
                for jj = 1:3
                   if isa(varargin{jj}, 'ballfun') == 0
                       varargin{jj} = ballfun( varargin{jj} );
                   end
                end

                fh{1} = varargin{1};
                fh{2} = varargin{2};
                fh{3} = varargin{3};
            end

            % Create a BALLFUNV from 3 function handles in spherical
            % coordinates
            if ( numel(varargin) == 4 )
                if strcmp(varargin{4}, 'spherical') || strcmp(varargin{4}, 'polar')
                    fh{1} = ballfun(varargin{1},'spherical');
                    fh{2} = ballfun(varargin{2},'spherical');
                    fh{3} = ballfun(varargin{3},'spherical');
                else
                    error('BALLFUNV:ballfunv', ...
                        'Input arguments not supported')
                end
            end

            F.comp = fh;
        end
    end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% STATIC METHODS:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    methods ( Access = public, Static = true )
        varargout = PT2ballfunv(P,T);
    end

    methods ( Access = private, Static = true )

    end

end


%% FAST Algorithm for Corner Detection
%
% In this demo, we will:
%
% * understand the basics of FAST algorithm
% * find corners using OpenCV functionalities for FAST algorithm.
%
% Sources:
%
% * <https://docs.opencv.org/3.2.0/df/d0c/tutorial_py_fast.html>
%

%% Theory
%
% We saw several feature detectors and many of them are really good. But when
% looking from a real-time application point of view, they are not fast
% enough. One best example would be SLAM (Simultaneous Localization and
% Mapping) mobile robot which have limited computational resources.
%
% As a solution to this, FAST (Features from Accelerated Segment Test)
% algorithm was proposed by Edward Rosten and Tom Drummond in their paper
% "Machine learning for high-speed corner detection" in 2006 (later revised it
% in 2010). A basic summary of the algorithm is presented below. Refer to the
% original paper for more details (All the images are taken from original
% paper).
%
%% Feature Detection using FAST
%
% * Select a pixel $p$ in the image which is to be identified as an interest
%   point or not. Let its intensity be $I_p$.
% * Select appropriate threshold value $t$.
% * Consider a circle of 16 pixels around the pixel under test (See the image
%   below)
%
% <<https://docs.opencv.org/3.2.0/fast_speedtest.jpg>>
%
% * Now the pixel $p$ is a corner if there exists a set of $n$ contiguous
%   pixels in the circle (of 16 pixels) which are all brighter than $I_p + t$,
%   or all darker than $I_p - t$ (Shown as white dash lines in the above
%   image). $n$ was chosen to be 12.
% * A *high-speed test* was proposed to exclude a large number of non-corners.
%   This test examines only the four pixels at 1, 9, 5 and 13 (First 1 and 9
%   are tested if they are too brighter or darker. If so, then checks 5 and
%   13). If $p$ is a corner, then at least three of these must all be brighter
%   than $I_p + t$ or darker than $I_p - t$. If neither of these is the case,
%   then $p$ cannot be a corner. The full segment test criterion can then be
%   applied to the passed candidates by examining all pixels in the circle.
%
% This detector in itself exhibits high performance, but there are several
% weaknesses:
%
% * It does not reject as many candidates for |n < 12|.
% * The choice of pixels is not optimal because its efficiency depends on
%   ordering of the questions and distribution of corner appearances.
% * Results of high-speed tests are thrown away.
% * Multiple features are detected adjacent to one another.
%
% First 3 points are addressed with a machine learning approach. Last one is
% addressed using non-maximal suppression.
%
%% Machine Learning a Corner Detector
%
% * Select a set of images for training (preferably from the target
%   application domain)
% * Run FAST algorithm in every images to find feature points.
% * For every feature point, store the 16 pixels around it as a vector. Do it
%   for all the images to get feature vector $P$.
% * Each pixel (say $x$) in these 16 pixels can have one of the following
%   three states:
%
% <<https://docs.opencv.org/3.2.0/fast_eqns.jpg>>
%
% * Depending on these states, the feature vector $P$ is subdivided into 3
%   subsets, $P_d$, $P_s$, $P_b$.
% * Define a new boolean variable, $K_p$, which is true if $p$ is a corner and
%   false otherwise.
% * Use the ID3 algorithm (decision tree classifier) to query each subset
%   using the variable $K_p$ for the knowledge about the true class. It
%   selects the $x$ which yields the most information about whether the
%   candidate pixel is a corner, measured by the entropy of $K_p$.
% * This is recursively applied to all the subsets until its entropy is zero.
% * The decision tree so created is used for fast detection in other images.
%
%% Non-maximal Suppression
%
% Detecting multiple interest points in adjacent locations is another problem.
% It is solved by using Non-maximum Suppression.
%
% * Compute a score function, $V$ for all the detected feature points. $V$ is
%   the sum of absolute difference between $p$ and 16 surrounding pixels
%   values.
% * Consider two adjacent keypoints and compute their $V$ values.
% * Discard the one with lower $V$ value.
%
%% Summary
%
% It is several times faster than other existing corner detectors.
%
% But it is not robust to high levels of noise. It is dependent on a
% threshold.
%

%% FAST Feature Detector in OpenCV
%
% It is called as any other feature detector in OpenCV. If you want, you can
% specify the threshold, whether non-maximum suppression to be applied or not,
% the neighborhood to be used etc.
%
% For the neighborhood, three flags are defined, |TYPE_5_8|, |TYPE_7_12|, and
% |TYPE_9_16|. Below is a simple code on how to detect and draw the FAST
% feature points.
%

%%
% load source image
img = cv.imread(fullfile(mexopencv.root(),'test','blox.jpg'), 'Grayscale',true);

%%
% create FAST object with default values, and find keypoints
fast = cv.FastFeatureDetector();
kp1 = fast.detect(img);
fprintf('%d keypoints with NonmaxSuppression\n', numel(kp1));

%%
% disable |NonmaxSuppression| and find keypoints
fast.NonmaxSuppression = false;
kp2 = fast.detect(img);
fprintf('%d keypoints without NonmaxSuppression\n', numel(kp2));

%%
% draw keypoints from both cases, and show results
out1 = cv.drawKeypoints(img, kp1, 'Color',[255 0 0]);
out2 = cv.drawKeypoints(img, kp2, 'Color',[255 0 0]);
subplot(121), imshow(out1), title('FAST w/ NonmaxSuppression')
subplot(122), imshow(out2), title('FAST w/o NonmaxSuppression')

%% Additional Resources
%
% * Edward Rosten and Tom Drummond, "Machine learning for high speed corner
%   detection" in 9th European Conference on Computer Vision, vol. 1, 2006,
%   pp. 430-443.
% * Edward Rosten, Reid Porter, and Tom Drummond, "Faster and better: a
%   machine learning approach to corner detection" in IEEE Trans. Pattern
%   Analysis and Machine Intelligence, 2010, vol 32, pp. 105-119.
%


%+========================================================================+
%|                                                                        |
%|            This script uses the GYPSILAB toolbox for Matlab            |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal & Francois Alouges (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                          |
%|             francois.alouges@polytechnique.edu                         |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : nrtHmxFemBemEFIEhalf.m                        |
%|    #    |   VERSION    : 0.40                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal & Francois Alouges            |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 14.03.2018                                    |
%| ( === ) |   SYNOPSIS   : Solve fem/bem with PEC scatering problem      |
%|  `---'  |                Volumic ans surfacic EFIE formulation         |
%+========================================================================+

% Cleaning
clear all
close all
clc

% Gypsilab path
run('../../addpathGypsilab.m')

%%% PREPARATION
disp('~~~~~~~~~~~~~ PREPARATION ~~~~~~~~~~~~~')

% Spherical mesh
mesh = msh('sphere_1e3.msh');

% Normalization interior sphere
mesh.vtx = mesh.vtx/0.8;

% Interior boundary
bound = mesh.bnd;
Iint  = (sum(bound.ctr.*bound.nrm,2) <= 0);
int   = swap(bound.sub(Iint));

% Final volumn
ctr = mesh.ctr;
vol = mesh.sub(ctr(:,3)>=0);

% Boundaries
tmp  = vol.bnd;
tmp1 = setdiff(int,tmp);
dvol = setdiff(tmp,int);
ext  = union(tmp1,dvol);
int  = intersect(int,tmp);

figure
plot(ext)
hold on
plotNrm(ext,'w')
plot(dvol,'b')
plot(int,'r')
axis equal
alpha(0.5)

% Domaine volumique
omega = dom(vol,4);

% Domaines surfacic
sigma = dom(dvol,3);
gamma = dom(ext,3);

% Frequency adjusted to maximum edge size
stp = mesh.stp;
k   = 1/stp(2);
c   = 299792458;
f   = (k*c)/(2*pi);
disp(['Frequency : ',num2str(f/1e6),' MHz']);

% Accuracy
tol = 1e-3;
disp(['Accuracy : ',num2str(tol)]);

% Incident direction and field
X0 = [0 0 -1];
E  = [0 1  0]; % Polarization (+x for Theta-Theta and +y for Phi-Phi)
H  = cross(X0,E);

% Incident Plane wave (electromagnetic field)
PWE{1} = @(X) exp(1i*k*X*X0') * E(1);
PWE{2} = @(X) exp(1i*k*X*X0') * E(2);
PWE{3} = @(X) exp(1i*k*X*X0') * E(3);

% Incident wave representation
figure
plot(ext,real(PWE{2}(ext.vtx)))
title('Incident wave')
xlabel('X');   ylabel('Y');   zlabel('Z');
axis equal
view(0,10)

%%% FORMUMATIONS
disp('~~~~~~~~~~~~~ FORMULATIONS ~~~~~~~~~~~~~')

% Green kernel
Gxy    = @(X,Y) femGreenKernel(X,Y,'[exp(ikr)/r]',k);
Hxy{1} = @(X,Y) femGreenKernel(X,Y,'grady[exp(ikr)/r]1',k) ;
Hxy{2} = @(X,Y) femGreenKernel(X,Y,'grady[exp(ikr)/r]2',k) ;
Hxy{3} = @(X,Y) femGreenKernel(X,Y,'grady[exp(ikr)/r]3',k) ;

% Surfacic finite elements for magnetic current
Jh = fem(ext,'RWG');
N1 = size(Jh.unk,1);

% Volumique finite elements for electric field
Eh = fem(vol,'NED');
Eh = dirichlet(Eh,int);
N2 = size(Eh.unk,1);

% Restriction matrix for regularization
[~,I1,I2] = intersect(Jh.unk,Eh.unk,'rows');
P         = sparse(I1,I2,1,size(Jh.unk,1),size(Eh.unk,1));

% Sparse operators
tic
rotErotE = integral(omega,curl(Eh),curl(Eh));
EE       = integral(omega,Eh,Eh);
JE       = integral(sigma,Jh,Eh);
toc

% Full operators
tic
JTJ = 1/(4*pi) .* integral(gamma, gamma, Jh, Gxy, Jh,tol) ...
    - 1/(4*pi*k^2) * integral(gamma, gamma, div(Jh), Gxy, div(Jh),tol) ;
JTJ = JTJ + 1/(4*pi) * regularize(gamma, gamma, Jh, '[1/r]', Jh) ...
      - 1/(4*pi*k^2) * regularize(gamma, gamma, div(Jh), '[1/r]', div(Jh));
toc

tic
JKExn = - 1/(4*pi) * integral(gamma, sigma, Jh, Hxy, nx(Eh),tol);
JKExn = JKExn - 1/(4*pi) * regularize(gamma, gamma, Jh, 'grady[1/r]', Jh) * P;
toc

% LHS = [A B ; C D]
A = 1i*k *  JTJ;
size(A)

B = JKExn - 0.5 * JE;
size(B)

C = JE.';
size(C)

D = 1/(1i*k) * (rotErotE - k^2*EE);
size(D)

% Right hand side
Y   = - integral(gamma,Jh,PWE);
RHS = [Y;zeros(size(D,1),1)];

%%% SOLVE LINEAR PROBLEM
disp('~~~~~~~~~~~~~ SOLVE LINEAR PROBLEM ~~~~~~~~~~~~~')

% Factorization LU H-Matrix
tic
[La,Ua] = lu(A);
toc

figure
subplot(1,2,1)
spy(La)
subplot(1,2,2)
spy(Ua)

% Shurr complement resolution
tic
Sm1V = @(V) Ua\(La\V);
SV   = @(V) A*V - B*(D\(C*V));
J    = gmres(SV,Y,[],tol,100,Sm1V);
E    = - D\(C*J);
toc

%%% INFINITE SOLUTION
disp('~~~~~~~~~~~~~ INFINITE RADIATION ~~~~~~~~~~~~~')

% Plane waves direction
Ninf  = 1e3;
theta = 2*pi/1e3 .* (1:Ninf)';
nu    = [sin(theta),zeros(size(theta)),cos(theta)];

% Green kernel function
xdoty = @(X,Y) X(:,1).*Y(:,1) + X(:,2).*Y(:,2) + X(:,3).*Y(:,3);
Ginf  = @(X,Y) exp(-1i*k*xdoty(X,Y));

% Magnetic current radiation
Tinf = integral(nu,gamma,Ginf,Jh);
Jinf = 1i*k/(4*pi)*cross(nu, cross([Tinf{1}*J, Tinf{2}*J, Tinf{3}*J], nu));

% Electric field radiation
Kinf = integral(nu,sigma,Ginf,nx(Eh));
Einf = 1i*k/(4*pi) * cross(nu, [-Kinf{1}*E, -Kinf{2}*E, -Kinf{3}*E] );

% Total electric field radiation
sol = Jinf - Einf;

% Radiation infinie de reference, convention e^(+ikr)/r
nMax = 100; refInf = zeros(Ninf,1);
if E(1) == 1
    for jj = 1:Ninf
        refInf(jj,:) = sphereMaxwell(1, -f, theta(jj), 0.0, nMax);
    end
else
    for jj = 1:Ninf
        [~,refInf(jj)] = sphereMaxwell(1, -f, theta(jj), pi/2, nMax);
    end
end
refInf = refInf ./ sqrt(4*pi);

% Radiations infinies en X
if E(1) == 1
    sol = sin(theta)'.*sol(:,3) - cos(theta)'.*sol(:,1);
else
    sol = sol(:,2);
end

% Erreur
eL2   = norm(refInf-sol,2)/norm(refInf,2)
eLINF = norm(refInf-sol,'inf')/norm(refInf,'inf')

% Representation graphique
figure
subplot(1,2,1)
plot(theta,20*log10(abs(sol)),'b',theta,20*log10(abs(refInf)),'r--')

subplot(1,2,2)
plot(theta,real(sol),'--b', theta,imag(sol),'--r', theta, real(refInf),':b', theta,imag(refInf),':r');
drawnow

disp('~~> Michto gypsilab !')