function [x cost info] = trustregions(problem, x, options)
% Riemannian trust-regions solver for optimization on manifolds.
%
% function [x cost info] = trustregions(problem)
% function [x cost info] = trustregions(problem, x0)
% function [x cost info] = trustregions(problem, x0, options)
% function [x cost info] = trustregions(problem, [], options)
%
% This is the Riemannian Trust-Region solver (with tCG inner solve), named
% RTR. This solver will attempt to minimize the cost function described in
% the problem structure. It requires the availability of the cost function
% and of its gradient. It will issue calls for the Hessian. If no Hessian
% nor approximate Hessian is provided, a standard approximation of the
% Hessian based on the gradient will be computed. If a preconditioner for
% the Hessian is provided, it will be used.
%
% For a description of the algorithm and theorems offering convergence
% guarantees, see the references below. Documentation for this solver is
% available online at:
%
% http://www.manopt.org/solver_documentation_trustregions.html
%
%
% The initial iterate is x0 if it is provided. Otherwise, a random point on
% the manifold is picked. To specify options whilst not specifying an
% initial iterate, give x0 as [] (the empty matrix).
%
% The two outputs 'x' and 'cost' are the last reached point on the manifold
% and its cost. Notice that x is not necessarily the best reached point,
% because this solver is not forced to be a descent method. In particular,
% very close to convergence, it is sometimes preferable to accept very
% slight increases in the cost value (on the order of the machine epsilon)
% in the process of reaching fine convergence. In practice, this is not a
% limiting factor, as normally one does not need fine enough convergence
% that this becomes an issue.
%
% The output 'info' is a struct-array which contains information about the
% iterations:
%   iter (integer)
%       The (outer) iteration number, or number of steps considered
%       (whether accepted or rejected). The initial guess is 0.
%	cost (double)
%       The corresponding cost value.
%	gradnorm (double)
%       The (Riemannian) norm of the gradient.
%	numinner (integer)
%       The number of inner iterations executed to compute this iterate.
%       Inner iterations are truncated-CG steps. Each one requires a
%       Hessian (or approximate Hessian) evaluation.
%	time (double)
%       The total elapsed time in seconds to reach the corresponding cost.
%	rho (double)
%       The performance ratio for the iterate.
%	rhonum, rhoden (double)
%       Regularized numerator and denominator of the performance ratio:
%       rho = rhonum/rhoden. See options.rho_regularization.
%	accepted (boolean)
%       Whether the proposed iterate was accepted or not.
%	stepsize (double)
%       The (Riemannian) norm of the vector returned by the inner solver
%       tCG and which is retracted to obtain the proposed next iterate. If
%       accepted = true for the corresponding iterate, this is the size of
%       the step from the previous to the new iterate. If accepted is
%       false, the step was not executed and this is the size of the
%       rejected step.
%	Delta (double)
%       The trust-region radius at the outer iteration.
%	cauchy (boolean)
%       Whether the Cauchy point was used or not (if useRand is true).
%   And possibly additional information logged by options.statsfun.
% For example, type [info.gradnorm] to obtain a vector of the successive
% gradient norms reached at each (outer) iteration.
%
% The options structure is used to overwrite the default values. All
% options have a default value and are hence optional. To force an option
% value, pass an options structure with a field options.optionname, where
% optionname is one of the following and the default value is indicated
% between parentheses:
%
%   tolgradnorm (1e-6)
%       The algorithm terminates if the norm of the gradient drops below
%       this. For well-scaled problems, a rule of thumb is that you can
%       expect to reduce the gradient norm by 8 orders of magnitude
%       (sqrt(eps)) compared to the gradient norm at a "typical" point (a
%       rough initial iterate for example). Further decrease is sometimes
%       possible, but inexact floating point arithmetic will eventually
%       limit the final accuracy. If tolgradnorm is set too low, the
%       algorithm may end up iterating forever (or at least until another
%       stopping criterion triggers).
%   maxiter (1000)
%       The algorithm terminates if maxiter (outer) iterations were executed.
%   maxtime (Inf)
%       The algorithm terminates if maxtime seconds elapsed.
%	miniter (3)
%       Minimum number of outer iterations (used only if useRand is true).
%	mininner (1)
%       Minimum number of inner iterations (for tCG).
%	maxinner (problem.M.dim() : the manifold's dimension)
%       Maximum number of inner iterations (for tCG).
%	Delta_bar (problem.M.typicaldist() or sqrt(problem.M.dim()))
%       Maximum trust-region radius. If you specify this parameter but not
%       Delta0, then Delta0 will be set to 1/8 times this parameter.
%   Delta0 (Delta_bar/8)
%       Initial trust-region radius. If you observe a long plateau at the
%       beginning of the convergence plot (gradient norm VS iteration), it
%       may pay off to try to tune this parameter to shorten the plateau.
%       You should not set this parameter without setting Delta_bar.
%	useRand (false)
%       Set to true if the trust-region solve is to be initiated with a
%       random tangent vector. If set to true, no preconditioner will be
%       used. This option is set to true in some scenarios to escape saddle
%       points, but is otherwise seldom activated.
%	kappa (0.1)
%       Inner kappa convergence tolerance.
%	theta (1.0)
%       Inner theta convergence tolerance.
%	rho_prime (0.1)
%       Accept/reject ratio : if rho is at least rho_prime, the outer
%       iteration is accepted. Otherwise, it is rejected. In case it is
%       rejected, the trust-region radius will have been decreased.
%       To ensure this, rho_prime must be strictly smaller than 1/4.
%   rho_regularization (1e3)
%       Close to convergence, evaluating the performance ratio rho is
%       numerically challenging. Meanwhile, close to convergence, the
%       quadratic model should be a good fit and the steps should be
%       accepted. Regularization lets rho go to 1 as the model decrease and
%       the actual decrease go to zero. Set this option to zero to disable
%       regularization (not recommended). See in-code for the specifics.
%   statsfun (none)
%       Function handle to a function that will be called after each
%       iteration to provide the opportunity to log additional statistics.
%       They will be returned in the info struct. See the generic Manopt
%       documentation about solvers for further information. statsfun is
%       called with the point x that was reached last, after the
%       accept/reject decision. See comment below.
%   stopfun (none)
%       Function handle to a function that will be called at each iteration
%       to provide the opportunity to specify additional stopping criteria.
%       See the generic Manopt documentation about solvers for further
%       information.
%   verbosity (2)
%       Integer number used to tune the amount of output the algorithm
%       generates during execution (mostly as text in the command window).
%       The higher, the more output. 0 means silent. 3 and above includes a
%       display of the options structure at the beginning of the execution.
%   debug (false)
%       Set to true to allow the algorithm to perform additional
%       computations for debugging purposes. If a debugging test fails, you
%       will be informed of it, usually via the command window. Be aware
%       that these additional computations appear in the algorithm timings
%       too.
%   storedepth (20)
%       Maximum number of different points x of the manifold for which a
%       store structure will be kept in memory in the storedb. If the
%       caching features of Manopt are not used, this is irrelevant. If
%       memory usage is an issue, you may try to lower this number.
%       Profiling may then help to investigate if a performance hit was
%       incured as a result.
%
% Notice that statsfun is called with the point x that was reached last,
% after the accept/reject decision. Hence: if the step was accepted, we get
% that new x, with a store which only saw the call for the cost and for the
% gradient. If the step was rejected, we get the same x as previously, with
% the store structure containing everything that was computed at that point
% (possibly including previous rejects at that same point). Hence, statsfun
% should not be used in conjunction with the store to count operations for
% example. Instead, you could use a global variable and increment that
% variable directly from the cost related functions. It is however possible
% to use statsfun with the store to compute, for example, alternate merit
% functions on the point x.
%
% See also: steepestdescent conjugategradient manopt/examples

% This file is part of Manopt: www.manopt.org.
% This code is an adaptation to Manopt of the original GenRTR code:
% RTR - Riemannian Trust-Region
% (c) 2004-2007, P.-A. Absil, C. G. Baker, K. A. Gallivan
% Florida State University
% School of Computational Science
% (http://www.math.fsu.edu/~cbaker/GenRTR/?page=download)
% See accompanying license file.
% The adaptation was executed by Nicolas Boumal.
%
% Change log:
%
%   NB April 3, 2013:
%       tCG now returns the Hessian along the returned direction eta, so
%       that we do not compute that Hessian redundantly: some savings at
%       each iteration. Similarly, if the useRand flag is on, we spare an
%       extra Hessian computation at each outer iteration too, owing to
%       some modifications in the Cauchy point section of the code specific
%       to useRand = true.
%
%   NB Aug. 22, 2013:
%       This function is now Octave compatible. The transition called for
%       two changes which would otherwise not be advisable. (1) tic/toc is
%       now used as is, as opposed to the safer way:
%       t = tic(); elapsed = toc(t);
%       And (2), the (formerly inner) function savestats was moved outside
%       the main function to not be nested anymore. This is arguably less
%       elegant, but Octave does not (and likely will not) support nested
%       functions.
%
%   NB Dec. 2, 2013:
%       The in-code documentation was largely revised and expanded.
%
%   NB Dec. 2, 2013:
%       The former heuristic which triggered when rhonum was very small and
%       forced rho = 1 has been replaced by a smoother heuristic which
%       consists in regularizing rhonum and rhoden before computing their
%       ratio. It is tunable via options.rho_regularization. Furthermore,
%       the solver now detects if tCG did not obtain a model decrease
%       (which is theoretically impossible but may happen because of
%       numerical errors and/or because of a nonlinear/nonsymmetric Hessian
%       operator, which is the case for finite difference approximations).
%       When such an anomaly is detected, the step is rejected and the
%       trust region radius is decreased.
%
%   NB Dec. 3, 2013:
%       The stepsize is now registered at each iteration, at a small
%       additional cost. The defaults for Delta_bar and Delta0 are better
%       defined. Setting Delta_bar in the options will automatically set
%       Delta0 accordingly. In Manopt 1.0.4, the defaults for these options
%       were not treated appropriately because of an incorrect use of the
%       isfield() built-in function.

% Verify that the problem description is sufficient for the solver.
if ~canGetCost(problem)
    warning('manopt:getCost', ...
            'No cost provided. The algorithm will likely abort.');
end
if ~canGetGradient(problem)
    warning('manopt:getGradient', ...
            'No gradient provided. The algorithm will likely abort.');
end
if ~canGetHessian(problem)
    warning('manopt:getHessian:approx', ...
            'No Hessian provided. Using an approximation instead.');
end

% Define some strings for display
tcg_stop_reason = {'negative curvature',...
                   'exceeded trust region',...
                   'reached target residual-kappa',...
                   'reached target residual-theta',...
                   'dimension exceeded',...
                   'model increased'};

% Set local defaults here
localdefaults.verbosity = 2;
localdefaults.maxtime = inf;
localdefaults.miniter = 3;
localdefaults.maxiter = 1000;
localdefaults.mininner = 1;
localdefaults.maxinner = problem.M.dim();
localdefaults.tolgradnorm = 1e-6;
localdefaults.kappa = 0.1;
localdefaults.theta = 1.0;
localdefaults.rho_prime = 0.1;
localdefaults.useRand = false;
localdefaults.rho_regularization = 1e3;

% Merge global and local defaults, then merge w/ user options, if any.
localdefaults = mergeOptions(getGlobalDefaults(), localdefaults);
if ~exist('options', 'var') || isempty(options)
    options = struct();
end
options = mergeOptions(localdefaults, options);

% Set default Delta_bar and Delta0 separately to deal with additional
% logic: if Delta_bar is provided but not Delta0, let Delta0 automatically
% be some fraction of the provided Delta_bar.
if ~isfield(options, 'Delta_bar')
    if isfield(problem.M, 'typicaldist')
        options.Delta_bar = problem.M.typicaldist();
    else
        options.Delta_bar = sqrt(problem.M.dim());
    end
end
if ~isfield(options,'Delta0')
    options.Delta0 = options.Delta_bar / 8;
end

% Check some option values
assert(options.rho_prime < 1/4, ...
        'options.rho_prime must be strictly smaller than 1/4.');
assert(options.Delta_bar > 0, ...
        'options.Delta_bar must be positive.');
assert(options.Delta0 > 0 && options.Delta0 < options.Delta_bar, ...
        'options.Delta0 must be positive and smaller than Delta_bar.');

% It is sometimes useful to check what the actual option values are.
if options.verbosity >= 3
    disp(options);
end

% Create a store database
storedb = struct();

tic();

% If no initial point x is given by the user, generate one at random.
if ~exist('x', 'var') || isempty(x)
    x = problem.M.rand();
end

%% Initializations

% k counts the outer (TR) iterations. The semantic is that k counts the
% number of iterations fully executed so far.
k = 0;

% initialize solution and companion measures: f(x), fgrad(x)
[fx fgradx storedb] = getCostGrad(problem, x, storedb);
norm_grad = problem.M.norm(x, fgradx);

% initialize trust-region radius
Delta = options.Delta0;

% Save stats in a struct array info, and preallocate
% (see http://people.csail.mit.edu/jskelly/blog/?x=entry:entry091030-033941)
if ~exist('used_cauchy', 'var')
    used_cauchy = [];
end
stats = savestats(problem, x, storedb, options, k, fx, norm_grad, Delta);
info(1) = stats;
info(min(10000, options.maxiter+1)).iter = [];

% ** Display:
if options.verbosity == 2
   fprintf(['%3s %3s      %5s                %5s     ',...
            'f: %e   |grad|: %e\n'],...
           '   ','   ','     ','     ', fx, norm_grad);
elseif options.verbosity > 2
   fprintf('************************************************************************\n');
   fprintf('%3s %3s    k: %5s     num_inner: %5s     %s\n',...
           '','','______','______','');
   fprintf('       f(x) : %e       |grad| : %e\n', fx, norm_grad);
   fprintf('      Delta : %f\n', Delta);
end

% **********************
% ** Start of TR loop **
% **********************
while true

	% Start clock for this outer iteration
    tic();

    % Run standard stopping criterion checks
    [stop reason] = stoppingcriterion(problem, x, options, info, k+1);

    % If the stopping criterion that triggered is the tolerance on the
    % gradient norm but we are using randomization, make sure we make at
    % least miniter iterations to give randomization a chance at escaping
    % saddle points.
    if stop == 2 && options.useRand && k < options.miniter
        stop = 0;
    end

    if stop
        if options.verbosity >= 1
            fprintf([reason '\n']);
        end
        break;
    end

    if options.verbosity > 2 || options.debug > 0
        fprintf('************************************************************************\n');
    end

    % *************************
    % ** Begin TR Subproblem **
    % *************************

    % Determine eta0
    if ~options.useRand
        % Pick the zero vector
        eta = problem.M.zerovec(x);
    else
        % Random vector in T_x M (this has to be very small)
        eta = problem.M.lincomb(x, 1e-6, problem.M.randvec(x));
        % Must be inside trust-region
        while problem.M.norm(x, eta) > Delta
            eta = problem.M.lincomb(x, sqrt(sqrt(eps)), eta);
        end
    end

    % solve TR subproblem
    [eta Heta numit stop_inner storedb] = ...
                     tCG(problem, x, fgradx, eta, Delta, options, storedb);
    srstr = tcg_stop_reason{stop_inner};

    % This is only computed for logging purposes, because it may be useful
    % for some user-defined stopping criteria. If this is not cheap for
    % specific application (compared to evaluating the cost), we should
    % reconsider this.
    norm_eta = problem.M.norm(x, eta);

    if options.debug > 0
        testangle = problem.M.inner(x, eta, fgradx) / (norm_eta*norm_grad);
    end

    % If using randomized approach, compare result with the Cauchy point.
    % Convergence proofs assume that we achieve at least the reduction of
    % the Cauchy point. After this if-block, either all eta-related
    % quantities have been changed consistently, or none of them have
    % changed.
    if options.useRand
        used_cauchy = false;
        % Check the curvature,
        [Hg storedb] = getHessian(problem, x, fgradx, storedb);
        g_Hg = problem.M.inner(x, fgradx, Hg);
        if g_Hg <= 0
            tau_c = 1;
        else
            tau_c = min( norm_grad^3/(Delta*g_Hg) , 1);
        end
        % and generate the Cauchy point.
        eta_c  = problem.M.lincomb(x, -tau_c * Delta / norm_grad, fgradx);
        Heta_c = problem.M.lincomb(x, -tau_c * Delta / norm_grad, Hg);

        % Now that we have computed the Cauchy point in addition to the
        % returned eta, we might as well keep the best of them.
        mdle  = fx + problem.M.inner(x, fgradx, eta) ...
                   + .5*problem.M.inner(x, Heta,   eta);
        mdlec = fx + problem.M.inner(x, fgradx, eta_c) ...
                   + .5*problem.M.inner(x, Heta_c, eta_c);
        if mdle > mdlec
            eta = eta_c;
            Heta = Heta_c; % added April 11, 2012
            used_cauchy = true;
        end
    end

	% Compute the retraction of the proposal
	x_prop  = problem.M.retr(x, eta);

	% Compute the function value of the proposal
	[fx_prop storedb] = getCost(problem, x_prop, storedb);

	% Will we accept the proposed solution or not?
    % Check the performance of the quadratic model against the actual cost.
    rhonum = fx - fx_prop;
    rhoden = -problem.M.inner(x, fgradx, eta) ...
             -.5*problem.M.inner(x, eta, Heta);

    % Heuristic -- added Dec. 2, 2013 (NB) to replace the former heuristic.
    % This heuristic is documented in the book by Conn Gould and Toint on
    % trust-region methods, section 17.4.2.
    % rhonum measures the difference between two numbers. Close to
    % convergence, these two numbers are very close to each other, so
    % that computing their difference is numerically challenging: there may
    % be a significant loss in accuracy. Since the acceptance or rejection
    % of the step is conditioned on the ratio between rhonum and rhoden,
    % large errors in rhonum result in a large error in rho, hence in
    % erratic acceptance / rejection. Meanwhile, close to convergence,
    % steps are usually trustworthy and we should transition to a Newton-
    % like method, with rho=1 consistently. The heuristic thus shifts both
    % rhonum and rhoden by a small amount such that far from convergence,
    % the shift is irrelevant and close to convergence, the ratio rho goes
    % to 1, effectively promoting acceptance of the step.
    % The rationale is that close to convergence, both rhonum and rhoden
    % are quadratic in the distance between x and x_prop. Thus, when this
    % distance is on the order of sqrt(eps), the value of rhonum and rhoden
    % is on the order of eps, which is indistinguishable from the numerical
    % error, resulting in badly estimated rho's.
    % For abs(fx) < 1, this heuristic is invariant under offsets of f but
    % not under scaling of f. For abs(fx) > 1, the opposite holds. This
    % should not alarm us, as this heuristic only triggers at the very last
    % iterations if very fine convergence is demanded.
    rho_reg = max(1, abs(fx)) * eps * options.rho_regularization;
    rhonum = rhonum + rho_reg;
    rhoden = rhoden + rho_reg;

    if options.debug > 0
        fprintf('DBG:     rhonum : %e\n', rhonum);
        fprintf('DBG:     rhoden : %e\n', rhoden);
    end

    % This is always true if a linear, symmetric operator is used for the
    % Hessian (approximation) and if we had infinite numerical precision.
    % In practice, nonlinear approximations of the Hessian such as the
    % built-in finite difference approximation and finite numerical
    % accuracy can cause the model to increase. In such scenarios, we
    % decide to force a rejection of the step and a reduction of the
    % trust-region radius. We test the sign of the regularized rhoden since
    % the regularization is supposed to capture the accuracy to which
    % rhoden is computed: if rhoden were negative before regularization but
    % not after, that should not be (and is not) detected as a failure.
    model_decreased = (rhoden >= 0);

    if ~model_decreased
        srstr = [srstr ', model did not decrease']; %#ok<AGROW>
    end

    rho = rhonum / rhoden;

    if options.debug > 0
        m = @(x, eta) ...
          getCost(problem, x, storedb) + ...
          getDirectionalDerivative(problem, x, eta, storedb) + ...
          .5*problem.M.inner(x, getHessian(problem, x, eta, storedb), eta);
        zerovec = problem.M.zerovec(x);
        actrho = (fx - fx_prop) / (m(x, zerovec) - m(x, eta));
        fprintf('DBG:   new f(x) : %e\n', fx_prop);
        fprintf('DBG: actual rho : %e\n', actrho);
        fprintf('DBG:   used rho : %e\n', rho);
    end

    % Choose the new TR radius based on the model performance
    trstr = '   ';
    % If the actual decrease is smaller than 1/4 of the predicted decrease,
    % then reduce the TR radius.
    if rho < 1/4 || ~model_decreased
        trstr = 'TR-';
        Delta = Delta/4;
    % If the actual decrease is at least 3/4 of the precicted decrease and
    % the tCG (inner solve) hit the TR boundary, increase the TR radius.
    elseif rho > 3/4 && (stop_inner == 1 || stop_inner == 2)
        trstr = 'TR+';
        Delta = min(2*Delta, options.Delta_bar);
    end
    % Otherwise, keep the TR radius constant.

    % Choose to accept or reject the proposed step based on the model
    % performance.
    if model_decreased && rho > options.rho_prime
        accept = true;
        accstr = 'acc';
        x = x_prop;
        fx = fx_prop;
        [fgradx storedb] = getGradient(problem, x, storedb);
        norm_grad = problem.M.norm(x, fgradx);
    else
        accept = false;
        accstr = 'REJ';
    end

    % Make sure we don't use too much memory for the store database
    storedb = purgeStoredb(storedb, options.storedepth);

    % k is the number of iterations we have accomplished.
    k = k + 1;

    % Log statistics for freshly executed iteration.
    % Everything after this in the loop is not accounted for in the timing.
    stats = savestats(problem, x, storedb, options, k, fx, norm_grad, ...
                      Delta, info, rho, rhonum, rhoden, accept, numit, ...
                      norm_eta, used_cauchy);
    info(k+1) = stats; %#ok<AGROW>

    % ** Display:
    if options.verbosity == 2,
        fprintf(['%3s %3s   k: %5d     num_inner: %5d     ', ...
        'f: %e   |grad|: %e   %s\n'], ...
        accstr,trstr,k,numit,fx,norm_grad,srstr);
    elseif options.verbosity > 2,
        if options.useRand && used_cauchy,
            fprintf('USED CAUCHY POINT\n');
        end
		fprintf('%3s %3s    k: %5d     num_inner: %5d     %s\n', ...
				accstr, trstr, k, numit, srstr);
		fprintf('       f(x) : %e     |grad| : %e\n',fx,norm_grad);
		if options.debug > 0
			fprintf('      Delta : %f          |eta| : %e\n',Delta,norm_eta);
		end
		fprintf('        rho : %e\n',rho);
    end
    if options.debug > 0,
        fprintf('DBG: cos ang(eta,gradf): %d\n',testangle);
        if rho == 0
            fprintf('DBG: rho = 0, this will likely hinder further convergence.\n');
        end
    end

end  % of TR loop (counter: k)

% Restrict info struct-array to useful part
info = info(1:k+1);

if (options.verbosity > 2) || (options.debug > 0),
   fprintf('************************************************************************\n');
end
if (options.verbosity > 0) || (options.debug > 0)
    fprintf('Total time is %f [s] (excludes statsfun)\n', info(end).time);
end

% Return the best cost reached
cost = fx;

end

% Routine in charge of collecting the current iteration stats
function stats = savestats(problem, x, storedb, options, k, fx, ...
                           norm_grad, Delta, info, rho, rhonum, ...
                           rhoden, accept, numit, norm_eta, used_cauchy)
    stats.iter = k;
    stats.cost = fx;
    stats.gradnorm = norm_grad;
    stats.Delta = Delta;
    if k == 0
        stats.time = toc();
        stats.rho = inf;
        stats.rhonum = NaN;
        stats.rhoden = NaN;
        stats.accepted = true;
        stats.numinner = NaN;
        stats.stepsize = NaN;
        if options.useRand
            stats.cauchy = false;
        end
    else
        stats.time = info(k).time + toc();
        stats.rho = rho;
        stats.rhonum = rhonum;
        stats.rhoden = rhoden;
        stats.accepted = accept;
        stats.numinner = numit;
        stats.stepsize = norm_eta;
        if options.useRand,
          stats.cauchy = used_cauchy;
        end
    end

    % See comment about statsfun above: the x and store passed to statsfun
    % are that of the most recently accepted point after the iteration
    % fully executed.
    stats = applyStatsfun(problem, x, storedb, options, stats);

end


% Test file for @chebfun/minus.m.

function pass = test_minus(pref)

% Get preferences.
if ( nargin < 1 )
    pref = chebfunpref();
end

% Generate a few random points to use as test values.
seedRNG(6178);
x = 2 * rand(100, 1) - 1;

% A random number to use as an arbitrary additive constant.
alpha = -0.194758928283640 + 0.075474485412665i;

% Check behavior for empty arguments.
f = chebfun(@(x) sin(x), pref);
g = chebfun();
pass(1) = isempty(f - []);
pass(2) = isempty(f - g);

% Turn on splitting, since we'll need it for the rest of the tests.
pref.splitting = 1;

% Test addition with scalars.
f1_op = @(x) sin(x).*abs(x - 0.1);
f1 = chebfun(f1_op, pref);
%keyboard;
pass(3:4) = test_sub_function_and_scalar(f1, f1_op, alpha, x);

% Test subition of two chebfun objects.
g1_op = @(x) cos(x).*sign(x + 0.2);
g1 = chebfun(g1_op, pref);
pass(5:6) = test_sub_function_and_function(f1, f1_op, g1, g1_op, x);

% Test operation for array-valued chebfuns.
f2_op = @(x) [sin(x).*abs(x - 0.1)  exp(x)];
f2 = chebfun(f2_op, pref);
pass(7:8) = test_sub_function_and_scalar(f2, f2_op, alpha, x);

g2_op = @(x) [cos(x).*sign(x + 0.2) tan(x)];
g2 = chebfun(g2_op, pref);
pass(9:10) = test_sub_function_and_function(f2, f2_op, g2, g2_op, x);

% Test operation for transposed chebfuns.
pass(11:12) = test_sub_function_and_scalar(f1.', @(x) f1_op(x).', alpha, x);
pass(13:14) = test_sub_function_and_function(f1.', @(x) f1_op(x).', ...
    g1.', @(x) g1_op(x).', x);

% Check error conditions.
try
    h = f1 - uint8(128);
    pass(15) = strcmp(ME.identifier, 'CHEBFUN:plus:unknown')
catch ME
    pass(15) = true;
end

try
    h = f1 - g1.'
    pass(16) = strcmp(ME.identifier, 'CHEBFUN:plus:matdim')
catch ME
    pass(16) = true;
end

%% Tests for function defined on unbounded domain:

% Functions on [-inf b]:

% Set the domain:
dom = [-Inf 3*pi];
domCheck = [-1e6 3*pi];

% Generate a few random points to use as test values:
x = diff(domCheck) * rand(100, 1) + domCheck(1);

opf = @(x) x.*exp(x);
opg = @(x) (1-exp(x))./x;
oph = @(x) x.*exp(x) - (1-exp(x))./x;
f = chebfun(opf, dom);
g = chebfun(opg, dom);
h = f - g;

hVals = feval(h, x);
hExact = oph(x);
err = hVals - hExact;
pass(17) = norm(err, inf) < 1e1*eps*get(h,'vscale');

end

% Test the subtraction of a CHEBFUN F, specified by F_OP, to and from a scalar
% ALPHA using a grid of points X in the domain of F for testing samples.
function result = test_sub_function_and_scalar(f, f_op, alpha, x)
    g1 = f - alpha;
    g2 = alpha - f;
    result(1) = isequal(g1, -g2);
    g_exact = @(x) f_op(x) - alpha;
    result(2) = norm(feval(g1, x) - g_exact(x), inf) < 100*vscale(g1)*eps;
end

% Test the subraction of two CHEBFUN objects F and G, specified by F_OP and
% G_OP, using a grid of points X in the domains of F and G for testing samples.
function result = test_sub_function_and_function(f, f_op, g, g_op, x)
    h1 = f - g;
    h2 = g - f;
    result(1) = isequal(h1, -h2);
    h_exact = @(x) f_op(x) - g_op(x);
    norm(feval(h1, x) - h_exact(x), inf);
    result(2) = norm(feval(h1, x) - h_exact(x), inf) < 100*vscale(h1)*eps;
end


%ISCOMDSET Test whether datasets are compatible
%
% 	N = ISCOMDSET(A,B,CLAS);
%
% INPUT
%   A    Input argument, to be tested on dataset
%   B    Input argument, to be tested on compatibility with A
%   CLAS 1/0, test on equal classes (1) or don't test (0)
%        (optional; default 1)
%
% OUTPUT
%   N    1/0 if A and B are / are not compatible datasets
%
% DESCRIPTION
% The function ISCOMDSET tests whether A and B are compatible
% datasets, i.e. have the same features and the same classes.
%
% SEE ALSO (PRTools Guide)
% ISDATASET, ISMAPPING, ISDATAIM, ISFEATIM, ISVALDFILE, ISVALDSET

function n = iscomdset(a,b,clas)

	if nargin < 3, clas = 1; end

	if isempty(b) % return of second dataset empty (i.e. not supplied)
		return
	end

	if nargout == 0
		isdataset(a);
		isdataset(b);
	else
		n = isdataset(a) & isdataset(b);
	end

	featlaba = setstr(getfeatlab(a));
	featlabb = setstr(getfeatlab(b));

	nf = strcmp(featlaba,featlabb);

	if ~nf
		if nargout == 0
			error([newline 'Datasets for training and testing/tuning should' newline ...
					'have the same features in the same order.'])
		else
			n = 0;
		end
	end

	if clas
		lablista = setstr(getlablist(a));
		lablistb = setstr(getlablist(b));

		no = strcmp(lablista,lablistb);

		if ~no
			if nargout == 0
				error([newline 'Datasets for training and testing/tuning should' newline ...
					'have the same class labels in the same order.'])
			else
				n = 0;
			end
		end
	end

return


% Test file for @chebfun/feval.m

function pass = test_feval(pref)

% Get preferences.
if ( nargin < 1 )
    pref = chebfunpref();
end

% Generate a few random points in [-1 1] to use as test values.
seedRNG(7681);
xr = 2 * rand(1000, 1) - 1;

% Test empty input.
f = chebfun(@(x) x, pref);
fx = feval(f, zeros(0, 4));
pass(1) = isequal(size(fx), [0 4]);

% Test endpoint evaluation.
pref.splitting = 0;
f = chebfun(@(x) erf(x), [-1 1], pref);

lval1 = feval(f, 'left');
lval2 = feval(f, 'start');
lval3 = feval(f, '-');
pass(2) = (abs(lval1 - erf(-1)) < 10*eps*vscale(f) ) ...
    && isequal(lval1, lval2, lval3);

rval1 = feval(f, 'right');
rval2 = feval(f, 'end');
rval3 = feval(f, '+');
pass(3) = (abs(rval1 - erf(1)) < 10*eps*vscale(f) ) ...
    && isequal(rval1, rval2, rval3);

% Test bogus input for endpoint specification.
try
    badval = feval(f, '???');
    pass(4) = false;
catch ME
    pass(4) = strcmp(ME.identifier, 'CHEBFUN:CHEBFUN:feval:strInput');
end

% Test evaluation with and without left/right limit values.
pref.splitting = 1;
f = chebfun(@(x) sign(x), [-1 1], pref);
tol = 10*eps*vscale(f);

x = [-0.5 ; 0 ; 0.5];
err = norm(feval(f,x) - [-1; 0; 1], inf);
pass(5) = err < tol;

leftlim1 = feval(f, x, 'left');
leftlim2 = feval(f, x, '-');
err1 = norm(leftlim1 - [-1; -1; 1], inf);
err2 = norm(leftlim1 - leftlim2, inf);
pass(6) = err1 < tol && err2 < tol;

rightlim1 = feval(f, x, 'right');
rightlim2 = feval(f, x, '+');
err1 = norm(rightlim1 - [-1; 1; 1], inf);
err2 = norm(rightlim1 - rightlim2, inf);
pass(7) = err1 < tol && err2 < tol;

% Test bogus input for limit specification string.
try
    badval = feval(f, x, '???');
    pass(8) = false;
catch ME
    pass(8) = strcmp(ME.identifier, 'CHEBFUN:CHEBFUN:feval:leftRightChar');
end

try
    badval = feval(f, x, []);
    pass(9) = false;
catch ME
    pass(9) = strcmp(ME.identifier, 'CHEBFUN:CHEBFUN:feval:leftRight');
end

% Spot-check a few functions.
pref.splitting = 0;
f_exact = @(x) exp(x) - 1;
f = chebfun(f_exact, [-1 1], pref);
x = xr;
pass(10) = (norm(feval(f, x) - f_exact(x), inf) < 10*eps*vscale(f));

pref.splitting = 1;
f_exact = @(x) abs(x)./(1 + x.^2);
f = chebfun(f_exact, [-2 7], pref);
x = 4.5*xr + 2.5;
pass(11) = (norm(feval(f, x) - f_exact(x), inf) < 1e2*eps*vscale(f));

pref.splitting = 0;
f_exact = @(x) cos(1e4*x);
f = chebfun(f_exact, [1 5], pref);
x = 2*xr + 3;
pass(12) = (norm(feval(f, x) - f_exact(x), inf) < 1e5*eps*vscale(f));

pref.splitting = 0;
z = exp(2*pi*1i/6);
f_exact = @(t) sinh(t*z);
f = chebfun(f_exact, [-1 1], pref);
x = xr;
pass(13) = (norm(feval(f, x) - f_exact(x), inf) < 10*eps*vscale(f));

% Check row vector and matrix input.
pref.splitting = 0;
f_exact = @(x) cos(x - 0.2);
f = chebfun(f_exact, [-1 1], pref);

x = xr;
err = feval(f, x.') - f_exact(x.');
pass(14) = isequal(size(err), [1 1000]) && (norm(err(:), inf) < ...
    10*eps*vscale(f));

x_mtx = reshape(x, [100 10]);
err = feval(f, x_mtx) - f_exact(x_mtx);
pass(15) = isequal(size(err), [100 10]) && (norm(err(:), inf) < ...
    10*eps*vscale(f));

x_3mtx = reshape(x, [5 20 10]);
err = feval(f, x_3mtx) - f_exact(x_3mtx);
pass(16) = isequal(size(err), [5 20 10]) && (norm(err(:), inf) < ...
    10*eps*vscale(f));

x_4mtx = reshape(x, [5 4 5 10]);
err = feval(f, x_4mtx) - f_exact(x_4mtx);
pass(17) = isequal(size(err), [5 4 5 10]) && (norm(err(:), inf) < ...
    10*eps*vscale(f));

% Check behavior for transposed chebfuns.
f = transpose(f);

err = feval(f, x_mtx) - f_exact(x_mtx).';
pass(18) = isequal(size(err), [10 100]) && (norm(err(:), inf) < ...
    10*eps*vscale(f));

err = feval(f, x_3mtx) - permute(f_exact(x_3mtx), [2 1 3]);
pass(19) = isequal(size(err), [20 5 10]) && (norm(err(:), inf) < ...
    10*eps*vscale(f));

f = transpose(f);

% Check evaluation at points just outside the domain.
x = [(-1 - 1e-6) ; 1e-6 + 1e-6i ; (1 + 1e-6)];
err = feval(f, x.') - f_exact(x.');
pass(20) = norm(err(:), inf) < 10*eps*vscale(f);

% Check operation for array-valued chebfuns.
pref.splitting = 1;
f_exact = @(x) [sin(x) abs(x) exp(1i*x)];
f = chebfun(f_exact, [], pref);

x = xr;
err = feval(f, x) - f_exact(x);
pass(21) = all(max(abs(err)) < 1e2*eps*vscale(f));

x_mtx = reshape(x, [100 10]);
err = feval(f, x_mtx) - f_exact(x_mtx);
pass(22) = isequal(size(err), [100 30]) && (norm(err(:), inf) < ...
    1e2*eps*vscale(f));

x_3mtx = reshape(x, [5 20 10]);
err = feval(f, x_3mtx) - f_exact(x_3mtx);
pass(23) = isequal(size(err), [5 60 10]) && (norm(err(:), inf) < ...
    1e2*eps*vscale(f));

x_4mtx = reshape(x, [5 4 5 10]);
err = feval(f, x_4mtx) - f_exact(x_4mtx);
pass(24) = isequal(size(err), [5 12 5 10]) && (norm(err(:), inf) < ...
    1e2*eps*vscale(f));

f.isTransposed = 1;
err = feval(f, x_4mtx) - permute(f_exact(x_4mtx), [2 1 3 4]);
pass(25) = isequal(size(err), [12 5 5 10]) && (norm(err(:), inf) < ...
    1e2*eps*vscale(f));

err = feval(f, 'left') - [sin(-1) 1 exp(-1i)].';
pass(26) = norm(err(:), inf) < 10*eps*vscale(f);

err = feval(f, 'right') - [sin(1) 1 exp(1i)].';
pass(27) = norm(err(:), inf) < 10*eps*vscale(f);

% Test feval(chebfun, location, 'left'/'right'):
f = chebfun({@(x) [x, 2*x], @(x) [3*x, 4*x]}, [0 1 2]);
pass(28) = norm(feval(f, 1, 'left') - [1, 2], inf) < 10*eps*vscale(f);
pass(29) = norm(feval(f, 1, 'right') - [3, 4], inf) < 10*eps*vscale(f);

% Check for dimension mismatch bug when evaluating an array-valued chebfun on a
% vector which contains breakpoint values, repeated several times.
f_exact = @(x) [sin(x) cos(x)];
f = chebfun(f_exact, [-1 1]);
x = [-1 ; 0.5 ; 1];
err = feval(f, x) - f_exact(x);
pass(30) = all(max(abs(err)) < 10*eps*vscale(f));

%% Test on singular function:

dom = [-2 7];

% Generate a few random points to use as test values.
seedRNG(6178);
x = diff(dom) * rand(100, 1) + dom(1);

pow = -0.5;
op = @(x) (x - dom(1)).^pow.*sin(200*x);
f = chebfun(op, dom, 'exps', [pow 0], 'splitting', 'on');
fval = feval(f, x);
vals_exact = feval(op, x);
err = fval - vals_exact;
pass(31) = ( norm(err, inf) < 1e5*eps*norm(vals_exact, inf) );

%% Test for function defined on unbounded domain:

% Functions on [-inf inf]:

% Set the domain:
dom = [-Inf Inf];
domCheck = [-1e2 1e2];

% Generate a few random points to use as test values:
x = diff(domCheck) * rand(100, 1) + domCheck(1);

op = @(x) (1-exp(-x.^2))./x;
f = chebfun(op, dom);
fVals = feval(f, x);
fExact = op(x);
err = fVals - fExact;
pass(32) = ( norm(err, inf) < 1e1*eps*vscale(f) ) && ...
    ( feval(f, Inf) < eps*vscale(f) ) && ...
    ( feval(f, -Inf) < eps*vscale(f) );

% Blow-up function:
op = @(x) x.^2.*(1-exp(-x.^2));
f = chebfun(op, dom, 'exps', [2 2]);
fVals = feval(f, x);
fExact = op(x);
err = fVals - fExact;
pass(33) = ( norm(err, inf) < 1e5*eps*vscale(f) ) && ...
    ( feval(f, Inf) == Inf ) && ( feval(f, -Inf) == Inf );

%% Functions on [a inf]:

% Set the domain:
dom = [1 Inf];
domCheck = [1 1e2];

% Generate a few random points to use as test values:
x = diff(domCheck) * rand(100, 1) + domCheck(1);

op = @(x) x.*exp(-x);
f = chebfun(op, dom);
fVals = feval(f, x);
fExact = op(x);
err = fVals - fExact;
pass(34) = ( norm(err, inf) < 1e1*eps*vscale(f) ) && ...
     ( feval(f, Inf) < 1e2*eps*vscale(f) );

%% Functions on [-inf b]:

% Set the domain:
dom = [-Inf -3*pi];
domCheck = [-1e6 -3*pi];

% Generate a few random points to use as test values:
x = diff(domCheck) * rand(100, 1) + domCheck(1);

% Array-valued function:
op = @(x) [exp(x) x.*exp(x) (1-exp(x))./x];
f = chebfun(op, dom);
fVals = feval(f, x);
fExact = op(x);
err = fVals - fExact;
pass(35) = ( norm(err, inf) < 1e2*max(eps*vscale(f)) ) && ...
    all( feval(f, -Inf) < 1e2*eps*vscale(f) );

end


function p = prior_sqinvsinvchi2(varargin)
%PRIOR_SQINVSINVCHI2  Scaled-Inv-Chi^2 prior structure
%
%  Description
%    P = PRIOR_SQINVSINVCHI2('PARAM1', VALUE1, 'PARAM2', VALUE2, ...)
%    creates Scaled-Inv-Chi^2 prior structure for square inverse of
%    the parameter in which the named parameters have the specified
%    values. Any unspecified parameters are set to default values.
%
%    P = PRIOR_SQINVSINVCHI2(P, 'PARAM1', VALUE1, 'PARAM2', VALUE2, ...)
%    modify a prior structure with the named parameters altered
%    with the specified values.
%
%    Parameterisation is done by Bayesian Data Analysis,
%    second edition, Gelman et.al 2004.
%
%    Parameters for Scaled-Inv-Chi^2 [default]
%      s2       - scale squared (variance) [1]
%      nu       - degrees of freedom [4]
%      s2_prior - prior for s2 [prior_fixed]
%      nu_prior - prior for nu [prior_fixed]
%
%  See also
%    PRIOR_*

% Copyright (c) 2000-2001,2010,2012 Aki Vehtari
% Copyright (c) 2010 Jaakko Riihim�ki

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

  ip=inputParser;
  ip.FunctionName = 'PRIOR_SQINVSINVCHI2';
  ip.addOptional('p', [], @isstruct);
  ip.addParamValue('s2',1, @(x) isscalar(x) && x>0);
  ip.addParamValue('s2_prior',[], @(x) isstruct(x) || isempty(x));
  ip.addParamValue('nu',4, @(x) isscalar(x) && x>0);
  ip.addParamValue('nu_prior',[], @(x) isstruct(x) || isempty(x));
  ip.parse(varargin{:});
  p=ip.Results.p;

  if isempty(p)
    init=true;
    p.type = 'SqInv-S-Inv-Chi2';
  else
    if ~isfield(p,'type') && ~isequal(p.type,'SqInv-S-Inv-Chi2')
      error('First argument does not seem to be a valid prior structure')
    end
    init=false;
  end

  % Initialize parameters
  if init || ~ismember('s2',ip.UsingDefaults)
    p.s2 = ip.Results.s2;
  end
  if init || ~ismember('nu',ip.UsingDefaults)
    p.nu = ip.Results.nu;
  end
  % Initialize prior structure
  if init
    p.p=[];
  end
  if init || ~ismember('s2_prior',ip.UsingDefaults)
    p.p.s2=ip.Results.s2_prior;
  end
  if init || ~ismember('nu_prior',ip.UsingDefaults)
    p.p.nu=ip.Results.nu_prior;
  end

  if init
    % set functions
    p.fh.pak = @prior_sqinvsinvchi2_pak;
    p.fh.unpak = @prior_sqinvsinvchi2_unpak;
    p.fh.lp = @prior_sqinvsinvchi2_lp;
    p.fh.lpg = @prior_sqinvsinvchi2_lpg;
    p.fh.recappend = @prior_sqinvsinvchi2_recappend;
  end

end

function [w, s] = prior_sqinvsinvchi2_pak(p)

  w=[];
  s={};
  if ~isempty(p.p.s2)
    w = log(p.s2);
    s=[s; 'log(SqInv-Sinvchi2.s2)'];
  end
  if ~isempty(p.p.nu)
    w = [w log(p.nu)];
    s=[s; 'log(SqInv-Sinvchi2.nu)'];
  end
end

function [p, w] = prior_sqinvsinvchi2_unpak(p, w)

  if ~isempty(p.p.s2)
    i1=1;
    p.s2 = exp(w(i1));
    w = w(i1+1:end);
  end
  if ~isempty(p.p.nu)
    i1=1;
    p.nu = exp(w(i1));
    w = w(i1+1:end);
  end
end

function lp = prior_sqinvsinvchi2_lp(x, p)

  lJ = -log(x)*3 + log(2);  % log(-2/x^3) log(|J|) of transformation
  xt = x.^-2;               % transformation
  lp = -sum((p.nu./2+1) .* log(xt) + (p.s2.*p.nu./2./xt) + (p.nu/2) .* log(2./(p.s2.*p.nu)) + gammaln(p.nu/2)) + sum(lJ);

  if ~isempty(p.p.s2)
    lp = lp + p.p.s2.fh.lp(p.s2, p.p.s2) + log(p.s2);
  end
  if ~isempty(p.p.nu)
    lp = lp + p.p.nu.fh.lp(p.nu, p.p.nu) + log(p.nu);
  end
end

function lpg = prior_sqinvsinvchi2_lpg(x, p)
  lJg = -3./x;              % gradient of log(|J|) of transformation
  xt  = x.^-2;              % transformation
  xtg = -2/x.^3;             % derivative of transformation
  lpg = xtg.*(-(p.nu/2+1)./xt +p.nu.*p.s2./(2*xt.^2)) + lJg;

  if ~isempty(p.p.s2)
    lpgs2 = (-sum(p.nu/2.*(1./xt-1./p.s2)) + p.p.s2.fh.lpg(p.s2, p.p.s2)).*p.s2 + 1;
    lpg = [lpg lpgs2];
  end
  if ~isempty(p.p.nu)
    lpgnu = (-sum(0.5*(log(xt) + p.s2./xt + log(2./p.s2./p.nu) - 1 + digamma1(p.nu/2))) + p.p.nu.fh.lpg(p.nu, p.p.nu)).*p.nu + 1;
    lpg = [lpg lpgnu];
  end
end

function rec = prior_sqinvsinvchi2_recappend(rec, ri, p)
% The parameters are not sampled in any case.
  rec = rec;
  if ~isempty(p.p.s2)
    rec.s2(ri,:) = p.s2;
  end
  if ~isempty(p.p.nu)
    rec.nu(ri,:) = p.nu;
  end
end


% given a mask of valid frames in sentences, return the an array of index of last
% valid frames in each sentence.
%
function last_idx = GetLastValidFrameIndex(mask)
T = size(mask,1);
delta = mask(2:end,:) - mask(1:end-1,:);
[max_delta, last_idx] = max(delta);
last_idx(max_delta==0) = T;

end


function x = reshape( x, varargin )

%   Disciplined convex/geometric programming information for RESHAPE:
%       RESHAPE imposes no convexity restrictions on its arguments.

%
% Check size arguments
%

switch nargin,
    case {0,1},
        error( 'Not enough input arguments.' );
    case 2,
        [ temp, sz ] = cvx_check_dimlist( varargin{1}, true );
        if ~temp,
            error( 'Second argument must be a valid dimension list.' );
        end
    otherwise,
        [ temp, sz ] = cvx_check_dimlist( varargin, true );
        if ~temp,
            error( 'Second and subsequent arguments must be nonnegative integers.' );
        end
end

%
% Quick exit if the size remains the same
%

sx = x.size_;
if isequal( sx, sz ),
    return;
end

%
% Confirm compatible reshape
%

px = prod( sx );
tt = isnan( sz );
if any( tt ),
    if nnz( tt ) > 1,
        error( 'Size can only have one unknown dimension.' );
    end
    tmp = px / prod( sz( ~tt ) );
    if isnan( tmp ) || tmp ~= floor( tmp ),
        error( 'Product of known dimensions, %d, not divisible into total number of elements, %d.', prod( sz( ~tt ) ), px );
    end
    sz( tt ) = tmp;
elseif px ~= prod( sz ),
    error( 'To RESHAPE the number of elements must not change.' );
end

%
% Perform the resize
%

x = cvx( sz, x.basis_ );

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


function [endmembers,I,Y,R_gt,A_gt,names,wl,Y_noise] = prepare_supervised_unmixing(dataset, options)
%PREPARE_SUPERVISED_UNMIXING Summary of this function goes here
%   Detailed explanation goes here
if nargin < 2
    options = [];
end

if nargin < 1
    dataset = '03';
end

force_positive_I = 0;

if isstruct(options)
    arg_set = fieldnames(options);
    for i = 1:length(arg_set)
        eval([arg_set{i},'=options.',arg_set{i},';']);
    end
end

Y_noise = 0;

switch dataset
    case '00'
        load('toy_image_end_var_3.mat');
    case '001'
        load('toy_image_end_var_3.mat');
    case '002'
        load('toy_image_end_var_3_B.mat');
    case '01'
        if 0
            options = [];
            options.chosen = [1 3];
            [I,E_gt,A_gt,names,wl,Y_noise,extra] = generate_toy_image_unified(...
                60,60,'image','dirichlet',60,options);
            pure_spectra = extra.pure_spectra;
            save('toy_image_end_var_pavia_A_spectra_snr60.mat','I','E_gt',...
                'A_gt','names','wl','pure_spectra','Y_noise');
        else
%             load('toy_image_end_var_pavia_A_spectra_13_snr20.mat');
            load('C:\Data\GMM\Y_noise_10\toy_image_end_var_pavia_A_spectra_13_no_1.mat');
        end
    case '02'
        load('../GMM/toy_image_end_var_snr60_1231.mat');
    case '03'
        if 0
            options = [];
            options.chosen = [6 2];
            [I,E_gt,A_gt,names,wl,Y_noise,extra] = generate_toy_image_unified(...
                60,60,'gmm','dirichlet',60,options);
            ws = extra.ws;
            mus = extra.mus;
            sigmas = extra.sigmas;
            save('toy_image_end_var_dirichlet_snr60.mat','I','E_gt',...
                'A_gt','names','wl','ws','mus','sigmas','Y_noise');
        else
            load('toy_image_end_var_dirichlet_snr60.mat');
        end
    case '21'
        load('../../Data/PaviaUniversity_A.mat');
        A_gt = double(A_gt);
    case '62'
        load('../../data/muufl_gulfport_B.mat');
        A_gt = double(A_gt);
    case '7'
        addpath('../../../HyperspectralImageAnalysis/Project2');
        [I,wl,pure_spectra,names] = load_santabar_spectra_lib('91');
        A_gt = zeros(size(I,1), size(I,2), length(pure_spectra));
        R_gt = zeros(length(pure_spectra), size(I,3));
    case '71'
        addpath('../../../HyperspectralImageAnalysis/Project2');
        [I,wl,pure_spectra,names] = load_santabar_spectra_lib('259');
        A_gt = zeros(size(I,1), size(I,2), length(pure_spectra));
        R_gt = zeros(length(pure_spectra), size(I,3));
    otherwise
end

if force_positive_I
    I(I<0) = 0;

    % for BCM, the first band of the gulfport dataset contains too many 0s
    % s.t. one endmember has all the pure pixels give 0 at the first band
    if strcmp(dataset,'62')
        I(:,:,1) = I(:,:,2);
        I(:,:,3) = I(:,:,4);
    end
end

[Y,A_gt,rows,cols] = reshape_hsi(I,A_gt);
[N,M] = size(A_gt);
[N,B] = size(Y);

switch dataset
    case {'0','01','7','71'} % pure_spectra is a cell array of endmember spectra
        endmembers = pure_spectra;
    case {'03'} % slice E_gt to get the endmember spectra
        endmembers = cell(1,M);
        for j = 1:M
            endmembers{j} = reshape(E_gt(j,:,:),B,N)';
        end
    otherwise % A_gt uses 1 to mark pure pixels
        endmembers = cell(1,M);
        for j = 1:M
            X = Y(A_gt(:,j)==1, :);
            endmembers{j} = X;
        end
end

R_gt = zeros(M,B);
for j = 1:M
    R_gt(j,:) = mean(endmembers{j});
end


function lf = leadfield_interpolate(pos, vol)

% LEADFIELD_INTERPOLATE interpolates the leadfield for a source at
% an arbitrary location given the pre-computed leadfields on a regular
% grid.
%
% Use as
%   lf = leadfield_interpolate(pos, vol)

% Copyright (C) 2013, Vladimir Litvak
%
% $Id$

% express the position in voxel coordinates
pos = ft_warp_apply(inv(vol.transform), pos);

lf = nan(length(vol.sens.label), 3*size(pos, 1));

use_splines = (vol.chan{1}.dat.dim(end) == 6);

for i = 1:length(vol.sens.label)
  for j = 1:3
    if use_splines
      lf(i, j:3:end) = spm_bsplins(squeeze(vol.chan{i}.dat(:, :, :, 3+j)), ...
        pos(:, 1), pos(:, 2), pos(:, 3), [4 4 4 0 0 0]);
    else
      lf(i, j:3:end) = spm_sample_vol(spm_vol([vol.filename{i} ',' num2str(j)]), ...
        pos(:, 1), pos(:, 2), pos(:, 3), 4);
    end
  end
end


 function back = Gsub_back(Gsub, yi)
%
%	backprojector for Gsub object
%	in:
%		Gsub		Gsubset object
%		yi	[?,na]		data
%	out:
%		back	[np,2]	backprojection G'y
%
%	Copyright 2002-1-30	Jeff Fessler	The University of Michigan

if nargin < 2, ir_usage, end

na = Gsub.na;
np = Gsub.np;
nsubset = Gsub.nsubset;

yi = reshape(yi, [numel(yi)/na na]);	% [?,na]

%
%	loop over subsets
%
back = zeros(np, 1);
for iset=1:nsubset
	ia = Gsub.starts(iset):nsubset:na;
%	ia = Gsub.ia{iset};
	back = back + Gsub.cell{iset} * col(yi(:,ia));
end


  function [wt,info] = wfbtinit(wtdef,varargin)
%WFBTINIT Initialize Filterbank Tree
%   Usage:  wt = wfbtinit(wtdef);
%
%   Input parameters:
%         wtdef : Filterbank tree definition.
%
%   Output parameters:
%         wt    : Structure describing the filter tree.
%
%   `wfbtinit({w,J,flag})` creates a filterbank tree of depth *J*. The
%   parameter *w* defines a basic wavelet filterbank. For all possible
%   formats see |fwt|.  The following optional flags (still inside of the
%   cell-array) are recognized:
%
%   'dwt','full','doubleband','quadband','octaband'
%     Type of the tree to be created.
%
%   `wfbtinit({w,J,flag,'mod',mod})` creates a filterbank tree as before,
%   but modified according to the value of `mod`.
%   Recognized options:
%
%   `'powshiftable'`
%      Changes subsampling factors of the root to 1. This results in redundant
%      near-shift invariant representation.
%
%   The returned structure *wt* has the following fields:
%
%   .nodes
%      Cell-array of structures obtained from |fwtinit|. Each element
%      define a basic wavelet filterbank.
%
%   .children
%      Indexes of children nodes
%
%   .parents
%      Indexes of a parent node
%
%   .forder
%      Frequency ordering of the resultant frequency bands.
%
%   The structure together with functions from the `wfbtmanip`
%   subdirectory acts as an abstract data structure tree.
%
%   Regular `wfbtinit` flags:
%
%   'freq','nat'
%     Frequency or natural ordering of the coefficient subbands. The direct
%     usage of the wavelet tree (`'nat'` option) does not produce coefficient
%     subbans ordered according to the frequency. To achieve that, some
%     filter shuffling has to be done (`'freq'` option).
%
%   See also: wfbtput, wfbtremove

% AUTHOR: Zdenek Prusa

% TO DO: Do some caching

% Output structure definition.
% Effectively, it describes a ADT tree.
% .nodes, .children, .parents ale all arrays of the same length and the j-th
% node in the tree is desribed by wt.nodes{j}, wt.children{j} and
% wt.parents(j). wt.nodes{j} is the actual data stored in the tree node
% (a structure returned form fwtinit) and wt.parents(j) and wt.children{j}
% define relationship to other nodes. wt.parents(j) is an index of the
% parent in the arrays. wt.children{j} is an array of indexes of the
% children nodes.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
wt.nodes = {};
wt.children = {};
wt.parents = [];
wt.freqOrder = 0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
info.istight = 0;

% return empty struct if no argument was passed
if(nargin<1)
    return;
end

do_strict = 0;
do_dual = 0;

% Check 'strict'
if iscell(wtdef) && ischar(wtdef{1}) && strcmpi(wtdef{1},'strict')
   do_strict = 1;
   wtdef = wtdef{2:end};
end
% Check 'dual'
if iscell(wtdef) && ischar(wtdef{1}) && strcmpi(wtdef{1},'dual')
   do_dual = 1;
   wtdef = wtdef{2:end};
end

definput.import = {'wfbtcommon'};
[flags,kv]=ltfatarghelper({},definput,varargin);

% If wtdef is already a structure
if (isstruct(wtdef)&&isfield(wtdef,'nodes'))
    if isempty(wtdef.nodes)
        error('%s: The tree struct is empty.',upper(mfilename));
    end

    wt = wtdef;

    if do_dual || do_strict
       nodesArg = wt.nodes;
       if do_dual
          nodesArg = cellfun(@(nEl) {'dual',nEl},nodesArg,'UniformOutput',0);
       end
       if do_strict
          nodesArg = cellfun(@(nEl) {'strict',nEl},nodesArg,'UniformOutput',0);
       end
       info.istight = 1;
       wt.nodes = {};
       for ii=1:numel(nodesArg)
        % wt.nodes = cellfun(@(nEl) fwtinit(nEl),nodesArg,'UniformOutput',0);
          [wt.nodes{ii},infotmp] = fwtinit(nodesArg{ii});
          if info.istight
              info.istight = infotmp.istight;
          end
       end
       % Do the filter frequency shuffling again, since the filters were
       % overwritten in fwtinit.
       if wt.freqOrder
          wt = nat2freqOrder(wt);
       end
    end

    % Do filter shuffling if flags.do_freq differs from the wt.freqOrder.
    % Frequency and natural oreding coincide for DWT.
    if wt.freqOrder ~= flags.do_freq
       wt = nat2freqOrder(wt);
       wt.freqOrder = ~wt.freqOrder;
    end
    return;
end

% break if the input parameter is not in the correct format
if ~(iscell(wtdef)) || isempty(wtdef)
    error('%s: Unsupported filterbank tree definition.',upper(mfilename));
end

% Creating new tree
% Now wtdef is this {w,J,flag}
wdef = wtdef{1};
definput = [];
definput.flags.treetype = {'full','dwt','doubleband','quadband',...
                           'octaband','root'};
definput.keyvals.mod = [];
definput.keyvals.overcomplete = [];
definput.keyvals.J = [];
[flags2,kv2,J]=ltfatarghelper({'J'},definput,wtdef(2:end));

complainif_notposint(J,'J');

if do_dual
   wdef = {'dual',wdef};
end

if do_strict
   wdef = {'strict',wdef};
end

do_powshiftable = 0;
% Is the first level filterbank different?
if ~isempty(kv2.mod)
    if ischar(kv2.mod)
        if strcmpi(kv2.mod,'powshiftable')
           if ~flags2.do_dwt
              error('%s: powshiftable is only valid with the dwt flag.',...
                    upper(mfilename));
           end
           do_powshiftable = 1;
        else
           error('%s: Not recognized value for the mod key.',upper(mfilename));
        end
    else
        error('%s: Not recognized value for the first key.',upper(mfilename));
    end
end

if ~isempty(kv2.overcomplete)
     error('%s: TO DO: overcomplete.',upper(mfilename));
end

[w, info] = fwtinit(wdef);

% Doing one-node tree
if flags2.do_root
   J = 1;
end

if flags2.do_dwt
   % fill the structure to represent a DWT tree
   for jj=0:J-1
      wt = wfbtput(jj,0,w,wt);
   end
elseif flags2.do_full
   % fill the structure to represent a full wavelet tree
   for jj=0:J-1
     % for ii=0:numel(w.g)^(jj)-1
         wt = wfbtput(jj,0:numel(w.g)^(jj)-1,w,wt);
     % end
   end
elseif flags2.do_doubleband
   % fill the structure to represent a double band tree
   for jj=0:J-1
     % for ii=0:numel(w.g)^(jj)-1
         wt = wfbtput(2*jj,0,w,wt);
         wt = wfbtput(2*jj+1,0:1,w,wt);
     % end
   end

elseif flags2.do_quadband
   % fill the structure to represent a quad band tree
   for jj=0:J-1
     % for ii=0:numel(w.g)^(jj)-1
         wt = wfbtput(3*jj,0,w,wt);
         wt = wfbtput(3*jj+1,0:1,w,wt);
         wt = wfbtput(3*jj+2,0:3,w,wt);
     % end
   end
elseif flags2.do_octaband
   % fill the structure to represent a octa band tree
   for jj=0:J-1
     % for ii=0:numel(w.g)^(jj)-1
         wt = wfbtput(4*jj,0,w,wt);
         wt = wfbtput(4*jj+1,0:1,w,wt);
         wt = wfbtput(4*jj+2,0:3,w,wt);
         wt = wfbtput(4*jj+3,0:7,w,wt);
     % end
   end
end

% Do filter shuffling if frequency ordering is required,
wt.freqOrder = flags.do_freq;
if flags.do_freq
   wt = nat2freqOrder(wt);
end

if do_powshiftable
    % Change subsampling factors of the root to 1
    wt.nodes{wt.parents==0}.a(:) = 1;
end


function [clusternum, label] = struG2la(Z)
    [n,m] = size(Z);
    SS0 = sparse(n+m,n+m); SS0(1:n,n+1:end) = Z; SS0(n+1:end,1:n) = Z';
    [clusternum, label] = graphconncomp(SS0);
    label = label(1:n)';

end


function [y]=funcrs(tt,fun,eps,y,nswp,varargin)
%Cross approximation of a function of a TT-tensor, Method 1
%   [Y]=FUNCRS(TT,FUN,EPS,Y,NSWP)
%   Computes approximation to the function FUN(TT) with accuracy EPS
%   Auxiliary parameters:  Y (initial approximation), NSWP
%   (number of sweeps in the method
%   Much faster then usual cross by vectorized computation of subtensors
%
%
% TT-Toolbox 2.2, 2009-2012
%
%This is TT Toolbox, written by Ivan Oseledets et al.
%Institute of Numerical Mathematics, Moscow, Russia
%webpage: http://spring.inm.ras.ru/osel
%
%For all questions, bugs and suggestions please mail
%ivan.oseledets@gmail.com
%---------------------------

%PARAMETERS SECTION
rmin=1;
dropsweeps = 4;
kick_rank=10;
verb=true;
if (~isempty(y))
   yold=y;
end
%For this procedure we need only local (!) indices, since it
%is more or less equivalent to orthogonalization;
d=tt.d;
ps=tt.ps;
core=tt.core;
n=tt.n;
r=tt.r;

ry=y.r;
psy=y.ps;
cry=y.core;
phi=cell(d+1,1);
phi{d+1}=1;
phi{1}=1;
%Warmup procedure: orthogonalize from right-to-left & maxvol
pos1=psy(d);
%Cores i
for i=d-1:-1:1
   cr=cry(pos1:pos1+ry(i+1)*n(i+1)*ry(i+2)-1);
   cr2=cry(pos1-ry(i)*n(i)*ry(i+1):pos1-1);
   cr2=reshape(cr2,[ry(i)*n(i),ry(i+1)]);
   cr=reshape(cr,[ry(i+1),n(i+1)*ry(i+2)]);
   cr=cr.';
   [cr,rm]=qr(cr,0);
   ry(i+1)=size(cr,2);
   ind=maxvol2(cr);
   r1=cr(ind,:);
   cr=cr/r1;
   cr=cr.';
   cry(pos1:pos1+ry(i+1)*n(i+1)*ry(i+2)-1)=cr(:);
   pos1=pos1-ry(i)*n(i)*ry(i+1);
   cr2=cr2*(r1*rm).';
   cry(pos1:pos1+ry(i)*n(i)*ry(i+1)-1)=cr2(:);
   %Take phi matrix; convolve from right with current cors of V
   cr0=core(ps(i+1):ps(i+2)-1);
   cr0=reshape(cr0,[r(i+1)*n(i+1),r(i+2)]);
   cr0=cr0*phi{i+2}; %cr0 is now r(i)*n(i)*ry(i+1);
   cr0=reshape(cr0,[r(i+1),n(i+1)*ry(i+2)]);
   phi{i+1}=cr0(:,ind);
 %  psy=cumsum([1;n.*ry(1:d).*ry(2:d+1)]);
%y.core=cry;
%y.r=ry;
%y.ps=psy;
%keyboard

   %Orthogonalization & maxvol is "local operation" (touches two
   %cores)
   %Computation of elements requries storage of phi matrices
end
%Truncate cry
%pos1=pos1-n(1)*r(2);
cry=cry(pos1:numel(cry));
y.core=cry;
y.r=ry;
y.ps=psy;
%keyboard
swp=1;

yold=[];
not_converged = true;
pos1=1;
last_sweep = false;
while ( swp < nswp && not_converged )
    max_er=0;
cry_old=cry;
psy=cumsum([1;n.*ry(1:d).*ry(2:d+1)]);
pos1=1;
 for i=1:d-1
     %fprintf('i=%d \n',i);
     %We care for two cores, with number i & number i+1, and use
     %psi(i) and psi(i+2) as a basis; also we will need to recompute
     %psi(i+1)
     ps1=phi{i}; ps2=phi{i+2};
     %Take current core; convolve it with
     %core=core(ps
     %Compute (!) superblock and function (!) of it
     cr1=core(ps(i):ps(i+1)-1);
     cr2=core(ps(i+1):ps(i+2)-1);
     cr1=reshape(cr1,[r(i),n(i)*r(i+1)]);
     cr1=ps1*cr1;
     cr2=reshape(cr2,[r(i+1)*n(i+1),r(i+2)]);
     cr2=cr2*ps2;
     cr1=reshape(cr1,[ry(i)*n(i),r(i+1)]);
     cr2=reshape(cr2,[r(i+1),n(i+1)*ry(i+2)]);
     cr=cr1*cr2;
     cr=reshape(cr,[ry(i)*n(i),n(i+1)*ry(i+2)]);
     cr=fun(cr); %Elements are evaluated here!
     cr=reshape(cr,[ry(i)*n(i),n(i+1)*ry(i+2)]);
     %Check for local approximation of cr for the error
     cry1=cry(pos1:pos1+ry(i)*n(i)*ry(i+1)-1);

     cry2=cry_old(psy(i+1):psy(i+2)-1);

     cry1=reshape(cry1,[ry(i)*n(i),ry(i+1)]);
     cry2=reshape(cry2,[ry(i+1),n(i+1)*ry(i+2)]);
     appr=cry1*cry2;
     er=norm(appr-cr,'fro')/norm(cr,'fro');
     max_er=max(er,max_er);
     %Compute SVD of cr

     if (mod(swp,dropsweeps)~=0)&&(swp>1)&&(~last_sweep)
         [u,s,v]=svd(cr-appr,'econ');
     else
         [u,s,v]=svd(cr,'econ');
     end;
     s=diag(s);
     r2=my_chop2(s,eps*norm(cr,'fro')/sqrt(d-1));
     s=s(1:r2); u=u(:,1:r2); v=v(:,1:r2);

     v=conj(v)*diag(s);

     if (mod(swp,dropsweeps)~=0)&&(swp>1)&&(~last_sweep)
         u = [cry1, u];
         v = [cry2.', v];
         [u,rv]=qr(u,0);
         v = v*(rv.');
     else
         if (~last_sweep)
             %Kick rank of u
             ur=randn(size(u,1),kick_rank);
             %Orthogonalize ur to u by Golub-Kahan reorth
             u=reort(u,ur);
             radd=size(u,2)-r2;
             if ( radd > 0 )
                 vr=zeros(size(v,1),radd);
                 v=[v,vr];
             end
         end;
     end;

     %vr=randn(size(v,1),kick_rank);
     %v=reort(v,vr);
     %radd=size(v,2)-rnew;
     %if ( radd > 0 )
     %    ur=zeros(size(u,1),radd);
     %    u=[u,ur];
     %end
     %rnew=rnew+radd;

     %u=[u,uadd]; v=[v,vadd];
     %[u,ru]=qr(u,0);
     %v=v*(ru.');
     r2=size(u,2);

     ind=maxvol2(u);
     r1=u(ind,:);
     u=u/r1; v=v*r1'; v=v.';
     %Compute new phi
     ry(i+1)=r2;
     cry(pos1:pos1+ry(i)*n(i)*ry(i+1)-1)=u(:);
     pos1=pos1+ry(i)*n(i)*ry(i+1);
     cry(pos1:pos1+ry(i+1)*n(i+1)*ry(i+2)-1)=v(:);

     %Now we have to: cr1 with phi from the left
     phi{i+1}=cr1(ind,:); %phi{i+1}=phi{i+1};

 end
  %Truncate local memory
  cry=cry(1:pos1+ry(d)*n(d)*ry(d+1)-1);
  psy=cumsum([1;n.*ry(1:d).*ry(2:d+1)]);
y.core=cry;
y.r=ry;
y.ps=psy;
%keyboard;
cry_old=cry;
%m=numel(cry);
%cry=[zeros(size(cry)),cry];
%pos1=pos1+m;
%cry2=cry_old(psy(i+1):psy(i+2)-1);
cry=cry(psy(d):psy(d+1)-1); %Start--only two cores
 for i=d-1:-1:1
     %fprintf('i=%d \n',i);
     %We care for two cores, with number i & number i+1, and use
     %psi(i) and psi(i+2) as a basis; also we will need to recompute
     %psi(i+1)
     ps1=phi{i}; ps2=phi{i+2};
     %Take current core; convolve it with
     %core=core(ps
     %Compute (!) superblock and function (!) of it
     cr1=core(ps(i):ps(i+1)-1);
     cr2=core(ps(i+1):ps(i+2)-1);
     cr1=reshape(cr1,[r(i),n(i)*r(i+1)]);
     cr1=ps1*cr1;
     cr2=reshape(cr2,[r(i+1)*n(i+1),r(i+2)]);
     cr2=cr2*ps2;
     cr1=reshape(cr1,[ry(i)*n(i),r(i+1)]);
     cr2=reshape(cr2,[r(i+1),n(i+1)*ry(i+2)]);
     cr=cr1*cr2;
     cr=reshape(cr,[ry(i)*n(i),n(i+1)*ry(i+2)]);
     cr=fun(cr); %Elements are evaluated here!
     cr=reshape(cr,[ry(i)*n(i),n(i+1)*ry(i+2)]);
     %Check for local approximation of cr for the error
     %cry1=cry(pos1-ry(i)*n(i)*ry(i+1):pos1-1);
     cry1=cry_old(psy(i):psy(i+1)-1);
     %cry2=cry(ry(:ry(i+1)*n(i+1)*ry(i+2));
     %cry1=cry(1:ry(i)*n(i)*ry(i+1));
     %cry2=cry(ry(i)*n(i)*ry(i+1)+1:ry(i)*n(i)*ry(i+1)+ry(i+1)*n(i+1)*ry(i+2));
     cry2=cry(1:ry(i+1)*n(i+1)*ry(i+2));
     cry(1:ry(i+1)*n(i+1)*ry(i+2))=[];
     %cry2=cry_old(psy(i+1):psy(i+2)-1);
     %cry(1:(ry(i+1)*n(i+1)*ry(i+2)))=[]; %Delete both of first cores
     %cry(1:ry(i)*n(i)*ry(i+1)+ry(i+1)*n(i+1)*ry(i+2))=[];
     cry1=reshape(cry1,[ry(i)*n(i),ry(i+1)]);
     cry2=reshape(cry2,[ry(i+1),n(i+1)*ry(i+2)]);
     appr=cry1*cry2;
     er=norm(appr-cr,'fro')/norm(cr,'fro');
     %er
     max_er=max(er,max_er);

     if (mod(swp,dropsweeps)~=0)&&(swp>1)&&(~last_sweep)
         [u,s,v]=svd(cr-appr,'econ');
     else
         %Compute SVD of cr
         [u,s,v]=svd(cr,'econ');
     end;
     s=diag(s);
     r2=my_chop2(s,eps*norm(cr,'fro')/sqrt(d-1));
     s=s(1:r2); u=u(:,1:r2); v=conj(v(:,1:r2));
     %Make it standard
     u=u*diag(s);

     if (mod(swp,dropsweeps)~=0)&&(swp>1)&&(~last_sweep)
         u = [cry1, u];
         v = [cry2.', v];
         [v,rv]=qr(v,0);
         u = u*(rv.');
     else
         if (~last_sweep)
             %Kick rank
             vr=randn(size(v,1),kick_rank);
             v=reort(v,vr);
             radd=size(v,2)-r2;
             if ( radd > 0 )
                 ur=zeros(size(u,1),radd);
                 u=[u,ur];
             end
         end;
     end;

     r2=size(v,2);
     ind=maxvol2(v);
     r1=v(ind,:);
     v=v/r1; v=v.';
     u=u*r1';
     %Compute new phi;
     ry(i+1)=r2;
     u=u(:); u=u'; v=v(:); v=v';
     cry=[u,v,cry];
     %keyboard;
     %We need new memory for
     %cry(pos1:pos1+ry(i+1)*n(i+1)*ry(i+2)-1)=v(:); %Here memory has to be (?) enlarged
     %if ( pos1 <= ry(i)*n(i)*ry(i+1) ) %Have to enlarge memory
     %  cry=cry(pos1:numel(cry));
     %  cry=[u(:)',cry];
     %  pos1=ry(i)*n(i)*ry(i+1)+1;
     %else
     %   pos1=pos1-ry(i)*n(i)*ry(i+1);
      %  cry(pos1:pos1+ry(i)*n(i)*ry(i+1)-1)=u(:);
     %end
     %Now we have to: cr1 with phi from the left
     phi{i+1}=cr2(:,ind); phi{i+1}=phi{i+1};

 end
 %Truncate local memory
 %cry=cry(pos1:numel(cry));

% keyboard;
  psy=cumsum([1;n.*ry(1:d).*ry(2:d+1)]);

y.core=cry;
y.r=ry;
y.ps=psy;
%keyboard;
if ( isempty(yold) )
 yold=y;
 er_nrm=1;
else

   er_nrm=norm(yold-y)/norm(y);
   yold=y;
end
if ( verb )
 fprintf('sweep=%d, er=%3.2e er_nrm=%3.2e \n',swp,max_er,er_nrm);
end
if (last_sweep)
    break;
end;
if (er_nrm<eps)
    last_sweep=true;
end;
swp=swp+1;
end
  psy=cumsum([1;n.*ry(1:d).*ry(2:d+1)]);

y.core=cry;
y.r=ry;
y.ps=psy;

end


function h = max( f, g, dim )
%MAX   Maximum value of a SEPARABLEAPPROX in one direction.
%   MAX(f) returns a chebfun representing the maximum of the SEPARABLEAPPROX along the
%   y direction, i.e, MAX(f) = @(x) max( f ( x, : ) )
%
%   MAX(f, [], dim) returns a CHEBFUN representing the maximum of f along the
%   DIM direction. If DIM = 1 is along the y-direction and DIM = 2 is along the
%   x-direction.
%
%   WARNING: This function is not always accurate to the expected precision.
%
%   For the global maximum use MAX2.
%
% See also MIN, MAX2, MIN2, MINANDMAX2.

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

% Empty check:
if ( isempty( f ) )
    error('CHEBFUN:SEPARABLEAPPROX:max:input', 'SEPARABLEAPPROX is empty');
end

% Default to max of one separableApprox:
if ( nargin < 2 )
    g = [];
end

% Default to maximum along the y direction:
if ( nargin < 3 )
    dim = 1;
end

% Do not allow max(F, G):
if ( nargin > 1 && ~isempty( g ) )
    error('CHEBFUN:SEPARABLEAPPROX:max:twoSeparableApproxInputs', ...
        'Unable to maximize two SEPARABLEAPPROX objects.');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% We have no idea how to achieve this in an efficient way. This
% is an attempt to return a result, but typically it won't be accurate to
% more than 4-5 digits.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

dom = f.domain;
n = 2049;
if ( dim == 1 )
    vals = sample(f, n, n);
    h = chebfun( max( vals ).', dom(1:2), 'splitting', 'on' );
    h = simplify( h.', [], 'globaltol' );
elseif ( dim == 2 )
    vals = sample(f, n, n);
    h = chebfun( max( vals, [], 2 ), dom(3:4), 'splitting', 'on' );
    h = simplify( h, [], 'globaltol' );
elseif ( dim == 0 )
    error('CHEBFUN:SEPARABLEAPPROX:max:dim', ...
        'Dimension argument must be a positive integer scalar within indexing range.')
else
   % return the SEPARABLEAPPROX. This is analogous to that MAX() command in
   % MATLAB.
   h = f;
end

end


function Cube = InitCube
% initialize the cube
Cube = [];
SmCube = ...
  [[0 0 0]; [1 0 0]; [0 1 0]; [1 1 0]; ...
   [0 0 1]; [1 0 1]; [0 1 1]; [1 1 1]];
Cube.Nodes = [];
for x=1:3,
  for y=1:3,
    for z=1:3,
      Cube.Nodes(:,1,(z-1)*9+(y-1)*3+x) = SmCube(:,1)*2+2*x-5;
      Cube.Nodes(:,2,(z-1)*9+(y-1)*3+x) = SmCube(:,2)*2+2*y-5;
      Cube.Nodes(:,3,(z-1)*9+(y-1)*3+x) = SmCube(:,3)*2+2*z-5;
    end
  end
end
Cube.Order = [[1 5 7 3];[2 4 8 6];[1 2 6 5];...
              [3 7 8 4];[1 3 4 2];[5 6 8 7]];
NumColor = [[1:3:25];[3:3:27];[1 2 3 10 11 12 19 20 21];...
  [7 8 9 16 17 18 25 26 27];[1:9];[19:27]];
ValColor = [[1 0 0];[1 0 1];[0 0 1];[0 1 0];[1 1 0];[1 1 1]];
Cube.Color = zeros(6,3,27); % black
for x=1:6,
  for y=1:9,
    Cube.Color(x,:,NumColor(x,y)) = ValColor(x,:);
  end
end
Cube.RotCubeCube = ... % for rotate cube
  [[ 7  8  9 16 17 18 25 26 27  4  5  6 13 14 15 22 23 24  1  2  3 10 11 12 19 20 21];...
   [19 20 21 10 11 12  1  2  3 22 23 24 13 14 15  4  5  6 25 26 27 16 17 18  7  8  9];...
   [19 10  1 22 13  4 25 16  7 20 11  2 23 14  5 26 17  8 21 12  3 24 15  6 27 18  9];...
   [ 3 12 21  6 15 24  9 18 27  2 11 20  5 14 23  8 17 26  1 10 19  4 13 22  7 16 25];...
   [ 3  6  9  2  5  8  1  4  7 12 15 18 11 14 17 10 13 16 21 24 27 20 23 26 19 22 25];...
   [ 7  4  1  8  5  2  9  6  3 16 13 10 17 14 11 18 15 12 25 22 19 26 23 20 27 24 21]];
Cube.RotCubeFlat = ... % for rotate cube
  [[1 2 5 6 4 3];...
   [1 2 6 5 3 4];...
   [6 5 3 4 1 2];...
   [5 6 3 4 2 1];...
   [3 4 2 1 5 6];...
   [4 3 1 2 5 6]];
Cube.RotLayerCube = ... % for rotate layer
  [[1:3:25];[3:3:27];[1 2 3 10 11 12 19 20 21];[7 8 9 16 17 18 25 26 27];[1:9];[19:27]];
Cube.NumColor = NumColor;
% Mouse polygons for any rotations
Cube.PgRotCubeX = ... % for rotate the cube around OX, OY, OZ
  [[168;114;171;225;168],[292;292;346;346;292],...
   [232;292;346;287;232],[292;347;347;292;292],...
   [232;171;119;180;232],[119;180;180;119;119],...
   [340;278;225;287;340],[119;180;180;119;119],...
   [240;240;180;180;240],[400;400;346;346;400],...
   [60;60;119;119;60],[292;292;240;240;292]];
Cube.PgRotCubeY(:,:,1) = ...
  [[373;345;323;350;373],[273;214;239;299;273],...
   [296;273;299;324;296],[89;117;176;148;89],...
   [296;323;294;272;296],[107;85;146;170;107],...
   [352;375;350;324;352],[234;209;272;294;234],...
   [123;187;209;146;123],[203;265;239;176;203],...
   [256;191;170;234;256],[148;214;187;123;148]];
Cube.PgRotLayX = ... % for rotate the layer
  [[240;292;232;240],[240;292;292;240],...
   [60;171;114;60],[240;292;292;240],...
   [220;168;225;220],[400;346;346;400],...
   [400;287;346;400],[400;347;347;400],...
   [400;340;287;400],[240;180;180;240],...
   [240;232;180;240],[240;180;180;240],...
   [60;119;171;60],[60;119;119;60],...
   [220;225;278;220],[60;119;119;60],...
   [60;60;119;60],[240;240;292;240],...
   [240;180;240;240],[400;346;400;400],...
   [240;240;180;240],[400;400;347;400],...
   [60;119;60;60],[240;292;240;240]];
Cube.PgRotLayY = ...
  [[246;273;296;246],[60;89;148;60],...
   [319;323;345;319],[246;214;273;246],...
   [398;373;350;398],[329;299;239;329],...
   [329;324;299;329],[142;176;117;142],...
   [329;352;324;329],[246;272;209;246],...
   [246;296;272;246],[60;146;85;60],...
   [319;294;323;319],[133;107;170;133],...
   [398;350;375;398],[319;234;294;319],...
   [319;256;234;319],[246;187;214;246],...
   [246;209;187;246],[329;239;265;329],...
   [60;123;146;60],[142;203;176;142],...
   [133;170;191;133],[60;148;123;60]];
Cube.hand = ...
  [[NaN,NaN,NaN,NaN,NaN,NaN,NaN,  1,  1,NaN,NaN,NaN,NaN,NaN,NaN,NaN];...
   [NaN,NaN,NaN,NaN,  1,  1,  1,  2,  2,  1,  1,  1,NaN,NaN,NaN,NaN];...
   [NaN,NaN,NaN,  1,  2,  2,  1,  2,  2,  1,  2,  2,  1,NaN,NaN,NaN];...
   [NaN,NaN,NaN,  1,  2,  2,  1,  2,  2,  1,  2,  2,  1,NaN,NaN,NaN];...
   [NaN,NaN,NaN,  1,  2,  2,  1,  2,  2,  1,  2,  2,  1,  1,  1,NaN];...
   [NaN,  1,  1,  1,  2,  2,  1,  2,  2,  1,  2,  2,  1,  2,  2,  1];...
   [  1,  2,  2,  1,  2,  2,  1,  2,  2,  1,  2,  2,  1,  2,  2,  1];...
   [  1,  2,  2,  1,  2,  2,  1,  2,  2,  1,  2,  2,  1,  2,  2,  1];...
   [  1,  2,  2,  1,  2,  2,  1,  2,  2,  1,  2,  2,  1,  2,  2,  1];...
   [  1,  2,  2,  1,  2,  2,  1,  2,  2,  1,  2,  2,  1,  2,  2,  1];...
   [  1,  2,  2,  2,  2,  2,  2,  2,  2,  2,  2,  2,  1,  2,  2,  1];...
   [  1,  2,  2,  2,  2,  2,  2,  2,  2,  2,  2,  2,  2,  2,  2,  1];...
   [NaN,  1,  2,  2,  2,  2,  2,  2,  2,  2,  2,  2,  2,  2,  2,  1];...
   [NaN,  1,  2,  2,  2,  2,  2,  2,  2,  2,  2,  2,  2,  2,  1,NaN];...
   [NaN,NaN,  1,  2,  2,  2,  2,  2,  2,  2,  2,  2,  2,  2,  1,NaN];...
   [NaN,NaN,  1,  2,  2,  2,  2,  2,  2,  2,  2,  2,  2,  2,  1,NaN]];
return


function c4_normal_01_test ( )

%*****************************************************************************80
%
%% C4_NORMAL_01_TEST tests C4_NORMAL_01.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 July 2006
%
%  Author:
%
%    John Burkardt
%
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'C4_NORMAL_01_TEST\n' );
  fprintf ( 1, '  C4_NORMAL_01 computes pseudorandom complex values\n' );
  fprintf ( 1, '  normally distributed in the unit circle.\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The initial seed is %d\n', seed );
  fprintf ( 1, '\n' );

  for i = 1 : 10
    [ r, seed ] = c4_normal_01 ( seed );
    fprintf ( 1, '  %6d  (%14f,  %14f)\n', i, real ( r ), imag ( r ) );
  end

  return
end


function [c1,c2] = match(c1,c2,toleranceInSeconds)

   %Retain matching elements of two correlation objects.
   %
   % [C1,C2] = MATCH(C1,C2) Returns those elements of c1 and c2 which have
   % matching trigger times. Typically this will be used to sync correlation
   % objects from two different stations covering the same time frames, or
   % sync'ing two channels from the same station.
   %
   % [C1,C2] = MATCH(C1,C2,TOLERANCE) allows traces to match when their
   % trigger times are within TOLERANCE seconds of each other. When this
   % parameter is not included, the default value is 1 second.
   %
   % CAVEATS: Note that if C2 has multiple traces which match a trigger time
   % in C1, there is no way of controling which single trace in C2 will be
   % considered a match. Similarly, if C1 contains multiple traces with the
   % same trigger time, a match for each one will be found in C2.

   % AUTHOR: Michael West, Geophysical Institute, Univ. of Alaska Fairbanks
   % $Date$
   % $Revision$

   narginchk(2,3);

   if ~exist('tolerance','var')
      toleranceInSeconds = 1;
   end

   assert(isnumeric(toleranceInSeconds), 'Tolerance must be a number in seconds')

   disp(['Tolerance for successful match is: ' num2str(toleranceInSeconds) ' seconds']);

   % FIND ELEMENTS THAT MATCH
   trig1 = c1.trig;
   trig2 = c2.trig;
   matchIndex = zeros(size(trig1));
   for n = 1:numel(trig1)
      [minVal,minIndex] = min(abs(trig2 - trig1(n))*86400);
      if minVal < toleranceInSeconds
         matchIndex(n) = minIndex;
      else
         matchIndex(n) = 0;
      end
   end

   % SUBSET FIRST CORRELATION OBJECT
   index = find(matchIndex);
   c1 = subset(c1,index);

   % SUBSET FIRST CORRELATION OBJECT
   index2 = matchIndex(index);
   c2 = subset(c2,index2);

end


% DEMSILHOUETTEAVERAGE Shows the average of the poses.

% FORMAT
% DESC show the average of the training poses from Agarwal and Triggs.
%
% SEEALSO : demSilhouetteGp1, demSilhouetteGp2
%
% COPYRIGHT : Neil D. Lawrence, 2008

% GP

randn('seed', 1e7)
rand('seed', 1e7)

dataSetName = 'silhouette';

capName = dataSetName;;
capName(1) = upper(capName(1));

% load data
[X, y, XTest, yTest] = mapLoadData(dataSetName);

yPred = repmat(mean(y), size(yTest, 1), 1);
xyzankurAnimCompare(yPred, yTest);

yDiff = (yPred - yTest);
rmsError = sqrt(sum(sum(yDiff.*yDiff))/prod(size(yDiff)));

counter = 0;
if printDiagram
  fileBase = ['dem' capName 'Average'];
  figure
  handle = xyzankurVisualise(yPred(1, :), 1);
  printPlot([fileBase], '../tex/diagrams', '../html')
end


function varargout = tapas_uniqc_example_slider4d()
%creates 4D dataset numerically and runs tapas_uniqc_slider4d to show capability of it
%
%    tapas_uniqc_example_slider4d()
%
% IN
%
% OUT
%
% EXAMPLE
%   tapas_uniqc_example_slider4d
%
%   See also

% Author: Lars Kasper
% Created: 2013-05-15
% Copyright (C) 2013 Institute for Biomedical Engineering, ETH/Uni Zurich.

Y = tapas_uniqc_create_shepp_logan_4d();
nSli = 1;
tapas_uniqc_slider4d(Y, @tapas_uniqc_plot_image_diagnostics, nSli);

if nargout > 1
    varargout{1} = Y;
end


function net = fcnInitializeModel16sGeneric(labelCount, net)
%FCNINITIALIZEMODEL16S Initialize the FCN-16S model from FCN-32

%% Remove the last layer
net.removeLayer('deconv32') ;

%% Add the first Deconv layer
filters = single(bilinear_u(4, 1, labelCount)) ;
net.addLayer('deconv2', ...
  dagnn.ConvTranspose('size', size(filters), ...
                      'upsample', 2, ...
                      'crop', [1 1 1 1], ...
                      'hasBias', false), ...
             'x38', 'x39', 'deconv1f') ;
f = net.getParamIndex('deconv1f') ;
net.params(f).value = filters ;
net.params(f).learningRate = 0 ;
net.params(f).weightDecay = 1 ;

%% Add skip layers on top of pool4
net.addLayer('skip4', ...
     dagnn.Conv('size', [1 1 512 labelCount], 'pad', 0), ...
     'x24', 'x40', {'skip4f','skip4b'});

f = net.getParamIndex('skip4f') ;
net.params(f).value = zeros(1, 1, 512, labelCount, 'single') ;
net.params(f).learningRate = 0.1 ;
net.params(f).weightDecay = 1 ;

f = net.getParamIndex('skip4b') ;
net.params(f).value = zeros(1, 1, labelCount, 'single') ;
net.params(f).learningRate = 2 ;
net.params(f).weightDecay = 1 ;

%% Add the summation layer
net.addLayer('sum1', dagnn.Sum(), {'x39', 'x40'}, 'x41') ;

%% Add deconvolutional layer implementing bilinear interpolation
filters = single(bilinear_u(32, labelCount, labelCount)) ;
net.addLayer('deconv16', ...
  dagnn.ConvTranspose('size', size(filters), ...
                      'upsample', 16, ...
                      'crop', 8, ...
                      'numGroups', labelCount, ...
                      'hasBias', false), ...
             'x41', 'prediction', 'deconv16f') ;

f = net.getParamIndex('deconv16f') ;
net.params(f).value = filters ;
net.params(f).learningRate = 0 ;
net.params(f).weightDecay = 1 ;

% Make sure that the output of the bilinear interpolator is not discared for
% visualization purposes
net.vars(net.getVarIndex('prediction')).precious = 1 ;

% empirical test
if 0
  figure(100) ; clf ;
  n = numel(net.vars) ;
  for i=1:n
    vl_tightsubplot(n,i) ;
    showRF(net, 'input', net.vars(i).name) ;
    title(sprintf('%s', net.vars(i).name)) ;
    drawnow ;
  end
end


function f = eval(SO3VF,rot,varargin)
%
% syntax
%   f = eval(SO3VF,rot)
%
% Input
%   rot - @rotation
%
% Output
%   f - @vector3d
%

% if isa(rot,'orientation')
%   ensureCompatibleSymmetries(SO3VF,rot)
% end

f = vector3d(SO3VF.SO3F.eval(rot)');
f = reshape(f',size(rot));

end


function Power(nlp, sys)

    domains = sys.Gamma.Nodes.Domain;

    for i=1:numel(domains)
        domain = domains{i};

        u = domain.Inputs.Control.u;
        dq = domain.States.dx;
        B = domain.Gmap.Control.u;

        P = tovector(norm(dq.*(B*u)).^2);
        power_fun = SymFunction(['power_' sys.Gamma.Nodes.Name{i}],P,{u,dq});
        rs_phase = getPhaseIndex(nlp,sys.Gamma.Nodes.Name{i});
        addRunningCost(nlp.Phase(rs_phase),power_fun,{'u','dx'});

    end
    nlp.update;
end


function [T, M, S, DISTR, df] = dtiDirTestStat(g1, g2, Y, mask)

% Computes voxel-wise test statistics for two groups from a data array of
% DTI directions.
%
%   [T, M, S, DISTR, df] = dtiDirTestStat(g1, g2, DIR_ARRAY, [MASK])
%
% Input:
%   g1, g2      List of indices that correspond to each group out of 1:N
%                   E.g: g1 = 1:7, g2 = 8:14, N = 14
%   DIR_ARRAY   Data array of size XxYxZx3xN (or nx3xN), where X, Y, Z are the volume
%                   dimensions and N is the number of subjects.
%                   The directions are contained in the dimension of size 3.
%   MASK        Optional XxYxZ binary array. Values of M and S are computed
%                   where mask = 1; in other voxels, M and S are set to NaN.
%                   Default is entire volume.
%
% Output:
%   T           XxYxZx1 (or nx1) array of test statistics (NaN where mask = 0)
%   M           XxYxZx3x2 (or nx3x2) array of mean directions for both groups (NaN where mask = 0)
%   S           XxYxZx1 (or nx1) array of dispersions (NaN where mask = 0)
%   DISTR       The string 'f'
%   df          The degrees of freedom of the f distribution
%
% Utilities:    ndfun.m, dtiEig.m, dti33to6.m
%
% WARNING: If using Pentium 4, eliminate NaN's from array before running
% (processor bug).
%
% Copyright by Armin Schwartzman, 2004

% HISTORY:
%   2004.06.23 ASH (armins@stanford.edu) wrote it.
%   2006.07.18 ASH (armins@stanford.edu) added indexed format capability.
%

% Check inputs
if (ndims(Y)==2 | ndims(Y)==3),
    Ind = 1;    % Data in indexed nx3xN format
    Y = shiftdim(Y, -2);
else
    Ind = 0;    % Data in XxYxZx3xN format
end
if (ndims(Y)<4 | ndims(Y)>5),
    error('Wrong input format');
end
if (~exist('mask')),
    mask = ones([size(Y,1) size(Y,2) size(Y,3)]);
end

% Computations
N1 = length(g1);
N2 = length(g2);
N  = N1 + N2;
p = size(Y, 4); % should be 3
DISTR = 'f';
df = [p-1, (p-1)*(N-2)];

Y = permute(Y, [4 6 5 1 2 3]); % permutation required by ndfun
Tbar1 = shiftdim(sum(ndfun('mult', Y(:,1,g1,:,:,:), permute(Y(:,1,g1,:,:,:), [2 1 3:6])), 3), 3) / N1;
Tbar2 = shiftdim(sum(ndfun('mult', Y(:,1,g2,:,:,:), permute(Y(:,1,g2,:,:,:), [2 1 3:6])), 3), 3) / N2;
Tbar = (N1 * Tbar1 + N2 * Tbar2)/(N1 + N2);
[vec1, val1] = dtiEig(dti33to6(Tbar1)); % ndfun('eig', Tbar1);
[vec2, val2] = dtiEig(dti33to6(Tbar2)); % ndfun('eig', Tbar2);
[vec, val] = dtiEig(dti33to6(Tbar)); % ndfun('eig', Tbar);
M = cat(5, vec1(:,:,:,:,1), vec2(:,:,:,:,1));
S = (N - N1*val1(:,:,:,1) - N2*val2(:,:,:,1)) / (N-2);;
T = (N1*val1(:,:,:,1) + N2*val2(:,:,:,1) - N*val(:,:,:,1)) ./ S;
M(~repmat(mask,[1 1 1 3 2])) = NaN;
S(~mask) = NaN;
T(~mask) = NaN;

% Adjust output
if Ind,
    T = shiftdim(T, 2);
    M = shiftdim(M, 2);
    S = shiftdim(S, 2);
end

return


function output = calllinprog(interfacedata)

% Standard input interface
options = interfacedata.options;
F_struc = interfacedata.F_struc;
c       = interfacedata.c;
K       = interfacedata.K;
Q       = interfacedata.Q;
lb      = interfacedata.lb;
ub      = interfacedata.ub;
x0      = interfacedata.x0;

showprogress('Calling LINPROG',options.showprogress);

if isempty(F_struc)
    Aeq = [];
    beq = [];
    A = [];
    b = [];
else
    Aeq = -F_struc(1:1:K.f,2:end);
    beq = F_struc(1:1:K.f,1);
    A =-F_struc(K.f+1:end,2:end);
    b = F_struc(K.f+1:end,1);
end

switch options.verbose
case 0
    options.linprog.Display = 'off';
case 1
    options.linprog.Display = 'final';
otherwise
    options.linprog.Display = 'iter';
end

if isfield(options.linprog,'LargeScale')
    if ~isequal(options.linprog.LargeScale,'on')
        c = full(c);
        A = full(A);
        b = full(b);
        Aeq = full(Aeq);
        beq = full(beq);
    end
end

if ~options.warmstart
    x0 = [];
end

if options.savedebug
    ops = options.linprog;
    save linprogdebug c A b Aeq beq lb ub ops x0
end

solvertime = tic;
[x,fmin,flag,output,lambda] = linprog(c, A, b, Aeq, beq, lb, ub, x0,options.linprog);
solvertime = toc(solvertime);
problem = 0;

% Internal format for duals
if isempty(lambda)
    D_struc = [];
else
    D_struc = [lambda.eqlin;lambda.ineqlin];
end
if isempty(x)
    x = zeros(length(c),1);
end

% Check, currently not exhaustive...
if flag==0
    problem = 3;
elseif flag == -2 | flag==-5
    problem = 1;
elseif flag == -3
    problem = 2;
else
    if flag>0
        problem = 0;
    else
        if any((A*x-b)>sqrt(eps)) | any( abs(Aeq*x-beq)>sqrt(eps))
            problem = 1; % Likely to be infeasible
        else
            if c'*x<-1e10 % Likely unbounded
                problem = 2;
            else          % Probably convergence issues
                problem = 5;
            end
        end
    end
end

% Save all data sent to solver?
if options.savesolverinput
    solverinput.A = A;
    solverinput.b = b;
    solverinput.Aeq = Aeq;
    solverinput.beq = beq;
    solverinput.c = c;
    solverinput.options = options.linprog;
else
    solverinput = [];
end

% Save all data from the solver?
if options.savesolveroutput
    solveroutput.x = x;
    solveroutput.fmin = fmin;
    solveroutput.flag = flag;
    solveroutput.output=output;
else
    solveroutput = [];
end

% Standard interface
output = createOutputStructure(x(:),D_struc,[],problem,interfacedata.solver.tag,solverinput,solveroutput,solvertime);


%-------------------------------------------------------------------------------------------------------------------%
%
% IB2d is an Immersed Boundary Code (IB) for solving fully coupled non-linear
% 	fluid-structure interaction models. This version of the code is based off of
%	Peskin's Immersed Boundary Method Paper in Acta Numerica, 2002.
%
% Author: Nicholas A. Battista
% Email:  nickabattista@gmail.com
% Date Created: May 27th, 2015
% Institution: UNC-CH
%
% This code is capable of creating Lagrangian Structures using:
% 	1. Springs
% 	2. Beams (*torsional springs, non-invariant beams*)
% 	3. Target Points
%	4. Muscle-Model (combined Force-Length-Velocity model, "Hill+(Length-Tension)")
%
% One is able to update those Lagrangian Structure parameters, e.g., spring constants, resting lengths, etc
%
% There are a number of built in Examples, mostly used for teaching purposes.
%
% If you would like us to add a specific muscle model, please let Nick (nickabattista@gmail.com) know.
%
%--------------------------------------------------------------------------------------------------------------------%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: updates the spring attributes!
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function springs_info = update_Springs(dt,current_time,xLag,yLag,springs_info)

%springs_info: col 1: starting spring pt (by lag. discretization)
%              col 2: ending spring pt. (by lag. discretization)
%              col 3: spring stiffness
%              col 4: spring resting lengths

%RL = springs_info(:,4); % resting-length vector

% Contraction Frequency
freq = 1.0;

% CHANGE RESTING LENGTH BTWN SIDES OF JELLYFISH BELL
springs_info(239:end,4) = abs( cos(freq*current_time*pi) );

%NOTE: not 2*pi*ft b/c of abs()


function [lat, long ] = gcj2wgs(gcjLat, gcjLng)
% Not exact
% Fully vectorized

    lat = gcjLat;
    long = gcjLng;

    inChina = ~outOfChina(gcjLat, gcjLng);

    [dlat, dlng] = delta(gcjLat(inChina), gcjLng(inChina));

    lat(inChina) = gcjLat(inChina) - dlat;
    long(inChina) = gcjLng(inChina) - dlng;

end


% load data
% load('../results/LightenedCNN_A_lfw.mat');      % model A
% load('../results/LightenedCNN_B_lfw.mat');      % model B
load('../results/LightenedCNN_C_lfw.mat');      % model C
load('lfw_pairs.mat');

% pos
for i = 1: length(pos_pair)
    feat1 = features(pos_pair(1, i), :)';
    feat2 = features(pos_pair(2, i), :)';
    pos_scores(i) = distance.compute_cosine_score(feat1, feat2);
%     pos_scores(i) = -distance.compute_L2_score(feat1, feat2);
end
pos_label = ones(1, length(pos_pair));

%neg
for i = 1: length(neg_pair)
    feat1 = features(neg_pair(1, i), :)';
    feat2 = features(neg_pair(2, i), :)';
    neg_scores(i) = distance.compute_cosine_score(feat1, feat2);
%     neg_scores(i) = -distance.compute_L2_score(feat1, feat2);
end
neg_label = -ones(1, length(neg_pair));

scores = [pos_scores, neg_scores];
label = [pos_label neg_label];

% ap
ap = evaluation.evaluate('ap', scores, label);

% roc
roc = evaluation.evaluate('roc', scores, label);

%% output
fprintf('ap:           %f\n', ap.measure);
fprintf('eer:          %f\n', roc.measure);
fprintf('tpr001:       %f\n', roc.extra.tpr001*100);
fprintf('tpr0001:      %f\n', roc.extra.tpr0001*100);
fprintf('tpr00001:     %f\n', roc.extra.tpr00001*100);
fprintf('tpr000001:    %f\n', roc.extra.tpr000001*100);
fprintf('tpr0:         %f\n', roc.extra.tpr0*100);
result = [ap.measure/100 roc.measure/100  roc.extra.tpr001 roc.extra.tpr0001 roc.extra.tpr00001 roc.extra.tpr000001 roc.extra.tpr0];


function nc2010_vbpca_mnistexperiment(flatW, rotate, ncomps)

M = load('mnist_all');

n = 100;

Y = double(M.train5');
inds = randperm(cols(Y));
Y = Y(:,inds(1:n));
size(Y)

m = rows(Y);

startupdatehyper = 1
maxiters = 1e4;

randn('state', 1);
rand('state', 1);

% Divide into training (90%) and test sets (10%)
testset = (rand(size(Y)) < 0.1);
Ytrain = Y;
Ytest = Y;
Ytrain(testset) = nan;
Ytest(~testset) = nan;

%size(Ytrain)

if flatW
  error('Using flat W might be problematic because m>n ???');
  disp('Using non-hierarchical flat prior for W');
  stringW = 'flatW';
else
  disp('Using hierarchical prior for W');
  stringW = 'hierW';
end

filename = sprintf(['/home/jluttine/matlab/neurocomputing2010/' ...
                    'nc2010_vbpca_mnistexperiment_%s_ncomps=%d_rotate=%d'], ...
                   stringW, ncomps, rotate);

results = vbpcamv(Ytrain, ncomps, 'testset', Ytest, 'maxiters', 1e7, ...
                  'rotate', rotate, 'startupdatehyper', startupdatehyper, ...
                  'startrotate', 1, 'autosavetime', 1, ...
                  'autosavefile', filename, 'fixw', flatW);


% fig_osem_mismatch.m
% examine effects of model mismatch on ML-OSEM
% Copyright 2002-6-10, Jeff Fessler, The University of Michigan

disp 'todo: NOT DONE!'

%
% generate data
%
if ~isvar('xfbp'), printm 'data'
	x = read_zubal_emis;
	[nx ny] = size(x);
	nb = 100;
	na = 10;

	arg = arg_pair('system', 12, 'nx', nx, 'ny', ny, ...
		'nb', nb, 'na', na, 'support', 'ellipse 0 0 60 60', ...
		'orbit', 180, 'orbit_start', 0, ...
		'pixel_size', 3, 'ray_spacing', 3, ...
		'obj2det_x', 2*nx, 'obj2det_y', 2*nx, ...
		'fwhm_detector', 0, ...
		'fwhm_collimator0', 0, ...
		'fwhm_slope', 0, ...
		'scale', 0);
prompt
end

%
% block object for block iterative
%
if ~isvar('Gb'), printm 'Gb'
	f.nblock = 5;
	Gb = Gblock(G, f.nblock);
prompt
end

%
% matlab iterations
%
if ~isvar('xmat'), printm 'matlab E-ML-OSEM'
	f.niter = 9;
	f.pixmax = 6;

	xinit = ones(size(xtrue));
	xmat = eml_osem(xinit(mask(:)), Gb, yi, ci, ri, f.niter, f.pixmax, ...
		[], 1.0);	% default fast precon matches aspire
	xmat = embed(xmat, mask);
	im clf, im(121, xmat, 'Matlab E-ML-OSEM iterations'), cbar horiz
prompt
end

if ~has_aspire, return, end

%
% aspire iterations
%
if ~isvar('xasp'), printm 'aspire E-ML-OSEM'

	f.init	= [f.dir 'init.fld'];
	f.out	= [f.dir 'out.fld'];
	fld_write(f.init, xinit, 'check', 0)
	if exist(f.out, 'file'), delete(f.out), end

	if ~isvar('f.sys_type')
		f.sys_type = ['2z@' f.wtr '@-'];
	end
	if ~isvar('n.n2')
		n.n2 = n.a;	% kludge
	end
	f.saver	= 'stack,1';
	f.alg	= sprintf('osemc,fast,%d,%d,1', f.nblock, n.n2);
	f.penal	= '-';
	f.method = sprintf('@%d@%s@%s', f.niter-1, f.alg, f.penal);
	f.com = sprintf(['i -chat 0 empl3 %s %s  %s %s 1 %s 1 %s -' ...
			' %s %s 0 1 %g 0 -'], ...
		f.out, f.init, f.yi, f.ci, f.ri, f.sys_type, ...
		f.method, f.saver, f.pixmax);
	os_run(f.com)

	xasp = double(fld_read(f.out));
	im(122, xasp, 'Aspire E-ML-OSEM iterations'), cbar horiz
end

if 1
	t = vcorrcoef(xasp, xmat);
	printf('corr. %g,%g', t, t-1)

	im clf, im(221, xmat, 'xhat matlab')
	im(222, xasp, 'xhat aspire')
	im(223, (xasp-xmat)/max(xmat(:)), 'aspire-matlab'), cbar

	t1 = eql_obj(xmat, G, yi(:), ci(:), ri(:), [], mask);
	t2 = eql_obj(xasp, G, yi(:), ci(:), ri(:), [], mask);

	if im
		subplot(224)
		plot(0:f.niter-1, t1-t1(1), '-o', 0:f.niter-1, t2-t1(1), '-x')
		xlabel iteration, ylabel '\Phi change', legend('mat', 'asp', 4)
		title(sprintf('E-ML-OSPS, Nsubset=%d', f.nblock))
	end
end


function A=linspace(x1,x2,N)
%LINPSPACE (overloaded)

if nargin < 3
    N = 100;
elseif isa(N,'sdpvar')
    error('SDPVAR/LINSPACE does not support N as a decision variable');
end

A = x1 + linspace(0,1,N)*(x2-x1);


function a = r8mat_diag_add_scalar ( n, a, s )

%*****************************************************************************80
%
%% R8MAT_DIAG_ADD_SCALAR adds a scalar to the diagonal of a matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 April 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of rows and columns of the matrix.
%
%    Input, real A(N,N), the matrix to be modified.
%
%    Input, real S, the value to be added to the diagonal
%    of the matrix.
%
%    Output, real A(N,N), the modified matrix.
%
  for i = 1 : n
    a(i,i) = a(i,i) + s;
  end

  return
end


function [ responses ] = PatchResponseCEN_mirror(patches, patch_experts_class, visibilities, patchExperts, window_size)
% As frontal faces are roughly symmetrical can compute the responses for
% two patches at the same time using only one of the landmark patch experts

    normalisationOptions = patchExperts.normalisationOptionsCol;
    patchSize = normalisationOptions.patchSize;

    responses = cell(size(patches, 1), 1);
    empty = zeros(window_size(1)-patchSize(1)+1, window_size(2)-patchSize(2)+1);

    % These landmark responses can be computed together
    mirror_inds = [1,17;2,16;3,15;4,14;5,13;6,12;7,11;8,10;18,27;19,26;20,25;21,24;22,23;...
                  32,36;33,35;37,46;38,45;39,44;40,43;41,48;42,47;49,55;50,54;51,53;60,56;59,57;...
                  61,65;62,64;68,66];

    for i = 1:numel(patches(:,1))
        if visibilities(i)
            % Do it only if not mirrored
            if(isempty(find(mirror_inds(:,2)==i, 1)))
                responses{i} = empty;

                col_norm = normalisationOptions.useNormalisedCrossCorr == 1;

                smallRegionVec = patches(i,:);
                smallRegion = reshape(smallRegionVec, window_size(1), window_size(2));

                patch = im2col_mine(smallRegion, patchSize)';

                % Add the mirrored version as well (it will be applied the
                % same way)
                mirr_id = mirror_inds(find(mirror_inds(:,1)==i,1),2);
                if(~isempty(mirr_id))
                    responses{mirr_id} = empty;
                    smallRegionVec_mirr = patches(mirr_id,:);
                    smallRegion_mirr = reshape(smallRegionVec_mirr, window_size(1), window_size(2));
                    patch_mirr = im2col_mine(fliplr(smallRegion_mirr), patchSize)';
                    patch = cat(1, patch, patch_mirr);
                end

                % Normalize
                if(col_norm)
                    mean_curr = mean(patch, 2);
                    patch_normed = patch - repmat(mean_curr, 1, patchSize(1)* patchSize(2));

                    % Normalising the patches using the L2 norm
                    scaling = sqrt(sum(patch_normed.^2,2));
                    scaling(scaling == 0) = 1;

                    patch_normed = patch_normed ./ repmat(scaling, 1, 11 * 11);

                    patch = patch_normed;
                end
                patch = patch';
                % Add bias
                patch_normed = cat(1, ones(1, size(patch,2)), patch);
                weights = patch_experts_class{i};
                % Where DNN will happen
                for w =1:numel(weights)/2

                    % mult and bias
                    patch_normed = weights{(w-1)*2+1}' * patch_normed + repmat(weights{(w-1)*2+2}', 1, size(patch_normed,2));

                    if w < 3
    %                    patch_normed(patch_normed < 0) = 0;
                       patch_normed = max(0, patch_normed);
                    else
                       patch_normed = 1./(1+exp(-patch_normed));
                    end

                end
                % If no mirroring took place
                if(isempty(mirr_id))
                    responses{i}(:) = reshape(patch_normed', window_size(1)-patchSize(1)+1, window_size(2)-patchSize(2)+1);
                else
                    patch_normed_1 = patch_normed(1:end/2);
                    patch_normed_2 = patch_normed(end/2+1:end);
                    responses{i}(:) = reshape(patch_normed_1', window_size(1)-patchSize(1)+1, window_size(2)-patchSize(2)+1);
                    responses{mirr_id}(:) = fliplr(reshape(patch_normed_2', window_size(1)-patchSize(1)+1, window_size(2)-patchSize(2)+1));
                end
            end
        end
    end

end


function truefalse = ch_is_digit ( c )

%*****************************************************************************80
%
% CH_IS_DIGIT returns TRUE if the character C is a digit.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 December 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, character C, a character.
%
%    Output, integer TRUEFALSE, is TRUE (1) if C is a digit, FALSE (0) otherwise.
%
  TRUE = 1;
  FALSE = 0;

  if ( '0' <= c & c <= '9' )
    truefalse = TRUE;
  else
    truefalse = FALSE;
  end

  return
end


function ivec = s_to_i4vec ( s, n )

%*****************************************************************************80
%
%% S_TO_I4VEC reads a integer vector from a string.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 June 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, character S(*), the string to be read.
%
%    Input, integer N, the number of values expected.
%
%    Output, integer IVEC(N), the values read from the string.
%
  i = 0;
  ivec = [];

  ilo = 1;

  while ( i < n )

    i = i + 1;

    [ ivec(i), lchar ] = s_to_i4 ( s(ilo:end) );

    ilo = ilo + lchar;

  end

  return
end