% This m-file produces the scales used in realized_range.  It can be run with more simulations to
% increase the precision of the estimated scales used.  If you do this you will have to copy and
% paste the results into the function since it uses a lookup table and interpolation.

% Clear
clear all
% Number of simulations
BB = 1000000;

% Number of prices to use in each interval.  All integer factors of 23400.  The asymptotic value is
% 4*log(2)

m1 = factor(23400);
m1 = [1 m1];
ms = m1';
for i=2:length(m1);
    temp = nchoosek(1:length(m1),i);
    ms = [ms;    unique(prod(m1(temp),2))]; %#ok<AGROW>
end
ms = unique(ms);
maxM = max(ms);

% Turn the number of prices per window in to a step size
gap = maxM./ms;
ms = ms+1;
% Initialize a place to hold results
M = length(ms);
MC = zeros(BB,M);
% BB simultions
tic
for j = 1:BB
    % Resample a single BM 1, 2, ..., 23400 times.
    x = [0;cumsum(randn(maxM,1)/sqrt(maxM))];
    for i=1:M
        x2 = x(1:gap(i):(maxM+1));
        MC(j,i)  = (max(x2)-min(x2))^2;
    end
    % Display the count and the expected time remaining.
    if mod(j,10000)==0
        t=toc;
        str = [num2str(j) ' iterations complete.  The elapsed time is ' num2str(t) '. The expected time remaining is ' num2str(BB*t/j) ];
        disp(str)
    end
end

% Some smoothing using a concave regression
y = mean(MC);
y(1) = 1;
orig_mean_MC=y;
n = length(y);
x = eye(n);
A1 = [eye(n-1) zeros(n-1,1)] + [zeros(n-1,1) -eye(n-1)];
A2 = [eye(n-2) zeros(n-2,2)] + [zeros(n-2,1) -2*eye(n-2) zeros(n-2,1)] + [zeros(n-2,2) eye(n-2)];
b = zeros(2*n-3,1);
concave_mean_MC=lsqlin(x,y,[A1;A2],b);
% Save the results.
%save realized_range_simulation_results orig_mean_MC concave_mean_MC ms


% GET THE QUATERNION BETWEEN TWO VECTORS
function [q] = qArgument(u,v)
q = zeros(4,1);
% Normalise the quaternion
u = u/norm(u);
v = v/norm(v);
% Get the axis vector
q(2:4) = cross(u,v);
% Define the rotation about that vector
q(1) = sqrt((norm(u)^2)*(norm(v)^2)) + dot(u,v);
% Normalise the quaternion
[q] = unit(q);
end


function [s] = gsp_filter_synthesis(G, filters, c, param)
%GSP_FILTER_SYNTHESIS Synthesis operator of a gsp filterbank
%   Usage:  s = gsp_filter_synthesis(G, filters, c);
%           s = gsp_filter_synthesis(G, filters, c, param);
%
%   Input parameters:
%         G         : Graph structure.
%         filters   : Set of spectral graph filters.
%         c         : Transform coefficients
%         param     : Optional parameter
%   Output parameters:
%         signal    : sythesis signal
%
%   'gsp_filter_synthesis(G,filters,c)' computes the synthesis
%   operator for coefficient $c$, where the atoms of the transform
%   dictionary are generalized translations of each graph spectral filter
%   to each vertex on the graph.
%
%   .. f = D * c
%
%   .. math:: f =  D c
%
%   where the columns of $D$ are $g_{i,m}=T_i g_m$, and $T_i$ is a
%   generalized translation operator applied to each filter
%   $\hat{g}_m(\cdot)$.
%
%   Each column of *c* is the response of the signal to one filter.
%
%   Example:::
%
%         Nf = 4;
%         G = gsp_sensor(30);
%         G = gsp_estimate_lmax(G);
%         G = gsp_estimate_lmax(G);
%         g = gsp_design_mexican_hat(G, Nf);
%         f = zeros(G.N,1);
%         f(1) = 1;
%         f = G.L^2*f;
%         ff = gsp_filter_analysis(G,g,f);
%         f2 = gsp_filter_synthesis(G,g,ff);
%         paramplot.show_edges = 1;
%         figure()
%         subplot(211)
%         gsp_plot_filter(G,g)
%         subplot(223)
%         gsp_plot_signal(G,f,paramplot);
%         subplot(224)
%         gsp_plot_signal(G,f2,paramplot);
%
%   Additional parameters
%   ---------------------
%
%   * *param.method*  : Select the method to be used for the computation.
%     * 'exact'     : Exact method using the graph Fourier matrix
%     * 'cheby'     : Chebyshev polynomial approximation
%     * 'lanczos'   : Lanczos approximation
%     Default: if the Fourier matrix is present: 'exact' otherwise 'cheby'
%   * *param.order* : Degree of the Chebyshev approximation
%     (default=30).
%   * *param.verbose* : Verbosity level (0 no log - 1 display warnings)
%     (default 1).
%
%   See also: gsp_filter_analysis gsp_filter_inverse
%
%   References: hammond2011wavelets
%

% Author: Nathanael Perraudin
% Testing: test_filter
% Date: 19 March 2014

% TODO: Perfect

% Read input parameters
if nargin < 4
    param = struct;
end

if iscell(G)
    NG = numel(G);
    s = cell(NG,1);
    for ii = 1:NG
        warning('Check what happen here')
       s{ii} = gsp_filter_synthesis(G{ii}, filters{ii}, c{ii}, param);
%         if iscell(s)
%             c{ii} = gsp_filter_analysis(G{ii}, fi{ii}, s{ii}, param);
%         else
%             c{ii} = gsp_filter_analysis(G{ii}, fi{ii}, s, param);
%         end
    end
    return
end

if isnumeric(filters)
    Nf = size(filters,2);
else
    Nf = numel(filters);
end

if isfield(param, 'exact')
    warning('param.exact is not used anymore. Please use param.method instead');
    if param.exact
        param.method = 'exact';
    else
        param.method = 'cheby';
    end
end

if ~isfield(param,'method')
    if gsp_check_fourier(G)
        param.method = 'exact';
    else
        param.method = 'cheby';
    end
end

if ~isfield(param,'order'); param.order = 30; end
if ~isfield(param,'verbose'); param.verbose = 1; end

if isfield(param, 'cheb_order')
    param.order = param.cheb_order;
    warning('param.cheb_order is not used anymore. Please use param.order instead');
end

switch param.method
    case 'exact'

        if ~gsp_check_fourier(G)
            if param.verbose
                warning(['GSP_FILTER_SYNTHESIS: The Fourier matrix is not ',...
                    'available. The function will compute it for you. ',...
                    'However, if you apply many time this function, you ',...
                    'should precompute it using the function: ',...
                    'gsp_compute_fourier_basis']);
            end
            G=gsp_compute_fourier_basis(G);
        end
        if isnumeric(filters)
            fie = filters;
        else
            fie = gsp_filter_evaluate(filters,G.e);
        end
