 function X = dtft(x, omega, varargin)
%function X = dtft(x, omega [,options])
%|
%| Compute d-dimensional DTFT of signal x at frequency locations omega
%|
%| in
%|	x	[(Nd) L]	signal values
%|	omega	[M dd]		frequency locations (radians), dd = numel(Nd)
%|
%| option
%|	'n_shift' [dd 1]	use [0:N-1]-n_shift (default [0 .. 0])
%|	'how'			'outer' (default) big outer product
%|				'loop' reduce memory use (slower)
%|				'arrayfun' uses arrayfun()
%|
%| out
%|	X	[M L]		DTFT values
%|
%| Requires enough memory to store M * prod(Nd) size matrices (for testing)
%|
%| Copyright 2001-9-17, Jeff Fessler, University of Michigan
%| 2013-03-22, Daniel Weller added arrayfun version and other improvements
%| 2013-03-27, JF converted to vararg

if nargin == 1 && streq(x, 'test'), dtft_test(0), return, end
if nargin == 1 && streq(x, 'time'), dtft_test(1), return, end
if nargin < 2, ir_usage(), end

arg.n_shift = 0;
arg.how = 'outer';
arg = vararg_pair(arg, varargin);

dd = size(omega, 2);
Nd = size(x);

if numel(arg.n_shift) == 1
	arg.n_shift = repmat(arg.n_shift, dd);
end
n_shift = arg.n_shift;
if numel(n_shift) ~= dd
	fail 'n_shift size bad')
end

if dd == 1 && numel(Nd) == 2 && Nd(2) == 1 % 1D
	Nd = Nd(1);
end

if length(Nd) == dd % just one image
	x = x(:);
elseif length(Nd) == dd+1 % multiple images
	Nd = Nd(1:(end-1));
	x = reshapee(x, prod(Nd), []); % [*Nd L]
else
	error 'bad input signal size'
end

% dsw alternative to the loop:
% nn = arrayfun(@(nd,nshift) (0:(nd-1))-nshift, Nd, n_shift, 'UniformOutput', false);
for id=1:dd
	nn{id} = [0:(Nd(id)-1)] - n_shift(id);
end

% nn = ndgrid_jf('cell', nn);
if dd > 1
	[nn{:}] = ndgrid(nn{:});
end

switch arg.how
case 'outer'
	X = dtft_outer(x, omega, nn);
case 'loop'
	X = dtft_loop(x, omega, Nd, nn);
case 'arrayfun'
	X = dtft_arrayfun(x, omega, nn);
otherwise
	fail('unknown how "%s"', arg.how)
end

% dtft_outer()
function X = dtft_outer(x, omega, nn);
X = 0;
dd = ncol(omega);
for id=1:dd % add up phases
	X = X + omega(:,id) * col(nn{id})'; % [M *Nd]
end
X = exp(-1i*X) * x;

% dtft_loop()
% loop way: slower but less memory
function X = dtft_loop(x, omega, Nd, nn);
M = nrow(omega);
X = zeros(numel(x)/prod(Nd),M); % [L M]
%t1 = col(nn{1})';
if ncol(omega) > 3
	fail 'only up to 3d done'
end
if ncol(omega) < 3
	for dd = (ncol(omega)+1) : 3
		nn{dd} = 0; % dummy 0's
	end
	omega(1,3) = 0; % trick: make '3d'
end
t1 = nn{1}(:)';
t2 = col(nn{2})';
t3 = col(nn{3})';
for mm=1:M
	tmp = omega(mm,1)*t1 + omega(mm,2)*t2 + omega(mm,3)*t3;
	X(:,mm) = exp(-1i * tmp) * x;
end
X = X.'; % [M L]

