% -------------------------------------------------------------------------
% Plot individual slopes of regressions, using conf. interval for x range
% -------------------------------------------------------------------------
function igls_plot_slopes(out, X, varargin)

    % data
    % -------------------------------------------------------------------
    N = out.sub; % subjects

    betas = out.beta_indiv([2 1], :);  % format for compatibility with mediation.m
    % slope is first row, intercept is 2nd

    groupbeta = out.beta([2 1]);

    w = ones(N, 1);
    if ~isempty(varargin)
        % we have stats structure with (maybe) weights
        if isfield(out, 'w')
            w = out.w;
            w = w .* N;
        end
    end

    ncols = 1;

    % plot
    % -------------------------------------------------------------------
    fh = create_figure('Slope_Plot');

    subplot(1, ncols, 1);
    linehan = plot_individual_slopes(betas, X, w(:,1));
    set(linehan, 'Color', [.7 .7 .7]);

    linehan_group = plot_individual_slopes(groupbeta, X(:), 3);
    set(linehan_group, 'LineWidth', 4);

    %xlabel(vnames{1}), ylabel(vnames{3});
    title('Slopes');

% %     subplot(1, ncols, 2);
% %     plot_individual_slopes(bbetas, M, w(:,2));
% %     xlabel(vnames{3}), ylabel(vnames{2});
% %     title('b: M->Y controlling X');
% %
% %     subplot(1, ncols, 3);
% %     plot_individual_slopes(cpbetas, X, w(:,3));
% %     xlabel(vnames{1}), ylabel(vnames{2});
% %     title('c'': X->Y controlling M');
% %
% %     subplot(1, ncols, 4);
% %     plot_individual_slopes(cbetas, X, w(:,4));
% %     xlabel(vnames{1}), ylabel(vnames{2});
% %     title('c: X->Y');
end

function linehan = plot_individual_slopes(betas, X, w)

    % sort so that we plot from lowest to highest weights
    [w, sorti] = sort(w);
    betas = betas(:,sorti);
    if iscell(X), X = X(sorti); else X = X(:,sorti); end

    % get colors based on weights
    N = size(betas, 2);
    minwt = .4;     % make sure all lines are visible.
    w = rescale_range(w, [minwt 1]);

    % line widths: median split, top half gets 1, bottom gets 1/2.
    linew = (w(:,1) >= median(w(:,1))) +.5;

    w = 1 - w;  % for colors, 0 is black
    colors = repmat([1 1 1], N, 1) .* repmat(w, 1, 3);
    colors(colors < 0) = 0; % include to remove rounding error

    for i = 1:N
        if iscell(X), x = X{i}; else x = X(:,i); end

        % 95% conf. interval for x
        [nanvec, x] = nanremove(x);
        mx = mean(x);
        s = std(x) * tinv(.975, length(x)-1);
        x = [mx - s mx + s];
        y = betas(2, i) + betas(1, i) * x;
        linehan(i) = plot(x, y, '-', 'Color', colors(i,:), 'LineWidth', linew(i));
    end

    drawnow
end

function rx = rescale_range(x, y)
    % re-scale x to range of y
    m = range(y)./range(x);

    if isinf(m)
        % no range/do not rescale
        rx = x;
    else
        b = y(1) - m * x(1);
        rx = m*x + b;
    end
end


function b = tril(a,k)
% function B = tril(A,K)
%
% DESCRIPTION
%   Extract lower triangular part of a polynomial matrix
%
% INPUTS
%   A: polynomial
%   K: integer
%
% OUTPUTS
%   B: polynomial
%
% SYNTAX
%   B=tril(A,K)
%     B contains the elements on and below the K-th diagonal of A. K=0
%     is the main diagonal, K>0 is above the main diagonal, and K<0 is
%     below the main diagonal.
%   B=tril(A)
%     This is the same as B=tril(A,0).
%
% See also triu, diag

% 11/24/2009: PJS  Initial Coding

if nargin==1
    k=0;
end

sza = size(a);
% a is a matrix --> b is a vector
if k>=sza(2)
    b = a;
    return
elseif k <= -sza(1)
    b = polynomial(zeros(sza));
    return;
end

% Use double/tril to find indices to zero out
idx = find( ~tril(ones(sza),k) );

% Assign values
b = a;
L.type = '()';
L.subs = {idx};
b = subsasgn(b,L,0);


function [out, revertClass] = tofloat(in)
%TOFLOAT Converts an image to floating point.
%   [OUT, REVERTCLASS] = TOFLOAT(IN) converts the input image IN to
%   floating-point. If IN is a double or single image, then OUT
%   equals IN.  Otherwise, OUT equals IM2DOUBLE(IN).  REVERTCLASS is
%   a function handle that can be used to convert back to the class
%   of IN.
%
%   Copyright 2002-2020 Gatesmark
%
%   This function, and other functions in the DIPUM Toolbox, are based
%   on the theoretical and practical foundations established in the
%   book Digital Image Processing Using MATLAB, 3rd ed., Gatesmark
%   Press, 2020.
%
%   Book website: http://www.imageprocessingplace.com
%   License: https://github.com/dipum/dipum-toolbox/blob/master/LICENSE.txt

identity = @(x) x;

todouble = @im2double;

converter_table = {'uint8',   todouble, @im2uint8
   'uint16',  todouble, @im2uint16
   'int16',   todouble, @im2int16
   'logical', todouble, @logical
   'double',  identity, identity
   'single',  identity, identity};

classIndex = find(strcmp(class(in), converter_table(:, 1)));

if isempty(classIndex)
   error('Unsupported input image class.');
end

out = converter_table{classIndex, 2}(in);

revertClass = converter_table{classIndex, 3};


function rs = center(s)

%tstoolbox/@signal/center
%   Syntax:
%     * center(s)
%
%   Center signal by removing it's mean.
%
% Copyright 1997-2001 DPI Goettingen, License http://www.physik3.gwdg.de/tstool/gpl.txt

narginchk(1,1);
if nargin  < 1, help(mfilename); end

N = dlens(s,1);
points = data(s);
c = core(points - repmat(mean(points), N,1));
rs = signal(c,s);

rs = addhistory(rs, ['Centered signal around zero']);
rs = addcommandlines(rs, 's = center(s');


function test_ft_plot_ortho

% MEM 1gb
% WALLTIME 00:10:00
% DEPENDENCY ft_plot_ortho

dat = randn(64, 64, 64);

figure
ft_plot_ortho(dat);