%         Nv = size(c,2);
%         s =zeros(G.N,size(c,2));
%         for ii=1:Nf
%             s = s + G.U * ...
%                 (repmat(fie(:,ii),1,Nv) ...
%                 .* (G.U' * c((1:G.N)+G.N * (ii-1),:)));
%         end

        chat = gsp_gft(G,gsp_vec2mat(c,numel(filters)));
        shat = squeeze(sum(bsxfun(@times, fie, chat), 2));
        s = gsp_igft(G, shat);

    case 'cheby'
        if ~isfield(G,'lmax');
            G = gsp_estimate_lmax(G);
            if param.verbose
                warning(['GSP_FILTER_ANALYSIS: The variable lmax is not ',...
                    'available. The function will compute it for you. ',...
                    'However, if you apply many time this function, you ',...
                    'should precompute it using the function: ',...
                    'gsp_estimate_lmax']);
            end
        end

        cheb_coeffs = gsp_cheby_coeff(G, filters,...
                param.order, param.order +1);

        s=zeros(G.N,size(c,2));

        for ii=1:Nf
            s = s + gsp_cheby_op(G,cheb_coeffs(:,ii),c((1:G.N)+G.N * (ii-1),:));
        end

    case 'lanczos'
        s=zeros(G.N,size(c,2));
        if ~iscell(filters)
            filters = {filters};
        end
        for ii=1:Nf
            s = s + gsp_lanczos_op(G, filters{ii}, c((1:G.N)+G.N * (ii-1),:), param);
        end

    otherwise
        error('Unknown method: please select exact, cheby or lanczos');
end

end


%DEMO_BAYESIANOPTIMIZATION3  A demonstration program for Bayesian
%                            optimization with constraints
%
% The set of BO demos
%  Part 1: see demo_bayesoptimization1
%  One dimensional example
%
%  Part 2: see demo_bayesoptimization3
%  Two dimensional example
%
%  Part 3: this file
%  Two dimensional example with constraints
%  * The implementation of constraints follows Gelbart et al. (2014)
%
%  References:
%    Jones, D., Schonlau, M., & Welch, W. (1998). Efficient global
%    optimization of expensive black-box functions. Journal of Global
%    Optimization, 13(4), 455-492. doi:10.1023/a:1008306431147
%
%    Michael A. Gelbart, Jasper Snoek, and Ryan P. Adams
%    (2014). Bayesian Optimization with Unknown Constraints.
%    http://arxiv.org/pdf/1403.5607v1.pdf
%
%    Snoek, J., Larochelle, H, Adams, R. P. (2012). Practical Bayesian
%    Optimization of Machine Learning Algorithms. NIPS 25
%
%  Copyright (c) 2015 Jarno Vanhatalo

% 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.

%%  Part 3:
%  Two dimensional example with constraints
% For testing purposes:
stack = dbstack;
if (~isempty(stack) && (strcmp(stack(end).name, 'runtestset') || strcmp(stack(end).name, 'runtests'))) test = 1; else test = 0; end;

rng(3)
% Construct function handles to objective function (fx) and two constraint
% functions (fxc, fxc2)
fx = @(x) -log( (mvnpdf([x(:,1) x(:,2)],[3.5 2.5], [1 0.3; 0.3 1]) + 0.3*mvnpdf([x(:,1) x(:,2)],[7 8], [3 0.5; 0.5 4])).*...
    mvnpdf([x(:,1) x(:,2)],[5 5], [100 0; 0 100])) ./15 -1;
fxc = @(x) ((x(:,1)-5) .^2 + (x(:,2)-5).^2 -1)/30;
fxc2 = @(x) ( (x(:,1)-5) .^2)./30 - 0.5;

% The upper and lower limits for the constraints
const = [0 0.8 ; -10 0.1];

% Help variables for visualization
lb=0;
ub=10;
[X,Y] = meshgrid(linspace(lb,ub,100),linspace(lb,ub,100));
xl = [X(:) Y(:)];
Z = reshape(fx(xl),100,100);
Zc1 = fxc(xl); Zc1(Zc1<const(1,1) | Zc1>const(1,2)) = nan;
Zc1(~isnan(Zc1))=1; Zc1 = reshape(Zc1,100,100);
Zc2 = fxc2(xl); Zc2(Zc2<const(2,1) | Zc2>const(2,2)) = nan;
Zc2(~isnan(Zc2))=1; Zc2 = reshape(Zc2,100,100);

% ----- conduct Bayesian optimization -----

% construct GP models for the objective function and constraint functions
cfc = gpcf_constant('constSigma2',10,'constSigma2_prior', prior_fixed);
cfse = gpcf_sexp('lengthScale',[1 1]);
cfl = gpcf_linear('coeffSigma2', 10);
cfl2 = gpcf_squared('coeffSigma2', 10, 'interactions', 'on');
lik = lik_gaussian('sigma2', 0.001, 'sigma2_prior', prior_fixed);
% GP model for objective function
%gp1 = gp_set('cf', {cfc, cfl, cfl2, cfse}, 'lik', lik);cfl, cfl2,
gp1 = gp_set('cf', {cfc, cfse}, 'lik', lik);
% GP models for constraint functions
gpc1 = {gp_set('cf', {cfc, cfse}, 'lik', lik, 'jitterSigma2', 1e-6),...
    gp_set('cf', {cfc, cfse}, 'lik', lik, 'jitterSigma2', 1e-6)};

% Set the options for optimizer of the acquisition function
optimf = @fmincon;
optdefault=struct('GradObj','on','LargeScale','on','Algorithm','interior-point','TolFun',1e-9,'TolX',1e-6, 'Display', 'iter');
opt=optimset(optdefault);
lb=[0 0];     % lower bound of the input space
ub=[10 10];   % upper bound of the input space

% draw initial points
% we assume that at first we don't have any observation from the objective
% function but only observations from constraint functions. Hence x and y
% are initialized to zero
x = [];
y = [];
xc1 = 10*rand(2,2);
yc1 = fxc(xc1);
xc2 = 10*rand(5,2);
yc2 = fxc2(xc2);

figure, % figure for visualization
i1 = 1;
maxiter = 25;
improv = inf;   % improvement between two successive query points
while i1 < maxiter && improv>1e-6
%while i1 < maxiter
    % Train the GP models and calculate variables that are needed when
    %   calculating the Expected improvement (Acquisition function)
    % Objective function
    if ~isempty(x)
        gp = gp_optim(gp1,x,y);
        [K, C] = gp_trcov(gp,x);
        invC = inv(C);
        a = C\y;
        fmin = min( fx(x) );
    else
        a=[];
        x=[];
        invC=[];
        fmin=[];
        gp=gp1;
    end
    % constrain function 1
    gpct = gp_optim(gpc1{1},xc1,yc1);
    [~, Cct] = gp_trcov(gpct,xc1);
    const1.gpc = gpct;
    const1.invCc = inv(Cct);
    const1.ac = Cct\yc1;
    const1.const = const(1,:);
    const1.xc = xc1;
    % constrain function 2
    gpct = gp_optim(gpc1{2},xc2,yc2);
    [~, Cct] = gp_trcov(gpct,xc2);
    const2.gpc = gpct;
    const2.invCc = inv(Cct);
    const2.ac = Cct\yc2;
    const2.const = const(2,:);
    const2.xc = xc2;

    % Calculate EI and the posterior of the functions for visualization
    if ~isempty(x)
        [Ef,Varf] = gp_pred(gp, x, y, xl);
        EI = expectedimprovement_eg(xl, gp, x, a, invC, fmin, const1, const2);
    else
        Ef = zeros(size(xl,1),1);
        Varf = zeros(size(xl,1),1);
        EI = zeros(size(xl,1),1);
    end
    [Efc1] = gp_pred(const1.gpc, xc1, yc1, xl);
    [Efc2] = gp_pred(const2.gpc, xc2, yc2, xl);

    % optimize acquisition function
    %  * Note! Opposite to the standard notation we minimize negative Expected
    %    Improvement since Matlab optimizers seek for functions minimum
    %  * Note! We alternate the acquisition function between Expected
    %    Improvement and expected variance. The latter helps the
    %    optimization so that it does not get stuck in local mode
    % Here we use multiple starting points for the optimization so that we
    % don't crash into suboptimal mode of acquisition function
%     if mod(i1,5)==0  %Do just exploration by finding the maimum variance location
%         fh_eg = @(x_new) expectedvariance_eg(x_new, gp, x, [], invC);
%     else
        fh_eg = @(x_new) expectedimprovement_eg(x_new, gp, x, a, invC, fmin, const1, const2);
%     end
    nstarts = 20;
    xstart = [repmat(lb,nstarts,1) + repmat(ub-lb,nstarts,1).*rand(nstarts,2) ]; %; repmat(x(indbest,:),2,1)+0.1*randn(2,size(x,2))
    for s1=1:nstarts
        x_new(s1,:) = optimf(fh_eg, xstart(s1,:), [], [], [], [], lb, ub, [], opt);
    end
    xnews = x_new;
    EIs = fh_eg(x_new);
    x_new = x_new( find(EIs==min(EIs),1), : );

    % New sample point
    x(end+1,:) = x_new;
    y(end+1,:) = fx(x(end,:));
    xc1(end+1,:) = x_new;
    yc1(end+1,:) = fxc(x(end,:));
    xc2(end+1,:) = x_new;
    yc2(end+1,:) = fxc2(x(end,:));

    % visualize
    clf
    % Plot the objective function
    subplot(2,4,1),hold on, title('Objective, query points')
    pcolor(X,Y,Z),shading flat
    clim = caxis;
    plot(x(1:end-1,1),x(1:end-1,2), 'rx', 'MarkerSize', 10),
    plot(x(end,1),x(end,2), 'ro', 'MarkerSize', 10, 'linewidth', 3)
    % Plot the posterior mean of the GP model
    subplot(2,4,2),hold on, title(sprintf('GP prediction, mean, iter: %d',i1))
    pcolor(X,Y,reshape(Ef,100,100)),shading flat
    caxis(clim)
    % Plot the posterior variance of GP model
    subplot(2,4,6),hold on, title('GP prediction, variance')
    pcolor(X,Y,reshape(Varf,100,100)),shading flat
    plot(xnews(:,1),xnews(:,2), 'ro', 'MarkerSize', 10)
    plot(x(end,1),x(end,2), 'ro', 'MarkerSize', 10, 'linewidth', 3)
    plot(x(1:end-1,1),x(1:end-1,2), 'rx', 'MarkerSize', 10),

    % The expected information
    subplot(2,4,5), hold on, title(sprintf('Expected improvement %.2e', min(EIs)))
    pcolor(X,Y,reshape(EI,100,100)),shading flat
    plot(xnews(:,1),xnews(:,2), 'ro', 'MarkerSize', 10)
    plot(x(end,1),x(end,2), 'ro', 'MarkerSize', 10, 'linewidth', 3)
    plot(x(1:end-1,1),x(1:end-1,2), 'rx'),

    % constraint 1
    subplot(2,4,3), hold on, title(sprintf('constraint 1'))
    pcolor(X,Y,Zc1),shading flat
    plot(xc1(1:end-1,1),xc1(1:end-1,2), 'rx', 'MarkerSize', 10);
    plot(xnews(:,1),xnews(:,2), 'ro', 'MarkerSize', 10);
    plot(xc1(end,1),xc1(end,2), 'ro', 'MarkerSize', 10, 'linewidth', 3);
    % constraint 2
    subplot(2,4,4), hold on, title(sprintf('constraint 2'))
    pcolor(X,Y,Zc2),shading flat
    l1= plot(xc2(1:end-1,1),xc2(1:end-1,2), 'rx', 'MarkerSize', 10);
    l2=plot(xnews(:,1),xnews(:,2), 'ro', 'MarkerSize', 10);
    l3=plot(xc2(end,1),xc2(end,2), 'ro', 'MarkerSize', 10, 'linewidth', 3);
    legend([l1,l2,l3], {'function evaluation points','local modes of acquisition function','The next query point'})

    % prediction of constraint 1
    subplot(2,4,7), hold on, title(sprintf('prediction for const 1'))
    Efc1(Efc1<const(1,1) | Efc1>const(1,2)) = nan;
    Efc1(~isnan(Efc1))=1; Efc1 = reshape(Efc1,100,100);
    pcolor(X,Y,reshape(Efc1,100,100)),shading flat
    plot(xnews(:,1),xnews(:,2), 'ro', 'MarkerSize', 10)
    plot(xc1(end,1),xc1(end,2), 'ro', 'MarkerSize', 10, 'linewidth', 3)
    plot(xc1(1:end-1,1),xc1(1:end-1,2), 'rx'),
    % prediction of constraint 2
    subplot(2,4,8), hold on, title(sprintf('prediction for const 2'))
    Efc2(Efc2<const(2,1) | Efc2>const(2,2)) = nan;
    Efc2(~isnan(Efc2))=1; Efc2 = reshape(Efc2,100,100);
    pcolor(X,Y,reshape(Efc2,100,100)),shading flat
    plot(xc2(1:end-1,1),xc2(1:end-1,2), 'rx', 'MarkerSize', 10),
    plot(xnews(:,1),xnews(:,2), 'ro', 'MarkerSize', 10)
    plot(xc2(end,1),xc2(end,2), 'ro', 'MarkerSize', 10, 'linewidth', 3)

    if length(y)>1
        improv = abs(y(end) - y(end-1));
    end
    i1=i1+1;

    if test == 0
        pause
    end
end


% GP_SAMPLE_PCG - Draw a sample from a conditional multivariate normal
%                 distribution using (preconditioned) conjugate gradient.
%
% The joint covariance matrix K is assumed to be of the form:
%
%   K = [  K1     K1
%          K1   (K1+K2) ]
%
% A sample is drawn from the conditional distribution
%
%   P(X1|X2) = N( X1 | K1*INV(K1+K2)*X2, K1 - K1*INV(K1+K2)*K1 )
%
% using (preconditioned) conjugate gradient method. This can be efficient
% for sparse covariance matrices.
%
% Usage:
%
% X1 = GP_SAMPLE_PCG(X2, K, K1, L1, L2)
% X1 = GP_SAMPLE_PCG(X2, K, K1, L1, L2, M)
%
% The matrices L1 and L2 are such that K1=L1*L1' and K2=L2*L2'. These can
% be found using, for instance, CHOL or LCHOL.
%
% One can also give function handles such that K(X), K1(X), L1(X) and L2(X)
% return the corresponding matrix-vector products.
%
% M is an optional preconditioner for the conjugate gradient method. To
% speed up the method, INV(M) should approximate INV(K1+K2). It can be
% extremely important to use a good preconditioner. One can also give a
% handle to a function which evaluates and returns M\X.
%
% Z1 = L1*RANDN(N1,1)
% Z2 = L2*RANDN(N2,1)
%
% One can also give a matrix S such that
%
%   K = [  K1       K1*S'
%         S*K1   S*(K1+K2)*S' ]
%
% and then a sample is drawn from the conditional Gaussian distribution
% with mean
%
%   E(X1|X2) = K1 * INV(S*(K1+K2)*S') * S*X2,
%
% and covariance
%
%   COV(X1|X2) = K1 - K1 * S' * INV(S*(K1+K2)*S') * S * K1 )
%
% For instance, S can be an identity matrix from which some rows have been
% removed. This would correspond to having missing values in X2. In any
% case, S should be such that matrix-vector products are fast to evaluate,
% thus, preferably sparse. At least for now, S can not be a function
% handle. Note: the preconditioner might become worse when missing values..
%
% Usage:
%
% X1 = GP_SAMPLE_PCG(X2, K, K1, L1, L2, M, S)

% Last modified 2010-10-29
% Copyright (c) Jaakko Luttinen (jaakko.luttinen@tkk.fi)

function z1 = gaussian_rand_conjgradmv(K1, K2, z1, z2, I, varargin)
%function z1 = gaussian_rand_conjgradmv(x2, K, K1, z1, z2, I, varargin)
% To reduce memory usage: x1=z1
%
% Large variables in memory: x2, z1, z2

options = struct( ...
    'maxiter', 100,  ...
    'tol',     1e-6, ...
    'verbose', false);

% Parse arguments
[options, errmsg] = argparse( options, varargin{:} );
error(errmsg);

if nargin < 6 || isempty(I)
  I = true(size(x2));
end

if isnumeric(K1)
  K1 = @(x) K1*x;
end
if isnumeric(K2)
  K2 = @(x) K2*x;
end

% Assume zero means for now.
%mu1 = 0;
%mu2 = 0;

% Observations are subtracted from Z2!!
%z2 = z2 - x2;
%z2 = z2 - x2 + mu2
%z2 = z2 + z1 - x2 + mu2
z2 = conjgradmv(K2, z2, I, ...
                'tol', options.tol, ...
                'maxiter', options.maxiter, ...
                'verbose', options.verbose);
z1 = z1 - K1(z2);
%z1 = mu1 + z1 - K1(z2);

% x1 = z1;


% GCMEX An efficient graph-cut based energy minimization
%
% [LABELS ENERGY ENERGYAFTER] =
%   GCMEX(CLASS, UNARY, PAIRWISE, LABELCOST,EXPANSION)
%
%   Runs a minimization starting with the labels for each node defined
%   by CLASS, with unary potentials UNARY and the structure of the
%   graph and pairwise potentials defined by PAIRWISE. LABELCOST
%   determines data costs in terms of the labels of adjacent nodes.
%
% Parameters:
%   CLASS:: A 1xN vector which specifies the initial labels of each
%     of the N nodes in the graph
%   UNARY:: A CxN matrix specifying the potentials (data term) for
%     each of the C possible classes at each of the N nodes.
%   PAIRWISE:: An NxN sparse matrix specifying the graph structure and
%     cost for each link between nodes in the graph.
%   LABELCOST:: A CxC matrix specifying the fixed label cost for the
%     labels of each adjacent node in the graph.
%   EXPANSION:: A 0-1 flag which determines if the swap or expansion
%     method is used to solve the minimization. 0 == swap,
%     1 == expansion. If ommitted, defaults to swap.
%
% Outputs:
%   LABELS:: A 1xN vector of the final labels.
%   ENERGY:: The energy of the initial labeling contained in CLASS
%   ENERGYAFTER:: The energy of the final labels LABELS
%
% How do I know if I should use swap or expansion? From GC_README.txt:
%   The expansion algorithm for energy minimization can be used
%   whenever for any 3 labels a,b,c V(a,a) + V(b,c) <= V(a,c)+V(b,a).
%   In other words, expansion algorithm can be used if the binary
%   energy for the expansion algorithm step is regular, using V.
%   Kolmogorov's terminology.
%
%   The swap algorithm for energy minimization can be used whenever
%   for any 2 labels a,b V(a,a) + V(b,b) <= V(a,b)+V(b,a). In other
%   words, swap algorithm can be used if the binary energy for the
%   swap algorithm step is regular, using V. Kolmogorov's terminology.
%
% GCMex Version 2.3.0


function [ fea, out ] = ex_planestress6( varargin )
%EX_PLANESTRESS6 Plane stress analysis of a pressure vessel.
%
%   [ FEA, OUT ] = EX_PLANESTRESS6( VARARGIN ) Model example for plain stress
%   approximation of a pressure vessel (annular cross section with symmetry).
%
%   Accepts the following property/value pairs.
%
%       Input       Value/{Default}        Description
%       -----------------------------------------------------------------------------------
%       E           scalar {207e9}         Modulus of elasticity
%       nu          scalar {0.27}          Poissons ratio
%       sfun        string {sflag1}        Shape function for displacements
%       iplot       scalar 0/{1}           Plot solution (=1)
%                                                                                .
%       Output      Value/(Size)           Description
%       -----------------------------------------------------------------------------------
%       fea         struct                 Problem definition struct
%       out         struct                 Output struct

% Copyright 2013-2022 Precise Simulation, Ltd.

cOptDef = { ...
  'E',        207e9; ...
  'nu',       0.27; ...
  'sfun',     'sflag1'; ...
  'iplot',    1; ...
  'igrid',    1; ...
  'tol',      0.1; ...
  'fid',      1 };
[got,opt] = parseopt( cOptDef, varargin{:} );
fid       = opt.fid;

% Geometry and grid.
fea.sdim = { 'x' 'y' };   % Coordinate names.
fea.grid = ringgrid( 12, 216, 100e-3, 120e-3 );
fea.grid = delcells( fea.grid, selcells( fea.grid, '(x<=eps) | (y<=eps)') );
if( opt.igrid~=1 )
  fea.grid = quad2tri( fea.grid );
end
n_bdr = max(fea.grid.b(3,:));   % Number of boundaries.

% Problem definition.
fea = addphys( fea, @planestress );
fea.phys.pss.eqn.coef{1,end} = { opt.nu };
fea.phys.pss.eqn.coef{2,end} = { opt.E  };
fea.phys.pss.sfun            = { opt.sfun opt.sfun };

% Boundary conditions.
bctype = mat2cell( zeros(2,n_bdr), [1 1], ones(1,n_bdr) );
bctype{1,4} = 1;
bctype{2,3} = 1;
fea.phys.pss.bdr.coef{1,5} = bctype;

bccoef = mat2cell( zeros(2,n_bdr), [1 1], ones(1,n_bdr) );
bccoef{1,1} = '-nx*1e4';
bccoef{2,1} = '-ny*1e4';
fea.phys.pss.bdr.coef{1,end} = bccoef;

% Parse and solve problem.
fea       = parsephys( fea );
fea       = parseprob( fea );                          % Check and parse problem struct.
fea.sol.u = solvestat( fea, 'fid', fid, 'icub', 1+str2num(strrep(opt.sfun,'sflag','')) );   % Call to stationary solver.

% Postprocessing.
s_disp = fea.phys.pss.eqn.vars{2,end};
if( opt.iplot>0 )
  figure
  postplot( fea, 'surfexpr', s_disp )
  title( 'Total displacement' )
end

% Error checking.
s_sx     = fea.phys.pss.eqn.vars{5,end};
s_sy     = fea.phys.pss.eqn.vars{6,end};
s_sxy    = fea.phys.pss.eqn.vars{7,end};
s_sp1    = fea.phys.pss.eqn.vars{8,end};
s_sp3    = fea.phys.pss.eqn.vars{10,end};
s_ez     = fea.phys.pss.eqn.vars{13,end};
s_ep1    = fea.phys.pss.eqn.vars{15,end};
s_ep2    = fea.phys.pss.eqn.vars{16,end};
s_ep3    = fea.phys.pss.eqn.vars{17,end};
v_disp   = evalexpr( s_disp, [100e-3 120e-3-2*sqrt(eps);0 0]+sqrt(eps), fea )';
v_dref   = [2.809e-8 2.635e-8];
[v_sx(1),v_sx(2)] = minmaxsubd( s_sx, fea );
v_sxref  = [-10000 55454];
[v_sy(1),v_sy(2)] = minmaxsubd( s_sy, fea );
v_syref  = [-10000 55454];
[v_sxy(1),v_sxy(2)] = minmaxsubd( s_sxy, fea );
v_sxyref = [-32730 0];
[v_sp1(1),v_sp1(2)] = minmaxsubd( s_sp1, fea );
v_sp1ref = [4.5e4 55454];
[v_sp3(1),v_sp3(2)] = minmaxsubd( s_sp3, fea );
v_sp3ref = [-1e4 0];
[v_ez(1),v_ez(2)] = minmaxsubd( s_ez, fea );
v_ezref = [-5.929e-8 -5.929e-8];
[v_ep1(1),v_ep1(2)] = minmaxsubd( s_ep1, fea );
v_ep1ref = [2.196e-7 2.809e-7];
[v_ep2(1),v_ep2(2)] = minmaxsubd( s_ep2, fea );
v_ep2ref = [-5.929e-8 -5.929e-8];
[v_ep3(1),v_ep3(2)] = minmaxsubd( s_ep3, fea );
v_ep3ref = [-1.206e-7 -5.929e-8];
out.err  = [ abs([v_dref-v_disp])./v_dref ;
             abs([v_sxref-v_sx])./v_sxref ;
             abs([v_syref-v_sy])./v_syref ;
             abs([v_sxyref(1)-v_sxy(1)])./v_sxyref(1) 0 ;
             abs([v_sp1ref(2)-v_sp1(2)])./v_sp1ref(2) 0 ;
             abs([v_sp3ref(1)-v_sp3(1)])./v_sp3ref(1) 0 ;
             abs([v_ezref(1)-v_ez(1)])./v_ezref(1) 0 ;
             abs([v_ep1ref(1)-v_ep1(1)])./v_ep1ref(1) 0 ;
             abs([v_ep2ref(1)-v_ep2(1)])./v_ep2ref(1) 0 ;
             abs([v_ep3ref(1)-v_ep3(1)])./v_ep3ref(1) 0 ];
out.pass = all( out.err(:) <= opt.tol );

if( nargout==0 )
  clear fea out
end


classdef ClosestVigdergauzSuperEllipseComputer < handle

    properties (Access = public)
        xopt
        error
    end

    properties (Access = private)
        txi
        rho
        q
        problem
        frames
        optimalExponent
    end

    methods (Access = public)

        function  obj = ClosestVigdergauzSuperEllipseComputer()
            obj.init();
            obj.solveProblem();
            obj.writeOptimizationVideo();
            obj.printVigdergauzSuperEllipse();
        end

    end

    methods (Access = private)

        function init(obj)
            obj.txi = pi/8;
            obj.rho = 0.8;
        end

        function solveProblem(obj)
            s.rho = obj.rho;
            s.txi = obj.txi;
            s.savingFrames = true;
            obj.optimalExponent = OptimalExponentClosestToVigergauz(s);
            obj.optimalExponent.compute();
            obj.xopt   = obj.optimalExponent.qOpt;
            obj.q      = obj.optimalExponent.qOpt;
            obj.error  = obj.optimalExponent.error;
            obj.frames = obj.optimalExponent.frames;
        end

        function writeOptimizationVideo(obj)
            v = VideoWriter('SuperEllipseToVigdergauz.avi');
            open(v);
            nf = 6;
            for iframe = 1:nf*(numel(obj.frames)-2)
                i = floor(iframe/nf)+1;
                frame = obj.frames{i};
                writeVideo(v,frame);
            end
            close(v);
        end

        function printVigdergauzSuperEllipse(obj)
            s.mx = obj.optimalExponent.mx;
            s.my = obj.optimalExponent.my;
            s.txi = obj.txi;
            s.rho = obj.rho;
            s.q   = obj.q;
            stressPrinter = VigdergauzSuperEllipseStressPrinter(s);
            stressPrinter.print();
        end

    end

end


function [ S, f, Serr ]= mtspectrumc_unequal_length_trials( data, movingwin, params, sMarkers )

% This routine computes the multi-taper spectrum for a given set of unequal length segments. It is
% based on modifications to the Chronux routines. The segments are continuously structured in the
% data matrix, with the segment boundaries given by markers. Below,
% movingwin is used in a non-overlaping way to partition each segment into
% various windows. Th spectrum is evaluated for each window, and then the
% window spectrum estimates averaged. Further averaging is conducted by
% repeating the process for each segment.
%
% Inputs:
%
%   data = data( samples, channels )- here segments must be stacked
%   as explained in the email
%   movingwin = [window winstep] i.e length of moving
%              window and step size. Note that units here have
%              to be consistent with units of Fs. If Fs=1 (ie normalized)
%              then [window winstep]should be in samples, or else if Fs is
%              unnormalized then they should be in time (secs).
%   sMarkers = N x 2 array of segment start & stop marks. sMarkers(n, 1) = start
%           sample index; sMarkers(n,2) = stop sample index for the nth segment
%   params = see Chronux help on mtspecgramc
%
% Output:
%
%       S       frequency x channels
%       f       frequencies x 1
%       Serr    (error bars) only for err(1)>=1
%
%

iwAvg = 1; % 0=no weighted average, 1=weighted average
debug = 1; % will display intermediate calcs.

if nargin < 2; error('avgSpectrum:: Need data and window parameters'); end;
if nargin < 3; params=[]; end;
if isempty( sMarkers ), error( 'avgSpectrum:: Need Markers...' ); end
[ tapers, pad, Fs, fpass, err, trialave, params ] = getparams( params );
if nargout > 2 && err(1)==0;
%   Cannot compute error bars with err(1)=0. change params and run again.
    error('avgSpectrum:: When Serr is desired, err(1) has to be non-zero.');
end;

% Set moving window parameters to no-overlapping
if abs(movingwin(2) - movingwin(1)) >= 1e-6, disp( 'avgSpectrum:: Warming: Window parameters for averaging should be non-overlapping. Set movingwin(2) = movingwin(1).' ); end

wLength = round( Fs * movingwin(1) ); % number of samples in window
wStep = round( movingwin(2) * Fs ); % number of samples to step through

% Check whether window lengths satify segment length > NW/2
if ( wLength < 2*tapers(1) ), error( 'avgSpectrum:: movingwin(1) > 2*tapers(1)' ); end

% Left align segment markers for easier coding
sM = ones( size( sMarkers, 1 ), 2 );
sM( :, 2 ) = sMarkers( :, 2 ) - sMarkers( :, 1 ) + 1;

% min-max segments
Nmax = max( sM(:,2) ); Nmin = min( sM(:,2) );
if ( Nmin < 2*tapers(1) ), error( 'avgSpectrum:: Smallest segment length > 2*tapers(1). Change taper settings' ); end

% max time-sample length will be the window length.
nfft = 2^( nextpow2( wLength ) + pad );
[ f, findx ] = getfgrid( Fs, nfft, fpass);

% Precompute all the tapers
sTapers = tapers;
sTapers = dpsschk( sTapers, wLength, Fs ); % compute tapers for window length

nChannels = size( data, 2 );
nSegments = size( sMarkers, 1 );

if debug
    disp( ['Window Length = ' num2str(wLength)] );
    disp( ['Window Step = ' num2str(wStep)] );
    disp( ' ' );
end

s = zeros( length(f), nChannels );
serr = zeros( 2, length(f), nChannels );
S = zeros( length(f), nChannels );
Serr = zeros( 2, length(f), nChannels );
nWins = 0;
for sg = 1 : nSegments
    % Window lengths & steps fixed above
    % For the given segment, compute the positions & number of windows
    N = sM(sg,2);
    wStartPos = 1 : wStep : ( N - wLength + 1 );
    nWindows = length( wStartPos );
    if nWindows
        nWins = nWins + nWindows; % for averaging purposes

        w=zeros(nWindows,2);
        for n = 1 : nWindows
            w(n,:) = [ wStartPos(n), (wStartPos(n) + wLength - 1) ]; % nWindows x 2. just like segment end points
        end

        % Shift window limits back to original sample-stamps
        w(:, 1) = w(:,1) + (sMarkers( sg, 1 ) - 1);
        w(:, 2) = w(:,2) + (sMarkers( sg, 1 ) - 1);

        if debug
            disp( ['Segment Start/Stop = ' num2str( w(1,1) ) ' ' num2str( w(end,2) ) ] );
            disp( ['Min / Max Window Positions = ' num2str( min(w(:,1)) ) ' ' num2str( max(w(:,1)) ) ] );
            disp( ['Total Number of Windows = ' num2str(nWindows) ]);
            disp( ' ' );
        end

        % Pile up window segments similar to segment pileup
        wData = zeros( wLength, nChannels, nWindows ); %initialize to avoid fragmentation
        for n = 1:nWindows
            %wData( :, :, n ) = detrend( data( w(n,1):w(n,2), : ), 'constant' );
            wData( :, :, n ) = detrend( data( w(n,1):w(n,2), : ) );
        end

        % J1 = frequency x taper x nWindows
        % J2 = frequency x taper x nWindows x nChannels
        J2 = zeros( length(f), tapers(2), nWindows, nChannels ); J2 = complex( J2, J2 );
        for c = 1 : nChannels
            J1 = mtfftc( squeeze(wData( :, c, : )), sTapers, nfft, Fs ); % FFT for the tapered data
            J2( :, :, :, c ) = J1(findx,:,:);
        end
        % J2 = frequency x taper x nWindows x nChannels
        % Inner mean = Average over tapers => frequency x nWindows x nChannels
        % Outer mean = Average over windows => frequency x nChannels
        dim1 = [length(f), nWindows, nChannels];
        dim2 = [length(f), nChannels];
        % s = frequency x nChannels
        s = reshape( squeeze( mean( reshape( squeeze( mean( conj(J2).*J2, 2 ) ), dim1), 2 ) ), dim2 );

        % Now treat the various "windowed data" as "trials"
        % serr = 2 x frequency x channels. Output from specerr = 2 x frequency x 1
        for c = 1 : nChannels
            serr( :, :, c ) = specerr( squeeze( s(:, c ) ), squeeze( J2(:,:,:, c ) ), err, 1 );
        end

        if iwAvg
            % Segment Weighted error estimates.
            S = S + nWindows*s;
            Serr = Serr + nWindows*serr;
        else
            S = S + s;
            Serr = Serr + serr;
        end

    else
        if debug, disp(['avgSpectrum:: Zero windows for segment: ' num2str(sg) ]); end
    end
end

% Segment Weighted error estimates.
% Only over those that had non-zero windows
if nWins && iwAvg
    S=S/nWins; Serr=Serr/nWins;
end
if ~nWins
    if debug, disp(['avgCoherence:: No segment long enough with movingwin parameters found. Reduce movingwin.' ]); end
end


% Test file for trigtech/sign.m

function pass = test_sign(pref)

if ( nargin < 1 )
    pref = trigtech.techPref();
end

testclass = trigtech();

% Test a positive function:
F = @(x) sin(pi*x) + 2;
f = testclass.make(@(x) F(x), [], pref);
h = sign(f);
pass(1) = normest(h - 1) < 10*eps;

% Test a negative function:
f2 = testclass.make(@(x) -F(x), [], pref);
h = sign(f2);
pass(2) = normest(h + 1) < 10*eps;

% Test a complex-valued function:
F = @(x) exp(1i*pi*x);
f = testclass.make(@(x) F(x), [], pref);
h = sign(f);
pass(3) = normest(h - f) < 10*eps;

% Test a complex array-valued function:
xx = linspace(-.95, .97);
F = @(x) [(2+sin(pi*x)).*exp(1i*pi*x), -(2+sin(pi*x)).*exp(1i*pi*x), 2+sin(pi*x)];
f = testclass.make(@(x) F(x), [], pref);
ff = feval(f, xx);
gg = ff./abs(ff);
h = sign(f);
hh = feval(h, xx);
pass(4) = norm(hh - gg, inf) < 10*eps;

end


close all
clear all

N=5;
names={'A','B','C','D','E'};
dag=zeros(N);
dag(1,2)=1;
dag(2,[3 4])=1;
dag([3 4],5)=1;
[xx yy] = draw_graph(dag,names,ones(N,1));
title('Cheng example.');

node_sizes=2*ones(1,N);
bnet=mk_bnet(dag,node_sizes);

if 1
  disp('Generating DataSet');
  bnet.CPD{1} = tabular_CPD(bnet, 1, [0.4 0.6]);
  bnet.CPD{2} = tabular_CPD(bnet, 2, [0.2 0.3 0.8 0.7]);
  bnet.CPD{3} = tabular_CPD(bnet, 3, [0.1 0.2 0.9 0.8]);
  bnet.CPD{4} = tabular_CPD(bnet, 4, [0.6 0.8 0.4 0.2]);
  bnet.CPD{5} = tabular_CPD(bnet, 5, [0.9 0.8 0.7 0.6 0.1 0.2 0.3 0.4]);
  m=10000;
  cheng = cell(N,m);
  for i=1:m
    cheng(:,i)=sample_bnet(bnet);
  end
  cheng = cell2num(cheng);
  %save -ascii cheng cheng
else
  load -ascii cheng.mat
end

%  profile clear
%  profile on
  [Phase_3, Phase_2, Phase_1, UPhase_3] = learn_struct_bnpc(cheng, node_sizes, 0.05, 0)
%  profile off
%  profile report report_cheng

figure
draw_graph(Phase_1,names,ones(N,1),xx,yy);
title('PhaseI');
figure
draw_graph(Phase_2,names,ones(N,1),xx,yy);
title('PhaseII');
%figure
%draw_graph(UPhase_3,names,ones(N,1),xx,yy);
%title('undirected PhaseIII');
figure
draw_graph(Phase_3,names,ones(N,1),xx,yy);
title('PhaseIII');


% ir_example_ct_lir1
% example of local impulse response (LIR) in CT
% Copyright 2012-07-28, Jeff Fessler, University of Michigan

if ~isvar('A'), printm 'setup geometry, image, sinogram'
	f.down = 4;
	ig = image_geom('nx', 512, 'fov', 30, 'down', f.down);
	ig.mask = ig.circ > 0;
	sg = sino_geom('par', 'nb', ig.nx, 'na', ig.nx, ...
			'dr', ig.dx, 'strip_width', 'd');
	sg.plot(ig);

	ell = [0 0 10 10 0 0.2;
                 0  6 3 3 0  0.2;
                 6  0 3 3 0  0.1
                 0 -6 3 3 0 -0.1;
                -6  0 3 3 0 -0.2;
		];
        xtrue = ellipse_im(ig, ell, 'oversample', 2);

%	A = Gtomo2_dscmex(sg, ig);
	A = Gtomo2_wtmex(sg, ig, 'nthread', jf('ncore'));
	ytrue = A * xtrue;
	wi = exp(-ytrue); % ideal Poisson weighting

	im plc 2 3
	clim = [0 0.4];
	im(1, xtrue, 'x', clim), cbar
	im(2, ytrue, 'y: ideal sinogram'), cbar
	im(3, wi, 'w: ideal weighting'), cbar
%	ir_savefig ir_example_ct_lir1_x_y_w
prompt
end

if ~isvar('yp'), printm 'yp'
	ix = round(ell(2:end, 1) / ig.dx + (ig.nx-1)/2);
	iy = round(ell(2:end, 2) / ig.dy + (ig.ny-1)/2);
	xp = ig.zeros;
	for ii=1:numel(ix)
		xp = xp + ig.unitv(ix(ii), iy(ii));
	end
	f.amp = 0.1;
	xp = f.amp * xp ;
	im(2, xtrue + xp)

	yp = A * xp;
	climp = [0 f.amp];
	im(3, xp, climp), cbar
	im(4, yp)
end

if ~isvar('fbp'), printm 'fbp'
	tmp = fbp2(sg, ig);
	fbp = fbp2(yp, tmp, 'window', 'boxcar,0.8');
	im(2, fbp, 'FBP', climp), cbar
prompt
end

if ~isvar('fw.fbp'), printm 'fw.fbp'
	ox = [-9:9]; oy = ox;
	fw.fun = @(x, ii) fwhm2(x(ix(ii)+ox, iy(ii)+oy));
	for ii=1:numel(ix)
		fw.fbp(ii) = fw.fun(fbp, ii);
	end
	pr fw.fbp
end

if ~isvar('kappa'), printm 'kappa: try to make resolution approximately uniform'
	kappa = sqrt( div0(A' * wi, A' * ones(size(wi))) );
	im(4, kappa), cbar
prompt
end

% use local psf to help select beta
if ~isvar('R1'), printm 'R1, R2'
	f.l2b = 0;
	W = Gdiag(wi);
	R1 = Reg1(ig.mask * kappa(end/2+1,end/2+1), 'beta', 2^f.l2b); % usual
%	qpwls_psf(A, R1, 1, ig.mask, W, 'loop', 1); % choose beta
	R2 = Reg1(kappa, 'beta', 2^f.l2b); % kappa
	qpwls_psf(A, R2, 1, ig.mask, W, 'loop', 1); % choose beta
prompt
end

%	psf = qpwls_psf(A, R1, 1, ig.mask, W);
%	init = conv2(psf, xp, 'same');
	init = fbp;
	im(init, climp)

if ~isvar('xpwls1'), printm 'pwls1'
	f.niter = 300;
	xpwls1 = pwls_pcg1(init(ig.mask), A, W, yp(:), R1, 'niter', f.niter);
	xpwls1 = ig.embed(xpwls1);
	im(3, xpwls1, 'PWLS R1'), cbar
end

if ~isvar('xpwls2'), printm 'pwls2'
	xpwls2 = pwls_pcg1(init(ig.mask), A, W, yp(:), R2, 'niter', f.niter);
	xpwls2 = ig.embed(xpwls2);
	im(4, xpwls2, 'PWLS R2'), cbar
end

if 1
	for ii=1:numel(ix)
		fw.pwls1(ii) = fw.fun(xpwls1, ii);
		fw.pwls2(ii) = fw.fun(xpwls2, ii);
	end
	pr fw.fbp
	pr fw.pwls1
	pr fw.pwls2
end

if 1
	climp = [-0.004 0.05];
%	climp = []
	im plc 1 3
	ax = [18 110 20 120];
	im(1, fbp, 'FBP', climp), axis(ax)%, cbar h
	fun = @(fw, ii) text(ix(ii), iy(ii)+15, ...
		sprintf('%3.1f', fw(ii)), 'horiz', 'center', 'color', 'green');
	for ii=1:numel(ix), fun(fw.fbp, ii); end
	im(2, xpwls1, 'PWLS standard R', climp), axis(ax)%, cbar h
	for ii=1:numel(ix), fun(fw.pwls1, ii); end
	im(3, xpwls2, 'PWLS modified R', climp), axis(ax)%, cbar h
	for ii=1:numel(ix), fun(fw.pwls2, ii); end

%	ir_savefig eps_c ir_example_ct_lir1_fwhm
end


function [ a, ipvt, rcond ] = r8mat_geco ( a, n )

%*****************************************************************************80
%
%% R8MAT_GECO factors a real matrix and estimates its condition number.
%
%  Discussion:
%
%    For the system A * X = B, relative perturbations in A and B
%    of size EPSILON may cause relative perturbations in X of size
%    EPSILON/RCOND.
%
%    If RCOND is so small that the logical expression
%      1.0 + RCOND == 1.0
%    is true, then A may be singular to working precision.  In particular,
%    RCOND is zero if exact singularity is detected or the estimate
%    underflows.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 June 2005
%
%  Author:
%
%    Original FORTRAN77 version by Cleve Moler.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Jack Dongarra, Jim Bunch, Cleve Moler, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979,
%    ISBN13: 978-0-898711-72-1,
%    LC: QA214.L56.
%
%  Parameters:
%
%    Input, real A(N,N), a matrix to be factored.
%
%    Input, integer N, the order of the matrix A.
%
%    Output, real A(LDA,N), the LU factorization of the matrix.
%
%    Output, integer IPVT(N), the pivot indices.
%
%    Output, real RCOND, an estimate of the reciprocal
%    condition number of A.
%

%
%  Compute the L1 norm of A.
%
  anorm = 0.0;
  for j = 1 : n
    anorm = max ( anorm, sum ( abs ( a(1:n,j) ) ) );
  end
%
%  Compute the LU factorization.
%
  [ a, ipvt, info ] = r8mat_gefa ( a, n );
%
%  RCOND = 1 / ( norm(A) * (estimate of norm(inverse(A))) )
%
%  estimate of norm(inverse(A)) = norm(Z) / norm(Y)
%
%  where
%    A * Z = Y
%  and
%    A' * Y = E
%
%  The components of E are chosen to cause maximum local growth in the
%  elements of W, where U'*W = E.  The vectors are frequently rescaled
%  to avoid overflow.
%
%  Solve U' * W = E.
%
  ek = 1.0;
  z(1:n) = 0.0;

  for k = 1 : n

    if ( z(k) ~= 0.0 )
      ek = - abs ( ek ) * r8_sign ( z(k) );
    end

    if ( abs ( a(k,k) ) < abs ( ek - z(k) ) )
      s = abs ( a(k,k) ) / abs ( ek - z(k) );
      z(1:n) = s * z(1:n);
      ek = s * ek;
    end

    wk = ek - z(k);
    wkm = -ek - z(k);
    s = abs ( wk );
    sm = abs ( wkm );

    if ( a(k,k) ~= 0.0 )
      wk = wk / a(k,k);
      wkm = wkm / a(k,k);
    else
      wk = 1.0;
      wkm = 1.0;
    end

    if ( k+1 <= n )

      for j = k+1 : n
        sm = sm + abs ( z(j) + wkm * a(k,j) );
        z(j) = z(j) + wk * a(k,j);
        s = s + abs ( z(j) );
      end

      if ( s < sm )
        t = wkm - wk;
        wk = wkm;
        z(k+1:n) = z(k+1:n) + t * a(k,k+1:n);
      end

    end

    z(k) = wk;

  end

  z(1:n) = z(1:n) / sum ( abs ( z(1:n) ) );
%
%  Solve L' * Y = W
%
  for k = n : -1 : 1

    z(k) = z(k) + z(k+1:n) * a(k+1:n,k);

    if ( 1.0 < abs ( z(k) ) )
      z(1:n) = z(1:n) / abs ( z(k) );
    end

    l = ipvt(k);

    t = z(l);
    z(l) = z(k);
    z(k) = t;

  end

  z(1:n) = z(1:n) / sum ( abs ( z(1:n) ) );

  ynorm = 1.0;
%
%  Solve L * V = Y.
%
  for k = 1 : n

    l = ipvt(k);

    t = z(l);
    z(l) = z(k);
    z(k) = t;

    z(k+1:n) = z(k+1:n) + t * a(k+1:n,k)';

    if ( 1.0 < abs ( z(k) ) )
      ynorm = ynorm / abs ( z(k) );
      z(1:n) = z(1:n) / abs ( z(k) );
    end

  end

  s = sum ( abs ( z(1:n) ) );
  z(1:n) = z(1:n) / s;
  ynorm = ynorm / s;
%
%  Solve U * Z = V.
%
  for k = n : -1 : 1

    if ( abs ( a(k,k) ) < abs ( z(k) ) )
      s = abs ( a(k,k) ) / abs ( z(k) );
      z(1:n) = s * z(1:n);
      ynorm = s * ynorm;
    end

    if ( a(k,k) ~= 0.0 )
      z(k) = z(k) / a(k,k);
    else
      z(k) = 1.0;
    end

    z(1:k-1) = z(1:k-1) - z(k) * a(1:k-1,k)';

  end
%
%  Normalize Z in the L1 norm.
%
  s = 1.0 / sum ( abs ( z(1:n) ) );
  z(1:n) = s * z(1:n);
  ynorm = s * ynorm;

  if ( anorm ~= 0.0 )
    rcond = ynorm / anorm;
  else
    rcond = 0.0;
  end

  return
end


function pred = ml_predictqda(trials, model)
% Prediction function for Quadratic Discriminant Analysis.
% Prediction = ml_predictqda(Trials, Model)
%
% In:
%   Trials  : the data a matrix, as in ml_predict
%
%   Model   : predictive model as produced by ml_trainqda
%
% Out:
%   Prediction  : discrete probability distribution, formatted as
%                 {'disc' [NxC] [Cx1]}, with element #2 being the per-class probability and
%                 element #3 the original target values per class
%                 thus, the expected target values are Prediction{2}*Prediction{3}
%
% Examples:
%   targets might look like this: [-1 -1 1 -1 1 -1 -1 1 -1 -1 1 -1 -1 1 ...]'
%
%   model = ml_trainqda(data,targets)
%   p = ml_predictqda(data, model); expectation = p{2}*p{3};
%   now expectation might look like this: [-0.6 -0.9 +0.4 -0.7 +0.8 -0.1 +0.5 +1.0 -0.9 +1.0 -1.0 -1.0 +1.0 ...]'
%
% See also:
%   ml_trainqda
%
%                           Christian Kothe, Swartz Center for Computational Neuroscience, UCSD
%                           2010-04-03

if isfield(model,'voted')
    % dispatch to the voter
    pred = ml_predictvote(trials,model);
else
    % pre-prune features
    trials = trials(:,model.featuremask);
    % pre-scale the data
    trials = hlp_applyscaling(trials,model.sc_info);
    % calculate the labels
    raw_labels = min(+1,max(-1,-(sum(((trials*model.q).*trials)') + model.l*trials' - model.c)))'; %#ok<UDIM>
    pred = {'disc', [(1-raw_labels)/2 1-(1-raw_labels)/2], model.classes};
end


function r = sqrt(a)
%SQRT         Taylor square root sqrt(a)
%

% written  05/21/09     S.M. Rump
% modified 08/26/12     S.M. Rump  global variables removed
%

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

  K1 = getappdata(0,'INTLAB_TAYLOR_ORDER') + 1;

  r = a;
  r.t(1,:) = sqrt(a.t(1,:));
  rt2 = 2*r.t(1,:);                     % almost 10 % faster
  for j=2:K1
    r.t(j,:) = ( a.t(j,:) - sum(r.t(2:j-1,:).*r.t(j-1:-1:2,:),1) ) ./ (rt2);
  end
% straight version is faster
%   for j=2:K1
%     if even(j)
%       r.t(j,:) = ( a.t(j,:)/2 - sum(r.t(2:(j/2),:).*r.t(j-1:-1:(j/2)+1,:),1) ) ./ r.t(1,:);
%     else
%       r.t(j,:) = ( a.t(j,:)/2 - sum(r.t(2:((j-1)/2),:).*r.t(j-1:-1:(j+3)/2,:),1) - ...
%                               (r.t((j+1)/2,:).^2)/2 ) ./ r.t(1,:);
%     end
%   end

  if rndold
    setround(rndold)
  end


%% FUNCTION split_data
%   Splitting multi-task data into training / testing by percentage.
%
%% INPUT
%   X: {n * d} * t - input matrix
%   Y: {n * 1} * t - output matrix
%   percent: percentage of the splitting range (0, 1)
%
%% OUTPUT
%   X_train: the split of X that has the specified percent of samples
%   Y_train: the split of Y that has the specified percent of samples
%   X_test: the split of X that has the remaining samples
%   Y_test: the split of Y that has the remaining samples
%   selIdx: the selection index of for X_train and Y_train for each task
%%

function [X_train, Y_train, X_test, Y_test, selIdx] = split_data(X, Y, percent)

if percent > 1 || percent < 0
    error('splitting percentage error')
end

task_num = length(X);

selIdx = cell(task_num, 0);
X_train = cell(task_num, 0);
Y_train = cell(task_num, 0);
X_test = cell(task_num, 0);
Y_test = cell(task_num, 0);

for t = 1:task_num
    task_sample_size = length(Y{t});
    tSelIdx = randperm(task_sample_size) < task_sample_size * percent;

    selIdx{t} = tSelIdx;

    X_train{t} = X{t}(tSelIdx,:);
    Y_train{t} = Y{t}(tSelIdx,:);
    X_test{t} = X{t}(~tSelIdx,:);
    Y_test{t} = Y{t}(~tSelIdx,:);

end


function stroud_test03 ( )

%*****************************************************************************80
%
%% TEST03 tests BALL_UNIT_07_3D, BALL_UNIT_14_3D, BALL_UNIT_15_3D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 April 2009
%
%  Author:
%
%    John Burkardt
%
  global FUNC_3D_INDEX;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST03\n' );
  fprintf ( 1, '  For integrals in the unit ball in 3D:\n' );
  fprintf ( 1, '  BALL_UNIT_07_3D uses a formula of degree 7;\n' );
  fprintf ( 1, '  BALL_UNIT_14_3D uses a formula of degree 14;\n' );
  fprintf ( 1, '  BALL_UNIT_15_3D uses a formula of degree 15.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Unit ball volume = %f\n', ball_unit_volume_nd ( 3 ) );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    Rule:      #7             #14           #15\n' );
  fprintf ( 1, '    F(X)\n' );
  fprintf ( 1, '\n' );

  num = function_3d_num ( );

  for i = 1 : num

    FUNC_3D_INDEX = i;

    result1 = ball_unit_07_3d ( 'function_3d' );
    result2 = ball_unit_14_3d ( 'function_3d' );
    result3 = ball_unit_15_3d ( 'function_3d' );

    fname = function_3d_name ( i );

    fprintf ( 1, '  %7s  %12f  %12f  %12f\n', ...
      fname, result1, result2, result3 );

  end

  return
end


% Jiao Xianjun (putaoshu@msn.com; putaoshu@gmail.com)
% convert a sequence to a mat. each column shift previos to left by 1
% for example input sequence 1 2 3 4 5 6 7, and len_body=4
% output matrix:
% 1 2 3 4
% 2 3 4 5
% 3 4 5 6
% 4 5 6 7

% A script of project: https://github.com/JiaoXianjun/multi-rtl-sdr-calibration

function r = lin2col_shift_mat(s, len_body)

if length(s) < len_body
    disp('Length of input is too short!');
    r = -1;
    return;
end

len_tail = length(s) - len_body;

r = toeplitz(s, [s(1) zeros(1, len_tail)]);
r = r((len_tail+1):end, end:-1:1);


% SCRIPT TEST FOR THE KINEMATIC PROBLEM FOR SERIAL ROBOTS

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.

close all

fprintf('\nTHE DEMO PRESENTS THE DIRECT AND INVERSE KINEMATIC PROBLEM')

%there are eight possible solutions for the inverse kinematic problem for most of these robots
n_solutions = 8;

%Try different configurations beware that, depending on the robot's topology
%not all the eight possible solutions will be feasible for an antropomorphic 6R robot.
q=[0 0 0 0 0 0]

%q=[0.2 0.8 -0.2 0.1 0.1 0.1]

%load robot parameters. You can try different robots
%robot=load_robot('KUKA', 'KR60_3'); n_solutions = 8;
robot=load_robot('MOTOMAN', 'MH6S'); n_solutions = 8;

%adjust 3D view as desired
adjust_view(robot)

%there are just 2 solutions for these robots and 4 DOF
%q = [pi/2 0.2 0.8 pi/4]
%robot=load_robot('kuka', 'KR5_scara_R350_Z200'); n_solutions = 2;
%robot=load_robot('example', 'scara'); n_solutions = 2;
%robot=load_robot('example', '2dofplanar'); n_solutions = 2;

%draw the robot
drawrobot3d(robot, q)

%Now compute direct kinematics for this position q
T = directkinematic(robot, q)

%Set to zero if you want to see the robot transparent
robot.graphical.draw_transparent=0;

%Set to one if you want to see the DH axes
%abb.graphical.draw_axes=1;

%Call the inversekinematic for this robot. All the possible solutions are
%stored at qinv. At least, one of the possible solutions should match q
qinv = inversekinematic(robot, T)

fprintf('\nNOW WE CAN REPRESENT THE DIFFERENT SOLUTIONS TO ACHIEVE THE SAME POSITION AND ORIENTATION\n')
fprintf('\nNote that some solutions may not be feasible since some joints may be out of range.\n')
correct=zeros(1,n_solutions);
%check that all of them are possible solutions!
for i=1:size(qinv,2),

    Ti = directkinematic(robot, qinv(:,i)) %Ti is constant for the different solutions

    % Note that all the solutions may not be feasible. Some of the joints may
    % be out of range. You can test this situation with test_joints
    test_joints(robot, qinv(:,i));

    %now draw the robot to see the solution
    drawrobot3d(robot, qinv(:,i))

    pause(1);

    k=sum(sum((T-Ti).^2));
    if k < 0.01 % a simple threshold to find differences in the solution
        correct(1,i)= 1;
    else
        correct(1,i)= 0; %uncorrect solution
        fprintf('\nERROR: One of the solutions seems to be uncorrect. Sum of errors: %f', i, k);
    end
end

fprintf('\n************** RESULTS **************')

%Display a message if any of the solutions is not correct
if sum(correct)==n_solutions
    fprintf('\nTEST 1--> OK: Every solution in qinv yields the same position/orientation T');
else
    fprintf('\nTEST 1--> ERROR: One or more of the solutions seem to be uncorrect.');
end

%Now, test if any of the solutions in qinv matches q
%find the solution that matches the initial q
%delta is just a squared sum of errors at each of the columns of the matrix
%which store the different solutions of qinv
delta=(repmat(q',[1 n_solutions])-qinv).^2;
i=find(sum(delta,1) < 0.01);
if ~isempty(i)
    fprintf('\nTEST 2--> OK!: Found a matching solution for the initial q.\n');
    solution=qinv(:,i)
else
    error_test2=1
    fprintf('\nTEST 2--> ERROR: Did not find a matching solution for the initial q.');
end

fprintf('\n************** ****** **************\n')


function faces = sphDelaunay(dirs)
%SPHDELAUNAY Computes the Delaunay triangulation on the unit sphere
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% SPHDELAUNAY.M - 10/10/2013
% Archontis Politis, archontis.politis@aalto.fi
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Convert to cartesian
N_vert = size(dirs, 1);
[tempx, tempy, tempz] = sph2cart(dirs(:,1), dirs(:,2), ones(N_vert,1));
U_vert = [tempx, tempy, tempz];

% Find the convex hull of the points on the sphere - in this special case
% the result equals the Delaunay triangulation of the points
faces = convhulln(U_vert);

% Invert the triangles
faces = faces(:, 3:-1:1);

% Shift the results to begin each triangle from the smallest entry
for n = 1:size(faces,1)
    tempface = faces(n,:);
    [~, minIdx] = min(tempface);
    faces(n, :) = circshift(tempface, [0 1-minIdx]);
end

% Sort through triangles with smaller entries first
faces = sortrows(faces, 1); % sort through first entry
maxentry = max(faces(:,1)); % sort through second entry
n = 1;
while n <= maxentry
    startIdx = find(faces(:,1) == n, 1, 'first');
    if ~isempty(startIdx)
        endIdx = find(faces(:,1) == n, 1, 'last');
        faces(startIdx:endIdx, :) = sortrows(faces(startIdx:endIdx, :), 2);
        n = n + 1;
    else
        n = n + 1;
    end
end


function F=msst(X,y,N)
%+++ Multi-Scale significance test
if nargin<3;N=1000;end

[n,p]=size(X);
r0=15/p;
ratio=[0.5 0.6 0.7 0.8 0.9 0.95];
for i=1:N
  for j=1:length(ratio)

    [Xcal,ycal]=traintestselect(X,y,ratio(j));
    kp=find(ycal==1);
    kn=find(ycal~=1);
    for k=1:p
%      [ptemp,h] = ranksum(Xcal(kp,k),Xcal(kn,k));
     [h,ptemp] = ttest2(Xcal(kp,k),Xcal(kn,k));
     P(i,j,k)=-log10(ptemp);
    end

  end
  fprintf('The %ith sampling finished.\n',i);
end
%+++ Output
F.P=P;


function [centers, radii] = SphericalHashing(data, bit)
%Spherical Hashing method by Jae-Pil Heo

    [N, D] = size(data);
    centers = random_center(data, bit);
    [O1, O2, radii, avg, stddev] = compute_statistics(data, centers);

    iter = 1;
    while true
        forces = zeros(bit, D);
        for i = 1:bit - 1
            for j = i + 1:bit
                force = 0.5 * (O2(i, j) - N / 4) / (N / 4) * (centers(i, :) - centers(j, :));
                forces(i, :) = forces(i, :) + force ./ bit;
                forces(j, :) = forces(j, :) - force ./ bit;
            end
        end
        centers = centers + forces;

        [O1, O2, radii, avg, stddev] = compute_statistics(data, centers);

        if avg <= 0.1 * N / 4 && stddev <= 0.15 * N / 4
            break;
        end
        if iter >= 100
            fprintf('iter exceed 100, avg = %f, stddev = %f\n', avg, stddev);
        end

        iter = iter + 1;
    end
    %fprintf('iteration = %d\n', iter);
end

function centers = random_center(data, bit)
    [N, D] = size(data);
    centers = zeros(bit, D);
    for i = 1:bit
        R = randperm(N);
        sample = data(R(1:5), :);
        sample = sum(sample, 1) / 5;
        centers(i, :) = sample(:);
    end
end

function [O1, O2, radii, avg, stddev] = compute_statistics(data, centers)
    [N, D] = size(data);
    bit = size(centers, 1);

    dist = EuDist2(centers,data);
    sort_dist = sort(dist, 2);
    radii = sort_dist(:, floor(N / 2));
    dist = dist <= repmat(radii, 1, N);
    dist = dist * 1.0;

    O1 = sum(dist, 2);
    avg = 0;
    avg2 = 0;
    O2 = dist * dist';
    for i = 1:bit-1
        for j = i + 1:bit
            avg = avg + abs(O2(i, j) - N / 4);
            avg2 = avg2 + O2(i, j);
        end
    end

    avg = avg / (bit * (bit - 1) / 2);
    avg2 = avg2 / (bit * (bit - 1) / 2);
    stddev = 0;
    for i = 1:bit - 1
        for j = i + 1:bit
            stddev = stddev + (O2(i, j) - avg2) ^ 2;
        end
    end
    stddev = sqrt(stddev / (bit * (bit - 1) / 2));
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% mcs.m %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% function [xbest,fbest,xmin,fmi,ncall,ncloc,flag]=mcs(fcn,data,u,v,prt,
% smax,nf,stop,iinit,local,gamma,hess)
% MCS global optimization for function defined by fcn in the
% n-dimensional box [u,v]
%
% Input:
% fcn = 'fun' 	name of function fun(data,x), x an n-vector
% data		data vector (or other data structure)
% [u,v]       	box in which the optimization is carried out (u, v
%             	n-vectors)
% prt		print level
% 		prt = 0: no printing
% 		prt = 1: # sweep, minimal nonempty level, # f-calls,
% 		best point and function value (default)
% 		prt > 1: only meaningful for test functions with known
% 		global minimizers
% 		in addition levels and function values of boxes
% 		containing the global minimizers of a test function
% smax        	number of levels (default: 5*n+10)
% nf         	maximum number of function evaluations (default: 50*n^2)
% stop         	stop(1) in ]0,1[:  relative error with which the known
%		 global minimum of a test function should be found
%		 stop(2) = fglob known global minimum of a test function
%		 stop(3) = safeguard parameter for absolutely small
%		 fglob
%		stop(1) >= 1: the program stops if the best function
%		 value has not been improved for stop(1) sweeps
%		stop(1) = 0: the user can specify a function value that
%		 should be reached
%                stop(2) = function value that is to be achieved
%             	(default: stop = 3*n)
% iinit       	parameter defining the initialization list
%             	= 0        corners and midpoint (default for finite u,v)
%             	= 1        safeguarded version *default otherwise)
% 		= 2        5u/6 + v/6, u/6 + 5v/6 and midpoint
%		= 3        initialization list with line searches
%             	otherwise  self-defined init. list (to be stored in
%			   init0.m)
%		for a self-defined initialization list, the user should
% 		provide an m-script file init0.m containing a matrix x0
%		with n rows and at least 3 columns and two n-vectors l
%		and L
%		the ith column of x0 contains the initialization list
%		values for the ith coordinate, their number is L(i), and
%		x0(i,l(i)) is the ith coordinate of the initial point
% local		local = 0: no local search
%		otherwise: maximal number of steps in local search
%		(default: 50)
% gamma		stopping criterion for local search (default: eps)
%           	the local search is stopped if abs(g)'*max(abs(x),
%		abs(xold)) < gamma*(f0-f)
% hess		sparsity pattern of the Hessian for local search
%		(default: hessian = ones(n,n))
%
% Output:
% xbest(1:n)  	current best point
% fbest    	function value at xbest
% xmin        	matrix with n rows; the columns are the points in the
%             	'shopping basket' (i.e. good points resp. local
%		minimizers)
% fmi         	function values corresponding to the 'shopping basket';
%		fmi(i) is the function value at xmin(:,i)
% ncall       	number of function evaluations
% ncloc		number of function evaluations used for local search
% flag        	specifies which stopping criterion has been used
%             	= 0  a (known) global minimum fglob of a test function
%                    has been found with the required relative error
%		     relerr
%             	= 1  the division procedure has been completed
%             	= 2  the maximum number nf of function calls has been
%                    reached without finding a known minimum with the
%                    required relative error or completing the division
%                    procedure
%		= 3  stop(1) sweeps without progress (for stop(1) >= 1)
%
% Uses the following m-files (directly or indirectly):
% addloc.m
% basket.m
% basket1.m
% bounds.m
% chkloc.m
% chrelerr.m
% chvtr.m
% csearch.m 	called by lsearch.m
% exgain.m
% fbestloc.m
% genbox.m
% hessian.m	called by triple.m
% init.m
% initbox.m
% initlist.m
% gls.m and its subprograms   called by lsearch.m
% lsearch.m
% minq.m and its subprograms  called by lsearch.m
% neighbor.m
% polint.m	called by exgain.m and initbox.m
% polint1.m	called by triple.m
% quadmin.m
% quadpol.m
% range.m	called by lsearch.m
% splinit.m
% split.m
% split1.m
% split2.m   	called by splrnk.m
% splrnk.m
% strtsw.m
% subint.m
% triple.m	called by lsearch.m
% updtf.m    	called by vertex.m
% updtoptl.m
% updtrec.m   	called by splinit.m and split.m
% vert1.m    	called by vertex.m
% vert2.m    	called by vertex.m
% vert3.m    	called by vertex.m
% vertex.m

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [xbest,fbest,xmin,fmi,ncall,ncloc,flag]=mcs(fcn,data,u,v,prt,smax,nf,stop,iinit,local,gamma,hess)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% global variables
global foptbox nbasket nboxes ncall nglob nsweep nsweepbest optlevel  record xglob xloc
% foptbox(1:nglob)  function value(s) of the box(es) containing the (a)
%             	global minimizer of a test function
% nbasket   	counter for boxes in the 'shopping basket'
% nboxes      	counter for boxes not in the 'shopping basket'
% nglob       	number of global minimizers of a test function
% nloc		(for local ~= 0) counter of points that have been used
% 		as starting points for a local search
% nsweep      	sweep counter
% nsweepbest    number of sweep in which fbest was updated for the last
%		time
% optlevel    	level(s) of the box(es) containing the (a) global
%             	minimum of a test function
% record(1:smax-1) record(i) points to the best non-split box at level i
%             	(record list)
% xglob(1:n,1:nglob)  xglob(:,i), i=1:nglob, are the global minimizers
% of a test function in [u,v]
% xloc(1:n,:)	(for local ~= 0) columns are the points that have been
%		used as starting points for local search
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
n = length(u);

% check box bounds
if ~isempty(find(v<u))
  error('incompatible box bounds')
elseif ~isempty(find(u==v))
  error('degenerate box bound')
end

% default values for the input parameters
if nargin < 5, prt = 1; end
if nargin < 6, smax = 5*n+10; end
if nargin < 7, nf = 50*n^2; end
if nargin < 8, stop = 3*n; end
if nargin < 9,
  if isempty(find(isinf(u))) & isempty(find(isinf(v)))
    iinit = 0;
  else
    iinit = 1;
  end
end
if nargin < 10, local = 50; end
if nargin < 11, gamma = eps; end
if nargin < 12, hess = ones(n,n); end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
xmin=[];fmi=[];		% avoid warnings in Matlab5

% initial values for the numbers of function calls (total number/local
% search)
ncall = 0;
ncloc = 0;

% some parameters needed for initializing large arrays
step1 = 10000;
step = 1000;
dim = step1;

% initialization of some large arrays
isplit = zeros(1,step1);
level = zeros(1,step1);
ipar = zeros(1,step1);
ichild = zeros(1,step1);
f = zeros(2,step1);
z = zeros(2,step1);
nogain = zeros(1,step1);

% definition of the initialization list
if iinit == 0
  x0(:,1) = u;
  x0(:,2) = (u+v)/2;
  x0(:,3) = v;
  l = 2*ones(n,1);
  L = 3*ones(n,1);
elseif iinit == 1
  for i = 1:n
    if u(i) >= 0
      x0(i,1) = u(i); [x0(i,2),x0(i,3)] = subint(u(i),v(i));x0(i,2) = 0.5*(x0(i,1)+x0(i,3));
    elseif v(i) <= 0
      x0(i,3) = v(i); [x0(i,2),x0(i,1)] = subint(v(i),u(i));x0(i,2) = 0.5*(x0(i,1)+x0(i,3));
    else
      x0(i,2) = 0; [xi,x0(i,1)] = subint(0,u(i)); [xi,x0(i,3)] = subint(0,v(i));
    end
  end
  l = 2*ones(n,1);
  L = 3*ones(n,1);
elseif iinit == 2
  x0(:,1) = (5*u + v)/6;
  x0(:,2) = 0.5*(u + v);
  x0(:,3) = (u + 5*v)/6;
  l = 2*ones(n,1);
  L = 3*ones(n,1);
elseif iinit == 3
  [x0,f0,l,L,istar,ncall1] = initlist(fcn,data,u,v);
  ncall = ncall + ncall1;
else
  init0 	%self-defined initialization list
  for i=1:size(x0,2)
    if ~isempty(find(x0(:,i)<u)) | ~isempty(find(x0(:,i)>v))
      error('incorrect initialization list')
    end
  end
end

% check whether there are infinities in the initialization list
if ~isempty(find(isinf(x0))), error('infinities in ititialization list'), end

% computation of the function values f0 appertaining to the init. list
% and the pointer istar to the best point in the initialization list
if iinit ~= 3
  [f0,istar,ncall1] = init(fcn,data,x0,l,L,n);
  ncall = ncall + ncall1;
end

% definition of the base vertex of the original box
for i = 1:n
  x(i) = x0(i,l(i));
end

% definition of the opposite vertex v1 of the original box
for i = 1:n
  if abs(x(i)-u(i)) > abs(x(i)-v(i))
    v1(i) = u(i);
  else
    v1(i) = v(i);
  end
end

% initialization of the record list, the counters nboxes, nbasket, m
% and nloc, xloc and the output flag
record = zeros(smax-1,1);
nboxes = 1;
nbasket = 0;
nbasket0 = 0;
nsweep = 0;
m = n;
record(1) = 1;
nloc = 0;
xloc = [];
flag = 1;

[ipar,level,ichild,f,isplit,p,xbest,fbest] = initbox(x0,f0,l,L,istar,u,v,prt);
% generates the boxes in the initialization procedure
f0min = fbest;
if stop(1) > 0 & stop(1) < 1
  flag = chrelerr(fbest,stop);
elseif stop(1) == 0
  flag = chvtr(fbest,stop(2));
end
if ~flag,return,end
% if the (known) minimum function value fglob has been found with the
% required tolerance, flag is set to 0 and the program is terminated

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
s = strtsw(smax,level,f(1,:));
% the vector record is updated, and the minimal level s containing
% non-split boxes is computed
nsweep = nsweep + 1;	% sweep counter

while s < smax & ncall + 1 <= nf
  par = record(s);   % the best box at level s is the current box
  [n0,x,y,x1,x2,f1,f2] = vertex(par,n,u,v,v1,x0,f0,ipar,isplit,ichild,z,f,l,L);
  % compute the base vertex x, the opposite vertex y, the 'neighboring'
  % vertices and their function values needed for quadratic
  % interpolation and the vector n0 indicating that the ith coordinate
  % has been split n0(i) times in the history of the box
  if s > 2*n*(min(n0)+1)
  % s 'large'
    [isplit(par),z(2,par)] = splrnk(n,n0,p,x,y);
    % splitting index and splitting value z(2,par) for splitting by
    % rank are computed
    % z(2,par) is set to Inf if we split according to the init. list
    splt = 1;  % indicates that the box is to be split
  else
    if nogain(par) % box has already been marked as not eligible for splitting
                   % by expected gain
      splt = 0;
    else
      [e,isplit(par),z(2,par)] = exgain(n,n0,l,L,x,y,x1,x2,f(1,par),f0,f1,f2);
      % splitting by expected gain
      % compute the expected gain vector e and the potential splitting
      % index and splitting value
      fexp = f(1,par) + min(e);
      if fexp < fbest
        splt = 1;
      else
        splt = 0;  % the box is not split since we expect no improvement
        nogain(par) = 1; % the box is marked as not eligible for splitting by expected gain
      end
    end
  end
  if splt == 1  % prepare for splitting
    i = isplit(par);
    level(par) = 0;
    if z(2,par) == Inf % prepare for splitting by initialization list
      m = m + 1;
      z(2,par) = m;
      [xbest,fbest,f0(:,m),xmin,fmi,ipar,level,ichild,f,flag,ncall1] = splinit(fcn,data,i,s,smax,par,x0,n0,u,v,x,y,x1,x2,L,l,xmin,fmi,ipar,level,ichild,f,xbest,fbest,stop,prt);
      ncall = ncall + ncall1;
    else  % prepare for default splitting
      z(1,par) = x(i);
      [xbest,fbest,xmin,fmi,ipar,level,ichild,f,flag,ncall1] = split(fcn,data,i,s,smax,par,n0,u,v,x,y,x1,x2,z(:,par),xmin,fmi,ipar,level,ichild,f,xbest,fbest,stop,prt);
      ncall = ncall + ncall1;
    end
    if nboxes > dim
% if the pre-assigned size of the `large' arrays has already been exceeded, these arrays are made larger
      isplit(nboxes+1:nboxes+step) = zeros(1,step);
      level(nboxes+1:nboxes+step) = zeros(1,step);
      ipar(nboxes+1:nboxes+step) = zeros(1,step);
      ichild(nboxes+1:nboxes+step) = zeros(1,step);
      z(:,nboxes+1:nboxes+step) = zeros(2,step);
      nogain(nboxes+1:nboxes+step) = zeros(1,step);
      f(:,nboxes+1:nboxes+step) = zeros(2,step);
      dim = nboxes + step;
    end
    if ~flag,break,end
  else  % splt=0: no splitting, increase the level by 1
    if s + 1 < smax
      level(par) = s + 1;
      updtrec(par,s+1,f(1,:));
    else
      level(par) = 0;
      nbasket = nbasket + 1;
      xmin(:,nbasket) = x;
      fmi(nbasket) = f(1,par);
    end
    if prt > 1
      [w1,w2] = bounds(n,n0,x,y,u,v);
      % compute lower and upper bounds of the box in order to be able
      % to check whether it contains a global minimizer
      iopt = [];
      % the vector iopt contains the indices of the global minimizers
      % contained in the box
      for iglob = 1:nglob
        if w1 <= xglob(:,iglob) & xglob(:,iglob) <= w2
          iopt = [iopt, iglob];
        end
        for iglob = 1:length(iopt)
          optlevel(iopt(iglob)) = s + 1;
        end
      end
    end
  end % of prepare for splitting
  s = s + 1;
  while s < smax
    if record(s) == 0
      s = s + 1;
    else
      break
    end
  end
  if s == smax  % if smax is reached, a new sweep is started
    if local,
      [fmi(nbasket0+1:nbasket),j] = sort(fmi(nbasket0+1:nbasket));
      xmin(:,nbasket0+1:nbasket) = xmin(:,nbasket0+j);
      xmin0 = [];
      fmi0 = [];
      for j = nbasket0+1:nbasket
        x = xmin(:,j);
        f1 = fmi(j);
        chkloc;
        if loc,
          addloc;
          [xbest,fbest,xmin,fmi,x,f1,loc,flag,ncall1] = basket(fcn,data,x,f1,xmin,fmi,xbest,fbest,stop,nbasket0);
          ncall = ncall + ncall1;
          if ~flag,break,end
          if loc,
            [xmin1,fmi1,nc,flag] = lsearch(fcn,data,x,f1,f0min,u,v,nf-ncall,stop,local,gamma,hess);
            ncall = ncall + nc;
            ncloc = ncloc + nc;
            if fmi1 < fbest
              xbest = xmin1;
              fbest = fmi1;
              nsweepbest = nsweep;
              if ~flag
                nbasket0 = nbasket0 + 1;
                nbasket = nbasket0;
                xmin(:,nbasket) = xmin1;
                fmi(nbasket) = fmi1;
                break
              end
              if stop(1) > 0 & stop(1) < 1
                flag = chrelerr(fbest,stop);
              elseif stop(1) == 0
                flag = chvtr(fbest,stop(2));
              end
              if ~flag,return,end
            end
            [xbest,fbest,xmin,fmi,loc,flag,ncall1] = basket1(fcn,data,xmin1,fmi1,xmin,fmi,xbest,fbest,stop,nbasket0);
            ncall = ncall + ncall1;
            if ~flag,break,end
            if loc,
              nbasket0 = nbasket0 + 1;
              xmin(:,nbasket0) = xmin1;
              fmi(nbasket0) = fmi1;
              fbestloc;
              if ~flag,
                nbasket = nbasket0; break
              end
            end
          end
        end
      end
      nbasket = nbasket0;
      if ~flag,break,end
    end
    s = strtsw(smax,level,f(1,:));
    if prt,
      if nsweep == 1
        fprintf('nsw  minl  ');
        if prt > 1
          fprintf('optl    fopt       ')
        end
        fprintf('nf     fbest        xbest\n')
      end
      minlevel=s;
      fprintf('%3i  %3i',nsweep,minlevel);
      if prt > 1
        fprintf('  %3i',optlevel);fprintf('  %10.3e',foptbox);
      end
      fprintf('  %5i  %10.3e',ncall,fbest);
      fprintf('  %10.4f',xbest);
      fprintf(1,'\n');
    end
    if stop(1) > 1
      if nsweep - nsweepbest >= stop(1),flag = 3; return,end
    end
    nsweep = nsweep + 1;
  end
end
if ncall >= nf
  flag = 2;
end
if local,
  if length(fmi) > nbasket
    xmin(:,nbasket+1:length(fmi)) = [];
    fmi(nbasket+1:length(fmi)) = [];
  end
end


function att = attenuationWater(f, T)
%ATTENUATIONWATER Calculate ultrasound attenuation in distilled water.
%
% DESCRIPTION:
%       attenuationWater calculates the ultrasonic absorption in distilled
%       water at a given temperature and frequency using a 7th order
%       polynomial fitted to the data given by Pinkerton (1949).
%
% USAGE:
%       att = attenuationWater(f, T)
%
% INPUTS:
%       f   - array of frequency values [MHz]
%       T   - water temperature [degC]
%
% OUTPUTS:
%       att - attenuation [dB/cm]
%
% ABOUT:
%       author      - Bradley E. Treeby
%       date        - 10th November 2008
%       last udpate - 10th November 2008
%
% REFERENCES:
%   [1] Pinkerton (1949) "The Absorption of Ultrasonic Waves in Liquids and
%       its Relation to Molecular Constitution," Proceedings of the
%       Physical Society. Section B, 2, 129-141
%
% This function is part of the k-Wave Toolbox (http://www.k-wave.org)
% Copyright (C) 2009-2014 Bradley Treeby and Ben Cox
%
% See also speedSoundWater

% This file is part of k-Wave. k-Wave is free software: you can
% redistribute it and/or modify it under the terms of the GNU Lesser
% General Public License as published by the Free Software Foundation,
% either version 3 of the License, or (at your option) any later version.
%
% k-Wave is distributed in the hope that it will be useful, but WITHOUT ANY
% WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
% FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License for
% more details.
%
% You should have received a copy of the GNU Lesser General Public License
% along with k-Wave. If not, see <http://www.gnu.org/licenses/>.

if T < 0 || T > 60
    disp('WARNING: Temperature outside range of experimental data');
end

% conversion factor between Nepers and dB
NEPER2DB = 8.686;

% coefficients for 7th order polynomial fit
a_0 = 56.723531840522710;
a_1 = -2.899633796917384;
a_2 = 0.099253401567561;
a_3 = -0.002067402501557;
a_4 = 2.189417428917596e-005;
a_5 = -6.210860973978427e-008;
a_6 = -6.402634551821596e-010;
a_7 = 3.869387679459408e-012;

% compute attenuation
a_on_fsqr = (a_0 + a_1*T + a_2*T.^2 + a_3*T.^3 + a_4*T.^4 + a_5*T.^5 + a_6*T.^6 + a_7*T.^7)*1e-17;
att = NEPER2DB*1e12*f.^2*a_on_fsqr;


function [sUnitVector, OUnitVector, vInfMag] = computeHyperSVectOVect(hSMA, hEcc, hInc, hRAAN, hArg, hTA, gmu)
%computeHyperSVectOVect Summary of this function goes here
%   Detailed explanation goes here
    [hRVect,hVVect]=getStatefromKepler(hSMA, hEcc, hInc, hRAAN, hArg, hTA, gmu);
    hHat = normVector(cross(hRVect, hVVect));

    flyByAngle=2*asin(1/hEcc);
    SigmaAngle=pi/2 - flyByAngle/2;
    hUnitVector=hHat;
    eVector=(norm(hVVect)^2/gmu - 1/norm(hRVect))*hRVect - (dot(hRVect,hVVect)/gmu)*hVVect;
    eUnitVect=eVector/norm(eVector);
    sUnitVector=cos(SigmaAngle)*eUnitVect + sin(SigmaAngle)*cross(hUnitVector,eUnitVect);
    BUnitVector=cross(sUnitVector,hUnitVector)/norm(cross(sUnitVector,hUnitVector));
    OUnitVector=cos(flyByAngle)*sUnitVector - sin(flyByAngle)*BUnitVector;

    sUnitVector = real(sUnitVector);
    OUnitVector = real(OUnitVector);

    vInfMag = sqrt(-gmu/hSMA);
end


%% Dense vs Sparse SNLE
clc
fun = @(x) [10*(x(2) - x(1)^2)
            sqrt(90)*(x(4)-x(3)^2)
            sqrt(10)*(x(2) + x(4) - 2)
            (1/sqrt(10))*(x(2) - x(4))];

x0 = [-30 -10 -30 -10]';

Opt = opti('fun',fun,'x0',x0,'options',optiset('solver','auto'))

[x,f,e,i] = solve(Opt)

%% Sparse above
fun = @(x) -1;
nleq = @(x) [10*(x(2) - x(1)^2)
            sqrt(90)*(x(4)-x(3)^2)
            sqrt(10)*(x(2) + x(4) - 2)
            (1/sqrt(10))*(x(2) - x(4))];
cl = zeros(4,1);
cu = zeros(4,1);

x0 = [-30 -10 -30 -10]';

Opt = opti('fun',fun,'nl',nleq,cl,cu,'x0',x0,'options',optiset('solver','auto'))

[x,f,e,i] = solve(Opt)

%% As above but new construct [no grad]
clc
nleq = @(x) [10*(x(2) - x(1)^2)
            sqrt(90)*(x(4)-x(3)^2)
            sqrt(10)*(x(2) + x(4) - 2)
            (1/sqrt(10))*(x(2) - x(4))];

x0 = [-30 -10 -30 -10]';

Opt = opti('nleq',nleq,'x0',x0,'options',optiset('solver','auto'))

[x,f,e,i] = solve(Opt)

%% As above but new construct [w grad DENSE]
clc
nleq = @(x) [10*(x(2) - x(1)^2)
            sqrt(90)*(x(4)-x(3)^2)
            sqrt(10)*(x(2) + x(4) - 2)
            (1/sqrt(10))*(x(2) - x(4))];
if (exist('syms.m','file'))
    [nljac,nljacstr] = symJac(nleq);

    x0 = [-30 -10 -30 -10]';

    Opt = opti('nleq',nleq,'nljac',nljac,'x0',x0,'options',optiset('solver','auto'))

    [x,f,e,i] = solve(Opt)
end

%% As above but new construct [w grad SPARSE]
clc
nleq = @(x) [10*(x(2) - x(1)^2)
            sqrt(90)*(x(4)-x(3)^2)
            sqrt(10)*(x(2) + x(4) - 2)
            (1/sqrt(10))*(x(2) - x(4))];
if (exist('syms.m','file'))
    [nljac,nljacstr] = symJac(nleq); nljac = @(x) sparse(nljac(x));

    x0 = [-30 -10 -30 -10]';

    Opt = opti('nleq',nleq,'nljac',nljac,'nljacstr',nljacstr,'x0',x0,'options',optiset('solver','auto'))

    [x,f,e,i] = solve(Opt)
end

%% As above but new construct [w grad SPARSE alt nl format]
clc
nleq = @(x) [10*(x(2) - x(1)^2)
            sqrt(90)*(x(4)-x(3)^2)
            sqrt(10)*(x(2) + x(4) - 2)
            (1/sqrt(10))*(x(2) - x(4))];
if (exist('syms.m','file'))
    [nljac,nljacstr] = symJac(nleq); nljac = @(x) sparse(nljac(x));

    x0 = [-30 -10 -30 -10]';

    Opt = opti('nleq',nleq,'nljac',nljac,'nljacstr',nljacstr,'x0',x0,'options',optiset('solver','auto'))

    [x,f,e,i] = solve(Opt)
end

%% As above but new construct [w grad SPARSE alt nl format w lin]
clc
nleq = @(x) [10*(x(2) - x(1)^2)
            sqrt(90)*(x(4)-x(3)^2)
            sqrt(10)*(x(2) + x(4) - 2)
            (1/sqrt(10))*(x(2) - x(4))];
if (exist('syms.m','file'))
    [nljac,nljacstr] = symJac(nleq); nljac = @(x) sparse(nljac(x));
    A = [0 1 0 1]; b = 2;

    x0 = [-30 -10 -30 -10]';

    Opt = opti('nleq',nleq,'ineq',A,b,'nljac',nljac,'nljacstr',nljacstr,'x0',x0,'options',optiset('solver','auto'))

    [x,f,e,i] = solve(Opt)
end

%% As above but new construct [w grad SPARSE alt nl format w lin + int]
clc
nleq = @(x) [10*(x(2) - x(1)^2)
            sqrt(90)*(x(4)-x(3)^2)
            sqrt(10)*(x(2) + x(4) - 2)
            (1/sqrt(10))*(x(2) - x(4))];
if (exist('syms.m','file'))
    [nljac,nljacstr] = symJac(nleq); nljac = @(x) sparse(nljac(x));
    A = [0 1 0 1]; b = 2;

    x0 = [-30 -10 -30 -10]';

    Opt = opti('nleq',nleq,'ivars',2,'lin',A,-Inf,b,'nljac',nljac,'nljacstr',nljacstr,'x0',x0,'options',optiset('solver','auto'))

    [x,f,e,i] = solve(Opt)
end

%% As above but new construct [w grad SPARSE alt nl format w ineq]
clc
nleq = @(x) [10*(x(2) - x(1)^2)
            sqrt(90)*(x(4)-x(3)^2)
            sqrt(10)*(x(2) + x(4) - 2)
            (1/sqrt(10))*(x(2) - x(4))
            x(3)];
if (exist('syms.m','file'))
    [nljac,nljacstr] = symJac(nleq); nljac = @(x) sparse(nljac(x));
    nlrhs = zeros(5,1);
    nle = [zeros(4,1);-1];
    A = [0 1 0 1]; b = 2;

    x0 = [-30 -10 -30 -10]';

    Opt = opti('nlmix',nleq,nlrhs,nle,'ineq',A,b,'nljac',nljac,'nljacstr',nljacstr,'x0',x0,'options',optiset('solver','auto'))

    [x,f,e,i] = solve(Opt)
end

%% Wiki Ex 1
clc
% System of Nonlinear Equations
 nleq = @(x) [ 2*x(1) - x(2) - exp(-x(1));
             -x(1) + 2*x(2) - exp(-x(2))];

% Starting Guess
 x0 = [-5;5];

% Create OPTI Object
 Opt = opti('nleq',nleq,'x0',x0)

% Solve the SNLE problem
[x,fval,exitflag,info] = solve(Opt)

%% Online Example
clc
% System of Nonlinear Equations
 nleq = @(x) [10*(x(2) - x(1)^2)
             sqrt(90)*(x(4) - x(3)^2)
             sqrt(10)*(x(2) + x(4) - 2)
             (1/sqrt(10))*(x(2) - x(4))];

% Nonlinear Equations Jacobian
 nlJac = @(x) sparse([-20*x(1),10,0,0
                      0,0,-6*10^(1/2)*x(3),3*10^(1/2)
                      0,10^(1/2),0,10^(1/2)
                      0,10^(1/2)/10,0,-10^(1/2)/10]);

% Jacobian Sparsity Pattern
 nlJacstr = @() sparse([1 1 0 0
                        0 0 1 1
                        0 1 0 1
                        0 1 0 1]);

% Starting Guess
 x0 = [-30;-10;-30;-10];

% Sparse SNLE OPTI Problem
 Opt = opti('nleq',nleq,'nlJac',nlJac,'nlJacstr',nlJacstr,'x0',x0)

  %Solve
[x,f,e,i] = solve(Opt)

%% SCNLE Example
clc

% Objective (Nonlinear Equations) Function
 fun = @(x) [ 2*x(1) - x(2) - exp(-x(1));
             -x(1) + 2*x(2) - exp(-x(2))];

% Bounds
 lb = [0.6;0];
 ub = [1;1];

% Starting Guess
 x0 = [-5;5];

% Create OPTI Object
 Opt = opti('nleq',fun,'bounds',lb,ub,'x0',x0,'options',optiset('display','iter'))

% Solve the SCNLE problem
[x,fval,exitflag,info] = solve(Opt)

%% Problem SCNLE
clc

A = randn(91);
B12 = randn(91,145);
B1 = randn(145,91); B2 = randn(145,91);
Constant = randn(91,1);
A_ieq = randn(12,91); b_ieq = randn(12,1);
lb = zeros(91,1); ub = 100*ones(91,1);
z0 = lb;

nleq = @(x) A*x + B12*((B1*x).*(B2*x)) + Constant;

Opt=opti('nleq',nleq,'ineq',A_ieq,b_ieq,'bounds',lb,ub,'x0',z0,'options',optiset('display','iter'))

solve(Opt)


function OUT2 = hillshade(DEM,varargin)

%HILLSHADE create hillshading from a digital elevation model (GRIDobj)
%
% Syntax
%
%     H = hillshade(DEM)
%     H = hillshade(DEM,'pn','pv',...)
%
% Description
%
%     Hillshading is a very powerful tool for relief depiction.
%     hillshade calculates a shaded relief for a digital elevation model
%     based on the angle between the surface and the incoming light beams.
%     If no output arguments are defined, the hillshade matrix will be
%     plotted with a gray colormap. The hillshading algorithm follows the
%     logarithmic approach to shaded relief representation of Katzil and
%     Doytsher (2003).
%
% Input
%
%     DEM       Digital elevation model (class: GRIDobj)
%
% Parameter name/value pairs
%
%     'azimuth'         azimuth angle, (default=315)
%     'altitude'        altitude angle, (default=60)
%     'exaggerate'      elevation exaggeration (default=1). Increase to
%                       pronounce elevation differences in flat terrain
%     'useblockproc'    true or {false}: use block processing
%                       (see function blockproc)
%     'useparallel'     true or {false}: use parallel computing toolbox
%     'blocksize'       blocksize for blockproc (default: 5000)
%
%
% Output
%
%     H         shaded relief (ranges between 0 and 1)
%
%
% Example
%
%     DEM = GRIDobj('srtm_bigtujunga30m_utm11.tif');
%     hillshade(DEM)
%
% References
%
%     Katzil, Y., Doytsher, Y. (2003): A logarithmic and sub-pixel approach
%     to shaded relief representation. Computers & Geosciences, 29,
%     1137-1142.
%
% See also: SURFNORM, IMAGESCHS
%
% Author: Wolfgang Schwanghart (w.schwanghart[at]geo.uni-potsdam.de)
% Date: 18. August, 2017

% Parse inputs
p = inputParser;
p.StructExpand  = true;
p.KeepUnmatched = false;
p.FunctionName = 'hillshade';
addParamValue(p,'azimuth',315,@(x) isscalar(x) && x>= 0 && x<=360);
addParamValue(p,'altitude',60,@(x) isscalar(x) && x>= 0 && x<=90);
addParamValue(p,'exaggerate',1,@(x) isscalar(x) && x>0);
addParamValue(p,'useparallel',true);
addParamValue(p,'blocksize',2000);
addParamValue(p,'useblockproc',true,@(x) isscalar(x));
parse(p,varargin{:});

OUT     = DEM;
OUT.Z   = [];

cs      = DEM.cellsize;
azimuth = p.Results.azimuth;
altitude = p.Results.altitude;
exaggerate = p.Results.exaggerate;

% Large matrix support. Break calculations in chunks using blockproc
if numel(DEM.Z)>(10001*10001) && p.Results.useblockproc;
    blksiz = bestblk(size(DEM.Z),p.Results.blocksize);
    padval = 'symmetric';
    Z      = DEM.Z;
    % The anonymous function must be defined as a variable: see bug 1157095
    fun   = @(x) hsfun(x,cs,azimuth,altitude,exaggerate);
    HS = blockproc(Z,blksiz,fun,...
                'BorderSize',[1 1],...
                'padmethod',padval,...
                'UseParallel',p.Results.useparallel);
    OUT.Z = HS;
else
    OUT.Z = hsfun(DEM.Z,cs,azimuth,altitude,exaggerate);
end

OUT.name = 'hillshade';
OUT.zunit = '';

if nargout == 0;
    OUT.Z = uint8(OUT.Z*255);
    imagesc(OUT);
    colormap(gray)
else
    OUT2 = OUT;
end

end
%% Subfunction
function H = hsfun(Z,cs,azimuth,altitude,exaggerate)

if isstruct(Z)
    Z = Z.data;
end

% correct azimuth so that angles go clockwise from top
azid = azimuth-90;

% use radians
altsource = altitude/180*pi;
azisource = azid/180*pi;

% calculate solar vector
[sx,sy,sz] = sph2cart(azisource,altsource,1);

% calculate surface normals
[Nx,Ny,Nz] = surfnorm(Z/cs*exaggerate);

% calculate cos(angle)
% H = [Nx(:) Ny(:) Nz(:)]*[sx;sy;sz];
% % reshape
% H = reshape(H,size(Nx));

H = Nx*sx + Ny*sy + Nz*sz;

% % usual GIS approach
% H = acos(H);
% % force H to range between 0 and 1
% H = H-min(H(:));
% H = H/max(H(:));

end


function y = pow_pos( x, p )

%POW_POS   Power of positive part.
%   POW_POS(X,P) = POS(X).^P = MAX(X,0).^P.
%   Both P and X must be real, and P must be greater than or equal to 1.
%
%   Disciplined convex programming information:
%       POW_POS(X,P) is convex and nondecreasing in X; so when used in CVX
%       expressions, X must be convex. P must be constant, real, and
%       greater than or equal to 1.

narginchk(2,2);
if ~isnumeric( x ) || ~isreal( x ) || ~isnumeric( p ) || ~isreal( p ),
    error( 'Arguments must be real.' );
elseif any( p(:) <= 1 ),
    error( 'Second argument must be greater than or equal to 1.\nFor other exponents, use POW_P instead.', 1 ); %#ok
end
y = max(x,0).^p;

% 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 n = normal(c, unit)
%NORMAL   Normal to a complex-valued CHEBFUN.
%   N = NORMAL(C) returns the normal vector to the curve C as a CHEBFUN with two
%   columns. The vector has the same magntiude as the curve's tangent vector.
%
%   N = NORMAL(C, 'unit') returns the unit normal vector to the curve C.

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

% [TODO]:  Reconsider how this function should behave in the presence of cusps
% once we have singfun in place.

n = -1i*diff(c);

if ( nargin > 1 )
    if ( strcmpi(unit, 'unit') )
        nrmn = norm(n);
        if ( nrmn == 0 )
            error('CHEBFUN:CHEBFUN:normal:zero', 'Normal vector is zero.');
        else
            n = n./nrmn;
        end
    else
        error('CHEBFUN:CHEBFUN:normal:args', ...
            'Second argument is not recognised.');
    end
end

% Return an array-valued CHEBFUN:
n = [real(n), imag(n)];

end


function clenshaw_curtis_set_test ( )

%*****************************************************************************80
%
%% CLENSHAW_CURTIS_SET_TEST tests CLENSHAW_CURTIS_SET.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 April 2015
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CLENSHAW_CURTIS_SET_TEST\n' );
  fprintf ( 1, '  CLENSHAW_CURTIS_SET sets up a Clenshaw-Curtis rule;\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Estimate the integral of sqrt(abs(x)) over [-1,+1].\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   N           Estimate             Error\n' );
  fprintf ( 1, '\n' );

  exact = 4.0 / 3.0;

  for n = 1 : 10

    [ x, w ] = clenshaw_curtis_set ( n );

    v(1:n,1) = sqrt ( abs ( x(1:n,1) ) );

    q = w' * v;
    e = abs ( q - exact );

    fprintf ( 1, '  %2d  %24.16g  %14.6e\n', n, q, e );

  end

  return
end