% dtft_arrayfun()
% by Dan Weller, 2013-03-27
function X = dtft_arrayfun(x, omega, nn);
nn = cellfun(@(x) col(x).',nn,'UniformOutput',false); % make row vectors
nn = cat(1,nn{:}); % [dd *Nd]

M = nrow(omega);
X = arrayfun(@(m) exp((-1i*omega(m,:)) * nn) * x, 1:M, 'UniformOutput', false); % each cell is [1 L]
X = cat(1,X{:}); % [M L]

% X = exp(-1i*(omega * nn)) * x; % [M L]

% dtft_test
% simple test
function dtft_test(do_time)
Nd = [4 6 5] * 2^(1+do_time);
n_shift = [1 3 2];
rng(0), x = randn(Nd); % test signal
o1 = 2*pi*[0:(Nd(1)-1)]'/Nd(1); % test with uniform frequency locations
o2 = 2*pi*[0:(Nd(2)-1)]'/Nd(2);
o3 = 2*pi*[0:(Nd(3)-1)]'/Nd(3);
[o1 o2 o3] = ndgrid(o1, o2, o3);
om = [o1(:) o2(:) o3(:)];
cpu etic
Xd = dtft(x, om, 'n_shift', n_shift);
cpu etoc outer
cpu etic
Xl = dtft(x, om, 'n_shift', n_shift, 'how', 'loop');
cpu etoc loop
cpu etic
Xa = dtft(x, om, 'n_shift', n_shift, 'how', 'arrayfun');
cpu etoc arrayfun
printm('loop max %% difference = %g', max_percent_diff(Xl,Xd))
printm('afun max %% difference = %g', max_percent_diff(Xa,Xd))
Xf = fftn(x);
Xf = Xf(:) .* exp(1i * (om * n_shift(:))); % phase shift
printm('fftn max %% difference = %g', max_percent_diff(Xf,Xd))


function [x, f, eflag, output, lambda] = miqps_cplex(H, c, A, l, u, xmin, xmax, x0, vtype, opt)
%MIQPS_CPLEX  Mixed Integer Quadratic Program Solver based on CPLEX.
%   [X, F, EXITFLAG, OUTPUT, LAMBDA] = ...
%       MIQPS_CPLEX(H, C, A, L, U, XMIN, XMAX, X0, VTYPE, OPT)
%   [X, F, EXITFLAG, OUTPUT, LAMBDA] = MIQPS_CPLEX(PROBLEM)
%   A wrapper function providing a standardized interface for using
%   CPLEXQP or CPLEXLP to solve the following QP (quadratic programming)
%   problem:
%
%       min 1/2 X'*H*X + C'*X
%        X
%
%   subject to
%
%       L <= A*X <= U       (linear constraints)
%       XMIN <= X <= XMAX   (variable bounds)
%
%   Inputs (all optional except H, C, A and L):
%       H : matrix (possibly sparse) of quadratic cost coefficients
%       C : vector of linear cost coefficients
%       A, L, U : define the optional linear constraints. Default
%           values for the elements of L and U are -Inf and Inf,
%           respectively.
%       XMIN, XMAX : optional lower and upper bounds on the
%           X variables, defaults are -Inf and Inf, respectively.
%       X0 : optional starting value of optimization vector X
%       VTYPE : character string of length NX (number of elements in X),
%               or 1 (value applies to all variables in x),
%               allowed values are 'C' (continuous), 'B' (binary),
%               'I' (integer), 'S' (semi-continuous), or 'N' (semi-integer).
%       OPT : optional options structure with the following fields,
%           all of which are also optional (default values shown in
%           parentheses)
%           verbose (0) - controls level of progress output displayed
%               0 = no progress output
%               1 = some progress output
%               2 = verbose progress output
%           skip_prices (0) - flag that specifies whether or not to
%               skip the price computation stage, in which the problem
%               is re-solved for only the continuous variables, with all
%               others being constrained to their solved values
%           price_stage_warn_tol (1e-7) - tolerance on the objective fcn
%               value and primal variable relative match required to avoid
%               mis-match warning message
%           cplex_opt - options struct for CPLEX, value in verbose
%                   overrides these options
%       PROBLEM : The inputs can alternatively be supplied in a single
%           PROBLEM struct with fields corresponding to the input arguments
%           described above: H, c, A, l, u, xmin, xmax, x0, vtype, opt
%
%   Outputs:
%       X : solution vector
%       F : final objective function value
%       EXITFLAG : CPLEXQP/CPLEXLP exit flag
%           (see CPLEXQP and CPLEXLP documentation for details)
%       OUTPUT : CPLEXQP/CPLEXLP output struct
%           (see CPLEXQP and CPLEXLP documentation for details)
%       LAMBDA : struct containing the Langrange and Kuhn-Tucker
%           multipliers on the constraints, with fields:
%           mu_l - lower (left-hand) limit on linear constraints
%           mu_u - upper (right-hand) limit on linear constraints
%           lower - lower bound on optimization variables
%           upper - upper bound on optimization variables
%
%   Note the calling syntax is almost identical to that of QUADPROG
%   from MathWorks' Optimization Toolbox. The main difference is that
%   the linear constraints are specified with A, L, U instead of
%   A, B, Aeq, Beq.
%
%   Calling syntax options:
%       [x, f, exitflag, output, lambda] = ...
%           miqps_cplex(H, c, A, l, u, xmin, xmax, x0, vtype, opt)
%
%       x = miqps_cplex(H, c, A, l, u)
%       x = miqps_cplex(H, c, A, l, u, xmin, xmax)
%       x = miqps_cplex(H, c, A, l, u, xmin, xmax, x0)
%       x = miqps_cplex(H, c, A, l, u, xmin, xmax, x0, vtype)
%       x = miqps_cplex(H, c, A, l, u, xmin, xmax, x0, vtype, opt)
%       x = miqps_cplex(problem), where problem is a struct with fields:
%                       H, c, A, l, u, xmin, xmax, x0, vtype, opt
%                       all fields except 'c', 'A' and 'l' or 'u' are optional
%       x = miqps_cplex(...)
%       [x, f] = miqps_cplex(...)
%       [x, f, exitflag] = miqps_cplex(...)
%       [x, f, exitflag, output] = miqps_cplex(...)
%       [x, f, exitflag, output, lambda] = miqps_cplex(...)
%
%
%   Example: (problem from from https://v8doc.sas.com/sashtml/iml/chap8/sect12.htm)
%       H = [   1003.1  4.3     6.3     5.9;
%               4.3     2.2     2.1     3.9;
%               6.3     2.1     3.5     4.8;
%               5.9     3.9     4.8     10  ];
%       c = zeros(4,1);
%       A = [   1       1       1       1;
%               0.17    0.11    0.10    0.18    ];
%       l = [1; 0.10];
%       u = [1; Inf];
%       xmin = zeros(4,1);
%       x0 = [1; 0; 0; 1];
%       opt = struct('verbose', 2);
%       [x, f, s, out, lambda] = miqps_cplex(H, c, A, l, u, xmin, [], x0, vtype, opt);
%
%   See also MIQPS_MASTER, CPLEXMIQP, CPLEXMILP, CPLEXQP, CPLEXLP,
%   CPLEX_OPTIONS.

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

%% check for CPLEX
% if ~have_feature('cplexqp')
%     error('miqps_cplex: requires the MATLAB interface for CPLEX');
% end

%%----- input argument handling  -----
%% gather inputs
if nargin == 1 && isstruct(H)       %% problem struct
    p = H;
    if isfield(p, 'opt'),   opt = p.opt;    else,   opt = [];   end
    if isfield(p, 'vtype'), vtype = p.vtype;else,   vtype = []; end
    if isfield(p, 'x0'),    x0 = p.x0;      else,   x0 = [];    end
    if isfield(p, 'xmax'),  xmax = p.xmax;  else,   xmax = [];  end
    if isfield(p, 'xmin'),  xmin = p.xmin;  else,   xmin = [];  end
    if isfield(p, 'u'),     u = p.u;        else,   u = [];     end
    if isfield(p, 'l'),     l = p.l;        else,   l = [];     end
    if isfield(p, 'A'),     A = p.A;        else,   A = [];     end
    if isfield(p, 'c'),     c = p.c;        else,   c = [];     end
    if isfield(p, 'H'),     H = p.H;        else,   H = [];     end
else                                %% individual args
    if nargin < 10
        opt = [];
        if nargin < 9
            vtype = [];
            if nargin < 8
                x0 = [];
                if nargin < 7
                    xmax = [];
                    if nargin < 6
                        xmin = [];
                    end
                end
            end
        end
    end
end

%% define nx, set default values for missing optional inputs
if isempty(H) || ~any(any(H))
    if isempty(A) && isempty(xmin) && isempty(xmax)
        error('miqps_cplex: LP problem must include constraints or variable bounds');
    else
        if ~isempty(A)
            nx = size(A, 2);
        elseif ~isempty(xmin)
            nx = length(xmin);
        else    % if ~isempty(xmax)
            nx = length(xmax);
        end
    end
else
    nx = size(H, 1);
end
if isempty(c)
    c = zeros(nx, 1);
end
if isempty(A) || (~isempty(A) && (isempty(l) || all(l == -Inf)) && ...
                                 (isempty(u) || all(u == Inf)))
    A = sparse(0,nx);           %% no limits => no linear constraints
end
nA = size(A, 1);                %% number of original linear constraints
if isempty(u)                   %% By default, linear inequalities are ...
    u = Inf(nA, 1);             %% ... unbounded above and ...
end
if isempty(l)
    l = -Inf(nA, 1);            %% ... unbounded below.
end
if isempty(xmin)                %% By default, optimization variables are ...
    xmin = -Inf(nx, 1);         %% ... unbounded below and ...
end
if isempty(xmax)
    xmax = Inf(nx, 1);          %% ... unbounded above.
end
if isempty(x0)
    x0 = zeros(nx, 1);
end

%% default options
if ~isempty(opt) && isfield(opt, 'verbose') && ~isempty(opt.verbose)
    verbose = opt.verbose;
else
    verbose = 0;
end

%% split up linear constraints
ieq = find( abs(u-l) <= eps );          %% equality
igt = find( u >=  1e10 & l > -1e10 );   %% greater than, unbounded above
ilt = find( l <= -1e10 & u <  1e10 );   %% less than, unbounded below
ibx = find( (abs(u-l) > eps) & (u < 1e10) & (l > -1e10) );
Ae = A(ieq, :);
be = u(ieq);
Ai  = [ A(ilt, :); -A(igt, :); A(ibx, :); -A(ibx, :) ];
bi  = [ u(ilt);    -l(igt);    u(ibx);    -l(ibx)];

%% grab some dimensions
nlt = length(ilt);      %% number of upper bounded linear inequalities
ngt = length(igt);      %% number of lower bounded linear inequalities
nbx = length(ibx);      %% number of doubly bounded linear inequalities

%% set up options struct for CPLEX
if ~isempty(opt) && isfield(opt, 'cplex_opt') && ~isempty(opt.cplex_opt)
    cplex_opt = cplex_options(opt.cplex_opt);
else
    cplex_opt = cplex_options;
end

vstr = have_feature('cplex', 'vstr');
vnum = have_feature('cplex', 'vnum');
vrb = max([0 verbose-1]);
if vrb && vnum > 12.002 && vnum < 12.007
    cplex_opt.diagnostics   = 'on';
end
if verbose > 2
    cplex_opt.display = 'iter';
elseif verbose > 1
    cplex_opt.display = 'on';
elseif verbose > 0
    cplex_opt.display = 'off';
end

if isempty(Ai) && isempty(Ae)
    unconstrained = 1;
    Ae = sparse(1, nx);
    be = 0;
else
    unconstrained = 0;
end

%% call the solver
if verbose
    alg_names = {
        'default',
        'primal simplex',
        'dual simplex',
        'network simplex',
        'barrier',
        'sifting',
        'concurrent'
    };
end
if isempty(vtype) || isempty(find(vtype == 'B' | vtype == 'I' | ...
        vtype == 'S' | vtype == 'N'))
    mi = 0;
else
    mi = 1;
    %% expand vtype to nx elements if necessary
    if length(vtype) == 1 && nx > 1
        vtype = char(vtype * ones(1,nx));
    end
end

if mi
    if isempty(H) || ~any(any(H))
        if verbose
            fprintf('CPLEX Version %s -- %s MILP solver\n', ...
                vstr, alg_names{cplex_opt.lpmethod+1});
        end
        [x, f, eflag, output] = ...
            cplexmilp(c, Ai, bi, Ae, be, [], [], [], xmin, xmax, vtype, x0, cplex_opt);
        lam = [];
    else
        if verbose
            fprintf('CPLEX Version %s --  %s MIQP solver\n', ...
                vstr, alg_names{cplex_opt.qpmethod+1});
        end
        %% ensure H is numerically symmetric
        if ~isequal(H, H')
            H = (H + H')/2;
        end
        [x, f, eflag, output] = ...
            cplexmiqp(H, c, Ai, bi, Ae, be, [], [], [], xmin, xmax, vtype, x0, cplex_opt);
    end
    lam = [];
else
    if isempty(H) || ~any(any(H))
        if verbose
            fprintf('CPLEX Version %s -- %s LP solver\n', ...
                vstr, alg_names{cplex_opt.lpmethod+1});
        end
        [x, f, eflag, output, lam] = ...
            cplexlp(c, Ai, bi, Ae, be, xmin, xmax, x0, cplex_opt);
    else
        if verbose
            fprintf('CPLEX Version %s --  %s QP solver\n', ...
                vstr, alg_names{cplex_opt.qpmethod+1});
        end
        %% ensure H is numerically symmetric
        if ~isequal(H, H')
            H = (H + H')/2;
        end
        [x, f, eflag, output, lam] = ...
            cplexqp(H, c, Ai, bi, Ae, be, xmin, xmax, x0, cplex_opt);
    end
end

%% workaround for eflag == 5 (which we have seen return infeasible results)
%%          cplexstatus: 6
%%    cplexstatusstring: 'non-optimal'
%%              message: 'Solution with numerical issues'
if eflag > 1
    warning('qps_cplex: Undocumented ''exitflag'' value (%d)\n          cplexstatus: %d\n    cplexstatusstring: ''%s''\n              message: ''%s''', eflag, output.cplexstatus, output.cplexstatusstring, output.message);
    if eflag == 5 && mi
        eflag = 1;      %% give it a try for the MI phase
    else
        eflag = -100 - eflag;
    end
end

%% check for empty results (in case optimization failed)
if isempty(x)
    x = NaN(nx, 1);
end
if isempty(f)
    f = NaN;
end
if isempty(lam)
    lam.ineqlin = NaN(length(bi), 1);
    lam.eqlin   = NaN(length(be), 1);
    lam.lower   = NaN(nx, 1);
    lam.upper   = NaN(nx, 1);
    mu_l        = NaN(nA, 1);
    mu_u        = NaN(nA, 1);
else
    mu_l        = zeros(nA, 1);
    mu_u        = zeros(nA, 1);
end
if unconstrained
    lam.eqlin = [];
end

%% negate prices depending on version
if vnum < 12.003
    lam.eqlin   = -lam.eqlin;
    lam.ineqlin = -lam.ineqlin;
end

%% repackage lambdas
kl = find(lam.eqlin < 0);   %% lower bound binding
ku = find(lam.eqlin > 0);   %% upper bound binding

mu_l(ieq(kl)) = -lam.eqlin(kl);
mu_l(igt) = lam.ineqlin(nlt+(1:ngt));
mu_l(ibx) = lam.ineqlin(nlt+ngt+nbx+(1:nbx));

mu_u(ieq(ku)) = lam.eqlin(ku);
mu_u(ilt) = lam.ineqlin(1:nlt);
mu_u(ibx) = lam.ineqlin(nlt+ngt+(1:nbx));

lambda = struct( ...
    'mu_l', mu_l, ...
    'mu_u', mu_u, ...
    'lower', lam.lower, ...
    'upper', lam.upper ...
);

if mi && eflag == 1 && (~isfield(opt, 'skip_prices') || ~opt.skip_prices)
    if verbose
        fprintf('--- Integer stage complete, starting price computation stage ---\n');
    end
    if isfield(opt, 'price_stage_warn_tol') && ~isempty(opt.price_stage_warn_tol)
        tol = opt.price_stage_warn_tol;
    else
        tol = 1e-7;
    end
    k = find(vtype == 'I' | vtype == 'B' | vtype == 'N' | ...
            (vtype == 'S' & x' == 0));
    x(k) = round(x(k));
    xmin(k) = x(k);
    xmax(k) = x(k);
    x0 = x;
    opt.cplex_opt.lpmethod = 1;     %% primal simplex
    opt.cplex_opt.qpmethod = 1;     %% primal simplex

    [x_, f_, eflag_, output_, lambda] = qps_cplex(H, c, A, l, u, xmin, xmax, x0, opt);
    if eflag ~= eflag_
        error('miqps_cplex: EXITFLAG from price computation stage = %d', eflag_);
    end
    if abs(f - f_)/max(abs(f), 1) > tol
        warning('miqps_cplex: relative mismatch in objective function value from price computation stage = %g', abs(f - f_)/max(abs(f), 1));
    end
    xn = x;
    xn(abs(xn)<1) = 1;
    [mx, k] = max(abs(x - x_) ./ xn);
    if mx > tol
        warning('miqps_cplex: max relative mismatch in x from price computation stage = %g (%g)', mx, x(k));
    end
    output.price_stage = output_;
end


function b = cptsl ( n, d, e, b )

%*****************************************************************************80
%
%% CPTSL solves a Hermitian positive definite tridiagonal linear system.
%
%  Discussion;
%
%    The system does not have to be factored first.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 May 2007
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Jim Bunch, Cleve Moler, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979,
%    ISBN13: 978-0-898711-72-1,
%    LC: QA214.L56.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, complex D(N), the diagonal of the matrix.
%
%    Input, complex E(N), the superdiagonal of the matrix in E(1:N-1).
%
%    Input, complex B(N), the right hand side.
%
%    Output, complex B(N), the solution.
%

%
%  Check for 1 x 1 case.
%
  if ( n == 1 )
    b(1) = b(1) / d(1);
    return
  end

  nm1 = n - 1;
  nm1d2 = floor ( ( n - 1 ) / 2 );

  if ( n ~= 2 )

    kbm1 = n - 1;
%
%  Zero top half of subdiagonal and bottom half of superdiagonal.
%
    for k = 1 : nm1d2
      t1 = conj ( e(k) ) / d(k);
      d(k+1) = d(k+1) - t1 * e(k);
      b(k+1) = b(k+1) - t1 * b(k);
      t2 = e(kbm1) / d(kbm1+1);
      d(kbm1) = d(kbm1) - t2 * conj ( e(kbm1) );
      b(kbm1) = b(kbm1) - t2 * b(kbm1+1);
      kbm1 = kbm1 - 1;
    end

  end

  kp1 = nm1d2 + 1;
%
%  Clean up for possible 2 x 2 block at center.
%
  if ( mod ( n, 2 ) == 0 )
    t1 = conj ( e(kp1) ) / d(kp1);
    d(kp1+1) = d(kp1+1) - t1 * e(kp1);
    b(kp1+1) = b(kp1+1) - t1 * b(kp1);
    kp1 = kp1 + 1;
  end
%
%  Back solve starting at the center, going towards the top and bottom.
%
  b(kp1) = b(kp1) / d(kp1);

  if ( n ~= 2 )

    k = kp1 - 1;
    ke = kp1 + nm1d2 - 1;

    for kf = kp1 : ke
      b(k) = ( b(k) - e(k) * b(k+1) ) / d(k);
      b(kf+1) = ( b(kf+1) - conj ( e(kf) ) * b(kf) ) / d(kf+1);
      k = k - 1;
    end

  end

  if ( mod ( n, 2 ) == 0 )
    b(1) = ( b(1) - e(1) * b(2) ) / d(1);
  end

  return
end


function c = tapas_hgf_jget_config
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Contains the configuration for the Hierarchical Gaussian Filter (HGF) model for the jumping
% Gaussian estimation task (JGET).
%
% The HGF is the model introduced in
%
% Mathys C, Daunizeau J, Friston, KJ, and Stephan KE. (2011). A Bayesian foundation
% for individual learning under uncertainty. Frontiers in Human Neuroscience, 5:39.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% The HGF configuration consists of the priors of parameters and initial values. All priors are
% Gaussian in the space where the quantity they refer to is estimated. They are specified by their
% sufficient statistics: mean and variance (NOT standard deviation).
%
% Quantities are estimated in their native space if they are unbounded (e.g., the omegas). They are
% estimated in log-space if they have a natural lower bound at zero (e.g., the sigmas).
%
% Parameters can be fixed (i.e., set to a fixed value) by setting the variance of their prior to
% zero. Aside from being useful for model comparison, the need for this arises whenever the scale
% and origin at the j-th level are arbitrary. This is the case if the observation model does not
% contain the representations mu_j and sigma_j. A choice of scale and origin is then implied by
% fixing the initial value mu_j_0 of mu_j and either kappa_j-1 or omega_j-1.
%
% Fitted trajectories can be plotted by using the command
%
% >> tapas_hgf_jget_plotTraj(est)
%
% where est is the stucture returned by fitModel. This structure contains the estimated
% perceptual parameters in est.p_prc and the estimated trajectories of the agent's
% representations (cf. Mathys et al., 2011). Their meanings are:
%
%         est.p_prc.mux_0      row vector of initial values of mu_x (in ascending order of levels)
%         est.p_prc.sax_0      row vector of initial values of sigma_x (in ascending order of levels)
%         est.p_prc.mua_0      row vector of initial values of mu_alpha (in ascending order of levels)
%         est.p_prc.saa_0      row vector of initial values of sigma_alpha (in ascending order of levels)
%         est.p_prc.kau        kappa_u
%         est.p_prc.omu        omega_u
%         est.p_prc.kax        row vector of kappa_x (in ascending order of levels)
%         est.p_prc.omx        row vector of omega_x (in ascending order of levels)
%         est.p_prc.kaa        row vector of kappa_alpha (in ascending order of levels)
%         est.p_prc.oma        row vector of omega_alpha (in ascending order of levels)
%
%         est.traj.mux         mux (rows: trials, columns: levels)
%         est.traj.sax         sigma_x (rows: trials, columns: levels)
%         est.traj.muxhat      prediction of mu_x (rows: trials, columns: levels)
%         est.traj.saxhat      precisions of predictions of x (rows: trials, columns: levels)
%         est.traj.wx          weighting factors for x (rows: trials, columns: levels)
%         est.traj.dax         volatility prediction errors in x (rows: trials, columns: levels)
%         est.traj.mua         mu_alpha (rows: trials, columns: levels)
%         est.traj.saa         sigma_alpha (rows: trials, columns: levels)
%         est.traj.muahat      prediction of mu_alpha (rows: trials, columns: levels)
%         est.traj.saahat      precisions of predictions of alpha (rows: trials, columns: levels)
%         est.traj.wa          weighting factors for alpha (rows: trials, columns: levels)
%         est.traj.daa         volatility prediction errors in alpha (rows: trials, columns: levels)
%         est.traj.dau         input prediction error
%
%
%
% Tips:
% - If you get an error saying that the prior means are in a region where model assumptions are
%   violated, lower the prior means of the omegas, starting with the highest level and proceeding
%   downwards.
%
% - Alternatives are lowering the prior means of the kappas, if they are not fixed, or adjusting
%   the values of the kappas or omegas, if any of them are fixed.
%
% - If the log-model evidence cannot be calculated because the Hessian poses problems, look at
%   est.optim.H and fix the parameters that lead to NaNs.
%
% - Your guide to all these adjustments is the log-model evidence (LME). Whenever the LME increases
%   by at least 3 across datasets, the adjustment was a good idea and can be justified by just this:
%   the LME increased, so you had a better model.
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2013-2014 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/>.

% Config structure
c = struct;

% Model name
c.model = 'hgf_jget';

% Number of levels (minimum: 2)
c.n_levels = 2;

% Sufficient statistics of Gaussian parameter priors

% PLACEHOLDER VALUES
% It is often convenient to set some priors to values
% derived from the inputs. This can be achieved by
% using placeholder values. The available placeholders
% are:
%
% 99991   Value of the first input
%         Usually a good choice for mux_0mu(1)
% 99992   Variance of the first 20 inputs
%         Usually a good choice for mux_0sa(1)
% 99993   Log-variance of the first 20 inputs
%         Usually a good choice for logsax_0mu(1) and mua_0mu(1)
% 99994   Log-variance of the first 20 inputs minus two
%         Usually a good choice for omxmu(1)

% Initial mus and sigmas
% Format: row vectors of length n_levels
% For all but the first level, this is usually best
% kept fixed to 1 (determines origin on x_i-scale).
c.mux_0mu = [99991, 1];
c.mux_0sa = [    0, 0];

c.logsax_0mu = [log(3), log(0.1)];
c.logsax_0sa = [      0,       0];

c.mua_0mu = [log(1), 1];
c.mua_0sa = [   0.1, 0];

c.logsaa_0mu = [log(3), log(0.1)];
c.logsaa_0sa = [      0,       0];

% Kappas
% Format: row vector of length n_levels-1 (except kappa_u: scalar)
% This should be fixed (preferably to 1) if the observation model
% does not use mu_i+1 (kappa then determines the scaling of x_i+1).
c.logkaumu = log(1);
c.logkausa = 0;

c.logkaxmu = [log(1)];
c.logkaxsa = [     0];

c.logkaamu = [log(1)];
c.logkaasa = [     0];

% Omegas
% Format: row vector of length n_levels (except omega_u: scalar)
c.omumu = 0;
c.omusa = 0;

c.omxmu = [  0,  -7];
c.omxsa = [5^2,   1];

c.omamu = [  0,  -7];
c.omasa = [5^2,   1];

% Gather prior settings in vectors
c.priormus = [
    c.mux_0mu,...
    c.logsax_0mu,...
    c.mua_0mu,...
    c.logsaa_0mu,...
    c.logkaumu,...
    c.logkaxmu,...
    c.logkaamu,...
    c.omumu,...
    c.omxmu,...
    c.omamu,...
         ];

c.priorsas = [
    c.mux_0sa,...
    c.logsax_0sa,...
    c.mua_0sa,...
    c.logsaa_0sa,...
    c.logkausa,...
    c.logkaxsa,...
    c.logkaasa,...
    c.omusa,...
    c.omxsa,...
    c.omasa,...
         ];

% Check whether we have the right number of priors
expectedLength = 8*c.n_levels;
if length([c.priormus, c.priorsas]) ~= 2*expectedLength;
    error('tapas:hgf:PriorDefNotMatchingLevels', 'Prior definition does not match number of levels.')
end

% Model function handle
c.prc_fun = @tapas_hgf_jget;

% Handle to function that transforms perceptual parameters to their native space
% from the space they are estimated in
c.transp_prc_fun = @tapas_hgf_jget_transp;

return;


function [ bboxes ] = scale_bboxes( bboxes, scale_factor )
% scale_bboxes: it scales the set bounding boxes bboxes by the scale_factor
% factor.
%
% INPUT:
% 1) bboxes: a N x 4 array with the input bounding box coordinates in the
% form of [x0,y0,x1,y1] (where (x0,y0) is the top-left corner and (x1,y1)
% the bottom left corner)
% 2) scale_factor: a 1 x 1 or 2 x 1 array with the scaling factor of the
% bounding boxes. If scale_factor is a 1 x 1 array then the same scaling
% factor will be applied on both the x and y axis. If scale_factor is a
% 2 x 1 (or 1 x 2) array then the bounding boxes will be scaled across the
% y dimension by scale_factor(1) and across the x dimension by
% scale_factor(2).
%
% OUTPUT:
% 1) bboxes: a N x 4 array with the output bounding box coordinates in the
% form of [x0,y0,x1,y1] (where (x0,y0) is the top-left corner and (x1,y1)
% the bottom left corners.
%
% This file is part of the code that implements the following paper:
% Title      : "LocNet: Improving Localization Accuracy for Object Detection"
% Authors    : Spyros Gidaris, Nikos Komodakis
% Institution: Universite Paris Est, Ecole des Ponts ParisTech
% ArXiv link : http://arxiv.org/abs/1511.07763
% code       : https://github.com/gidariss/LocNet
%
% AUTORIGHTS
% --------------------------------------------------------
% Copyright (c) 2016 Spyros Gidaris
%
% Title     : "LocNet: Improving Localization Accuracy for Object Detection"
% ArXiv link: http://arxiv.org/abs/1511.07763
% Licensed under The MIT License [see LICENSE for details]
% ---------------------------------------------------------

if numel(scale_factor) == 1, scale_factor(2) = scale_factor(1); end
assert(numel(scale_factor) == 2);
scale_factor = single(scale_factor);

bboxes_center      = [(bboxes(:,1)+bboxes(:,3)), (bboxes(:,2)+bboxes(:,4))]/2;
bboxes_width_half  = (bboxes(:,3) - bboxes(:,1))/2;
bboxes_width_half  = bboxes_width_half * scale_factor(2);

bboxes_height_half = (bboxes(:,4) - bboxes(:,2))/2;
bboxes_height_half = bboxes_height_half * scale_factor(1);

bboxes = round([bboxes_center(:,1) - bboxes_width_half, ...
                bboxes_center(:,2) - bboxes_height_half, ...
                bboxes_center(:,1) + bboxes_width_half, ...
                bboxes_center(:,2) + bboxes_height_half]);
end


function [y,nz] = ompdenoise1(params,msgdelta)
%OMPDENOISE1 OMP denoising of 1-D signals.
%  OMPDENOISE1 denoises a 1-dimensional signal using OMP denoising. The
%  function syntax is identical to OMPDENOISE, but it runs significantly
%  faster on 1-D signals. OMPDENOISE1 requires somewhat more memory than
%  OMPDENOISE (approximately the size of the input signal), so if memory is
%  limited, OMPDENOISE can be used instead.
%
%  See also OMPDENOISE.

%  Ron Rubinstein
%  Computer Science Department
%  Technion, Haifa 32000 Israel
%  ronrubin@cs
%
%  August 2009

% parse input arguments %

x = params.x(:);
D = params.dict;
blocksize = params.blocksize;

% maxval %
if (isfield(params,'maxval'))
  maxval = params.maxval;
else
  maxval = 1;
end

% gain %
if (isfield(params,'gain'))
  gain = params.gain;
else
  gain = 1.15;
end

% maxatoms %
if (isfield(params,'maxatoms'))
  maxatoms = params.maxatoms;
else
  maxatoms = floor(blocksize/2);
end

% stepsize %
if (isfield(params,'stepsize'))
  stepsize = params.stepsize;
else
  stepsize = 1;
end
if (any(stepsize<1))
  error('Invalid step size.');
end

% noise mode %
if (isfield(params,'noisemode'))
  switch lower(params.noisemode)
    case 'psnr'
      sigma = maxval / 10^(params.psnr/20);
    case 'sigma'
      sigma = params.sigma;
    otherwise
      error('Invalid noise mode specified');
  end
elseif (isfield(params,'sigma'))
  sigma = params.sigma;
elseif (isfield(params,'psnr'))
  sigma = maxval / 10^(params.psnr/20);
else
  error('Noise strength not specified');
end

% lambda %
if (isfield(params,'lambda'))
  lambda = params.lambda;
else
  lambda = maxval/(10*sigma);
end

% msgdelta %
if (nargin <2)
  msgdelta = 5;
end
if (msgdelta<=0)
  msgdelta = -1;
end

epsilon = sqrt(blocksize) * sigma * gain;   % target error for omp

MEM_LOW = 1;
MEM_NORMAL = 2;
MEM_HIGH = 3;

if (isfield(params,'memusage'))
  switch lower(params.memusage)
    case 'low'
      memusage = MEM_LOW;
    case 'normal'
      memusage = MEM_NORMAL;
    case 'high'
      memusage = MEM_HIGH;
    otherwise
      error('Invalid memory usage mode');
  end
else
  memusage = MEM_NORMAL;
end

% compute G %

G = [];
if (memusage >= MEM_NORMAL)
  G = D'*D;
end

% verify dictionary normalization %

if (isempty(G))
  atomnorms = sum(D.*D);
else
  atomnorms = diag(G);
end
if (any(abs(atomnorms-1) > 1e-2))
  error('Dictionary columns must be normalized to unit length');
end

% denoise the signal %

% process the signal in batches to conserve memory
% choose batchsize so im2colstep returns a matrix of approximately the same
% size as the signal
batchsize = ceil(length(x)*stepsize/blocksize + blocksize);

y = zeros(size(x));
ids = 1:min(batchsize,length(x));
nz = 0;

blocknum = floor((length(x)-blocksize)/stepsize) + 1;
processedblocks = 0;
tid = timerinit('ompdenoise', blocknum);
while (length(ids)>=blocksize)

  % extract the signal blocks
  blocks = im2colstep(x(ids),[blocksize 1],[stepsize 1]);

  % remove DC
  [blocks, dc] = remove_dc(blocks,'columns');

  % denoise the blocks
  if (memusage == MEM_LOW)
    gamma = omp2(D,blocks,[],epsilon,'maxatoms',maxatoms,'checkdict','off');
  else
    gamma = omp2(D'*blocks,sum(blocks.*blocks),G,epsilon,'maxatoms',maxatoms,'checkdict','off');
  end
  nz = nz + nnz(gamma);
  cleanblocks = add_dc(D*gamma, dc, 'columns');

  y(ids) = y(ids) + col2imstep(cleanblocks, [length(ids) 1], [blocksize 1], [stepsize 1]);
  ids = ids + floor((batchsize-blocksize)/stepsize)*stepsize + stepsize;
  if (ids(end)>length(x))
    ids = ids(ids<=length(x));
  end

  % display status
  if (msgdelta>0)
    processedblocks = processedblocks + size(blocks,2);
    timereta(tid, processedblocks, msgdelta);
  end

end

if (msgdelta>0)
  timereta(tid, blocknum);
end

cnt = countcover(size(x),[blocksize 1],[stepsize 1]);
y = (y+lambda*x)./(cnt + lambda);
y = reshape(y,size(params.x));


function [varargout]=patchPointDist(V,F,Vp,dL)

methodOpt='near-norm';

switch methodOpt
    case 'near-norm'

        [Nn,Vn]=patchNormal(F,V); %Get face normals and normal vector origins
        [Dn,indMin]=minDist(Vp,Vn); %Get distances to origins and find closest faces

        %Attempt to find orthogonal projection to closest face
        N=Nn(indMin,:); %Order the normals
        W=Vp-Vn(indMin,:);
        Dm=(dot(N,W,2));
        D=abs(Dm);
        Dp=Dm(:,ones(1,3)).*N;
        Vpd=Vp-Dp;

        %Test if projection is valid
        TR = triangulation(F,V);
        B = cartesianToBarycentric(TR,indMin,Vpd);
        logicOutside=any(B<-dL,2);

        %Replace invalid projections with face centre points
        D(logicOutside)=Dn(logicOutside); %Replace distances
        Dp(logicOutside,:)=Vp(logicOutside,:)-Vn(indMin(logicOutside),:); %Replace difference vectors

    otherwise
        error('Wrong method option chosen');
end

switch nargout
    case 1
        varargout{1}=D;
    case 2
        varargout{1}=D;
        varargout{2}=Dp;
    case 3
        varargout{1}=D;
        varargout{2}=Dp;
        varargout{3}=logicOutside;
end

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


function blas2_test01 ( )

%*****************************************************************************80
%
%% BLAS2_TEST01 tests DGEMV.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 February 2014
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BLAS2_TEST01\n' );
  fprintf ( 1, '  For a general matrix A,\n' );
  fprintf ( 1, '  DGEMV computes y := alpha * A * x + beta * y\n' );
  fprintf ( 1, '  or             y := alpha * A'' * x + beta * y.\n' );
%
%  y = alpha * A * x + beta * y
%
  trans = 'N';
  m = 5;
  n = 4;
  alpha = 2.0;
  lda = m;
  a = r8mat_test ( trans, lda, m, n );
  x = zeros ( n, 1 );
  for i = 1 : n
    x(i) = i;
  end
  incx = 1;
  beta = 3.0;
  y = zeros ( m, 1 );
  for i = 1 : m
    y(i) = 10 * i;
  end
  incy = 1;

  r8mat_print ( m, n, a, '  Matrix A:' );
  r8vec_print ( n, x, '  Vector X:' );
  r8vec_print ( m, y, '  Vector Y:' );

  y = dgemv ( trans, m, n, alpha, a, lda, x, incx, beta, y, incy );

  r8vec_print ( m, y, '  Result Y = alpha * A  * x + beta * y' );
%
%  y = alpha * A' * x + beta * y
%
  trans = 'T';
  m = 5;
  n = 4;
  alpha = 2.0;
  lda = m;
  a = r8mat_test ( trans, lda, n, m );
  x = zeros ( m, 1 );
  for i = 1 : m
    x(i) = i;
  end
  incx = 1;
  beta = 3.0;
  y = zeros ( n, 1 );
  for i = 1 : n
    y(i) = 10 * i;
  end
  incy = 1;

  r8mat_print ( m, n, a, '  Matrix A:' );
  r8vec_print ( m, x, '  Vector X:' );
  r8vec_print ( n, y, '  Vector Y:' );

  y = dgemv ( trans, m, n, alpha, a, lda, x, incx, beta, y, incy );

  r8vec_print ( n, y, '  Result Y = alpha * A'' * x + beta * y' );

  return
end


function T = sptoeplitz(col, row)
%SPTOEPLITZ   Sparse Toeplitz matrix.
%   SPTOEPLITZ(C, R) produces a sparse nonsymmetric Toeplitz matrix having
%   C as its first column and R as its first row. Neither C nor R needs to
%   be sparse. No full-size dense matrices are formed.
%
%   SPTOEPLITZ(R) is a sparse symmetric/Hermitian Toeplitz matrix.
%
%   Examples:
%     sptoeplitz( real( (1i).^(0:8) ) )   % 9x9, 41 nonzeros
%     sptoeplitz( [-2 1 zeros(1,9998)] ); % classic 2nd difference
%
% See also TOEPLITZ, SPDIAGS.

% Based on SPTOEPLITZ.M on the Mathworks File Exchange,
% Copyright (c) 2006 by Tobin Driscoll (tobin.driscoll@gmail.com).

% Developer note: This is needed by multiplication operators.

% This part is borrowed from built-in Toeplitz.
if ( nargin < 2 ) % Symmetric case
  col(1) = conj(col(1));
  row = col;
  col = conj(col);
else
  if ( col(1) ~= row(1) )
    warning('MATLAB:sptoeplitz:DiagonalConflict',['First element of ' ...
      'input column does not match first element of input row. ' ...
      '\n         Column wins diagonal conflict.'])
  end
end

% Size of result.
m = length(col(:));
n = length(row(:));

% Only use toeplitz if you have too... fairly slow.
if ( (m < 2e3) && (n < 2e3) )
    % Note: nnz(col) -- Number of nonzero elements in col
    if ( nnz(col) == 1 && nnz(row) == 1 )
        Ic = find(col);
        Ir = find(row);
        if ( Ic == 1 )
            T = spdiags(col(Ic)*ones(m, 1), 0, m, n);
        else
            T = spdiags([col(Ic)*ones(m, 1) row(Ir)*ones(n , 1)], ...
                [-Ic + 1, Ir - 1], m, n);
        end
    else
        T = toeplitz(col, row);
        T = sparse(T);
    end
else
    % Locate the nonzero diagonals.
    [ic, jc, sc] = find(col(:));
    row(1) = 0;  % not used
    [ir, jr, sr] = find(row(:));

    % Use spdiags for construction.
    d = [ ir - 1; 1 - ic ];
    B = repmat( [ sr; sc ].', min(m, n), 1 );
    T = spdiags(B, d, m, n);
end

end


function bk = r4_besks ( xnu, x, nin )

%*****************************************************************************80
%
%% R4_BESKS evaluates a sequence of K Bessel functions at X.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real XNU, the order of the first function.
%    |XNU| < 1.
%
%    Input, real X, the argument.
%
%    Input, integer NIN, the absolute value of NIN indicates
%    the number of terms to compute.
%    If NIN < 0, successive values of NU count DOWN from XNU.
%    If NIN > 0, successive values of NU count UP from XNU.
%
%    Output, real BK(abs(NIN)), the K Bessel functions.
%
  persistent xmax

  if ( isempty ( xmax ) )
    xmax = - log ( r4_mach ( 1 ) );
    xmax = xmax + 0.5 * log ( 3.14 * 0.5 / xmax );
  end

  bk = r4_beskes ( xnu, x, nin );

  expxi = exp ( - x );
  n = abs ( nin );

  bk(1:n) = expxi * bk(1:n);

  return
end


function M = legendreMass(k)
% Compute the mass matrix using legendre polinomials of order 'k' as a base
% for our expansion.

M = diag(2./(2*(0:k)+1));


function pde = TorusTime(mum,mup,sigm,sigp,epsm,epsp,omega,x0,y0,z0,r1,r2)
%% USAGE: polynomial solution for Poisson equation
%  Last Modified: 02/21/2020 by Xu Zhang

%% PDE Structure
pde = struct('intf',@intf,...
    'f1',@f1,'f2',@f2,'f3',@f3,...
    'fm1',@fm1,'fm2',@fm2,'fm3',@fm3,...
    'fp1',@fp1,'fp2',@fp2,'fp3',@fp3,...
    'Mu',@Mu,'Mum',@Mum,'Mup',@Mup,'one',@one,...
    'Epslon',@Epslon,'Epslonm',@Epslonm,'Epslonp',@Epslonp,...
    'Sig',@Sig,'Sigm',@Sigm,'Sigp',@Sigp);

pde.mum = mum;
pde.mup = mup;
pde.sigm = sigm;
pde.sigp = sigp;
pde.epsm = epsm;
pde.epsp = epsp;
%% interface function
    function u = intf(x,y,z)
        u = (sqrt((x-x0).^2+(y-y0).^2)-r2).^2 + (z-z0).^2 - r1^2;
    end

%% exact solution

%% right hand side function
    function u = f1(x,y,z,t)
        u = fm1(x,y,z,t);
        id = intf(x,y,z) > 0;
        u(id) = fp1(x(id),y(id),z(id),t);
    end
    function u = f2(x,y,z,t)
        u = fm2(x,y,z,t);
        id = intf(x,y,z) > 0;
        u(id) = fp2(x(id),y(id),z(id),t);
    end
    function u = f3(x,y,z,t)
        u = fm3(x,y,z,t);
        id = intf(x,y,z) > 0;
        u(id) = fp3(x(id),y(id),z(id),t);
    end

    function u = fm1(x,y,z,t)
        u = -2*pi*omega*sin(myacos(x,y))*cos(2*pi*omega*t);
    end
    function u = fm2(x,y,z,t)
        u = 2*pi*omega*cos(myacos(x,y))*cos(2*pi*omega*t);
    end
    function u = fm3(x,y,z,t)
        u = zeros(size(x));
    end
    function u = fp1(x,y,z,t)
        u = zeros(size(x));
    end
    function u = fp2(x,y,z,t)
        u = zeros(size(x));
    end
    function u = fp3(x,y,z,t)
        u = zeros(size(x));
    end
    function theta = myacos(x,y)
        theta = (2*pi*(1-sign(y))/2 + sign(y).*acos(x./sqrt(x.^2+y.^2))).*abs(sign(y))+...
            (1-abs(sign(y))).*((1-x)/2*pi);
    end

%% Diffusion coefficient function
    function u = Mu(x,y,z)
        u = Mum(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Mup(x(id),y(id),z(id));
    end
    function u = Mum(x,y,z)
        u = mum^(-1)*ones(size(x));
    end
    function u = Mup(x,y,z)
        u = mup^(-1)*ones(size(x));
    end

%% Mass coefficient function
    function u = Epslon(x,y,z)
        u = Epslonm(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Epslonp(x(id),y(id),z(id));
    end
    function u = Epslonm(x,y,z)
        u = epsm*ones(size(x));
    end
    function u = Epslonp(x,y,z)
        u = epsp*ones(size(x));
    end

    function u = Sig(x,y,z)
        u = Sigm(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Sigp(x(id),y(id),z(id));
    end
    function u = Sigm(x,y,z)
        u = sigm*ones(size(x));
    end
    function u = Sigp(x,y,z)
        u = sigp*ones(size(x));
    end

%% Other function
    function u = one(x,y,z)
        u = ones(size(x));
    end
end


function [P,T] = cubic_flat_eval(C,tol)
  % CUBIC_FLAT_EVAL Recursively subdivide a cubic Bezier curve until each
  % segment spans a region of the curve that is locally flat up to a given
  % tolerance (i.e., this computes an adaptive refinement).
  %
  % [P,T] = cubic_flat_eval(C,tol)
  %
  % Inputs:
  %   C  4 by dim list of control points
  % Outputs:
  %   P  #P by dim list of evaluated points
  %   T  #T list of corresponding parameter values
  %
  % See also: cubic_eval, cubic_is_flat, cubic_split
  %
  if cubic_is_flat(C,tol)
    P = C([1 4],:);
    T = [0 1];
  else
    [C1,C2] = cubic_split(C,0.5);
    [P1,T1] = cubic_flat_eval(C1,tol);
    [P2,T2] = cubic_flat_eval(C2,tol);
    P = [P1;P2(2:end,:)];
    T = [T1*0.5;0.5+T2(2:end,:)];
  end
end


function X = cs_qleft (V, Beta, p, Y)
%CS_QLEFT apply Householder vectors on the left.
%   X = cs_qleft(V,Beta,p,Y) computes X = Hn*...*H2*H1*P*Y = Q'*Y where Q is
%   represented by the Householder vectors V, coefficients Beta, and
%   permutation p.  p can be [], which denotes the identity permutation.
%
%   Example:
%       Prob = UFget ('HB/well1033') ; A = Prob.A ; [m n] = size (A) ;
%       b = rand (m,1) ;
%       [V,beta,p,R] = cs_qr (A) ; % QR factorization of A(p,:)
%       b1 = cs_qleft (V, beta, p, b) ;
%       x1 = R (1:n,1:n) \ b1 (1:n) ;
%       x2 = A\b ;
%       norm (x1-x2)
%
%   See also CS_QR, CS_QRIGHT.

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

[m2 n] = size (V) ;
[m ny] = size (Y) ;
X = Y ;
if (m2 > m)
    if (issparse (Y))
        X = [X ; sparse(m2-m,ny)] ;
    else
        X = [X ; zeros(m2-m,ny)] ;
    end
end
if (~isempty (p))
    X = X (p,:) ;
end
for k = 1:n
    X = X - V (:,k) * (Beta (k) * (V (:,k)' * X)) ;
end


function partition=new_partitions(zones,sigma,r,N,x)
%sigma=1;
%r=8.4;
T=r/2/sigma;
kappa=size(zones);
kappa=kappa(1);
%width=sigma*(2*N/r-1);
%x=-width:1/200:width;

partition=zeros(kappa,length(x));

if kappa==1
  k=kappa;
  for j=1:length(x)
      if x(j)<=(zones(k,1)-1)/T+sigma
        partition(k,j)=0;
      elseif (zones(k,1)-1)/T+sigma<x(j) & x(j)<-sigma
        partition(k,j)=rho((-x(j)-sigma)/(-2*sigma-(zones(k,1)-1)/T));
      elseif -sigma<=x(j) & x(j)<=sigma
        partition(k,j)=1;
      elseif sigma<x(j) & x(j)<(zones(k,2)+1)/T-sigma
        partition(k,j)=rho((x(j)-sigma)/((zones(k,2)+1)/T-2*sigma));
      else
        partition(k,j)=0;
      end
  end
else
for k=1:kappa
  if k==1
    for j=1:length(x)
      if x(j)<=(zones(k,1)-1)/T+sigma
        partition(k,j)=0;
      elseif (zones(k,1)-1)/T+sigma<x(j) & x(j)<-sigma
        partition(k,j)=rho((-x(j)-sigma)/(-2*sigma-(zones(k,1)-1)/T));
      elseif -sigma<=x(j) & x(j)<=(zones(k+1,1)-1)/T+sigma
        partition(k,j)=1;
      elseif (zones(k+1,1)-1)/T+sigma<x(j) & x(j)<(zones(k,2)+1)/T-sigma
	partition(k,j)=rho((x(j)-((zones(k+1,1)-1)/T+sigma))/((zones(k,2)-zones(k+1,1)+2)/T-2*sigma));
%      elseif (1-N)/T+sigma<x(j) & x(j)<(1-N/2)/T
%        partition(k,j)=1-rho((-x(j)+(1-N/2)/T)/(N/T/2-sigma))/2;
%      elseif x(j)==(1-N/2)/T
%        partition(k,j)=1/2;
%      elseif (1-N/2)/T<x(j) & x(j)<1/T-sigma
%        partition(k,j)=rho((x(j)-(1-N/2)/T)/(N/T/2-sigma))/2;
      else
        partition(k,j)=0;
      end
    end
  elseif k==kappa
    for j=1:length(x)
      if x(j)<=(zones(k,1)-1)/T+sigma
        partition(k,j)=0;
      elseif (zones(k,1)-1)/T+sigma<x(j) & x(j)<(zones(k-1,2)+1)/T-sigma
        partition(k,j)=1-rho((x(j)-((zones(k,1)-1)/T+sigma))/((zones(k-1,2)-zones(k,1)+2)/T-2*sigma));
%      elseif sigma-1/T<x(j) & x(j)<(N/2-1)/T
%        partition(k,j)=rho((-x(j)+(N/2-1)/T)/(N/T/2-sigma))/2;
%      elseif x(j)==(N/2-1)/T
%        partition(k,j)=1/2;
%      elseif (N/2-1)/T<x(j) & x(j)<(N-1)/T-sigma
%        partition(k,j)=1-rho((x(j)-(N/2-1)/T)/(N/T/2-sigma))/2;
      elseif (zones(k-1,2)+1)/T-sigma<=x(j) & x(j)<=sigma
        partition(k,j)=1;
      elseif sigma<x(j) & x(j)<(zones(k,2)+1)/T-sigma
        partition(k,j)=rho((x(j)-sigma)/((zones(k,2)+1)/T-2*sigma));
      else
        partition(k,j)=0;
      end
    end
  else
    for j=1:length(x)
      if x(j)<=(zones(k,1)-1)/T+sigma
        partition(k,j)=0;
      elseif (zones(k,1)-1)/T+sigma<x(j) & x(j)<(zones(k-1,2)+1)/T-sigma
        partition(k,j)=1-rho((x(j)-((zones(k,1)-1)/T+sigma))/((zones(k-1,2)-zones(k,1)+2)/T-2*sigma));
%      elseif (-N+k-1)/T+sigma<x(j) & x(j)<(k-1-N/2)/T
%        partition(k,j)=rho((-x(j)+(k-1-N/2)/T)/(N/T/2-sigma))/2;
%      elseif x(j)==(k-1-N/2)/T
%        partition(k,j)=1/2;
%      elseif (k-1-N/2)/T<x(j) & x(j)<(k-1)/T-sigma
%        partition(k,j)=1-rho((x(j)-(k-1-N/2)/T)/(N/T/2-sigma))/2;
      elseif (zones(k-1,2)+1)/T-sigma<=x(j) & x(j)<=(zones(k+1,1)-1)/T+sigma
        partition(k,j)=1;
      elseif (zones(k+1,1)-1)/T+sigma<x(j) & x(j)<(zones(k,2)+1)/T-sigma
        partition(k,j)=rho((x(j)-((zones(k+1,1)-1)/T+sigma))/((zones(k,2)-zones(k+1,1)+2)/T-2*sigma));
%      elseif (-N+k)/T+sigma<x(j) & x(j)<(k-N/2)/T
%        partition(k,j)=1-rho((-x(j)+(k-N/2)/T)/(N/T/2-sigma))/2;
%      elseif x(j)==(k-N/2)/T
%        partition(k,j)=1/2;
%      elseif (k-N/2)/T<x(j) & x(j)<k/T-sigma
%        partition(k,j)=rho((x(j)-(k-N/2)/T)/(N/T/2-sigma))/2;
      else
        partition(k,j)=0;
      end
    end
  end
%  plot(x,partition(k,:))
%  axis([min(x) max(x) 0 1])
%  [k kappa]
%  zones(k,:)
%  pause
end
end
%plot(x,partition'); %'
%pause

%total=zeros(size(x));
%for j=1:N
%total=total+partition(j,:);
%end
%spy(1-total);


function A = affineBuild(trans, rot, scale, skew)
%
% A = affineBuild(trans, rot, scale, skew)
%
% Builds an affine (orthogonal linear) transformation matrix from the four
% componets: translations, rotations scales and skews.
%
% trans, rot, scale, skew should all be 1x3 vectors specifying x,y,z.
% Rotations are in radians, and can be described as pitch (x-rotation),
% roll (y-rotation) and yaw (z-rotation).
%
% The transformations are applied in the following order:
%
% 1) translations
% 2) rotations
% 3) scaling
% 4) skews
%
% The form of the transform assumes a PRE-multiplication format:
% Y = A*X where X and Y are 4 x n arrays of n coordinates.
%
% HISTORY:
%   2004.03.12 RFD (bob@white.stanford.edu) shamelessly copied the core
%   algorithm from spm99's spm_matrix.m.
%   2005.07.08 ras (sayres at stanford edu) imported into mrVista 2.0
%   2005.07.28 ras enorces double class for inputs
if ~exist('skew','var') | isempty(skew), skew = [0 0 0];    end
if ~exist('scale','var') | isempty(scale), scale = [1 1 1]; end
if ~exist('rot','var') | isempty(rot), rot = [0 0 0];       end
if ~exist('trans','var') | isempty(trans), trans = [0 0 0]; end
if ~isa(trans,'double'), trans = double(trans);             end
if ~isa(rot,'double'), rot = double(rot);                   end
if ~isa(scale,'double'), scale = double(scale);             end
if ~isa(skew,'double'), skew = double(skew);                end

A  = eye(4);

A  = A*[1 	0 	0  trans(1);
        0 	1 	0  trans(2);
        0 	0 	1  trans(3);
        0 	0 	0  1];

A  = A*[1   0            0            0;
        0   cos(rot(1))  sin(rot(1))  0;
        0  -sin(rot(1))  cos(rot(1))  0;
        0   0    	      0           1];

A  = A*[cos(rot(2))  0  sin(rot(2))  0;
        0    	     1  0            0;
       -sin(rot(2))  0  cos(rot(2))  0;
        0            0  0            1];

A  = A*[cos(rot(3))  sin(rot(3))  0  0;
       -sin(rot(3))  cos(rot(3))  0  0;
        0            0            1  0;
        0     	     0    	      0  1];

A  = A*[scale(1) 0         0         0;
        0        scale(2)  0         0;
        0        0         scale(3)  0;
        0        0         0         1];

A  = A*[1  skew(1)  skew(2)  0;
        0  1        skew(3)  0;
        0  0        1        0;
        0  0        0        1];

return;


function fismat = genfis4(Xin, Xout, fistype, sig_par, varargin)

%GENFIS4 Generates a FIS using CART
%
%   Given separate sets of input and output data, GENFIS4 generates a fuzzy
%   inference system (FIS) using CART algorithm. GENFIS4 accomplishes this
%   by extracting a set of rules that models the data behavior. The rule
%   extraction method first uses TREEINFO to determine the number
%   of rules and membership functions for the antecedents and consequents.
%
%   FIS = GENFIS4(XIN, XOUT) returns a Sugeno-type FIS given input data XIN
%   and output data XOUT. The matrices XIN and XOUT have one column per FIS
%   input and output, respectively.
%
%   FIS = GENFIS4(XIN, XOUT, TYPE) returns FIS of type specified by the
%   argument TYPE. It can take one of two values: 'mamdani' or 'sugeno'.
%
%   FIS = GENFIS4(XIN, XOUT, TYPE, SIGM_PAR) allows you to specify the
%   sigmoid parameters in the argument SIGM_PAR. The parameter is used to
%   form input membership function. The larger SIGM_PAR the closer
%   behaviour of fuzzy CART to behaviour of "crisp" CART. It should be
%   positive scalar or vector. The latter should have size equal to number
%   of FIS inputs. This argument also takes the value 'auto' in which case
%   GENFIS4 uses additional information containing in data set to calculate
%   SIGM_PAR.
%
%   FIS = GENFIS4(XIN, XOUT, TYPE, SIGM_PAR, VARARGIN) allows you to
%   specify options for the CART algorithm. Type HELP CLASSREGTREE and
%   HELP TEST for a list of options that can be specified for the CART
%   algorithm.
%
%   Examples:
%
%       Xin1 = 7 * rand(50, 1);
%       Xin2 = 20 * rand(50, 1) - 10;
%       Xin = [Xin1 Xin2];
%       Xout = 5 * rand(50, 1);
%       fis = genfis4(Xin, Xout);
%
%       fis = genfis4(Xin, Xout, 'mamdani', [10, 1]);
%       specifies the type of FIS and the sigmoid parameters desired.
%
%       fis = genfis4(Xin, Xout, 'mamdani', 'auto', {'minparent', 15});
%       specifies the type of FIS, the sigmoid parameters desired and CART
%       options.
%
%   See also CLASSREGTREE, TEST, TREEINFO, GENFIS3, ANFISX

%   Per Konstantin A. Sidelnikov, 2009.

%%%%%%%%%%%%%%%%%%%%
% Some constants
%%%%%%%%%%%%%%%%%%%%

% Number of standard deviations (can be changed)
NSTD = 1;
% Area under the gauss curve in the
% [mu - NSTD * sigma, mu + NSTD * sigma]
AREA = erf(NSTD / sqrt(2));
% Scaling factor
SCALE = log((1 + AREA) / (1 - AREA)) / NSTD;

%%%%%%%%%%%%%%%%%%%%
% Number of input arguments checking
%%%%%%%%%%%%%%%%%%%%

if nargin < 2
    error('FuzzyLogic:missingparams', ...
        'genfis4 requires input and output data to build a FIS.');
end

if nargin < 3
    fistype = 'sugeno';
end

if nargin < 4
    sig_par = 'auto';
end

% hardcoded for now
in_mftype = 'sigmf';
out_mftype = 'gaussmf';

%%%%%%%%%%%%%%%%%%%%%%%%%
% IO checking
%%%%%%%%%%%%%%%%%%%%%%%%%

% Check Xin, Xout
[numData, numInp] = size(Xin);
[numData2, numOutp] = size(Xout);

if numData ~= numData2
    % There's a mismatch in the input and output data matrix dimensions
    if numData == numOutp
        % The output data matrix should have been transposed, we'll fix it
        Xout = Xout';
        numOutp = numData2;
    else
        error('FuzzyLogic:dimensionmismatch', ...
            'Mismatched input and output data matrices.');
    end
end

if numOutp > 1
    error('FuzzyLogic:outputmismatch', ...
        'Output data must be a vector.');
end

%%%%%%%%%%%%%%%%%%%%
% Checking sig_par
%%%%%%%%%%%%%%%%%%%%

% Convert sig_par (if necessary) to numeric array
arrayNaN = NaN(1, numInp);
if ~isnumeric(sig_par)
    if ~isequal(sig_par, 'auto')
        error('FuzzyLogic:sigmoid', ...
            'Set sigmoid parameter to ''auto'' or a value greater than 0');
    end
    sig_par = arrayNaN;
elseif ~isscalar(sig_par)
    tmp = arrayNaN;
    tmp(1 : length(sig_par)) = sig_par;
    sig_par = tmp;
else
    sig_par = repmat(sig_par, 1, numInp);
end
% Avoid negative sig_par
sig_par = abs(sig_par);
% Length of sig_par must be the same as number of inputs
if length(sig_par) ~= numInp
    error('FuzzyLogic:sigmoid', ...
        ['Set number of sigmoid parameters to ', ...
        '1 or %d (number of inputs)'], numInp);
end

%%%%%%%%%%%%%%%%%%%%
% Creating tree
%%%%%%%%%%%%%%%%%%%%
ti = treeinfo(Xin, Xout, varargin{:});

n_node = numel(ti.node);
n_leaf = numel(ti.leaf);

in_mf = zeros(1, n_node);

%%%%%%%%%%%%%%%%%%%%%%
% Building FIS
%%%%%%%%%%%%%%%%%%%%%

% Initialize a FIS
str = sprintf('%s%g%g', fistype, numInp, numOutp);
fismat = newfis(str, fistype);

% Loop through and add inputs
for ind = 1 : numInp
    fismat = addvar(fismat, ...
        'input', ['in' num2str(ind)], minmax(Xin(:, ind)'));
end

% Loop through and add mf's
for ind = 1 : n_node
    var = ti.node(ind).variable;
    cut = ti.node(ind).cutpoint;
    s = ti.node(ind).sample;
    mfparams = computemfparams(in_mftype, Xin(s, var), ...
        cut, sig_par(var), SCALE);

    fismat = addmf(fismat, 'input', var, ...
        ['larger', num2str(ind)], in_mftype, mfparams);

    in_mf(ind) = length(fismat.input(var).mf);
end

% Add output
fismat = addvar(fismat, 'output', 'out', minmax(Xout'));

switch fistype
    case 'sugeno'
        % Loop through and add mf's
        for ind = 1 : n_leaf
            s = ti.leaf(ind).sample;
            mfparams = computemfparams('linear', Xin(s, :), Xout(s));

            fismat = addmf(fismat, 'output', 1, ...
                ['class', num2str(ind)], 'linear', mfparams);
        end
    case 'mamdani'
        % Loop through and add mf's
        for ind = 1 : n_leaf
            s = ti.leaf(ind).sample;
            mfparams = computemfparams(out_mftype, [], Xout(s));

            fismat = addmf(fismat, 'output', 1, ...
                ['class', num2str(ind)], out_mftype, mfparams);
        end
    otherwise
        error('FuzzyLogic:unknownfistype', ...
            'Unknown fistype specified');
end

% Create rules
rulelist = cell(n_leaf, 4);
for ind = 1 : n_leaf
    n = ti.branch(ind).nodes;
    e = ti.branch(ind).ineqs;

    var = [ti.node(n).variable];

    rulelist{ind, 1} = [var; in_mf(n) .* e];
    rulelist{ind, 2} = ind;
    rulelist{ind, 3} = 1;
    rulelist{ind, 4} = 1;
end

fismat = addrulex(fismat, rulelist);

function mfparams = computemfparams(mf, X, y, sig_par, scale)
%   This subfunction computes parameters of input and
%   output membership functions dependeding on mf's value.

switch mf
    case 'sigmf'
        % NaN's value of sig_par means its automatic calculation
        if isnan(sig_par)
            sigma = std(X);
            sig_par = scale / sigma;
        end
        mfparams = [sig_par, y];
    case 'gaussmf'
        sigma = std(y);
        % Check if y is a scalar or consists of identical elements
        if sigma == 0
            sigma = sqrt(eps);
        end
        c = mean(y);
        mfparams = [sigma, c];
    case 'linear'
        numData = size(X, 1);
        A = [X, ones(numData, 1)];
        % Using pinv instead of ldivide avoids
        % warning if A is close to singular
        mfparams = (pinv(A) * y)';
    otherwise
        error('FuzzyLogic:invalidmftype', ...
            'Unknown type of membership function specified.');
end

function pr = minmax(p)
%MINMAX Ranges of matrix rows.
%
%  Syntax
%
%    pr = minmax(p)
%
%  Description
%
%    MINMAX(P) takes one argument,
%      P - RxQ matrix.
%    and returns the Rx2 matrix PR of minimum and maximum values
%    for each row of P.
%
%    Alternately, P can be an MxN cell array of matrices.  Each matrix
%    P{i,j} should have Ri rows and Q columns.  In this case, MINMAX returns
%    an Mx1 cell array where the mth matrix is an Rix2 matrix of the
%    minimum and maximum values of elements for the matrics on the
%    ith row of P.
%
%  Examples
%
%    p = [0 1 2; -1 -2 -0.5]
%    pr = minmax(p)
%
%    p = {[0 1; -1 -2] [2 3 -2; 8 0 2]; [1 -2] [9 7 3]};
%    pr = minmax(p)

if iscell(p)
    m = size(p, 1);
    pr = cell(m, 1);
    for i = 1 : m
        pr{i} = minmax([p{i, :}]);
    end
elseif isa(p, 'double')
    pr = [min(p, [], 2), max(p, [], 2)];
else
    error('Argument has illegal type.');
end


function [data, Ht] = rarch_simulate(T,C,parameters,p,q,type)
% Simulation of RARCH(p,q) multivariate volatility model of Noureldin, Shephard and Sheppard
%
% USAGE:
%  [DATA,HT] = rarch_simulate(T,C,PARAMETERS,P,Q,TYPE)
%
% INPUTS:
%   T          - Either a scalar containing the length of the series to simulate, or a T by K matrix
%                  of simulated random variables.  The default is to use standard normal random
%                  variables.  Providing a T by K matrix allows other distributions to be used.
%   C          - Unconditional covariance of the data
%   PARAMETERS - Vector of parameters governing the dynamics.  The form of the parameters depends on the TYPE.
%                  'Scalar':
%                  [a(1) ... a(p) b(1) ... b(q)]'  (all scalars)
%                  'CP' :
%                  [diag(A(:,:,1))' ... diag(A(:,:,p))' theta]' (theta is the scalar persistence)
%                  'Diagonal'
%                  [diag(A(:,:,1))' ... diag(A(:,:,p))' diag(B(:,:,1))' ... diag(B(:,:,p))']'
%   P          - Positive, scalar integer representing the number of symmetric innovations
%   Q          - Non-negative, scalar integer representing the number of conditional covariance
%                  lags.  When using 'CP' model, 0<=q<=1
%   TYPE       - String, one of :
%                  'Scalar' (Default)
%                  'CP' (Common Persistence)
%                  'Diagonal'
%
% OUTPUTS:
%   DATA   - A T by K matrix of simulated data
%   HT     - A [K K T] dimension matrix of conditional covariances
%
% COMMENTS:
%   The dynamics of a RARCH model are identical to that of a BEKK, except
%   that the model evolves in the rotated space.
%
%   G(:,:,t) = (eye(K) - sum(A.^2,3) - sum(B.^2,3)) +
%       A(:,:,1)*OP(:,:,t-1)*A(:,:,1) + ... A(:,:,p)*OP(:,:,t-1)*A(:,:,p) +
%       B(:,:,1)*G(:,:,t-1)*B(:,:,1) + ... B(:,:,p)*OP(:,:,t-1)*B(:,:,p)
%
%   where in the scalar model A(:,:,i) = a(i)*eye(K), B(:,:,j)=b(j)*eye(K)
%   and in the CP model, B(:,:,j) = theta - sum(A.^2,3).  OP is the outer product of the
%   unconditionally standardized data.
%
% EXAMPLES:
%   % Scalar with A.^2=.05 and B.^2=.93
%   [data,Ht] = rarch_simulate(1000,eye(2)+1,sqrt([.05,.93]),1,1,'Scalar')
%   % Diagonal
%   [data,Ht] = rarch_simulate(1000,eye(2)+1,sqrt([.05 .07 .93 .88]),1,1,'Diagonal')
%   % Common Persistence, note uses theta (sqrt(A.^2+B.^2) not B)
%   [data,Ht] = rarch_simulate(1000,eye(2)+1,sqrt([.05 .07 .99]),1,1,'CP')
%
% See also RARCH

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 1    Date: 3/27/2012

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
k = size(C,1);
if isscalar(T)
    e = randn(2*T,k);
else
    e = T;
    if size(e,2)~=k
        error('T must have K columns when providing simulated random numbers.')
    end
    T = size(e,1);
    e = [e(ceil(rand(T,1)*T),:);e];
end

if strcmpi(type,'Scalar')
    type = 1;
elseif strcmpi(type,'CP')
    type = 2;
elseif strcmpi(type,'Diagonal')
    type = 3;
else
    error('TYPE must be ''Scalar'', ''CP'' or  ''Diagonal''.')
end

if type==2 && q>1
    error('Q must be either 0 or 1 for the ''CP'' model.')
end

switch type
    case 1
        count = p+q;
    case 2
        count = p*k+q;
    case 3
        count = (p+q)*k;
end
if length(parameters)~=count
    error('PARAMETERS does not have the expected number of elements.')
end
[C,A,B] = rarch_parameter_transform(parameters,p,q,k,C,type,false);
if max(diag(sum(A.^2,3)+sum(B.^2,3)))>=1
    warning('MFE:nonstationary','The parameters do not correspond to the stationary region.')
end
if type==2 && q==1 && min(B(:))<0
    error('When using ''CP'', the common persistence parameter Theta must satisfy Theta^2>=sum(A.^2,3)')
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Gt = repmat(eye(k),[1 1 2*T]);
intercept = eye(k) - sum(A.^2,3) - sum(B.^2,3);
backCast = eye(k);
for i=1:(2*T)
    Gt(:,:,i) = intercept;
    for j=1:p
        if (i-j)<=0
            Gt(:,:,i) = Gt(:,:,i) + A(:,:,j)*backCast*A(:,:,j);
        else
            Gt(:,:,i) = Gt(:,:,i) + A(:,:,j)*(e(i-j,:)'*e(i-j,:))*A(:,:,j);
        end
    end
    for j=1:q
        if (i-j)<=0
            Gt(:,:,i) = Gt(:,:,i) + B(:,:,j)*backCast*B(:,:,j);
        else
            Gt(:,:,i) = Gt(:,:,i) + B(:,:,j)*Gt(:,:,i-j)*B(:,:,j);
        end
    end
    Gt12 = Gt(:,:,i)^(0.5);
    e(i,:) = e(i,:)*Gt12;
end

data = zeros(size(e));
Ht = zeros(k,k,T);
C12 = C^(0.5);
for i=1:length(e)
    data(i,:) = e(i,:)*C12;
    Ht(:,:,i) = C12*Gt(:,:,i)*C12;
end

data = data(T+1:2*T,:);
Ht = Ht(:,:,T+1:2*T);


%DEMO_SVI_REGRESSION  A toy data regression example for sparse SVI GP
%
%  Description
%    Demonstration of sochastic variational inference GP model regression.
%    The problem is similar as in the Hensman et. al (2013). The dataset is
%    made synthetically in two dimensions using sinusoidal functions.
%
%  See also
%    DEMO_SVI_CLASSIFIC
%
%  References:
%    Hensman, J., Fusi, N. and Lawrence, N. D. (2013). Gaussian processes
%    for big data. arXiv preprint arXiv:1309.6835.

% Copyright (c) 2014 Tuomas Sivula

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

% Generate toy data
n = 2000;
s2 = 0.01;
% Training samples
x = rand(n,2)*2*pi - pi;
y = sin(x(:,1)).*sin(x(:,2)) + sqrt(s2).*randn(n,1);
% Test samples
nt = 100;
xt = rand(nt,2)*2*pi - pi;
yt = sin(xt(:,1)).*sin(xt(:,2));

% Prediction grid
[X1,X2] = meshgrid(-4:0.2:4, -4:0.2:4);
grid = [X1(:), X2(:)];

% Build gp structure
gp = gp_set('lik',lik_gaussian, 'cf', gpcf_sexp, 'latent_method', 'SVI');
% Optimise
nu = 50;    % The number of inducing inputs
maxi = 150; % The maximum number of iteration rounds
[gp, diagnosis] = svigp(gp,x,y,'xt',xt,'yt',yt,'nu',nu,'maxiter',maxi);
% Predict
Eft = gpsvi_pred(gp,x,y,grid);

% ------- Plot --------
% Prediction
figure()
contour(X1, X2, reshape(Eft, size(X1)), 16)
hold on
scatter(x(:,1), x(:,2), 20, y, 'filled')
scatter(gp.X_u(:,1), gp.X_u(:,2), 30)
legend('Eft', 'data', 'Z')
% Convergence analysis
figure()
subplot(3,1,1)
plot(mean(diagnosis.e,2))
title('energy')
subplot(3,1,2)
plot(diagnosis.mlpd)
title('mean log predictive density')
subplot(3,1,3)
plot(diagnosis.rmse)
title('root mean square error')
xlabel('iteration')