function [  ] = salientNumCnt( filesDir, totalNum )
%SALIENTNUMCNT Summary of this function goes here
%   Detailed explanation goes here

    allPoints = zeros(1,totalNum);

    fileExt = '*.txt';

    files = dir(fullfile(filesDir,fileExt));

    for i=0:length(files)-1
        i
        fileCnt = num2str(i);
        fileName = [filesDir, fileCnt, '.txt'];

        file_t = fopen(fileName);
        pointID = fscanf(file_t, '%d');
        fclose(file_t);

        allPoints(sub2ind(size(allPoints), ones(1, length(pointID)), (pointID)')) = 1;

    end

    disp('salient predicted:');
    disp(sum(allPoints));

end


function test_suite = test_spatial2

%   Run specific demo and save values for comparison.
%
%   See also
%     TEST_ALL, DEMO_SPATIAL2

initTestSuite;

function testDemo
% Set random number stream so that failing isn't because randomness. Run
% demo & save test values.
prevstream=setrandstream(0);

disp('Running: demo_spatial2')
demo_spatial2
Ef = Ef(1:100);
Varf = Varf(1:100);
C = C(1:50, 1:50);
path = which('test_spatial2.m');
path = strrep(path,'test_spatial2.m', 'testValues');
if ~(exist(path, 'dir') == 7)
    mkdir(path)
end
path = strcat(path, '/testSpatial2');
save(path, 'Ef', 'Varf', 'C');

% Set back initial random stream
setrandstream(prevstream);
drawnow;clear;close all

% Compare test values to real values.

function testPredictionsEP
values.real = load('realValuesSpatial2.mat', 'Ef', 'Varf');
values.test = load(strrep(which('test_spatial2.m'), 'test_spatial2.m', 'testValues/testSpatial2.mat'), 'Ef', 'Varf');
assertElementsAlmostEqual(mean(values.test.Ef), mean(values.real.Ef), 'relative', 0.1);
assertElementsAlmostEqual(mean(values.test.Varf), mean(values.real.Varf), 'relative', 0.1);

function testCovarianceMatrix
values.real = load('realValuesSpatial2.mat', 'C');
values.test = load(strrep(which('test_spatial2.m'), 'test_spatial2.m', 'testValues/testSpatial2.mat'), 'C');
assertElementsAlmostEqual(mean(values.real.C), mean(values.test.C), 'relative', 0.1);


function pass = test_emptyObjects()
% Does each chebfun3 command deal with empty chebfun3 objects,
% appropriately?

% Check things work for empty chebfun3 objects.
f = chebfun3();
try
    f + f;
    2*f;
    f*2;
    f.^2;
    2.^f;
    f.^f;
    sqrt(f);
    sum(f);
    sum2(f);
    sum3(f);
    integral(f);
    diff(f);
    sin(f);
    cos(f);
    sinh(f);
    f.^f + f;
    tucker(f)
    mean(f);
    max3(f);
    norm(f);
    minandmax3(f);
    permute(f);
    pass = 1;
catch
    pass = 0;
end
end


function X = tenzeros(varargin)
%TENZEROS Create zeros tensor.
%
%   X = TENZEROS(SZ) forms a tensor of size SZ with all zeros.
%
%   TENZEROS(SZ) is equivalent to TENSOR(ZEROS(SZ(1),SZ(2),...),SZ).
%
%   See also TENSOR, ZEROS.
%
%MATLAB Tensor Toolbox.
%Copyright 2015, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2015) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

if nargin == 1
    sz = varargin{1};
else
    sz = cell2mat(varargin);
end

if isempty(sz)
    X = tensor;
    return;
end

if nargin == 2
    order = sz;
    dim = varargin{1};
    sz = dim * ones(1,order);
end

data = zeros([sz 1 1]);
X = tensor(data,sz);


%  This program is free software; you can redistribute it and/or modify
%  it under the terms of the GNU General Public License as published by
%  the Free Software Foundation; either version 2 of the License, or
%  (at your option) any later version.
%
%  This program is distributed in the hope that it will be useful,
%  but WITHOUT ANY WARRANTY; without even the implied warranty of
%  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%  GNU General Public License for more details.
%
%  You should have received a copy of the GNU General Public License
%  along with this program; if not, write to the Free Software
%  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
%function tfrrit
%TFRRIT Unit test for the function TFRRI.

%       O. Lemoine, March 1996.

N=128;

% Covariance by translation in time
t1=60; t2=70; f=0.3;
sig1=amgauss(N,t1).*fmconst(N,f,t1);
sig2=amgauss(N,t2).*fmconst(N,f,t2);
tfr1=tfrri(sig1);
tfr2=tfrri(sig2);
[tr,tc]=size(tfr1);
tfr=tfr1-tfr2(:,modulo((1:tc)-t1+t2,tc));
if any(any(abs(tfr)>sqrt(eps))),
 error('tfrri test 1 failed');
end

% Energy conservation
sig=noisecg(N);
tfr=tfrri(sig);
Es=norm(sig)^2;
Etfr=sum(mean(tfr));
if abs(Es-Etfr)>sqrt(eps),
 error('tfrri test 2 failed');
end

% Time-marginal
sig=noisecg(N);
tfr=tfrri(sig);
ip1=abs(sig).^2;
ip2=mean(tfr)';
if any(abs(ip1-ip2)>sqrt(eps)),
 error('tfrri test 3 failed');
end

% Frequency-marginal
sig=noisecg(N);
tfr=tfrri(sig);
FFT=fft(sig);
psd1=abs(FFT(2:N/2)).^2/N;
psd2=real(mean(tfr(2:N/2,:)'))';
if any(abs(psd1-psd2)>sqrt(eps)),
 error('tfrri test 4 failed');
end

% Unitarity
x1=amgauss(N).*fmlin(N);
x2=amexpo2s(N);
tfr1=tfrri(x1);
tfr2=tfrri(x2);
cor1=abs(x1'*x2)^2;
cor2=real(sum(sum(tfr1.*conj(tfr2)))/N);
if abs(cor1-cor2)>sqrt(eps),
 error('tfrri test 5 failed');
end

% Conservation of the time support (wide-sense)
sig=[zeros(N/4,1);fmlin(N/2);zeros(N/4,1)];
tfr=tfrri(sig);
if sum(any(abs(tfr(:,1:N/4-1))>sqrt(eps))) | ...
   sum(any(abs(tfr(:,(3*N/4+1):N))>sqrt(eps))),
 error('tfrri test 6 failed');
end

% time localization
t0=30; sig= ((1:N)'==t0);
tfr=tfrri(sig);
[ik,jk]=find(tfr~=0.0);
if any(jk~=t0)|any(ik'-(1:N)),
 error('tfrri test 7 failed');
end;

N=127;

% Covariance by translation in time
t1=61; t2=70; f=0.3;
sig1=amgauss(N,t1).*fmconst(N,f,t1);
sig2=amgauss(N,t2).*fmconst(N,f,t2);
tfr1=tfrri(sig1);
tfr2=tfrri(sig2);
[tr,tc]=size(tfr1);
tfr=tfr1-tfr2(:,modulo((1:tc)-t1+t2,tc));
if any(any(abs(tfr)>1e-7)),
 error('tfrri test 8 failed');
end

% Energy conservation
sig=noisecg(N);
tfr=tfrri(sig);
Es=norm(sig)^2;
Etfr=sum(mean(tfr));
if abs(Es-Etfr)>sqrt(eps),
 error('tfrri test 9 failed');
end

% Time-marginal
sig=noisecg(N);
tfr=tfrri(sig);
ip1=abs(sig).^2;
ip2=mean(tfr)';
if any(abs(ip1-ip2)>sqrt(eps)),
 error('tfrri test 10 failed');
end

% Frequency-marginal
sig=noisecg(N);
tfr=tfrri(sig);
FFT=fft(sig);
psd1=abs(FFT(2:fix(N/2))).^2/N;
psd2=real(mean(tfr(2:fix(N/2),:)'))';
if any(abs(psd1-psd2)>sqrt(eps)),
 error('tfrri test 11 failed');
end

% Unitarity
x1=amgauss(N).*fmlin(N);
x2=amexpo2s(N);
tfr1=tfrri(x1);
tfr2=tfrri(x2);
cor1=abs(x1'*x2)^2;
cor2=real(sum(sum(tfr1.*conj(tfr2)))/N);
if abs(cor1-cor2)>sqrt(eps),
 error('tfrri test 12 failed');
end

% Conservation of the time support (wide-sense)
sig=[zeros(round(N/4),1);fmlin(round(N/2));zeros(round(N/4),1)];
tfr=tfrri(sig);
if sum(any(abs(tfr(:,1:round(N/4)-1))>sqrt(eps))) | ...
   sum(any(abs(tfr(:,(round(3*N/4)+2):N))>sqrt(eps))),
 error('tfrri test 13 failed');
end

% time localization
t0=30; sig= ((1:N)'==t0);
tfr=tfrri(sig);
[ik,jk]=find(tfr~=0.0);
if any(jk~=t0)|any(ik'-(1:N)),
 error('tfrri test 14 failed');
end;


function r8_to_r8_discrete_test ( )

%*****************************************************************************80
%
%% R8_TO_R8_DISCRETE_TEST tests R8_TO_R8_DISCRETE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 April 2009
%
%  Author:
%
%    John Burkardt
%
  ndx = 19;
  rhi = 10.0;
  rlo = 1.0;
  test_num = 15;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8_TO_R8_DISCRETE_TEST\n' );
  fprintf ( 1, '  R8_TO_R8_DISCRETE maps numbers to a discrete set\n' );
  fprintf ( 1, '  of equally spaced numbers in an interval.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of discrete values = %d\n', ndx );
  fprintf ( 1, '  Real interval: [%f, %f]\n', rlo, rhi );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  R   RD\n' );
  fprintf ( 1, '\n' );

  seed = 123456789;

  rlo2 = rlo - 2.0;
  rhi2 = rhi + 2.0;

  for test = 1 : test_num
    [ r, seed ] = r8_uniform_ab ( rlo2, rhi2, seed );
    rd = r8_to_r8_discrete ( r, rlo, rhi, ndx );
    fprintf ( 1, '  %14f  %14f\n', r, rd );
  end

  return
end


function [ y2, m2, d2, f2 ] = ymdf_prev_islamic ( y1, m1, d1, f1 )

%*****************************************************************************80
%
%% YMDF_PREV_ISLAMIC returns the Islamic YMDF date of the previous day.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 April 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y1, M1, D1, real F1,
%    the YMDF date.
%
%    Output, integer Y2, M2, D2, real F2,
%    yesterday's YMDF date.
%
  y2 = y1;
  m2 = m1;
  d2 = d1 - 1;
  f2 = f1;

  [ y2, m2, d2 ] = day_borrow_islamic ( y2, m2, d2 );

  return
end


function sF = rdivide(sF1, sF2)
%
% Syntax
%   sF = sF1/sF2
%   sF = sF1/a
%   sF = a/sF1
%
% Input
%  sF1, sF2 - @S2FunHarmonic
%  a - double
%
% Output
%  sF - @S2FunHarmonic
%

if isnumeric(sF1)
  f = @(v) sF1./sF2.eval(v);
  sF = S2FunHarmonic.quadrature(f);
elseif isnumeric(sF2)
  sF = sF1.*(1./sF2);
else
  f = @(v) sF1.eval(v)./sF2.eval(v);
  sF = S2FunHarmonic.quadrature(f);
end

end


function t_pnes_master(quiet)
%T_PNES_MASTER  Tests of PNE solvers via PNES_MASTER().

%   MP-Opt-Model
%   Copyright (c) 2010-2020, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%
%   This file is part of MP-Opt-Model.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://github.com/MATPOWER/mp-opt-model for more info.

if nargin < 1
    quiet = 0;
end

%%  alg         name        check       opts
cfg = {
    {'DEFAULT', 'default',  []          []  },
};

n = 188;

t_begin(n*length(cfg), quiet);

for k = 1:length(cfg)
    alg   = cfg{k}{1};
    name  = cfg{k}{2};
    check = cfg{k}{3};
    opts  = cfg{k}{4};
    if ~isempty(check) && ~have_feature(check)
        t_skip(n, sprintf('%s not installed', name));
    else
        %% default start point
        x0 = [-1;0;0];
        opt = struct( ...
            'verbose', 0, ...
            'alg', alg, ...
            'stop_at', 0.7, ...
            'parameterization', 1, ...
            'nleqs_opt', struct('tol', 1e-9), ...
            'adapt_step', 0, ...
            'step_max', 10, ...
            'target_lam_tol', 1e-6, ...
            'nose_tol', 1e-6, ...
            'adapt_step_tol', 1e-2 );
%         opt.plot = struct('level', 2, 'idx', 2);

        t = sprintf('%s - TARGET_LAM = 0.7 (natural) : ', name);
        [x, f, e, out, jac] = pnes_master(@f1p, x0, opt);
        it = 14;
        t_is(e, 1, 12, [t 'exitflag']);
        t_is(x, [-1.931782106; -1.268217894; 0.7], 6, [t 'x - final']);
        t_is(f, [0;0], 10, [t 'f']);
        t_is(out.x(:,1), [-3;4;0], 8, [t 'out.x(:,1)']);
        t_is(out.max_lam, 0.7, 10, [t 'out.max_lam']);
        t_is(out.iterations, it, 12, [t 'out.iterations']);
        t_is(size(out.lam), [1,it+1], 12, [t 'size(out.lam)']);
        t_is(size(out.lam_hat), [1,it+1], 12, [t 'size(out.lam_hat)']);
        t_is(size(out.x), [3,it+1], 12, [t 'size(out.x)']);
        t_is(size(out.x_hat), [3,it+1], 12, [t 'size(out.x_hat)']);
        t_is(size(out.steps), [1,it+1], 12, [t 'size(out.steps)']);
        t_is(length(out.events), 1, 12, [t 'length(out.events)']);
        t_is(out.events.k, it, 12, [t 'out.events.k']);
        t_is(out.events.idx, 1, 12, [t 'out.events.idx']);
        t_str_match(out.events.name, 'TARGET_LAM', [t 'out.events.name']);
        t_str_match(out.done_msg, sprintf('Reached desired lambda 0.7 in %d continuation steps', it), [t 'out.done_msg']);
        t_ok(isfield(out, 'corrector'), [t 'out.corrector exists']);
        t_is(out.corrector.iterations, 3, 12, [t 'out.corrector.iterations']);
        t_str_match(out.corrector.message, sprintf('Newton''s method converged in %d iterations.', 3), [t 'out.corrector.message']);

        t = sprintf('%s - TARGET_LAM = 0.7 (arc len) : ', name);
        opt.adapt_step = 1;
        opt.parameterization = 2;
        [x, f, e, out, jac] = pnes_master(@f1p, x0, opt);
        it = 9;
        t_is(e, 1, 12, [t 'exitflag']);
        t_is(x, [-1.931782106; -1.268217894; 0.7], 6, [t 'x - final']);
        t_is(f, [0;0], 10, [t 'f']);
        t_is(out.x(:,1), [-3;4;0], 8, [t 'out.x(:,1)']);
        t_is(out.max_lam, 0.7, 10, [t 'out.max_lam']);
        t_is(out.iterations, it, 12, [t 'out.iterations']);
        t_is(size(out.lam), [1,it+1], 12, [t 'size(out.lam)']);
        t_is(size(out.lam_hat), [1,it+1], 12, [t 'size(out.lam_hat)']);
        t_is(size(out.x), [3,it+1], 12, [t 'size(out.x)']);
        t_is(size(out.x_hat), [3,it+1], 12, [t 'size(out.x_hat)']);
        t_is(size(out.steps), [1,it+1], 12, [t 'size(out.steps)']);
        t_is(length(out.events), 1, 12, [t 'length(out.events)']);
        t_is(out.events.k, it, 12, [t 'out.events.k']);
        t_is(out.events.idx, 1, 12, [t 'out.events.idx']);
        t_str_match(out.events.name, 'TARGET_LAM', [t 'out.events.name']);
        t_str_match(out.done_msg, sprintf('Reached desired lambda 0.7 in %d continuation steps', it), [t 'out.done_msg']);

        t = sprintf('%s - TARGET_LAM = 0.7 (pseudo arc len) : ', name);
        opt.parameterization = 3;
        [x, f, e, out, jac] = pnes_master(@f1p, x0, opt);
        it = 9;
        t_is(e, 1, 12, [t 'exitflag']);
        t_is(x, [-1.931782106; -1.268217894; 0.7], 6, [t 'x - final']);
        t_is(f, [0;0], 10, [t 'f']);
        t_is(out.x(:,1), [-3;4;0], 8, [t 'out.x(:,1)']);
        t_is(out.max_lam, 0.7, 10, [t 'out.max_lam']);
        t_is(out.iterations, it, 12, [t 'out.iterations']);
        t_is(size(out.lam), [1,it+1], 12, [t 'size(out.lam)']);
        t_is(size(out.lam_hat), [1,it+1], 12, [t 'size(out.lam_hat)']);
        t_is(size(out.x), [3,it+1], 12, [t 'size(out.x)']);
        t_is(size(out.x_hat), [3,it+1], 12, [t 'size(out.x_hat)']);
        t_is(size(out.steps), [1,it+1], 12, [t 'size(out.steps)']);
        t_is(length(out.events), 1, 12, [t 'length(out.events)']);
        t_is(out.events.k, it, 12, [t 'out.events.k']);
        t_is(out.events.idx, 1, 12, [t 'out.events.idx']);
        t_str_match(out.events.name, 'TARGET_LAM', [t 'out.events.name']);
        t_str_match(out.done_msg, sprintf('Reached desired lambda 0.7 in %d continuation steps', it), [t 'out.done_msg']);

        t = sprintf('%s - FULL : ', name);
        opt.stop_at = 'FULL';
        p = struct('fcn', @f1p, 'x0', x0, 'opt', opt);
        [x, f, e, out, jac] = pnes_master(p);
        it = 34;
        t_is(e, 1, 12, [t 'exitflag']);
        t_is(x, [2;-1;0], 10, [t 'x - final']);
        t_is(f, [0;0], 10, [t 'f']);
        t_is(out.x(:,1), [-3;4;0], 8, [t 'out.x(:,1)']);
        t_is(out.max_lam, 1.04127275, 8, [t 'out.max_lam']);
        t_is(out.iterations, it, 12, [t 'out.iterations']);
        t_is(size(out.lam), [1,it+1], 12, [t 'size(out.lam)']);
        t_is(size(out.lam_hat), [1,it+1], 12, [t 'size(out.lam_hat)']);
        t_is(size(out.x), [3,it+1], 12, [t 'size(out.x)']);
        t_is(size(out.x_hat), [3,it+1], 12, [t 'size(out.x_hat)']);
        t_is(size(out.steps), [1,it+1], 12, [t 'size(out.steps)']);
        t_is(length(out.events), 1, 12, [t 'length(out.events)']);
        t_is(out.events.k, it, 12, [t 'out.events.k']);
        t_is(out.events.idx, 1, 12, [t 'out.events.idx']);
        t_str_match(out.events.name, 'TARGET_LAM', [t 'out.events.name']);
        t_str_match(out.done_msg, sprintf('Traced full continuation curve in %d continuation steps', it), [t 'out.done_msg']);

        t = sprintf('%s - FULL (max_it) : ', name);
        opt.max_it = 25;
        p = struct('fcn', @f1p, 'x0', x0, 'opt', opt);
        [x, f, e, out, jac] = pnes_master(p);
        opt.max_it = 2000;
        it = 25;
        t_is(e, 1, 12, [t 'exitflag']);
        t_is(x, [0.64152370; -4.588447338; 0.82448727], 8, [t 'x - final']);
        t_is(f, [0;0], 8, [t 'f']);
        t_is(out.x(:,1), [-3;4;0], 8, [t 'out.x(:,1)']);
        t_is(out.max_lam, 1.04127275, 8, [t 'out.max_lam']);
        t_is(out.iterations, it, 12, [t 'out.iterations']);
        t_is(size(out.lam), [1,it+1], 12, [t 'size(out.lam)']);
        t_is(size(out.lam_hat), [1,it+1], 12, [t 'size(out.lam_hat)']);
        t_is(size(out.x), [3,it+1], 12, [t 'size(out.x)']);
        t_is(size(out.x_hat), [3,it+1], 12, [t 'size(out.x_hat)']);
        t_is(size(out.steps), [1,it+1], 12, [t 'size(out.steps)']);
        t_is(length(out.events), 0, 12, [t 'length(out.events)']);
        t_str_match(out.done_msg, sprintf('Reached maximun number of continuation steps (opt.max_it = %d)', it), [t 'out.done_msg']);

        t = sprintf('%s - NOSE (arc len): ', name);
        p.opt.stop_at = 'NOSE';
        p.opt.parameterization = 2;
        [x, f, e, out, jac] = pnes_master(p);
        it = 18;
        t_is(e, 1, 12, [t 'exitflag']);
        t_is(x, [-0.5; -4.75; 1.04166667], 6, [t 'x - final']);
        t_is(f, [0;0], 10, [t 'f']);
        t_is(out.x(:,1), [-3;4;0], 8, [t 'out.x(:,1)']);
        t_is(out.max_lam, 1.04166666667, 10, [t 'out.max_lam']);
        t_is(out.iterations, it, 12, [t 'out.iterations']);
        t_is(size(out.lam), [1,it+1], 12, [t 'size(out.lam)']);
        t_is(size(out.lam_hat), [1,it+1], 12, [t 'size(out.lam_hat)']);
        t_is(size(out.x), [3,it+1], 12, [t 'size(out.x)']);
        t_is(size(out.x_hat), [3,it+1], 12, [t 'size(out.x_hat)']);
        t_is(size(out.steps), [1,it+1], 12, [t 'size(out.steps)']);
        t_is(length(out.events), 1, 12, [t 'length(out.events)']);
        t_is(out.events.k, it, 12, [t 'out.events.k']);
        t_is(out.events.idx, 1, 12, [t 'out.events.idx']);
        t_str_match(out.events.name, 'NOSE', [t 'out.events.name']);
        t_str_match(out.done_msg, sprintf('Reached limit in %d continuation steps, lambda = 1.042.', it), [t 'out.done_msg']);

        t = sprintf('%s - NOSE (pseudo arc len) : ', name);
        p.opt.parameterization = 3;
        [x, f, e, out, jac] = pnes_master(p);
        it = 18;
        t_is(e, 1, 12, [t 'exitflag']);
        t_is(x, [-0.5; -4.75; 1.04166667], 6, [t 'x - final']);
        t_is(f, [0;0], 10, [t 'f']);
        t_is(out.x(:,1), [-3;4;0], 8, [t 'out.x(:,1)']);
        t_is(out.max_lam, 1.04166666667, 10, [t 'out.max_lam']);
        t_is(out.iterations, it, 12, [t 'out.iterations']);
        t_is(size(out.lam), [1,it+1], 12, [t 'size(out.lam)']);
        t_is(size(out.lam_hat), [1,it+1], 12, [t 'size(out.lam_hat)']);
        t_is(size(out.x), [3,it+1], 12, [t 'size(out.x)']);
        t_is(size(out.x_hat), [3,it+1], 12, [t 'size(out.x_hat)']);
        t_is(size(out.steps), [1,it+1], 12, [t 'size(out.steps)']);
        t_is(length(out.events), 1, 12, [t 'length(out.events)']);
        t_is(out.events.k, it, 12, [t 'out.events.k']);
        t_is(out.events.idx, 1, 12, [t 'out.events.idx']);
        t_str_match(out.events.name, 'NOSE', [t 'out.events.name']);
        t_str_match(out.done_msg, sprintf('Reached limit in %d continuation steps, lambda = 1.042.', it), [t 'out.done_msg']);

        t = sprintf('%s - NOSE (opp dir) : ', name);
        p.x0 = [1;-1;0];
        [x, f, e, out, jac] = pnes_master(p);
        it = 20;
        t_is(e, 1, 12, [t 'exitflag']);
        t_is(x, [-0.5; -4.75; 1.04166667], 5, [t 'x - final']);
        t_is(f, [0;0], 10, [t 'f']);
        t_is(out.x(:,1), [2;-1;0], 8, [t 'out.x(:,1)']);
        t_is(out.max_lam, 1.04166666667, 10, [t 'out.max_lam']);
        t_is(out.iterations, it, 12, [t 'out.iterations']);
        t_is(size(out.lam), [1,it+1], 12, [t 'size(out.lam)']);
        t_is(size(out.lam_hat), [1,it+1], 12, [t 'size(out.lam_hat)']);
        t_is(size(out.x), [3,it+1], 12, [t 'size(out.x)']);
        t_is(size(out.x_hat), [3,it+1], 12, [t 'size(out.x_hat)']);
        t_is(size(out.steps), [1,it+1], 12, [t 'size(out.steps)']);
        t_is(length(out.events), 1, 12, [t 'length(out.events)']);
        t_is(out.events.k, it, 12, [t 'out.events.k']);
        t_is(out.events.idx, 1, 12, [t 'out.events.idx']);
        t_str_match(out.events.name, 'NOSE', [t 'out.events.name']);
        t_str_match(out.done_msg, sprintf('Reached limit in %d continuation steps, lambda = 1.042.', it), [t 'out.done_msg']);

        t = sprintf('%s - FULL warmstart, before SWITCH : ', name);
        opt.parameterization = 3;
        opt.adapt_step = 1;
        opt.adapt_step_ws = 0.25;
        opt.stop_at = 'FULL';
        opt.adapt_step_tol = 5e-3;
        opt.callbacks = {@pne_callback_test1};
        opt.events = {{'SWITCH!', @pne_event_test1, 1e-6}};
        opt.output_fcn = @pne_output_fcn_test1;
        % opt.verbose = 4;
        % opt.plot = struct('level', 2, 'yname', 'y', 'idx', [1;2]);
        x0 = [-1;0;0];
        [x, f, e, out, jac] = pnes_master(@f1p, x0, opt);
        it = 10;
        t_is(e, 1, 12, [t 'exitflag']);
        t_is(x, [-2; -1; 2/3], 6, [t 'x - final']);
        t_is(f, [0;0], 10, [t 'f']);
        t_ok(isfield(out, 'warmstart') && ~isempty(out.warmstart), [t 'out.warmstart exists']);
        t_is(out.max_lam, 2/3, 10, [t 'out.max_lam']);
        t_is(out.iterations, it, 12, [t 'out.iterations']);
        t_is(size(out.lam), [1,it+1], 12, [t 'size(out.lam)']);
        t_is(size(out.lam_hat), [1,it+1], 12, [t 'size(out.lam_hat)']);
        t_is(size(out.y), [2,it+1], 12, [t 'size(out.y)']);
        t_is(size(out.y_hat), [2,it+1], 12, [t 'size(out.y_hat)']);
        t_is(size(out.steps), [1,it+1], 12, [t 'size(out.steps)']);
        t_ok(isstruct(out.events), [t 'out.events is struct']);
        t_is(out.events.k, it, 12, [t 'out.events.k']);
        t_is(out.events.idx, 1, 12, [t 'out.events.idx']);
        t_str_match(out.events.name, 'SWITCH!', [t 'out.events.name']);
        t_str_match(out.events.msg, 'ZERO detected for SWITCH! event', [t 'out.events.msg']);
        t_is(out.warmstart.cont_steps, it, 12, [t 'out.warmstart.cont_steps']);
        t_is(out.warmstart.default_step, 0.64432407, 8, [t 'out.warmstart.default_step']);
        t_ok(isa(out.warmstart.parm, 'function_handle'), [t 'out.warmstart.parm is function']);
        t_ok(isa(out.warmstart.default_parm, 'function_handle'), [t 'out.warmstart.default_parm is function']);
        t_ok(isstruct(out.warmstart.cbs), [t 'out.warmstart.cbs is struct']);
        t_ok(isstruct(out.warmstart.cbs.default), [t 'out.warmstart.cbs.default is struct']);
        t_is(out.warmstart.cbs.default.iterations, it, 12, [t 'out.warmstart.cbs.default.iterations']);
        t_ok(isstruct(out.warmstart.events), [t 'out.warmstart.events is struct']);
        t_is(out.warmstart.events.k, it, 12, [t 'out.warmstart.events.k']);
        t_is(out.warmstart.events.idx, 1, 12, [t 'out.warmstart.events.idx']);
        t_str_match(out.warmstart.events.name, 'SWITCH!', [t 'out.warmstart.events.name']);
        t_str_match(out.warmstart.events.msg, 'ZERO detected for SWITCH! event', [t 'out.warmstart.events.msg']);
        t_str_match(out.done_msg, sprintf('Reached switching point in %d continuation steps', it), [t 'out.done_msg']);
        t_ok(isfield(out, 'corrector'), [t 'out.corrector exists']);
        t_is(out.corrector.iterations, 3, 12, [t 'out.corrector.iterations']);
        t_str_match(out.corrector.message, sprintf('Newton''s method converged in %d iterations.', 3), [t 'out.corrector.message']);

        t = sprintf('%s - FULL warmstart, after SWITCH : ', name);
        ws = out.warmstart;
        ws.x  = [ws.x(end)/2;  ws.x];
        ws.xp = [ws.xp(end)/2; ws.xp];
        ws.z = [0; ws.z];
        ws.zp = [0; ws.zp];
        x0 = ws.x;      %% ignored for warm start
        opt.warmstart = ws;
        opt.events = {{'SNOUT!', @pne_event_test2, 1e-6}};
        opt.callbacks = {};
        [x, f, e, out, jac] = pnes_master(@f2p, x0, opt);
        it = 49;
        t_is(e, 1, 12, [t 'exitflag']);
        t_is(x, [0; 6; -5; 0], 6, [t 'x - final']);
        t_is(f, [0;0;0], 10, [t 'f']);
        t_is(out.y(:,1), [-3;4], 8, [t 'out.y(:,1)']);
        t_is(out.max_lam, 1.04166666667, 10, [t 'out.max_lam']);
        t_is(out.iterations, it, 12, [t 'out.iterations']);
        t_is(size(out.lam), [1,it+1], 12, [t 'size(out.lam)']);
        t_is(size(out.lam_hat), [1,it+1], 12, [t 'size(out.lam_hat)']);
        t_is(size(out.y), [2,it+1], 12, [t 'size(out.y)']);
        t_is(size(out.y_hat), [2,it+1], 12, [t 'size(out.y_hat)']);
        t_is(size(out.steps), [1,it+1], 12, [t 'size(out.steps)']);
        t_is(length(out.events), 3, 12, [t 'length(out.events)']);
        t_is(out.events(1).k, 10, 12, [t 'out.events(1).k']);
        t_is(out.events(1).idx, 1, 12, [t 'out.events(1).idx']);
        t_str_match(out.events(1).name, 'SWITCH!', [t 'out.events(1).name']);
        t_str_match(out.events(1).msg, 'ZERO detected for SWITCH! event', [t 'out.events(1).msg']);
        t_is(out.events(2).k, 28, 12, [t 'out.events(2).k']);
        t_is(out.events(2).idx, 1, 12, [t 'out.events(2).idx']);
        t_str_match(out.events(2).name, 'SNOUT!', [t 'out.events(2).name']);
        t_str_match(out.events(2).msg, 'ZERO detected for SNOUT! event', [t 'out.events(2).msg']);
        t_is(out.events(3).k, it, 12, [t 'out.events(3).k']);
        t_is(out.events(3).idx, 1, 12, [t 'out.events(3).idx']);
        t_str_match(out.events(3).name, 'TARGET_LAM', [t 'out.events(3).name']);
        t_str_match(out.events(3).msg, 'ZERO detected for TARGET_LAM event', [t 'out.events(3).msg']);
        t_str_match(out.done_msg, sprintf('Traced full continuation curve in %d continuation steps', it), [t 'out.done_msg']);
        t_ok(isfield(out, 'corrector'), [t 'out.corrector exists']);
        t_is(out.corrector.iterations, 3, 12, [t 'out.corrector.iterations']);
        t_str_match(out.corrector.message, sprintf('Newton''s method converged in %d iterations.', 3), [t 'out.corrector.message']);
    end
end

t_end;

% lam = 1;
% m = 6;
% xs = [-m:0.2:m];
% ys = xs;
% [xx, yy] = meshgrid(xs, ys);
% zz0 = zeros([size(xx), 2]);
% zz = zz0;
% for i = 1:length(xs)
%     for j = 1:length(ys)
%         zz(i, j, :) = f1p([xx(i, j); yy(i, j); lam]);
% %         zz(i, j, :) = f2([xx(i, j); yy(i, j)]);
%     end
% end
%
% figure
% ax = gca;
% surf(ax, xx, yy, squeeze(zz(:, :, 1)))
% hold on
% surf(ax, xx, yy, squeeze(zz(:, :, 2)))
% surf(ax, m*[-1 -1; 1 1], m*[-1 1; -1 1], [0 0; 0 0])
% hold off

%% 2-d problem with 2 solutions
%% from https://www.chilimath.com/lessons/advanced-algebra/systems-non-linear-equations/
function [f, J] = f1(x)
f = [  x(1)   + x(2) - 1;
      -x(1)^2 + x(2) + 5    ];
if nargout > 1
    J = [1 1; -2*x(1) 1];
end

%% parameterized 2-d problem
%% based on https://www.chilimath.com/lessons/advanced-algebra/systems-non-linear-equations/
function [f, J] = f1p(x)
if nargout < 2
    f = f1(x(1:2));
else
    [f, J] = f1(x(1:2));
    J = [J [6;0]];
end
f = f + [6*x(3); 0];

%% parameterized 3-d problem, based on f1p
function [f, J] = f2p(x)
x1 = [x(3)+2; x(2)-2; x(4)];
if nargout < 2
    f = [2*x(1)-x(4); f1p(x1)];
else
    [f, JJ] = f1p(x1);
    J = [  2      0        0       -1;
          [0; 0] JJ(:, 2) JJ(:, 1) JJ(:, 3) ];
end
f = [ 2*x(1)-x(4); f ];

%% example custom event function 1 (target lambda == 2/3)
function efv = pne_event_test1(cx, opt)
tlam = 2/3;
efv = cx.x(end) - tlam;

%% example custom event function 2 (nose point)
function efv = pne_event_test2(cx, opt)
efv = cx.z(end);

%% example custom callback function (exit for warmstart on SWITCH! event)
function [nx, cx, s] = pne_callback_test1(k, nx, cx, px, s, opt)
if k <= 0 || s.done, return; end    %% skip if initialize, finalize or done
tlam = 2/3;
ev = pne_detected_event(s.events, 'SWITCH!');
if ~isempty(ev)
    if ev.zero              %% prepare to terminate
        s.done = 1;
        s.done_msg = sprintf('Reached switching point in %d continuation steps', k);
        s.warmstart = struct(); %% signal that we want to exit, then resume
    else                    %% set step-size & parameterization to terminate next time
        cx.this_parm = @pne_pfcn_natural;   %% change to natural parameterization
        cx.this_step = tlam - cx.x(end);
        ev.msg = sprintf('%s\n  step %d to overshoot lambda = %g, reduce step size and set natural param', ev.msg, k, tlam);
    end
end

function [names, vals] = pne_output_fcn_test1(x, x_hat)
%% [names, vals] = pne_output_fcn_default(x, x_hat)
%% names = pne_output_fcn_default()
names = {'y_hat', 'y'};
if nargin
    if length(x) == 3
        k = [1;2];
    else %% == 3
        k = [2;3];
    end
    vals = {x_hat(k), x(k)};
end

% %% another 2-d problem
% %% from Christi Patton Luks, https://www.youtube.com/watch?v=pJG4yhtgerg
% function [f, J] = f2(x)
% m = 10;
% m1 = 0; %2;
% m2 = 0; %5;
% f = [  x(1)^2 +   x(1)*x(2)   - 10 + m1;
%        (x(2)   + 3*x(1)*x(2)^2 - 57) / m  ] + m2;
% if nargout > 1
%     J = sparse([    2*x(1)+x(2)    x(1);
%                     (3*x(2)^2) / m (6*x(1)*x(2)+1) / m    ]);
% end


function V = variablesPack(X, I, U, P, H)

N = length(H);
nx = size(X,1);
ni = size(I,1);
np = size(P,1);

nv = nvars(N, nx, ni, nu, np);

[X_indizes, I_indizes, U_indizes, P_indizes, H_indizes] = ocl.simultaneous.indizes(N, nx, ni, nu, np);

V = zeros(nv, 1);
V(X_indizes) = X;
V(I_indizes) = I;
V(U_indizes) = U;
V(P_indizes) = P;
V(H_indizes) = H;


% Test file for @chebfun/times.m.

function pass = test_times(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;

% Random numbers to use as arbitrary multiplicative constants.
alpha = -0.194758928283640 + 0.075474485412665i;

%% SCALAR-VALUED

% Check operation for empty inputs.
f = chebfun(@sin, [-1 1], pref);
pass(1) = isempty(f.*[]);
pass(2) = isempty(f.*chebfun());

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

% Test multiplication by scalars.
f1_op = @(x) sin(x).*abs(x - 0.1);
f1 = chebfun(f1_op, pref);
pass(3:4) = test_mult_function_by_scalar(f1, f1_op, alpha, x);

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

%% ARRAY-VALUED

% 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_mult_function_by_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_mult_function_by_function(f2, f2_op, g2, g2_op, x);

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

%% QUASIMATRICES
f2q = quasimatrix(f2_op, pref);
pass(15:16) = test_mult_function_by_scalar(f2q, f2_op, alpha, x);

% Quasi * array-cheb
pass(17:18) = test_mult_function_by_function(f2q, f2_op, g2, g2_op, x);

% Quasi * quasi
g2q = quasimatrix(g2_op, pref);
pass(19:20) = test_mult_function_by_function(f2q, f2_op, g2q, g2_op, x);

%% Check error conditions.

try
    h = f1.*'X';
    pass(21) = false;
catch ME
    pass(21) = strcmp(ME.identifier, 'CHEBFUN:CHEBFUN:times:unknown');
end

try
    h = f1.*f1.';
    pass(22) = false;
catch ME
    pass(22) = strcmp(ME.identifier, 'CHEBFUN:CHEBFUN:times:matdim');
end

try
    h = f1.*g2q.';
    pass(23) = false;
catch ME
    pass(23) = strcmp(ME.identifier, 'CHEBFUN:CHEBFUN:times:matdim');
end

%% 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);

%% Case of a scalar and a function:
c = 3;
pow = -0.5;
op = @(x) (x - dom(2)).^pow.*sin(x);
op_exact = @(x) c*(x - dom(2)).^pow.*sin(x);
f = chebfun(op, dom, 'exps', [0 pow], 'splitting', 'on');
g = c.*f;
g_exact = chebfun(op_exact, dom, 'exps', [0 pow], 'splitting', 'on');

err = norm(feval(g, x) - feval(g_exact, x), inf);
pass(24) = ( err < 50*eps*norm(feval(g_exact, x), inf) );

%% Case of two functions: piecewise smooth chebfun - splitting on.
pow1 = -0.3;
pow2 = -0.5;
op1 = @(x) (x - dom(2)).^pow1.*sin(100*x);
op2 = @(x) (x - dom(2)).^pow2.*cos(300*x);
op_exact = @(x) (x - dom(2)).^(pow1+pow2).*sin(100*x).*cos(300*x);
f = chebfun(op1, dom, 'exps', [0 pow1], 'splitting', 'on');
g = chebfun(op2, dom, 'exps', [0 pow2] , 'splitting', 'on');
h = f.*g;
h_exact = chebfun(op_exact, dom, 'exps', [0 pow1+pow2], 'splitting', 'on');

err = norm(feval(h, x) - feval(h_exact, x), inf);
pass(25) = ( err < 1e4*eps*...
    norm(feval(h_exact, x), inf) );

%% Tests 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);

opf = @(x) x.^2.*exp(-x.^2);
opg = @(x) (1-exp(-x.^2))./x;
oph = @(x) x.*exp(-x.^2).*(1-exp(-x.^2));
f = chebfun(opf, dom);
g = chebfun(opg, dom);
h = f.*g;
hVals = feval(h, x);
hExact = oph(x);
err = hVals - hExact;
pass(26) = norm(err, inf) < 10*eps*get(f,'vscale');

%% Test multiplication between a CHEBFUN and a TRIGFUN.

dom = [0 pi 2*pi];

% 1. One column case.
f = chebfun(@(x) x.^2, dom, pref);
g = chebfun(@(x) cos(x), [dom(1) dom(end)], 'periodic');
h1 = f.*g;
% We want the result to use the same tech as the one used by f.
pass(27) = strcmpi(func2str(get(h1.funs{1}.onefun, 'tech')), ...
                   func2str(get(f.funs{1}.onefun, 'tech')));
h2 = chebfun(@(x) (x.^2).*cos(x), dom, pref);
pass(28) = norm(h1-h2, inf) < 1e1*eps*get(h2,'vscale');

% 2. Quasimatrix case.
f = chebfun(@(x) [cos(x), sin(x)], [dom(1) dom(end)], 'periodic');
g = chebfun(@(x) [x, x.^3], dom, pref);
h1 = f.*g;
% We want the result to use the same tech as the one used by g.
pass(29) = strcmpi(func2str(get(h1(:,1).funs{1}.onefun, 'tech')), ...
                   func2str(get(g(:,1).funs{1}.onefun, 'tech')));
pass(30) = strcmpi(func2str(get(h1(:,2).funs{1}.onefun, 'tech')), ...
                   func2str(get(g(:,2).funs{1}.onefun, 'tech')));
h2 = chebfun(@(x) [x.*cos(x), x.^3.*sin(x)], dom, pref);
pass(31) = norm(h1-h2, inf) < 1e2*eps*get(h2,'vscale');

end

%% The tests

% Test the multiplication of a chebfun F, specified by F_OP, by a scalar ALPHA
% using a grid of points X in [-1  1] for testing samples.
function result = test_mult_function_by_scalar(f, f_op, alpha, x)
    g1 = f .* alpha;
    g2 = alpha .* f;
    result(1) = isequal(g1, g2);
    g_exact = @(x) f_op(x) .* alpha;
    err = feval(g1, x) - g_exact(x);
    result(2) = norm(err(:), inf) < 1e2*max(vscale(g1)*eps);

end

% Test the multiplication of two chebfuns F and G, specified by F_OP and G_OP,
% using a grid of points X in [-1  1] for testing samples.
function result = test_mult_function_by_function(f, f_op, g, g_op, x)
    h1 = f .* g;
    h2 = g .* f;
    result(1) = norm(h1 - h2) < 10*max(vscale(h1)*eps);
    h_exact = @(x) f_op(x) .* g_op(x);
    err = feval(h1, x) - h_exact(x);
    result(2) = norm(err(:), inf) < 1e2*max(vscale(h1)*eps);

end


%+========================================================================+
%|                                                                        |
%|            This script uses the GYPSILAB toolbox for Matlab            |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2018.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : nrtRayTheatre.m                               |
%|    #    |   VERSION    : 0.41                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 01.04.2018                                    |
%| ( === ) |   SYNOPSIS   : Ray tracing with theatre                      |
%|  `---'  |                                                              |
%+========================================================================+

% Cleaning
clear all
close all
clc

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

% Parameters
Xsrc  = [0 8 1.7];
Xmes  = [0 -20 8];
Nray  = 1e5
rad   = 1

% Read mesh
mesh        = msh('theatre.mesh');
mesh.col(:) = 100; % Rough concrete (Bobran, 1973)

% Extract toit
ctr = mesh.ctr;
mesh = mesh.sub(ctr(:,3)<14);

% Initialize ray
ray1 = ray(mesh,Xsrc,Nray);

% Graphical sphere
[X,Y,Z] = sphere(50);
X = Xmes(1) + rad*X; Y = Xmes(2) + rad*Y; Z = Xmes(3) + rad*Z;

% Graphical representation
figure
plot(mesh,'w')
hold on
plot(ray1)
surf(X,Y,Z,ones(size(X)))
axis equal
xlabel('X');   ylabel('Y');   zlabel('Z');
view(0,45)
alpha(0.5)

% Maximum distances
r1000 = rad/2 * sqrt(Nray/1000);
r100  = rad/2 * sqrt(Nray/100);
r10   = rad/2 * sqrt(Nray/10);
rMax  = rad/2 * sqrt(Nray/2);

% Ray-tracing
tic
ray1 = ray1.tracer(30,rMax);
toc
% plot(ray1)

% Images sources
tic
[img,nrg] = ray1.image(Xmes,rad,rMax);
toc

% Data
r   = sqrt(sum(img.^2,2));
sol = mean(nrg,2);

% Impacts
ray2 = ray(mesh,Xmes,img);
ray2 = ray2.tracer;
plot(ray2)
plot(msh(ray2.pos{2},(1:length(ray2))',10*log10(sol)));
axis equal
colorbar

% Energy in dB
figure
plot(r,10*log10(sol),'+b')
hold on
plot([r1000 r1000],[-60,0],'k--')
text(r1000,-55,' n = 1000')
plot([r100 r100],[-60,0],'k--')
text(r100,-57,' n = 100')
plot([r10 r10],[-60,0],'k--')
text(r10,-55,' n = 10')
plot([rMax rMax],[-60,0],'k--')
text(rMax,-55,' n = 1')
xlabel('Distance source mesure')
ylabel('Energie mesuree (dB)')
title('Energie mesuree selon la distance de mesure')
grid on

% Audio file
[audio,fs] = audioread('anechoicVoice.wav');

% Fir from images
T = floor(r/340*fs) + 1;
for i = 1:size(nrg,2)
    fir8(:,i) = accumarray(T,nrg(:,i),[max(T) 1]);
end

% Bank filtering
dir8 = ray1.bank(256,fs);
rir  = 0;
for i = 1:size(dir8,2)
    rir = rir + fftfilt(dir8(:,i),fir8(:,i));
end

% Graphical representation
figure
plot(rir)

% Audio rendering (5 seconds)
out = fftfilt(rir,audio(1:5*fs));
% sound(out,fs)

disp('~~> Michto gypsilab !')


function [caNodeIndices, vResolution_] = ex_CreateIndexCatalog(mCatalog, mPolygon, bMap, nGriddingMode, nNumberEvents, fRadius, fSizeRectHorizontal, fSizeRectDepth)
% function [caNodeIndices] = ex_CreateIndexCatalog(mCatalog, mPolygon, bMap, nGriddingMode,
%                                                  nNumberEvents, fRadius, fSizeRectHorizontal, fSizeRectDepth)
% -------------------------------------------------------------------------------------------------------------
% Creates a cell-array with subcatalogs for every grid node defined by mPolygon. These subcatalogs
%   contain only indices to the earthquake "rows" in mCatalog.
%
% Input parameters:
%   mCatalog              Earthquake catalog
%   mPolygon              Polygon (defined by ex_selectgrid)
%   bMap                  Calculate cell-array for a map (=1) or a cross-section (=0)
%   nGriddingMode         Mode of creating grid node subcatalogs
%                         0: Constant number of events
%                         1: Constant radius
%                         2: Rectangular grid node samples
%   nNumberEvents         Number of events per grid node (nGriddingMode == 0)
%   fRadius               Radius of grid node sample (nGriddingMode == 1)
%   fSizeRectHorizontal   Latitude/horizontal size of rectangle (nGriddingMode == 2)
%   fSizeRectDepth        Longitude/depth size of rectangle (nGriddingMode == 2)
%
% Output parameters:
%   caNodeIndices         Cell-array with index-catalogs per grid node of mPolygon
%
% Danijel Schorlemmer
% June 17, 2002

global bDebug;
if bDebug
  report_this_filefun(mfilename('fullpath'));
end

% Create the catalogs for each node with pointers to the overall catalog
nNumberNodes_ = length(mPolygon(:,1));
caNodeIndices = cell(nNumberNodes_, 1);

% If cross-section calculate the length along cross-section
if ~bMap
  [nRow_, nColumn_] = size(mCatalog);
  vXSecX_ = mCatalog(:,nColumn_);  % length along x-section
  vXSecY_ = (-1) * mCatalog(:,7);  % depth of hypocenters
end

% vResolution give the radius (for nGriddingMode = 0) and no. of events
% (for nGriddingMode = 1)
vResolution_(:,1)=ones(nNumberNodes_,1)*nan;

% Loop over all points of the polygon
for nNode_ = 1:nNumberNodes_
  % Get the grid node coordinates
  fX_ = mPolygon(nNode_, 1);
  fY_ = mPolygon(nNode_, 2);

  if (nGriddingMode == 0) | (nGriddingMode == 1)  % Fixed radius or fixed number
    % Calculate distance from center point
    if bMap
      vDistances_ = sqrt(((mCatalog(:,1)-fX_)*cos(pi/180*fY_)*111).^2 + ((mCatalog(:,2)-fY_)*111).^2);
    else
      vDistances_ = sqrt(((vXSecX_ - fX_)).^2 + ((vXSecY_ - fY_)).^2);
    end
    if nGriddingMode == 0 % Fixed number
      if length(mCatalog(:,1)) == 0
        caNodeIndices{nNode_} = [];
        % NaN for no events
        vResolution_(nNode_) = nan;
      elseif nNumberEvents > length(mCatalog(:,1))
        caNodeIndices{nNode_} = vIndices(1:length(mCatalog(:,1)));
        % take the maximal distance for all eq. in the catalog
        vResolution_(nNode_) = max(vdistances_);
      else
        % Use first nNumberEvents events
        [vTmp, vIndices] = sort(vDistances_);
        caNodeIndices{nNode_} = vIndices(1:nNumberEvents);
        % radius of the nNumberEvents-th event in the sorted vDistances_
        vResolution_(nNode_) = vTmp(nNumberEvents);
      end
    else % Fixed radius
      % Use all events within fRadius
      caNodeIndices{nNode_} = find(vDistances_ <= fRadius);
      vResolution_(nNode_) = length(find(vDistances_ <= fRadius));
    end
  else % Rectangular gridding (nGriddingMode == 2)
    if bMap
      vSel_ = ((mCatalog(:,1) >= (fX_ - fSizeRectHorizontal/2)) & (mCatalog(:,1) < (fX_ + fSizeRectHorizontal/2)) & ...
        (mCatalog(:,2) >= (fY_ - fSizeRectDepth/2)) & (mCatalog(:,2) < (fY_ + fSizeRectDepth/2)));
      vResolution_(nNode_) = length(find(vSel_ > 0))
    else
      vSel_ = ((vXSecX_ >= (fX_ - fSizeRectHorizontal/2)) & (vXSecX_ < (fX_ + fSizeRectHorizontal/2)) & ...
        (vXSecY_ >= (fY_ - fSizRectDepth/2)) & (vXSecY_ < (fY_ + fSizeRectDepth/2)));
      vResolution_(nNode_) = length(find(vSel_ > 0))
    end
    caNodeIndices{nNode_} = find(vSel_ == 1);
  end
end; % of for nNode_


function [ kids, ntypelist, layers ] = roomhierarchy( data, RELATIONS, NODETYPES )
%ROOMHIERARCHY

obblist = data.obblist;
Rmat = data.Rmat;
Dmat = data.Dmat;
labellist = data.labellist;
sur = data.sur;
objnum = size(obblist,2);

%% build support tree
supportparent = zeros(size(obblist,2),1);
for i = 1:objnum
    rlist = Rmat(:,i);
    pp = find(rlist==RELATIONS.SUPPORT1);
    if(length(pp)>1)
        pd = Dmat(i,pp);
        [~,I] = min(pd);
        supportparent(i) = pp(I);
    end
    if(length(pp)==1)
        supportparent(i) = pp;
    end
end

%% compute the layer values
% nobjlist = length(supportparent);% add the floor
% layers = zeros(size(obblist,2),1);
% layers(nobjlist(1)) = 1;
% while(length(nobjlist)>0)
%     i = nobjlist(1);
%     nobjlist = nobjlist(2:end);
%     ind = find(supportparent==i);
%     nobjlist = [nobjlist;ind];
%     layers(ind) = layers(i)+1;
% end

%% split by surround relation
parent = [supportparent(:)';zeros(1,length(supportparent))+NODETYPES.SUPPORT];
flooridx = strmatch('floor',labellist,'exact');
parent(1,flooridx) = 0;
% a central object may be involved with multiple surround relations,
% but it can have only one parent
for i = 1:objnum
    % find the possible parents, sur pair id, distance in the relation
    pdlist = [];
    for j = 1:length(sur)
        slist = sur{j};
        if(slist(1)==i&&length(slist)>2)
            t = tabulate(supportparent(slist(2:end)));
            if(size(t,2)>1)
                [~,pp] = max(t(:,2));
            else
                pp = 1;
            end
            pd = [t(pp,1),j,Dmat(slist(1),slist(2))];
            pdlist = [pdlist;pd];
        end
    end
    surind = [];
    if(size(pdlist,1)>0)
        ind = find(pdlist(:,1)==parent(1,i));
        surind = pdlist(ind,2);
    end

    if(length(surind)>1)
        slist = sur{surind(1)};
        surchildlist = slist(2:end);
        innodelist = [];
        for j = 1:length(surchildlist)
            plist = [surchildlist(j)];
            for k = 2:length(surind)
                sklist = sur{surind(k)};
                sklist = sklist(2:end);
                dlist = Dmat(surchildlist(j),sklist);
                [~,I] = min(dlist);
                plist = [plist,sklist(I)];
            end
            parent(:,size(parent,2)+1) = [0;NODETYPES.COOCCUR]; % note here may make mistake
            parent(1,plist) = size(parent,2);
            obblist(:,size(obblist,2)+1) = updateOBB(obblist,plist);
            innodelist = [innodelist;size(parent,2)];
        end
%         parent(innodelist) = i;% assign the central obj as their parent
        innodelist = [i;innodelist(:)];
        parent(:,size(parent,2)+1) = [parent(1,i);NODETYPES.SURROUND];
        parent(1,innodelist) = size(parent,2); % assign the central obj as their parent
        obblist(:,size(obblist,2)+1) = updateOBB(obblist,innodelist);
    end

    if(length(surind)==1)
        slist = sur{surind(1)};
        parent(:,size(parent,2)+1) = [parent(1,i);NODETYPES.SURROUND];
        parent(1,slist) = size(parent,2);
        obblist(:,size(obblist,2)+1) = updateOBB(obblist,slist);
    end
end

%% create the distmat for all the current nodes
distmat = Dmat;
for i = 1+objnum:size(parent,2)
    ind = find(parent(1,:)==i);
    dm = distmat(ind,:);
    if(length(ind)>1)
        dm = min(dm);
    end
    if(length(ind)>0)
        distmat = [distmat; dm];
        dm = [dm(:);0];
        distmat = [distmat,dm];
    end
end

wallidx = strmatch('wall',labellist,'exact');
flooridx = strmatch('floor',labellist,'exact');
if(length(wallidx)>0)
    %% create wall nodes
    %% shoud be changed to obj cluster algorithm
    floorchildlist = find(parent(1,:)==flooridx);
    floorchildlist = setdiff(floorchildlist,wallidx);
    [ clusters ] = objclustering( obblist, distmat, wallidx, floorchildlist );
     for j = 1:length(clusters)
        list = clusters{j};
        parent(1,list) = wallidx(j);
    end
%     for i = 1:length(floorchildlist)
%         cid = floorchildlist(i);
%         distlist = distmat(cid,wallidx);
%         [~,I] = min(distlist);
%         parent(:,cid) = [wallidx(I);parent(2,cid)];
%     end
%
end
parent(2,wallidx) = NODETYPES.WALL;
parent(2,flooridx) = NODETYPES.ROOM;%%%%%%%%%

% colorlist = {'r','g','b','k'};
% for i = 1:length(clusters)
%     c = clusters{i};
%     draw3dOBB_v2(obblist(:,wallidx(i)),colorlist{i});
%     for j = 1:length(c)
%         draw3dOBB_v2(obblist(:,c(j)),colorlist{i});
%     end
% end

[ kids, ntypelist ] = organize_binarytree( parent, distmat, objnum, NODETYPES, flooridx);

%% check and correct the ntypelist
for i = 1:length(ntypelist)
    flag = ntypelist(i);
    if(flag==NODETYPES.COOCCUR)
        k = kids{i};
        if(k(1)<size(Rmat,1)&k(2)<size(Rmat,1))
            rel = Rmat(k(1),k(2));
            if(rel==RELATIONS.SUPPORT1)
                ntypelist(i) = NODETYPES.SUPPORT;
            end
            if(rel==RELATIONS.SUPPORT2)
                ntypelist(i) = NODETYPES.SUPPORT;
                tmp = k(1);
                k(1) = k(2);
                k(2) = tmp;
            end
            kids{i} = k;
        end
    end
end

end


function voxelThicknessV = deduceVoxelThicknesses(scanNum, planC)
%"deduceVoxelThicknesses"
%   Deduces voxel thicknesses when the treatment planning system provides
%   only zValues.
%
%   Note:  these voxel thicknesses may well be different than CT slice
%   thicknesses as read out by the CT scanners!  This is because slice
%   thickness on a CT scanner is not determined by slice spacing necessarily,
%   whereas we are assigning voxels absolute volumes which they are to
%   represent in 3-D.  Therefore, our slice thicknesses need to be consistent
%   with z values, and every point in the CT scan needs to be assigned to
%   a unique voxel.
%
%   The planC argument is optional.  If not specified, the global planC is
%   used.
%
%Created            :  30 Apr 03, JOD
%Latest modifications: 08 May 03, JOD, check for thicknesses < 0.
%                      28 May 03, JOD, raised tolerance for consistency
%                                      check to 10000 * eps due to failure
%                                      at 100 * eps.
%                      05 Sep 03, JOD, changed assignment of thickness for
%                                       first and last slices: previously
%                                       not consistent and produced an error.
%                      02 Feb 16, JRA, Added multiscan support.
%Usage:
%   function voxelThicknessV = deduceVoxelThicknesses(scanNum, planC)
%
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
% CERR is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
% without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
% See the GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with CERR.  If not, see <http://www.gnu.org/licenses/>.

if ~exist('planC')
    global planC
end

indexS = planC{end};

zValuesV = [planC{indexS.scan}(scanNum).scanInfo(:).zValue];

voxelThicknessV = ones(size(zValuesV)) * NaN;

for i = 2 : length(zValuesV) - 1

  nextDelta = abs(zValuesV(i+1) - zValuesV(i));

  lastDelta = abs(zValuesV(i) - zValuesV(i-1));

  if nextDelta == lastDelta

    voxelThicknessV(i) = nextDelta;

  else

    %split thicknesses:
    voxelThicknessV(i) = 0.5 * lastDelta + 0.5 * nextDelta;

  end

end

if length(zValuesV) > 1
    voxelThicknessV(1) = abs(zValuesV(2) - zValuesV(1));  %JOD, 5 Sept 03

    voxelThicknessV(end) = abs(zValuesV(end) - zValuesV(end - 1)); %JOD, 5 Sept 03
else
    %voxelThicknessV = optS.alternateLimitUniformCTSliceSpacing;
    voxelThicknessV = 1; % dummy value for single slice
end

%Check

if any(isnan(voxelThicknessV))
  error('Error in determining slice thicknesses: not all slices were assigned thicknesses')
end

if length(zValuesV)>1 && abs(sum(voxelThicknessV) - (abs(zValuesV(end)-zValuesV(1)) + 0.5 * voxelThicknessV(1) + 0.5 * voxelThicknessV(end))) > 10000 * eps
  error('Voxel thicknesses inconsistent with z values.')
end


function Fo = blockframeaccel(F, Lb, varargin)
%BLOCKFRAMEACCEL Precompute structures for block processing
%   Usage: F = blockframeaccel(F,Lb);
%
%   `F=blockframeaccel(F,Lb)` has to be called for each frame object prior to
%   entering the main loop where |blockana| and |blocksyn| are called.
%   The function works entirely like |frameaccel| but in addition, it prepares
%   structures for the processing of a consecutive stream of blocks.
%
%      `'sliwin',sliwin`   : Slicing window. `sliwin` have to be a window
%                            of length *2Lb* or a string accepted
%                            by the |firwin| function. It is used only in
%                            the slicing window approach. The default is
%                            `'hann'`.
%
%      `'zpad',zpad`   : Number of zero samples the block will be padded
%                        after it is windowed by a slicing window. This
%                        does not affect the synthesis windowing.

complainif_notenoughargs(nargin,2,'BLOCKFRAMEACCEL');
complainif_notvalidframeobj(F,'BLOCKFRAMEACCEL');

definput.flags.blockalg = {'naive','sliced','segola'};
definput.keyvals.sliwin = [];
definput.keyvals.zpad = 0;
[flags,kv]=ltfatarghelper({},definput,varargin);

isSliProp = ~isempty(kv.sliwin) || kv.zpad~=0;
assert(~(~flags.do_sliced && isSliProp),...
   sprintf(['%s: Definig slicing window properties without setting the',...
            ' ''sliced'' flag.'], mfilename));

if flags.do_sliced
   if isempty(kv.sliwin)
      kv.sliwin = 'hann';
   end

   if ~isnumeric(kv.sliwin)
      kv.sliwin = fftshift(sqrt(firwin(kv.sliwin,2*Lb)));
   end

   Fo = frameaccel(F,2*Lb+2*kv.zpad);
   Fo.sliwin = kv.sliwin;
   Fo.zpad = kv.zpad;
elseif flags.do_segola
   % Determine window length without calling frameaccel
   % Fo = frameaccel(F,Lb);
   winLen = framefirlen(F);

   if winLen==-1
      error(['%s: Segment overlap cannot be used with this frame.,'...
             ' It does not have FIR windows.'],upper(mfilename));
   end

   switch(F.type)
      case {'fwt'}
         Fo = frameaccel(F,Lb);
         Fo.a = F.g.a(:);
      case {'dgt','dgtreal'}
         Fo = frameaccel(F,Lb+winLen-1+F.a);
%       case {'filterbank','filterbankreal','ufilterbank','ufilterbankreal'}
%          lcma =  filterbanklength(1,F.a(:,1));
%          Fo = frameaccel(F,Lb+winLen-1+lcma);
%          assert(all(Fo.a(:,2)==1), '%s: Fractional subsampling is not supported',upper(mfilename) );
%          Fo.lcma =  lcma;
      case {'dwilt'}
         Fo = frameaccel(F,Lb+winLen-1+2*F.M);
         Fo.a = 2*Fo.M;
      case {'wmdct'}
         Fo = frameaccel(F,Lb+winLen-1+F.M);
         Fo.a = Fo.M;
      otherwise
	 error('%s: Unsupported frame for segola.',upper(mfilename));
   end

   % This is important otherwise we would get 0 coefficients for some
   % blocks.
   assert(max(Fo.a(:,1)) <= Lb ,sprintf(['%s: Time step %i is bigger than the',...
      ' block length %i.'],upper(mfilename),max(Fo.a(:,1)),Lb));

   Fo.winLen = winLen;

elseif flags.do_naive
   Fo = frameaccel(F,Lb);
end

Fo.blockalg = flags.blockalg;

function winLen = framefirlen(F)
%FRAMEFIRLEN Frame window/filter length
%
%   Function returns length of the longest FIR window/filter. The function
%   returns -1 if the frame does not have FIR windows.

winLen = -1;
info = [];
switch(F.type)
      case {'dgt','dgtreal'}
        [~, info] =  gabwin(F.g,F.a,F.M,[],F.kv.lt);
      case {'dwilt','wmdct'}
        [~, info] = wilwin(F.g,F.M,[],upper(mfilename));
      case {'filterbank','ufilterbank'}
        [~, ~,info]  = filterbankwin(F.g,F.a);
      case {'filterbankreal','ufilterbankreal'}
        [~, ~,info]  = filterbankwin(F.g,F.a,'real');
      case 'fwt'
        winLen = (F.g.a(1)^F.J-1)/(F.g.a(1)-1)*(numel(F.g.g{1}.h)-1)+1;
end;

if ~isempty(info) && isfield(info,'isfir') && info.isfir
   if isfield(info,'longestfilter')
      winLen = info.longestfilter;
   else
      winLen = max(info.gl);
   end
end


%% Intialise Parameters
function init_params()
global El_lim_low El_lim_up LB UB El_range PopulationSize P Generations iteration;
global El vel NumVer pbest_loc gbest_loc pbest_val EV gbest_val_record;
global FitVal gbest_val ev av d n p t delta_t k_pb_i k_pb_f k_gb_i k_gb_f w
El_lim_low(1,1:NumVer)=LB;
El_lim_up(1,1:NumVer)=UB;
El_range(1,1:NumVer)= El_lim_up - El_lim_low;
P=PopulationSize;% number of particles
iteration=Generations;% number of iterations
% Intialise Pbest of all agents & Gbest
El =zeros(1,NumVer,P);
vel =zeros(1,NumVer,P);
pbest_loc=zeros(1,NumVer,P);
gbest_loc=zeros(1,NumVer);
pbest_val=zeros(P);
EV=P*iteration;
gbest_val_record(1:EV) = 0;
FitVal = 0.0;
gbest_val = 0.0;
ev = 0;
av = 0;
d=0; n=0; p=0; t=0;
delta_t=1;
% Set parameters of the optimization problem
k_pb_i = 2.5;
k_pb_f = 0.5;
k_gb_i = 0.5;
k_gb_f = 2.5;
w = 0.9;
end


function [pvec, pstruct] = tapas_hgf_whatworld_transp(r, ptrans)
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2013 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Number of states whose contingencies have to be learned
ns = r.c_prc.n_states;

% Number of elements of the transition matrix
ntr = ns^2;

pvec    = NaN(1,length(ptrans));
pstruct = struct;

pvec(1:ntr)        = ptrans(1:ntr);                     % mu2_0
pstruct.mu2_0      = pvec(1:ntr);
pvec(ntr+1:2*ntr)  = exp(ptrans(ntr+1:2*ntr));          % sa2_0
pstruct.sa2_0      = pvec(ntr+1:2*ntr);
pvec(2*ntr+1)      = ptrans(2*ntr+1);                   % mu3_0
pstruct.mu3_0      = pvec(2*ntr+1);
pvec(2*ntr+2)      = exp(ptrans(2*ntr+2));              % sa3_0
pstruct.sa3_0      = pvec(2*ntr+2);
pvec(2*ntr+3)      = tapas_sgm(ptrans(2*ntr+3),r.c_prc.kaub); % ka
pstruct.ka         = pvec(2*ntr+3);
pvec(2*ntr+4)      = ptrans(2*ntr+4);                   % om
pstruct.om         = pvec(2*ntr+4);
pvec(2*ntr+5)      = tapas_sgm(ptrans(2*ntr+5),r.c_prc.thub); % th
pstruct.th         = pvec(2*ntr+5);

return;


%%******************************************************************
%% checkdepconst: compute AAt to determine if the
%%             constraint matrices Ak are linearly independent.
%%
%% [At,b,y,idxB,neardepconstr,feasible,AAt] =
%%         checkdepconstr(blk,At,b,y,rmdepconstr);
%%
%% rmdepconstr = 1, if want to remove dependent columns in At
%%             = 0, otherwise.
%%
%% idxB = indices of linearly independent columns of original At.
%% neardepconstr = 1 if there is nearly dependent columns in At
%%            = 0, otherwise.
%% Note: the definition of "nearly dependent" is dependent on the
%%       threshold used to determine the small diagonal elements in
%%       the LDLt factorization of A*At.
%%*****************************************************************
%% SDPT3: version 4.0
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*****************************************************************

   function  [At,b,y,idxB,neardepconstr,feasible,AAt] = ...
              checkdepconstr(blk,At,b,y,rmdepconstr);

   global existlowrank printlevel
   global nnzmatold
%%
%% compute AAt
%%
   m = length(b);
   AAt = sparse(m,m);  numdencol = 0; UU = [];
   for p = 1:size(blk,1)
      pblk = blk(p,:);
      if strcmp(pblk{1},'s');
         m1 = size(At{p,1},2); m2 = m - m1;
         if (m2 > 0)
            if (m2 > 0.3*m); AAt = full(AAt); end
            dd = At{p,3};
            lenn = size(dd,1);
            idxD = [0; find(diff(dd(:,1))); lenn];
            ss = [0, cumsum(pblk{3})];
            if (existlowrank)
               AAt(1:m1,1:m1) = AAt(1:m1,1:m1) + At{p,1}'*At{p,1};
               for k = 1:m2
                  idx = [ss(k)+1 : ss(k+1)];
                  idx2 = [idxD(k)+1: idxD(k+1)];
                  ii = dd(idx2,2)-ss(k); %% undo cumulative indexing
                  jj = dd(idx2,3)-ss(k);
                  len = pblk{3}(k);
                  Dk = spconvert([ii,jj,dd(idx2,4); len,len,0]);
                  tmp = svec(pblk,At{p,2}(:,idx)*Dk*At{p,2}(:,idx)');
                  tmp2 = AAt(1:m1,m1+k) + (tmp'*At{p,1})';
                  AAt(1:m1,m1+k) = tmp2;
                  AAt(m1+k,1:m1) = tmp2';
               end
            end
            DD = spconvert([dd(:,2:4); sum(pblk{3}),sum(pblk{3}),0]);
            VtVD = (At{p,2}'*At{p,2})*DD;
            VtVD2 = VtVD'.*VtVD;
            for k = 1:m2
               idx0 = [ss(k)+1 : ss(k+1)];
               %%tmp = VtVD(idx0,:)' .* VtVD(:,idx0);
               tmp = VtVD2(:,idx0);
               tmp = tmp*ones(length(idx0),1);
               tmp3 = AAt(m1+[1:m2],m1+k) + mexqops(pblk{3},tmp,ones(length(tmp),1),1);
               AAt(m1+[1:m2],m1+k) = tmp3;
            end
         else
            AAt = AAt + abs(At{p,1})'*abs(At{p,1});
         end
      else
         decolidx = checkdense(At{p,1}');
         if ~isempty(decolidx);
            n2 = size(At{p,1},1);
            dd = ones(n2,1);
            len= length(decolidx);
            dd(decolidx) = zeros(len,1);
            AAt = AAt + (abs(At{p,1})' *spdiags(dd,0,n2,n2)) *abs(At{p,1});
            tmp = At{p,1}(decolidx,:)';
            UU = [UU, tmp];
            numdencol = numdencol + len;
         else
            AAt = AAt + abs(At{p,1})'*abs(At{p,1});
         end
      end
   end
   if (numdencol > 0) & (printlevel)
      fprintf('\n number of dense column in A = %d',numdencol);
   end
   numdencol = size(UU,2);
%%
%%
%%
   feasible = 1; neardepconstr = 0;
   if ~issparse(AAt); AAt = sparse(AAt); end
   nnzmatold = mexnnz(AAt);
   rho = 1e-15;
   diagAAt = diag(AAt);
   mexschurfun(AAt,rho*max(diagAAt,1));
   [L.R,indef,L.perm] = chol(AAt,'vector');
   L.d = full(diag(L.R)).^2;
   if (indef)
      msg = 'AAt is not pos. def.';
      idxB = [1:m];
      neardepconstr = 1;
      if (printlevel); fprintf('\n checkdepconstr: %s',msg); end
      return;
   end
%%
%% find independent rows of A
%%
   dd = zeros(m,1);
   idxB = [1:m]';
   dd(L.perm) = abs(L.d);
   idxN = find(dd < 1e-13*mean(L.d));
   ddB = dd(setdiff([1:m],idxN));
   ddN = dd(idxN);
   if ~isempty(ddN) & ~isempty(ddB) & (min(ddB)/max(ddN) < 10)
      %% no clear separation of elements in dd
      %% do not label constraints as dependent
      idxN = [];
   end
   if ~isempty(idxN)
      neardepconstr = 1;
      if (printlevel)
         fprintf('\n number of nearly dependent constraints = %1.0d',length(idxN));
      end
      if (numdencol==0)
         if (rmdepconstr)
            idxB = setdiff([1:m]',idxN);
            if (printlevel)
               fprintf('\n checkdepconstr: removing dependent constraints...');
            end
            [W,resnorm] = findcoeffsub(blk,At,idxB,idxN);
   	    tol = 1e-8;
            if (resnorm > sqrt(tol))
               idxB = [1:m]';
               neardepconstr = 0;
               if (printlevel)
                  fprintf('\n checkdepconstr: basis rows cannot be reliably identified,');
                  fprintf(' abort removing nearly dependent constraints');
               end
               return;
            end
            tmp = W'*b(idxB) - b(idxN);
            nnorm = norm(tmp)/max(1,norm(b));
            if (nnorm > tol)
               feasible = 0;
               if (printlevel)
                  fprintf('\n checkdepconstr: inconsistent constraints exist,');
                  fprintf(' problem is infeasible.');
               end
            else
               feasible = 1;
               for p = 1:size(blk,1)
                  At{p,1} = At{p,1}(:,idxB);
               end
               b = b(idxB);
               y = y(idxB);
               AAt = AAt(idxB,idxB);
            end
	 else
            if (printlevel)
               fprintf('\n To remove these constraints,');
               fprintf(' re-run sqlp.m with OPTIONS.rmdepconstr = 1.');
            end
         end
      else
         if (printlevel)
            fprintf('\n warning: the sparse part of AAt may be nearly singular.');
         end
      end
   end
%%*****************************************************************************
%% findcoeffsub:
%%
%% [W,resnorm] = findcoeffsub(blk,At,idXB,idXN);
%%
%% idXB = indices of independent columns of At.
%% idxN = indices of   dependent columns of At.
%%
%% AB = At(:,idxB); AN = At(:,idxN) = AB*W
%%
%% SDPT3: version 3.0
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last modified: 2 Feb 01
%%*****************************************************************************

   function [W,resnorm] = findcoeffsub(blk,At,idxB,idxN);

   AB = []; AN = [];
   for p = 1:size(blk,1)
      AB = [AB; At{p,1}(:,idxB)];
      AN = [AN; At{p,1}(:,idxN)];
   end
   [m,n] = size(AB);
%%
%%-----------------------------------------
%% find W so that AN = AB*W
%%-----------------------------------------
%%
   [L,U,P,Q] = lu(sparse(AB));
   rhs  = P*AN;
   Lhat = L(1:n,:);
   W = Q*( U \ (Lhat \ rhs(1:n,:)));
   resnorm = norm(AN-AB*W,'fro')/max(1,norm(AN,'fro'));
%%*****************************************************************************


function [gradKeypointS,gradKeypointV,gradKeypointAlpha,gradOcclusionS,...
    gradOcclusionV,gradOcclusionAlpha,occlusionCount,gradSilS,gradSilV,...
    gradSilAlpha,gradShapePriorS,gradShapePriorV,gradShapePriorAlpha] ...
    = computeGrads(S,V,alpha,stateFiles,lambda,shapePriorInstance,corenum,numcores,params)

numpoints = size(S,1);
K = size(V,2);
numimages = length(stateFiles);

gradOcclusionS = zeros(size(S));
gradOcclusionV = zeros(size(V));
gradOcclusionAlpha = zeros(size(alpha));
occlusionCount = zeros(numpoints,1);

gradSilS = zeros(size(S));
gradSilV = zeros(size(V));
gradSilAlpha = zeros(size(alpha));

gradKeypointS = zeros(size(S));
gradKeypointV = zeros(size(V));
gradKeypointAlpha = zeros(size(alpha));

gradShapePriorS = zeros(size(S));
gradShapePriorV = zeros(size(V));
gradShapePriorAlpha = zeros(size(alpha));

%t_occ=0;t_sil=0;t_prior=0;
%t_kp = 0;

for i=1:numimages
    if(mod(i,numcores)==(corenum-1))

        load(stateFiles{i});
        if(params.opt.normalizeByViews)
            gradWeight = getGradWtFromClusters(state.viewClusters,state.globalID);
            gradWeight = gradWeight^(0.5);
        else
            gradWeight = 1;
        end
        S_i = S + reshape(V*alpha(:,i),numpoints,3);

        %% Keypoint Consistency
        %tic;
        kpGrad = keypointGrad(S_i,state,params);
        kpGrad = lambda(6)*kpGrad;
        gradKeypointS = gradKeypointS + kpGrad;
        gradKeypointV = gradKeypointV + kpGrad(:)*(alpha(:,i))';
        gradKeypointAlpha(:,i) = V'*kpGrad(:);
        %t_kp = t_kp + toc;

        %% Occlusion Reasoning
        %tic;
        [occGrad,occCount] = occlusionGradOptimal(S_i,state); %we also keep track of how many constraints each point violates
        occGrad = lambda(1)*occGrad;
        occGrad = occGrad*gradWeight;
        occlusionCount = occCount + occlusionCount;
        gradOcclusionS = gradOcclusionS + occGrad;
        gradOcclusionV = gradOcclusionV + occGrad(:)*(alpha(:,i))';
        gradOcclusionAlpha(:,i) = V'*occGrad(:);
        %t_occ = t_occ+toc;

        %% Silhouette Consistency
        %tic;
        silGrad = lambda(2)*silhouetteGrad(S_i,state,params); %we also keep track of how many constraints each point violates
        silGrad = silGrad*500; % because we divide by number of points on silhouette
        silGrad = silGrad*gradWeight;
        gradSilS = gradSilS + silGrad;
        gradSilV = gradSilV + silGrad(:)*(alpha(:,i))';
        gradSilAlpha(:,i) = V'*silGrad(:);
        %t_sil = t_sil + toc;

        %% Shape Prior
        %tic;
        shapePriorGrad = lambda(5)*shapePriorInstance(S_i);
        gradShapePriorS = gradShapePriorS + shapePriorGrad;
        gradShapePriorV = gradShapePriorV + shapePriorGrad(:)*(alpha(:,i))'; %%Verify
        gradShapePriorAlpha(:,i) = V'*shapePriorGrad(:); %% Verify
        %t_prior = t_prior + toc;
    end
end
end


function [estimate] = ft_inverse_harmony(sourcemodel, sens, headmodel, dat, varargin)

% FT_INVERSE_HARMONY computes a linear estimate of the current in a distributed
% source model using a mesh harmonic based low-pass filter.
%
% Use as
%   [estimate] = ft_inverse_harmony(sourcemodel, sens, headmodel, dat, ...)
% where
%   sourcemodel is the input source model, see FT_PREPARE_SOURCEMODEL
%   sens        is the gradiometer or electrode definition, see FT_DATATYPE_SENS
%   headmodel   is the volume conductor definition, see FT_PREPARE_HEADMODEL
%   dat         is the data matrix with the ERP or ERF
% and
%   estimate    contains the estimated source parameters
%
% Additional input arguments should be specified as key-value pairs and can include
%   'noisecov'             = Nchan x Nchan matrix with noise covariance
%   'noiselambda'          = scalar value, regularisation parameter for the noise covariance matrix (default=0)
%   'filter_order'         = scalar, order of the mesh Butterwirth filter
%   'filter_bs'            = scalar, stop-band of the mesh Butterworth filter
%   'number_harmonics'     = Integer, number of mesh harmonics used (can be empty, the default will then be identity)
%   'lambda'               = scalar, regularisation parameter (can be empty, it will then be estimated from snr)
%   'snr'                  = scalar, signal to noise ratio
%   'scalesourcecov'       = 'no' or 'yes', scale the source covariance matrix R such that trace(leadfield*R*leadfield')/trace(C)=1
%   'connected_components' = number of connected components of the source mesh (1 or 2)
%   'prewhiten'            = 'no' or 'yes', prewhiten the leadfield matrix with the noise covariance matrix C
%
% These options influence the forward computation of the leadfield
%   'reducerank'      = 'no' or number  (default = 3 for EEG, 2 for MEG)
%   'backproject'     = 'yes' or 'no', in the case of a rank reduction this parameter determines whether the result will be backprojected onto the original subspace (default = 'yes')
%   'normalize'       = 'no', 'yes' or 'column' (default = 'no')
%   'normalizeparam'  = parameter for depth normalization (default = 0.5)
%   'weight'          = number or Nx1 vector, weight for each dipole position to compensate for the size of the corresponding patch (default = 1)
%
% This implements
% - Petrov Y (2012) Harmony: EEG/MEG Linear Inverse Source Reconstruction in the
%   Anatomical Basis of Spherical Harmonics. PLoS ONE 7(10): e44439.
%   doi:10.1371/journal.pone.0044439
%
% See also FT_SOURCEANALYSIS, FT_PREPARE_HEADMODEL, FT_PREPARE_SOURCEMODEL

% TODO implement the following options
% - keepleadfield

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

if mod(nargin-4,2)
  % the first 4 arguments are fixed, the other arguments should come in pairs
  ft_error('invalid number of optional arguments');
end

% get the optional input arguments, or use defaults
noisecov       = ft_getopt(varargin, 'noisecov');
lambda         = ft_getopt(varargin, 'lambda');  % can be empty, it will then be estimated based on SNR
noiselambda    = ft_getopt(varargin, 'noiselambda', []);
snr            = ft_getopt(varargin, 'snr');     % is used to estimate lambda if lambda is not specified
keepfilter     = istrue(ft_getopt(varargin, 'keepfilter', false));
dowhiten       = istrue(ft_getopt(varargin, 'prewhiten',  false));
doscale        = istrue(ft_getopt(varargin, 'scalesourcecov', false));

% construct the low-level options for the leadfield computation as key-value pairs, these are passed to FT_COMPUTE_LEADFIELD
leadfieldopt = {};
leadfieldopt = ft_setopt(leadfieldopt, 'reducerank',     ft_getopt(varargin, 'reducerank'));
leadfieldopt = ft_setopt(leadfieldopt, 'backproject',    ft_getopt(varargin, 'backproject'));
leadfieldopt = ft_setopt(leadfieldopt, 'normalize',      ft_getopt(varargin, 'normalize'));
leadfieldopt = ft_setopt(leadfieldopt, 'normalizeparam', ft_getopt(varargin, 'normalizeparam'));
leadfieldopt = ft_setopt(leadfieldopt, 'weight',         ft_getopt(varargin, 'weight'));

% flags to avoid calling isfield repeatedly in the loop over grid positions (saves a lot of time)
hasmom        = isfield(sourcemodel, 'mom');
hasleadfield  = isfield(sourcemodel, 'leadfield');
hasfilter     = isfield(sourcemodel, 'filter');

if isempty(lambda) && isempty(snr) && ~hasfilter
  ft_error('either lambda or snr should be specified');
elseif ~isempty(lambda) && ~isempty(snr)
  ft_error('either lambda or snr should be specified, not both');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% find the dipole positions that are inside/outside the brain
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if ~isfield(sourcemodel, 'inside')
  if hasfilter
    sourcemodel.inside = ~cellfun(@isempty, sourcemodel.filter);
  elseif hasleadfield
    sourcemodel.inside = ~cellfun(@isempty, sourcemodel.leadfield);
  else
    sourcemodel.inside = ft_inside_headmodel(sourcemodel.pos, headmodel);
  end
end

% convert to logical representation
sourcemodel = fixinside(sourcemodel);

% keep the original details on inside and outside positions
originside = sourcemodel.inside;
origpos    = sourcemodel.pos;

% select only the dipole positions inside the brain for scanning
sourcemodel.pos    = sourcemodel.pos(originside,:);
sourcemodel.inside = true(size(sourcemodel.pos,1),1);

if hasmom
  sourcemodel.mom = sourcemodel.mom(:,originside);
end

if hasfilter
  ft_info('using precomputed filters\n');
  sourcemodel.filter = sourcemodel.filter(originside);
elseif hasleadfield
  ft_info('using precomputed leadfields\n');
  sourcemodel.leadfield = sourcemodel.leadfield(originside);
else
  ft_info('computing forward model on the fly\n');
  if hasmom
    for i=size(sourcemodel.pos,1)
      % compute the leadfield for a fixed dipole orientation
      sourcemodel.leadfield{i} = ft_compute_leadfield(sourcemodel.pos(i,:), sens, headmodel, leadfieldopt{:}) * sourcemodel.mom(:,i);
    end
  else
    for i=1:size(sourcemodel.pos,1)
      % compute the leadfield
      sourcemodel.leadfield{i} = ft_compute_leadfield(sourcemodel.pos(i,:), sens, headmodel, leadfieldopt{:});
    end
  end
end

% Harmony parameters
num_dip      = size(sourcemodel.leadfield{1}, 2);
num_pos      = size(sourcemodel.leadfield, 2);
num_comp     = ft_getopt(varargin, 'connected_components', 1);
num_harm     = ft_getopt(varargin, 'number_harmonics',     5);
filt_ord     = ft_getopt(varargin, 'filter_order',         5);
lc           = ft_getopt(varargin, 'filter_bs',            5);

% Create the leadfield matrix

% count the number of channels and leadfield components
Nchan   = size(sourcemodel.leadfield{1},1);
Nsource = 0;
for i=1:size(sourcemodel.pos,1)
  Nsource = Nsource + size(sourcemodel.leadfield{i}, 2);
end

% concatenate the leadfield components of all sources into one large matrix
lf = zeros(Nchan, Nsource);
n = 1;
for i=1:size(sourcemodel.pos,1)
  cbeg = n;
  cend = n + num_dip - 1;
  lf(:,cbeg:cend) = sourcemodel.leadfield{i};
  n = n + size(sourcemodel.leadfield{i}, 2);
end

% Create brain harmonics matrix

% Mesh harmonics
[dum,H,d] = mesh_spectrum(sourcemodel,num_harm,num_comp);

% Elimination of pathological zeroth frequencies and concatenating
% harmonics numbers
l = [];
for n = 1:num_comp
  d{n}(1) = 0;
  H{n}(:,1) = 1/sqrt(numel(H{n}(:,1)));
  d_cat = [];
  for m = 1:num_dip
    d_cat = [d_cat; d{n}];
  end
  l = [l; -d_cat];
end

% Total harmonics matrix
if num_comp == 1
  H_blk = H{1};
elseif num_comp == 2
  H_blk = blkdiag(H{1},H{2});
else
  ft_error('this function only supports meshes with 1 or 2 connected components')
end

if num_dip == 1
  H_tot = H_blk;
elseif num_dip == 2
  H_tot = blkdiag(H_blk,H_blk);
elseif num_dip == 3
  H_tot = blkdiag(H_blk,H_blk,H_blk);
else
  ft_error('this function only supports source model with 1, 2 or 3 dimensional source values')
end

% Create harmonic leadfield
lf_h = zeros(size(lf,1),num_harm,num_comp,num_dip); %leadifield in the harmonic domain

for k = 1:num_comp
  for j = 1:num_dip
    ind = (j-1) + 1:num_dip:num_dip*num_pos;
    lf_h(:,:,k,j) = lf(:,ind)*H{k};
  end
end

lf_h = reshape(lf_h,size(lf,1),num_comp*num_harm*num_dip);

% Create harmonic source covariance matrix
q = 1./sqrt(1 + (l/lc).^(2*filt_ord));
sourcecov = diag(q);

% Compute the Harmony spatial filters

% count the number of channels and leadfield components
Nchan   = size(sourcemodel.leadfield{1},1);
Nsource = 0;
for i=1:size(sourcemodel.pos,1)
  Nsource = Nsource + size(sourcemodel.leadfield{i}, 2);
end

% Take the rieal part of the noise cross-spectral density matrix
if isreal(noisecov)
  ft_info('taking the real part of the noise cross-spectral density matrix\n');
  noisecov = real(noisecov);
end

% compute the inverse of the forward model, this is where prior information
% on source and noise covariance would be useful
if isempty(noisecov)
  % use an unregularised minimum norm solution, i.e. using the Moore-Penrose pseudoinverse
  ft_warning('computing a unregularised minimum norm solution. This typically does not work due to numerical accuracy problems');
  w = pinv(lf_h);
elseif ~isempty(noisecov)
  ft_info('using the noise covariance for regularisation\n');
  % the noise covariance has been given and can be used to regularise the solution
  if isempty(sourcecov)
    sourcecov = speye(Nsource);
  end
  % rename some variables for consistency with the publications
  A = lf_h;
  R = sourcecov;
  C = noisecov;

  if dowhiten
    ft_info('prewhitening the leadfields using the noise covariance\n');

    % compute the prewhitening matrix
    if ~isempty(noiselambda)
      ft_info('using a regularized noise covariance matrix\n');
      % note: if different channel types are present, one should probably load the diagonal with channel-type specific stuff
      [U,S,V] = svd(C+eye(size(C))*noiselambda);
    else
      [U,S,V] = svd(C);
    end

    Tol     = 1e-12;
    diagS   = diag(S);
    sel     = find(diagS>Tol.*diagS(1));
    P       = diag(1./sqrt(diag(S(sel,sel))))*U(:,sel)'; % prewhitening matrix
    A       = P*A; % prewhitened leadfields
    C       = eye(size(P,1)); % prewhitened noise covariance matrix
  end

  if doscale
    % estimate sourcecov such that trace(ARA')/trace(C) = 1 (see
    % http://martinos.org/mne/manual/mne.html. In the case of prewhitening
    % C reduces to I (and then lambda^2 ~ 1/SNR); note that in mixed
    % channel type covariance matrices prewhitening should be applied in
    % order for this to make sense (otherwise the diagonal elements of C
    % have different units)
    ft_info('scaling the source covariance\n');
    scale = trace(A*(R*A'))/trace(C);
    R     = R./scale;
  end

  if ~isempty(snr)
    % the regularisation parameter can be estimated from the noise covariance,
    % see equation 6 in Lin et al. 2004
    lambda = trace(A * R * A')/(trace(C)*snr^2);
  end

  if dowhiten
    % as documented on MNE website, this is replacing the part of the code below, it gives
    % more stable results numerically.
    Rc      = chol(R, 'lower');
    [U,S,V] = svd(A * Rc, 'econ');
    s  = diag(S);
    ss = s ./ (s.^2 + lambda);
    w  = Rc * V * diag(ss) * U';

    % unwhiten the filters to bring them back into signal subspace
    w = w*P;

  else
    % equation 5 from Lin et al 2004 (this implements Dale et al 2000, and Liu et al. 2002)
    denom = (A*R*A'+(lambda^2)*C);
    if cond(denom)<1e12
      w = R * A' / denom;
    else
      ft_info('taking pseudo-inverse due to large condition number\n');
      w = R * A' * pinv(denom);
    end
  end

end % if empty noisecov

% for each of the timebins, estimate the source strength
w_x = H_tot*w; % The Harm matrix project the Harmony level filters back to the source locations

if isreal(dat)
  ft_info('the input consists of time-series: computing the dipole moments\n')
  mom = w_x * dat;
  mom_ind = 1;
elseif size(dat,1)==size(dat,2)&&sum(sum(dat-dat'))<10^-5*sum(diag(dat))
  ft_info('the input consists of a cross-spectral density: computing source-level power\n')
  pow_tot = real(sum((w_x*dat).*w_x,2));
  pow = 0;
  for j = 1:num_dip
    pow = pow + pow_tot((1 + (j-1)*end/num_dip):(j*end/num_dip));
    mom_ind = 0;
  end
else
  ft_info('the input consists of Fourier coefficients: computing source-level Fourier coefficients\n')
  mom = w_x*dat; % The Harm matrix project the Harmonic activity back to the source locations
  mom_ind = 1;
end

% assign the estimated source strength to each dipole
if mom_ind == 1
  n = 1;
  for i=1:size(sourcemodel.pos,1)
    cbeg = n;
    cend = n + size(sourcemodel.leadfield{i}, 2) - 1;
    estimate.mom{i} = mom(cbeg:cend,:);
    n = n + size(sourcemodel.leadfield{i}, 2);
  end
end

% compute power (over the three orientations) at each location and for each time

if mom_ind == 1
  estimate.pow = nan(size(sourcemodel.pos,1), size(dat,2));
  for i=1:size(sourcemodel.pos,1)
    estimate.pow(i,:) = sum(abs(estimate.mom{i}).^2, 1);
  end
else
  estimate.pow = pow;
end

% deal with keepfilter option
if keepfilter && ~hasfilter
  % spatial filters have been computed, store them in the output
  % re-assign spatial filter to conventional 1 cell per dipole location
  n = 1;
  for i=1:size(sourcemodel.pos,1)
    cbeg = n;
    cend = n + size(sourcemodel.leadfield{i}, 2) - 1;
    estimate.filter{i} = w(cbeg:cend,:);
    n    = n + size(sourcemodel.leadfield{i}, 2);
  end
elseif keepfilter
  estimate.filter = sourcemodel.filter;
end

% deal with noise covariance
% if ~isempty(noisecov) && ~hasfilter
%   % compute estimate of the projected noise
%   n = 1;
%   for i=1:size(sourcemodel.pos,1)
%     cbeg = n;
%     cend = n + size(sourcemodel.leadfield{i}, 2) - 1;
%     estimate.noisecov{i} = w(cbeg:cend,:)*noisecov*w(cbeg:cend,:)';
%     n    = n + size(sourcemodel.leadfield{i}, 2);
%   end
% elseif ~isempty(noisecov)
%   % compute estimate of the projected noise
%   for i=1:size(sourcemodel.pos,1)
%     estimate.noisecov{i} = sourcemodel.filter{i}*noisecov*sourcemodel.filter{i}';
%   end
%end

% reassign the estimated values over the inside and outside grid positions
estimate.inside  = originside;
estimate.pos     = origpos;
if isfield(estimate, 'pow')
  estimate.pow( originside,:) = estimate.pow;
  estimate.pow(~originside,:) = nan;
end
if isfield(estimate, 'mom')
  estimate.mom( originside) = estimate.mom;
  estimate.mom(~originside) = {[]};
end
if isfield(estimate, 'noisecov')
  estimate.noisecov( originside) = estimate.noisecov;
  estimate.noisecov(~originside) = {[]};
end
if isfield(estimate, 'filter')
  estimate.filter( originside) = estimate.filter;
  estimate.filter(~originside) = {[]};
end


%                               SYMBADA = getsymbada(At,Ajc,DAt,psdblkstart)
% GETSYMBADA
%   Ajc points to start of PSD-nonzeros per column
%   DAt.q has the nz-structure of ddotA.
%
% ******************** INTERNAL FUNCTION OF SEDUMI ********************
%
% See also sedumi, partitA, getada1, getada2.

function SYMBADA = getsymbada(At,Ablkjc,DAt,psdblkstart)
%
% This file is part of SeDuMi 1.1 by Imre Polik and Oleksandr Romanko
% Copyright (C) 2005 McMaster University, Hamilton, CANADA  (since 1.1)
%
% Copyright (C) 2001 Jos F. Sturm (up to 1.05R5)
%   Dept. Econometrics & O.R., Tilburg University, the Netherlands.
%   Supported by the Netherlands Organization for Scientific Research (NWO).
%
% Affiliation SeDuMi 1.03 and 1.04Beta (2000):
%   Dept. Quantitative Economics, Maastricht University, the Netherlands.
%
% Affiliations up to SeDuMi 1.02 (AUG1998):
%   CRL, McMaster University, Canada.
%   Supported by the Netherlands Organization for Scientific Research (NWO).
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc.,  51 Franklin Street, Fifth Floor, Boston, MA
% 02110-1301, USA
%

Alpq = spones(extractA(At,Ablkjc,0,3,1,psdblkstart(1)));
Ablks = findblks(At,Ablkjc,3,[],psdblkstart);
if spars(Ablks)==1 || spars(Alpq)==1 || (~isempty(DAt.q) && spars(DAt.q)==1)
    SYMBADA=sparse(ones(size(At,2),size(At,2)));
else
    SYMBADA = Alpq' * Alpq + Ablks'*Ablks + DAt.q'*DAt.q;
end


function [zValues,zCosts,zGuesses,inConvHull,meanMax,exitFlags] = ...
    findTDistributedProjections_fmin(data,trainingData,...
                                        trainingEmbedding,parameters)
%findTDistributedProjections_fmin is called by runEmbbedings to find the
%optimal embeddings of a set of wavelet amplitudes into a previously
%defined t-SNE embedding

    readout = 5000;

    perplexity = parameters.perplexity;
    sigmaTolerance = parameters.sigmaTolerance;
    maxNeighbors = parameters.maxNeighbors;
    batchSize = parameters.embedding_batchSize;
    maxOptimIter = parameters.maxOptimIter;

    N = length(data(:,1));
    zValues = zeros(N,2);
    zGuesses = zeros(N,2);
    zCosts = zeros(N,1);
    batches = ceil(N/batchSize);
    inConvHull = false(N,1);
    meanMax = zeros(N,1);
    exitFlags = zeros(N,1);

    options = optimset('Display','off','maxiter',maxOptimIter);

    for j=1:batches
        fprintf(1,'\t Processing batch #%4i out of %4i\n',j,batches);
        idx = (1:batchSize) + (j-1)*batchSize;
        idx = idx(idx <= N);
        current_guesses = zeros(length(idx),2);
        current = zeros(length(idx),2);
        currentData = data(idx,:);
        tCosts = zeros(size(idx));
        current_poly = false(length(idx),1);

        D2 = findListKLDivergences(currentData,trainingData);
        current_meanMax = zeros(length(idx),1);

        parfor i=1:length(idx)

            if mod(i,readout) == 0
                fprintf(1,'\t\t Image #%5i\n',i);
            end

            [~,p] = returnCorrectSigma_sparse(D2(i,:),perplexity,sigmaTolerance,maxNeighbors);
            idx2 = p>0;
            z = trainingEmbedding(idx2,:);
            [~,maxIdx] = max(p);
            a = sum(bsxfun(@times,z,p(idx2)'));

            guesses = [a;trainingEmbedding(maxIdx,:)];

            b = zeros(2,2);
            c = zeros(2,1)
            flags = zeros(2,1);

            q = convhull(z);
            q = z(q,:);

            [b(1,:),c(1),flags(1)] = fminsearch(@(x)calculateKLCost(x,z,p(idx2)),guesses(1,:),options);
            [b(2,:),c(2),flags(2)] = fminsearch(@(x)calculateKLCost(x,z,p(idx2)),guesses(2,:),options);
            polyIn = inpolygon(b(:,1),b(:,2),q(:,1),q(:,2));

            if sum(polyIn) > 0
                pp = find(polyIn);
                [~,mI] = min(c(polyIn));
                mI = pp(mI);
                current_poly(i) = true;
            else
                [~,mI] = min(c);
                current_poly(i) = false;
            end

            exitFlags(i) = flags(mI);
            current_guesses(i,:) = guesses(mI,:);
            current(i,:) = b(mI,:);
            tCosts(i) = c(mI);
            current_meanMax(i) = mI;

        end

        zGuesses(idx,:) = current_guesses;
        zValues(idx,:) = current;
        zCosts(idx) = tCosts;
        inConvHull(idx) = current_poly;
        meanMax(idx) = current_meanMax;

    end

    zValues(~inConvHull,:) = zGuesses(~inConvHull,:);


function varargout = solvemp(P,x,y,options)
%SOLVEMP Computes solution to multi-parametric optimization problem
%
% min_z(x)   h(x,z)
% subject to
%            F(x,z) > 0
%
%
% [SOL, DIAGNOSTIC,Z,HPWF,ZPWF] = SOLVEMP(P,options,x,y)
%
% SOL        : Multi-parametric solution (see MPT toolbox)
%
% DIAGNOSTIC : struct with diagnostic information
%
% Z          : SDPVAR object with the detected decision variable z
%
% HPWF       : The value function as a pwf function
%
% ZPWF       : The optimal decision variable as a pfw function
%
% Input
%    P        : Optimization model
%    options  : solver options. See SDPSETTINGS. Can be [].
%    x        : Parametric variables
%    y        : Requested decision variables (subset of z)
%
% NOTE : If you are solving a problem leading to an mpMILP, the
% output SOL will be a set-valued map. To obtain the minimal
% solution (without so called overlaps), run removeOverlaps(SOL). If you
% have requested the 5th output ZPWF, overlaps are automatically removed.
% If your problem leads to an mpMIQP,  the output SOL will also be a
% set-valued map, but there is currently no way in MPT to obtain a
% non-overlapping solution. To use the solution in MPT, the command
% mpt_mergeCS(SOL) can be useful. Notice that the fifth output argument
% not will be available for mpMIQP problems.
%
% See also PARAMETRIC, SDPSETTINGS, YALMIPERROR

if nargin < 4
    options = sdpsettings;
else
    options = P.Options;
end

[sol,info] = solvemp(P.Constraints,P.Objective,options,x,y)