function [K,C,strain,A,M] = linear_elasticity_stiffness(V,F,varargin)
  %
  % [K,C,strain,A,M] = linear_elasticity_stiffness(V,F)
  %
  % Inputs:
  %   V  #V by d list of vertex positions
  %   F  #F by d+1 list of  element indices into V
  %   Optional:
  %     'Lambda'  followed by first Lamé parameter {1.7423333}, scalar
  %       (homogeneous) or #F by 1 list of per-element values
  %     'Mu'  followed by shear modulus {0.0115}, scalar (homogeneous) or #F by
  %       1 list of per-element values
  %     'Young'  followed by Young's modulus, scalar (homogeneous) or #F by 1
  %       list of per-element values
  %     'Nu'  followed by Poisson's ratio, scalar (homogeneous) or #F by 1 list
  %       of per-element values
  % Outputs:
  %   K  #V*d by #V*d sparse stiffness matrix
  %   C  #F**(d*(d+1)/2) by #F**(d*(d+1)/2) sparse constituitive model matrix
  %   strain  #F*(d*(d+1)/2) by #V*d sparse strain matrix
  %   A  #F*(d*(d+1)/2) by #F*(d*(d+1)/2) diagonal element area matrix
  %   M  #V*d by #V*d sparse mass matrix
  %

  % Silicone rubber: http://www.azom.com/properties.aspx?ArticleID=920
  mu = 0.0115;
  % Bulk modulus
  K = 1.75;
  lambda = K-2/3*mu;
  young = [];
  nu = [];
  % Map of parameter names to variable names
  params_to_variables = containers.Map( ...
    {'Lambda','Mu','Nu','Young'}, ...
    {'lambda','mu','nu','young'});
  v = 1;
  while v <= numel(varargin)
    param_name = varargin{v};
    if isKey(params_to_variables,param_name)
      assert(v+1<=numel(varargin));
      v = v+1;
      % Trick: use feval on anonymous function to use assignin to this workspace
      feval(@()assignin('caller',params_to_variables(param_name),varargin{v}));
    else
      error('Unsupported parameter: %s',varargin{v});
    end
    v=v+1;
  end

  assert( ...
    (~isempty(lambda) && ~isempty(mu))||(~isempty(young) && ~isempty(nu)), ...
    'Must define either lambda and mu or young and nu');
  if (~isempty(young) && ~isempty(nu))
    lambda = young.*nu./((1+nu).*(1-2.*nu));
    mu = .5.*young./(1+nu);
  end

  dim = size(V,2);

  % This matches the matlab code by Jonas Koko, in
  % "Vectorized Matlab Codes for Linear Two-Dimensional Elasticity"

  % Gradient/divergence operator
  G = grad(V,F);

  I = speye(size(F,1));
  Z = sparse(size(F,1),size(V,1));
  lambda = diag(sparse(lambda));
  mu = diag(sparse(mu));
  % Strain tensor
  %
  %   ϵ = ½(∇u + (∇u)')
  %   ϵ = ½ // ∂u₁/∂x₁  ∂u₂/∂x₁ \  + / ∂u₁/∂x₁  ∂u₁/∂x₂ \\
  %         \\ ∂u₁/∂x₂  ∂u₂/∂x₂ /    \ ∂u₂/∂x₁  ∂u₂/∂x₂ //
  %
  %                                "Voigt" notation
  %   ϵ₁₁ = ∂u₁/∂x₁              = ϵ₁
  %   ϵ₂₂ = ∂u₂/∂x₂              = ϵ₂
  %   ϵ₁₂ = ½(∂u₂/∂x₁ + ∂u₁/∂x₁) = ½ ϵ₃
  %   ϵ₂₁ = ϵ₁₂                  = ½ ϵ₃
  %
  switch dim
  case 2
    G1 = G(1:size(F,1),:);
    G2 = G(size(F,1)+(1:size(F,1)),:);
    % 3#F by 2#V
    strain = [G1 Z;Z G2;G2 G1];
    % Stiffness tensor
    %
    %    σ = C:ϵ        %  A:B = Aij Bij
    %                   %      = ∑∑ Aij Bij, where in this case Aij is a 2x2
    %                   %                    matrix, and Bij is a scalar
    %
    % For each face we have:
    %
    %    2x2 = 2x2x2x2 2x2
    %    σf  = Cf : ϵf
    %    σ = ∑∑ Cij ϵij, where Cij is a 2x2 matrix
    %    σkl = ∑∑ Cijkl ϵij, where Cijkl is a scalar
    %
    % But really ϵf and σf are just 3 distinct values:
    %
    %   σ₁ = [ϵ₁ ϵ₂ ϵ₃] [ c₁₁ ; c₁₂ ; c₁₃ ]
    %   σ₂ = [ϵ₁ ϵ₂ ϵ₃] [ c₂₁ ; c₂₂ ; c₂₃ ]
    %   σ₃ = [ϵ₁ ϵ₂ ϵ₃] [ c₃₁ ; c₃₂ ; c₃₃ ]
    %
    %    /σ₁\     /c₁₁ c₁₂ c₁₃\  /ϵ₁\
    %   | σ₂ | = | c₂₁ c₂₂ c₂₃ || ϵ₂ |
    %    \σ₃/     \c₃₁ c₃₂ c₃₃/  \ϵ₃/
    %
    % So if σ is a 3#F by 1 vector and ϵ is a 3#F vector then:
    %
    %   σ = C ϵ
    %        /C₁₁ C₁₂ C₁₃\  /ϵ₁\
    %   σ = | C₂₁ C₂₂ C₂₃ || ϵ₂ |
    %        \C₃₁ C₃₂ C₃₃/  \ϵ₃/
    %
    %  where C is 3#F by 3#F matrix and Cij = diagonal #F by #F matrix.
    %
    % For Isotropic homogeneous media, we have that:
    %
    %   σij = λ δij ϵkk + 2μ ϵij
    %   σij = λ δij (∑ ϵkk) + 2μ ϵij
    %
    % where λ is Lamé's first parameter and μ is the shear modulus: the bulk
    % modulus is thus K := λ + ⅔ μ
    %
    % Or in Voigt notation:
    %
    %   σ₁ = σ₁₁ = λ (ϵ₁ + ϵ₂) + 2μ ϵ₁
    %   σ₂ = σ₂₂ = λ (ϵ₁ + ϵ₂) + 2μ ϵ₂
    %   σ₃ = σ₁₂ = λ (ϵ₁ + ϵ₂) + 2μ ϵ₁₂
    %            = λ (ϵ₁ + ϵ₂) + μ ϵ₃
    %
    %        //λ  λ 0\   /2μ  0  0\\
    %  σ =  || λ  λ 0 |+|  0 2μ  0 || ϵ
    %        \\0  0 0/   \ 0  0  μ//
    %
    %
    %Z = sparse(size(F,1),size(F,1));
    %I = speye(size(F,1));
    %C = lambda*[[I I Z;I I Z;Z Z Z]] + mu*[2*I Z Z;Z 2*I Z;Z Z I];
    %C = lambda*[1 1 0;1 1 0;0 0 0] + mu*diag([2 2 1]);
    %C = kroneye(C,size(F,1));
    C = [ ...
      (lambda+2*mu)*I        lambda*I  0*I; ...
            lambda*I (lambda+2*mu)*I  0*I; ...
                  0*I             0*I mu*I];
    %   ∇⋅σ = /∇⋅/σ₁₁\  ∇⋅/σ₁₂\\
    %         \  \σ₂₁/    \σ₂₂//
    %
    % If D is the divergence operator then D is 2#V by 3#F, where σ is 3#F by 1
    % vectorized stress tensor using Voigt notation:
    %
    %   X = D σ
    %
    A = diag(sparse(doublearea(V,F)/2));
  case 3
    G1 = G(1:size(F,1),:);
    G2 = G(size(F,1)+(1:size(F,1)),:);
    G3 = G(2*size(F,1)+(1:size(F,1)),:);
    strain = [G1 Z Z;Z G2 Z; Z Z G3; Z G3 G2; G3 Z G1; G2 G1 Z];
    C = [ ...
        (lambda+2*mu)*I        lambda*I        lambda*I     0*I     0*I     0*I ; ...
        lambda*I (lambda+2*mu)*I        lambda*I     0*I     0*I     0*I ; ...
        lambda*I        lambda*I (lambda+2*mu)*I     0*I     0*I     0*I ; ...
        0*I             0*I             0*I    mu*I     0*I     0*I ; ...
        0*I             0*I             0*I     0*I    mu*I     0*I ; ...
        0*I             0*I             0*I     0*I     0*I    mu*I ];
    A = diag(sparse(volume(V,F)));
  end
  Z = sparse(size(V,1),size(F,1));
  D = strain';
  A = repdiag(A,dim*(dim+1)/2);
  K = D * A * C * strain;

  M = massmatrix(V,F);
  M = repdiag(M,size(V,2));

end


function [old_dirs,old_stps,Hdiag,Bcompact] = lbfgsUpdate(y,s,corrections,debug,old_dirs,old_stps,Hdiag)

%B0 = eye(length(y))/Hdiag;
S = old_dirs(:,2:end);
Y = old_stps(:,2:end);
k = size(Y,2);
L = zeros(k);
for j = 1:k
    for i = j+1:k
        L(i,j) = S(:,i)'*Y(:,j);
    end
end
D = diag(diag(S'*Y));
N = [S/Hdiag Y];
M = [S'*S/Hdiag L;L' -D];

ys = y'*s;
Bs = s/Hdiag - N*(M\(N'*s)); % Product B*s
sBs = s'*Bs;

eta = .02;
if ys < eta*sBs
    if debug
        fprintf('Damped Update\n');
    end
    theta = min(max(0,((1-eta)*sBs)/(sBs - ys)),1);
    y = theta*y + (1-theta)*Bs;
end

numCorrections = size(old_dirs,2);
if numCorrections < corrections
    % Full Update
    old_dirs(:,numCorrections+1) = s;
    old_stps(:,numCorrections+1) = y;
else
    % Limited-Memory Update
    old_dirs = [old_dirs(:,2:corrections) s];
    old_stps = [old_stps(:,2:corrections) y];
end

% Update scale of initial Hessian approximation
Hdiag = (y'*s)/(y'*y);


function varargout=sde_correlate(c,r1)
%SDE_CORRELATE  Correlated values.
%   R2 = SDE_CORRELATE(C, R1) returns the matrix R2 of M correlated values given
%   the N-by-N correlation matrix C and the M-by-N matrix of (uncorrelated)
%   values R1. The first column of R2 is always SQRT(C(1,1)) times the first
%   column of R1.
%
%   [R2, D] = SDE_CORRELATE(C, R1) also returns the N-by-N diffusion matrix, D,
%   created from the correlation matrix, C.
%
%   D = SDE_CORRELATE(C) without a second input argument returns only the N-by-N
%   diffusion matrix, D, created from the correlation matrix, C.
%
%   Note:
%       C may either be a symmetric positive semidefinite correlation matrix or
%       covariance matrix.
%
%   Example:
%       % Plot uncorrelated and correlated normally-distributed points
%       r1 = randn(1e4,2); corr(r1)
%       r2 = sde_correlate([1 -0.8;-0.8 1],r1); corr(r2)
%       figure; plot(r1(:,1),r1(:,2),'b.',r2(:,1),r2(:,2),'r*'); axis equal;
%
%   See also: SDE_DECORRELATE, RAND, RANDN, RANDSTREAM, RANDSTREAM/RANDN

%   Andrew D. Horchler, horchler @ gmail . com, Created 5-20-13
%   Revision: 1.2, 7-17-13

if nargout > min(nargin,2)
    error('SDETools:sde_correlate:TooManyOutputs',...
          'Too many output arguments for number of supplied inputs.');
end

if isempty(c) || isempty(r1)
    if nargin == 1
        varargout{1} = [];
    else
        varargout{2} = [];
    end
else
    [m,n] = size(c);
    if ndims(c) ~= 2 || m ~= n              %#ok<ISMAT>
        error('SDETools:sde_correlate:NonSquareMatrix',...
              'The correlation matrix must be square.');
    end

    if sde_isdiag(c)
        num = rank(c);
        c = diag(c);
        if any(c) < 0
            error('SDETools:sde_correlate:NegativeDiagonal',...
                 ['All elements of the diagonal of the correlation matrix '...
                  'must be non-negative.']);
        end

        d = sqrt(c);
        isDiag = isscalar(c);
    else
        [d,num] = cholcov(c);
        if isempty(d)
            error('SDETools:sde_correlate:NonSymmetricSemiDefinite',...
                 ['The correlation matrix must be a symmetric positive '...
                  'semidefinite matrix.']);
        end
        isDiag = false;
    end

    if nargin == 1
        if num ~= m
            warning('SDETools:sde_correlate:NonFullRank1',...
                    'The correlation matrix is not full rank.');
        end

        if isDiag
            varargout{1} = diag(d);
        else
            varargout{1} = d;
        end
    else
        if ndims(r1) ~= 2 || size(r1,2) ~= m	%#ok<ISMAT>
            error('SDETools:sde_correlate:DimensionMismatch',...
                 ['The number of columns in the matrix of normally '...
                  'distributed values must equal the dimension of the '...
                  'correlation matrix.']);
        end
        if num ~= m
            warning('SDETools:sde_correlate:NonFullRank2',...
                   ['The correlation matrix is not full rank. Only the '...
                    'first %d columns of the input values will be '...
                    'correlated.'],num);
        end

        if isDiag
            varargout{1} = bsxfun(@mtimes,r1,d);
            if nargout == 2
                varargout{2} = diag(d);
            end
        else
            varargout{1} = r1*d;
            if nargout == 2
                varargout{2} = d;
            end
        end
    end
end


function CPD = deterministic_CPD(bnet, self, fname, pfail)
% DETERMINISTIC_CPD Make a tabular CPD representing a (noisy) deterministic function
%
% CPD = deterministic_CPD(bnet, self, fname)
% This calls feval(fname, pvals) for each possible vector of parent values.
% e.g., suppose there are 2 ternary parents, then pvals =
%  [1 1], [2 1], [3 1],   [1 2], [2 2], [3 2],   [1 3], [2 3], [3 3]
% If v = feval(fname, pvals(i)), then
%  CPD(x | parents=pvals(i)) = 1 if x==v, and = 0 if x<>v
% e.g., suppose X4 = X2 AND (NOT X3). Then
%    bnet.CPD{4} = deterministic_CPD(bnet, 4, inline('((x(1)-1) & ~(x(2)-1)) + 1'));
% Note that x(1) refers pvals(1) = X2, and x(2) refers to pvals(2)=X3
% See also boolean_CPD.
%
% CPD = deterministic_CPD(bnet, self, fname, pfail)
% will put probability mass 1-pfail on f(parents), and distribute pfail over the other values.
% This is useful for simulating noisy deterministic functions.
% If pfail is omitted, it is set to 0.
%

if nargin==0
  % This occurs if we are trying to load an object from a file.
  CPD = tabular_CPD(bnet, self);
  return;
elseif isa(bnet, 'deterministic_CPD')
  % This might occur if we are copying an object.
  CPD = bnet;
  return;
end

if nargin < 4, pfail = 0; end

ps = parents(bnet.dag, self);
ns = bnet.node_sizes;
psizes = ns(ps);
self_size = ns(self);

psucc = 1-pfail;

CPT = zeros(prod(psizes), self_size);
pvals = zeros(1, length(ps));
for i=1:prod(psizes)
  pvals = ind2subv(psizes, i);
  x = feval(fname, pvals);
  %fprintf('%d ', [pvals x]); fprintf('\n');
  if psucc == 1
    CPT(i, x) = 1;
  else
    CPT(i, x) = psucc;
    rest = mysetdiff(1:self_size, x);
    CPT(i, rest) = pfail/length(rest);
  end
end
CPT = reshape(CPT, [psizes self_size]);

CPD = tabular_CPD(bnet, self, 'CPT',CPT, 'clamped',1);


function check = reciprocal_check ( a, b )

%*****************************************************************************80
%
%% RECIPROCAL_CHECK checks the parameters of the Reciprocal CDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 October 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, the parameters of the PDF.
%    0.0 < A <= B.
%
%    Output, logical CHECK, is true if the parameters are legal.
%
  if ( a <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'RECIPROCAL_CHECK - Fatal error!\n' );
    fprintf ( 1, '  A <= 0.0\n' );
    check = 0;
    return
  end

  if ( b < a ) then
    fprintf ( 1, '\n' );
    fprintf ( 1, 'RECIPROCAL_CHECK - Fatal error!\n' );
    fprintf ( 1, '  B < A\n' );
    check = 0;
    return
  end

  check = 1;

  return
end


function N=patchnormals(FV)
% This function PATCHNORMALS calculates the normals of a triangulated
% mesh. PATCHNORMALS calls the patchnormal_double.c mex function which
% first calculates the normals of all faces, and after that calculates
% the vertice normals from the face normals weighted by the angles
% of the faces.
%
% N=patchnormals(FV);
%
% Inputs,
%   FV : A struct containing FV.faces with a facelist Nx3 and FV.vertices
%        with a Nx3 vertices list. Such a structure is created by Matlab
%        Patch function
% Outputs,
%   N : A Mx3 list with the normals of all vertices
%
% Example,
%   % Compile the c-coded function
%   mex patchnormals_double.c -v
%
%   % Load a triangulated mesh of a sphere
%   load sphere;
%
%   % Calculate the normals
%   N=patchnormals(FV);
%
%   % Show the normals
%   figure, patch(FV,'FaceColor',[1 0 0]); axis square; hold on;
%   for i=1:size(N,1);
%       p1=FV.vertices(i,:); p2=FV.vertices(i,:)+10*N(i,:);
%       plot3([p1(1) p2(1)],[p1(2) p2(2)],[p1(3) p2(3)],'g-');
%   end
%
% Function is written by D.Kroon University of Twente (June 2009)

sizev=size(FV.vertices);
% Check size of vertice array
if((sizev(2)~=3)||(length(sizev)~=2))
    error('patchnormals:inputs','The vertice list is not a m x 3 array')
end

sizef=size(FV.faces);
% Check size of vertice array
if((sizef(2)~=3)||(length(sizef)~=2))
    error('patchnormals:inputs','The vertice list is not a m x 3 array')
end

% Check if vertice indices exist
if(max(FV.faces(:))>size(FV.vertices,1))
    error('patchnormals:inputs','The face list contains an undefined vertex index')
end

% Check if vertice indices exist
if(min(FV.faces(:))<1)
    error('patchnormals:inputs','The face list contains an vertex index smaller then 1')
end

[Nx,Ny,Nz]=patchnormals_double(double(FV.faces(:,1)),double(FV.faces(:,2)),double(FV.faces(:,3)),double(FV.vertices(:,1)),double(FV.vertices(:,2)),double(FV.vertices(:,3)));

N=zeros(length(Nx),3);
N(:,1)=Nx;
N(:,2)=Ny;
N(:,3)=Nz;


function [parameters, LL, ht, VCVrobust, VCV, scores, diagnostics] = tarch(epsilon, p, o, q, error_type, tarch_type, startingvals, options)
% TARCH(P,O,Q) parameter estimation with different error distributions:
% Normal, Students-T, Generalized Error Distribution, Skewed T
% Estimation of ARCH or GARCH models if o=0 and tarch_type=2
% Estimation of TARCH or GJR asymmetric models if o>0 and tarch_type=1 or 2
%
% USAGE:
%   [PARAMETERS] = tarch(EPSILON,P,O,Q)
%   [PARAMETERS,LL,HT,VCVROBUST,VCV,SCORES,DIAGNOSTICS] =
%                                   tarch(EPSILON,P,O,Q,ERROR_TYPE,TARCH_TYPE,STARTINGVALS,OPTIONS)
%
% INPUTS:
%   EPSILON      - A column of mean zero data
%   P            - Positive, scalar integer representing the number of symmetric innovations
%   O            - Non-negative scalar integer representing the number of asymmetric innovations (0
%                    for symmetric processes)
%   Q            - Non-negative, scalar integer representing the number of lags of conditional
%                    variance (0 for ARCH)
%   ERROR_TYPE   - [OPTIONAL] The error distribution used, valid types are:
%                    'NORMAL'    - Gaussian Innovations [DEFAULT]
%                    'STUDENTST' - T distributed errors
%                    'GED'       - Generalized Error Distribution
%                    'SKEWT'     - Skewed T distribution
%   TARCH_TYPE   - [OPTIONAL] The type of variance process, either
%                    1 - Model evolves in absolute values
%                    2 - Model evolves in squares [DEFAULT]
%   STARTINGVALS - [OPTIONAL] A (1+p+o+q), plus 1 for STUDENTST OR GED (nu), plus 2 for SKEWT
%                    (nu,lambda), vector of starting values.
%                     [omega alpha(1) ... alpha(p) gamma(1) ... gamma(o) beta(1) ... beta(q) [nu lambda]]'.
%   OPTIONS      - [OPTIONAL] A user provided options structure. Default options are below.
%
% OUTPUTS:
%   PARAMETERS   - A 1+p+o+q column vector of parameters with
%                  [omega alpha(1) ... alpha(p) gamma(1) ... gamma(o) beta(1) ... beta(q) [nu lambda]]'.
%   LL           - The log likelihood at the optimum
%   HT           - The estimated conditional variances
%   VCVROBUST    - Robust parameter covariance matrix
%   VCV          - Non-robust standard errors (inverse Hessian)
%   SCORES       - Matrix of scores (# of params by t)
%   DIAGNOSTICS  - Structure of optimization outputs and other values useful for functions calling TARCH.
%
% COMMENTS:
% The following (generally wrong) constraints are used:
%   (1) omega > 0
%   (2) alpha(i) >= 0 for i = 1,2,...,p
%   (3) gamma(i) + alpha(i) > 0 for i=1,...,o
%   (3) beta(i)  >= 0 for i = 1,2,...,q
%   (4) sum(alpha(i) + 0.5*gamma(j) + beta(k)) < 1 for i = 1,2,...p and
%   j = 1,2,...o, k=1,2,...,q
%   (5) nu>2 of Students T and nu>1 for GED
%   (6) -.99<lambda<.99 for Skewed T
%
%   The conditional variance, h(t), of a TARCH(P,O,Q) process is modeled as follows:
%
%    g(h(t)) = omega
%            + alpha(1)*f(r_{t-1}) + ... + alpha(p)*f(r_{t-p})+...
%            + gamma(1)*I(t-1)*f(r_{t-1}) +...+ gamma(o)*I(t-o)*f(r_{t-o})+...
%            beta(1)*g(h(t-1)) +...+ beta(q)*g(h(t-q))
%
%     where f(x) = abs(x)  if tarch_type=1
%          g(x) = sqrt(x) if tarch_type=1
%          f(x) = x^2     if tarch_type=2
%          g(x) = x       if tarch_type=2
%
%   Default Options
%    options  =  optimset('fminunc');
%    options  =  optimset(options , 'TolFun'      , 1e-005);
%    options  =  optimset(options , 'TolX'        , 1e-005);
%    options  =  optimset(options , 'Display'     , 'iter');
%    options  =  optimset(options , 'Diagnostics' , 'on');
%    options  =  optimset(options , 'LargeScale'  , 'off');
%    options  =  optimset(options , 'MaxFunEvals' , '400*numberOfVariables');
%
%  See also TARCH_LIKELIHOOD, TARCH_CORE, TARCH_PARAMETER_CHECK, TARCH_STARTING_VALUES,
%  TARCH_TRANSFORM, TARCH_ITRANSFORM
%
%  You should use the MEX files (or compile if not using Win64 Matlab) as they provide speed ups of
%  approx 100 times relative to the m file.

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
switch nargin
    case 4
        [p,o,q,error_type,tarch_type,startingvals,options]=tarch_parameter_check(epsilon, p, o, q);
    case 5
        [p,o,q,error_type,tarch_type,startingvals,options]=tarch_parameter_check(epsilon, p, o, q, error_type);
    case 6
        [p,o,q,error_type,tarch_type,startingvals,options]=tarch_parameter_check(epsilon, p, o, q, error_type, tarch_type);
    case 7
        [p,o,q,error_type,tarch_type,startingvals,options]=tarch_parameter_check(epsilon, p, o, q, error_type, tarch_type, startingvals);
    case 8
        [p,o,q,error_type,tarch_type,startingvals,options]=tarch_parameter_check(epsilon, p, o, q, error_type, tarch_type, startingvals, options);
    otherwise
        error('Number of inputs must be between 4 and 8');
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%Initial setup
m  =  max([p o q]);

%Augment the data with back casts to avoid costly memory allocations
if tarch_type==1
    %fepsilon is f(epsilon), as above
    fepsilon   =  [mean(abs(epsilon))*ones(m,1) ; abs(epsilon)];
    %fIepsilon is fepsilon*(epsilon<0)
    fIepsilon   =  [0.5*mean(abs(epsilon))*ones(m,1) ; abs(epsilon).*(epsilon<0)];

    % Local back casting
    back_cast_length = max(floor(length(epsilon)^(1/2)),1);
    back_cast_weights = .05*(.9.^(0:back_cast_length ));
    back_cast_weights = back_cast_weights/sum(back_cast_weights);
    back_cast = back_cast_weights*(abs(epsilon(1:back_cast_length+1)));
    if back_cast==0
        back_cast=mean(abs(epsilon));
    end
else
    %fepsilon is f(epsilon), as above
    fepsilon   =  [mean(epsilon.^2)*ones(m,1) ; epsilon.^2];
    %fIepsilon is fepsilon*(epsilon<0)
    fIepsilon   =  [0.5*mean(epsilon.^2)*ones(m,1) ; epsilon.^2.*(epsilon<0)];
    % Local back casting
    back_cast_length = max(floor(length(epsilon)^(1/2)),1);
    back_cast_weights = .05*(.9.^(0:back_cast_length ));
    back_cast_weights = back_cast_weights/sum(back_cast_weights);
    back_cast = back_cast_weights*((epsilon(1:back_cast_length+1)).^2);
    if back_cast==0
        back_cast=mean(epsilon.^2);
    end
end
epsilon_augmented=[zeros(m,1);epsilon];
%Compute the length of the augmented epsilon
T     = size(fepsilon,1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Starting values
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%This flag is for the robustness check below, if the user supplies starting
%values, there will be no robustness check
if isempty(startingvals)
    startingflag=0;
else
    startingflag=1;
end
%Grid search for starting values.
[startingvals,nu,lambda,~,ordered_parameters]=tarch_starting_values(startingvals,epsilon_augmented,fepsilon,fIepsilon,p,o,q,T,error_type,tarch_type);
%Finally, initialize the starting values
startingvals = [startingvals; nu; lambda];
%Transform the starting vals
[garch_params_transformed,nu_transformed,lambda_transformed]=tarch_transform(startingvals,p,o,q,error_type);
%Re-append nu, lambda
startingvals_transformed = [garch_params_transformed; nu_transformed; lambda_transformed];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Starting values
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Estimate the parameters. Note the 1 in the last argument to indicate it
% is a constrained optimization

%LL0 is used to make sure the log likelihood improves
LL0=tarch_likelihood(startingvals_transformed,epsilon_augmented,fepsilon,fIepsilon,p,o,q,error_type,tarch_type,back_cast,T,1);
%Parameter estimation
[parameters,LL,exitflag,output]=fminunc('tarch_likelihood',startingvals_transformed,options,epsilon_augmented,fepsilon,fIepsilon,p,o,q,error_type,tarch_type,back_cast,T,1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Estimation Robustness
%This portion of the code is to make sure that the optimization converged
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%This is the case where the optimization did not converge, but improved on
%the initial log likelihood
if  exitflag<=0 && LL<LL0
    %Try more iterations, only do more iterations if the final likelihood is
    %actually better than the initial

    %Increase the max iterations and max fun evals
    %Also switch to steepest descent
    if ischar(options.MaxFunEvals)
        options.MaxIter=2*100*length(parameters);
    else
        options.MaxIter=2*options.MaxIter;
    end
    if ischar(options.MaxFunEvals)
        options.MaxFunEvals=4*100*length(parameters);
    else
        options.MaxFunEvals=2*options.MaxFunEvals;
    end
    options.HessUpdate='steepdesc';
    % Estimate the parameters.
    [parameters,LL,exitflag,output]=fminunc('tarch_likelihood',parameters,options,epsilon_augmented,fepsilon,fIepsilon,p,o,q,error_type,tarch_type,back_cast,T,1);
end

%If the optimization still hasn't converged, try other starting values
if startingflag==0 && exitflag<=0
    %Keep track of the final estimates, if nothing converges, we will
    %return the
    robust_parameters(1,:)=parameters';
    %Also keep the LL
    robust_LL = zeros(2,1);
    robust_LL(1) = LL;
    %Keep track of the iteration
    index=2;
    while exitflag<=0
        %This condition checks that we haven't converged
        %OR that the best objective is worse than best grid search
        %Sort the original grid search log likelihoods and parameters

        startingvals=[ordered_parameters(index,:)' ; nu; lambda];
        %Transform the starting vals
        [garch_params_transformed,nu_transformed,lambda_transformed]=tarch_transform(startingvals,p,o,q,error_type);
        %Reappend nu
        startingvals_transformed = [garch_params_transformed; nu_transformed; lambda_transformed];

        LL0=tarch_likelihood(startingvals_transformed,epsilon_augmented,fepsilon,fIepsilon,p,o,q,error_type,tarch_type,back_cast,T,1);
        options.HessUpdate='bfgs';
        %Try the second set of starting values
        [parameters,LL,exitflag,output]=fminunc('tarch_likelihood',startingvals_transformed,options,epsilon_augmented,fepsilon,fIepsilon,p,o,q,error_type,tarch_type,back_cast,T,1);
        if  exitflag<=0 && LL<LL0
            %Again, if the LL improved, try more iterations
            %Increase the max iterations and max fun evals
            %Also switch to steepest descent
            options.MaxIter=2*options.MaxIter;
            options.MaxFunEvals=2*options.MaxFunEvals;
            options.HessUpdate='steepdesc';
            % Estimate the parameters.
            [parameters,LL,exitflag,output]=fminunc('tarch_likelihood',parameters,options,epsilon_augmented,fepsilon,fIepsilon,p,o,q,error_type,tarch_type,back_cast,T,1);
        end
        %Save the parameter estimates
        robust_parameters(index,:)=parameters';
        robust_LL(index)=LL;
        %Increment the index
        index=index+1;

        if index>size(ordered_parameters,1);
            %save the best LL and parameters and break
            warning('MFEToolbox:Convergence','Convergence not achieved.  Use results with caution');
            [LL,index]=min(robust_LL);
            parameters=robust_parameters(index,:)';
            break
        end
    end
end

%Transform the parameters from the real line to the restricted space
[parameters,nu,lambda]=tarch_itransform(parameters,p,o,q,error_type);
parameters=[parameters;nu;lambda];
%Compute the log likelihood if needed
if nargout>1
    [LL, ~, ht]=tarch_likelihood(parameters,epsilon_augmented,fepsilon,fIepsilon,p,o,q,error_type,tarch_type,back_cast,T);
    LL=-LL;
end

%Compute standard errors using RobustVCV if needed.
if nargout>3
    nw=0; %No newey west on scores
    [VCVrobust,A,~,scores,hess]=robustvcv('tarch_likelihood',parameters,nw,epsilon_augmented,fepsilon,fIepsilon,p,o,q,error_type,tarch_type,back_cast,T);
    VCV=hess^(-1)/(T-m);
    diagnostics.A = A;
end

%Report diagnostics in case requested
diagnostics.EXITFLAG=exitflag;
diagnostics.ITERATIONS=output.iterations;
diagnostics.FUNCCOUNT=output.funcCount;
diagnostics.MESSAGE=output.message;
diagnostics.m = m;
diagnostics.T = T;
diagnostics.fdata = fepsilon;
diagnostics.fIdata = fIepsilon;
diagnostics.back_cast = back_cast;


function [err, time] = spincomp(S, N, dt, pref)
%SPINCOMP  Compare time-stepping schemes in 1D/2D/3D.
%   SPINCOMP(S, N, DT, PREF) compares the time-stepping schemes in PREF.SCHEME
%   applied to the PDE specified by the SPINOP/SPINOP2/SPINOP3 S, using a fixed
%   number N of grid points and different time-steps DT. The results are shown
%   in a pair of log-log plots. PREF is a SPINPREF/SPINPREF2/SPINPREF3 object.
%
%   [ERR, TIME] = SPINCOMP(S, N, DT, PREF) returns the errors and the computer
%   times.
%
% Example 1: Compare the ETDRK schemes for 1D Kuramoto-Sivashinsky equation
%
%   dom = [0 32*pi]; tspan = [0 30];
%   u0 = chebfun('cos(x/16).*(1 + sin(x/16))',dom,'trig');
%   S = spinop('KS');
%   S.init = u0; S.tspan = tspan;
%   N = 256; dt = [1, 5e-1, 2e-1, 1e-1, 5e-2, 2e-2];
%   pref = spinpref;
%   pref.scheme = {'etdrk4', 'exprk5s8', 'krogstad', 'friedli', ...
%      'hochbruck-ostermann', 'minchev', 'strehmel-weiner'};
%   spincomp(S, N, dt, pref);
%
% Example 2: Compare the PREDICTOR-CORRECTOR schemes for 2D Gray-Scott equations
%
%   G = 3; dom = G*[0 1 0 1]; tspan = [0 10];
%   u01 = chebfun2(@(x,y) 1-exp(-150*((x-G/2).^2 + (y-G/2).^2)), dom, 'trig');
%   u02 = chebfun2(@(x,y) exp(-150*((x-G/2).^2 + 2*(y-G/2).^2)), dom, 'trig');
%   u0 = chebmatrix(u01);
%   u0(2,1) = u02;
%   S = spinop2('GS'); S.init = u0; S.tspan = tspan;
%   N = 128; dt = [1, 5e-1, 2e-1, 5e-2, 2e-2];
%   pref = spinpref2;
%   pref.scheme = {'pec423', 'pecec433', 'pec524', 'pecec534', 'pec625', ...
%      'pecec635', 'pec726', 'pecec736'};
%   spincomp(S, N, dt, pref);
%
% See also SPINOP, SPINOP2, SPINOP3, SPINSCHEME.
%
% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% Get the schemes:
schemes = pref.scheme;
nSchemes = length(schemes);

% Get the time-steps:
timesteps = dt;
nTimesteps = length(timesteps);

% Convert the initial conditiona to a CHEBMATRIX:
u0 = S.init;
if ( isa(u0, 'chebfun') == 1 || isa(u0, 'chebfun2') == 1 || ...
    isa(u0, 'chebfun3') == 1)
    u0 = chebmatrix(u0);
elseif ( isa(u0, 'chebfun2v') == 1 || isa(u0, 'chebfun3v') == 1 )
    temp = chebmatrix(u0(1));
    for k = 2:size(u0, 1)
        temp(k,1) = u0(k);
    end
    u0 = temp;
end
nVars = length(u0);

% Create a grid to compare exact and computed solutions:
dom = u0{1}.domain;
xx = trigpts(32, dom);
if ( isa(u0{1}, 'chebfun') == 1 )
    dim = 1;
elseif ( isa(u0{1}, 'chebfun2') == 1 )
    dim = 2;
    [xx, yy] = meshgrid(xx);
elseif ( isa(u0{1}, 'chebfun3') == 1 )
    dim = 3;
    [xx, yy, zz] = meshgrid(xx);
end

% First, estimate the exact solution using a very small time step (half the
% smallest time-step) and with PECEC736 (7th-order multistep scheme):
if ( isa(pref, 'spinpref') == 1 )
    prefu = spinpref();
elseif ( isa(pref, 'spinpref2') == 1 )
    prefu = spinpref2();
elseif ( isa(pref, 'spinpref3') == 1 )
    prefu = spinpref3();
end
prefu.scheme = 'pecec736';
prefu.plot = 'off';
prefu.M = pref.M;
uexact = spinoperator.solvepde(S, N, min(timesteps)/2, prefu);
if ( isa(uexact, 'chebmatrix') == 0 )
    uexact = chebmatrix(uexact);
end

% Scale (i.e., maximum amplitude) of the exact solution:
scale = 0;
for i = 1:nVars
    if ( dim == 1 )
        scale = max(scale, max(abs(uexact{i}(xx))));
    elseif( dim == 2 )
        scale = max(scale, max(max(abs(uexact{i}(xx,yy)))));
    elseif ( dim == 3 )
        scale = max(scale, max(max(max(abs(uexact{i}(xx,yy,zz))))));
    end
end

% Second, compute solutions for different schemes and time-steps:
err = zeros(nTimesteps, nSchemes);
time = zeros(nTimesteps, nSchemes);
for k = 1:nTimesteps
    for l = 1:nSchemes
        prefu.scheme = schemes{l};
        [u, ~, t] = spinoperator.solvepde(S, N, timesteps(k), prefu);
        time(k,l) = t;
        if ( isa(u, 'chebmatrix') == 0 )
            u = chebmatrix(u);
        end
        isNan = isNanTest(u, dim);
        if ( isNan == 1 )
            err(k,l) = NaN;
        else
            for i = 1:nVars
                if ( dim == 1 )
                    temp = abs(u{i}(xx) - uexact{i}(xx));
                elseif( dim == 2 )
                    temp = abs(u{i}(xx,yy) - uexact{i}(xx,yy));
                elseif ( dim == 3 )
                    temp = abs(u{i}(xx,yy,zz) - uexact{i}(xx,yy,zz));
                end
                temp = max(temp(:));
                err(k,l) = max(err(k,l), temp);
            end
        end
    end
end
err = err/scale; % Relative error
tspan = S.tspan;
TF = tspan(end);
timesteps = timesteps/TF; % Relative time-steps

% Plot Accuarcy vs time-step:
figure, subplot(1, 2, 1)
labels = cell(nSchemes, 1);
for l = 1:nSchemes
    labels{l} = schemes{l};
    if ( l > 1 )
        hold on
    end
    if ( l < 8 )
        loglog(timesteps, err(:, l), '.-', 'linewidth', 2, 'markersize', 30)
    elseif ( l < 15 )
        loglog(timesteps, err(:, l), 'x-', 'linewidth', 2, 'markersize', 10)
    else
        loglog(timesteps, err(:, l), 'o-', 'linewidth', 2, 'markersize', 10)
    end
end
left = 10^floor(log10(min(timesteps)));
right = 10^ceil(log10(max(timesteps)));
down = max(min(1e-10, 10^floor(log10(min(err(:))))), 1e-12);
up = min(max(1e0, 10^ceil(log10(max(err(:))))), 1e2);
set(gca, 'fontsize', 16), axis([left right down up])
xlabel('Relative time-step'), ylabel(sprintf('Relative error at t = %.3f', TF))
legend(labels, 'Location', 'NorthWest')

% Plot Accuarcy vs computer time:
subplot(1, 2, 2)
for l = 1:nSchemes
    if ( l > 1 )
        hold on
    end
    if ( l < 8 )
        loglog(time(:,l), err(:, l), '.-', 'linewidth', 2, 'markersize', 30)
   elseif ( l < 15 )
        loglog(time(:,l), err(:, l), 'x-', 'linewidth', 2, 'markersize', 10)
    else
        loglog(time(:,l), err(:, l), 'o-', 'linewidth', 2, 'markersize', 10)
    end
end
left = 10^(floor(log10(min(min(time)))));
right = 10^(ceil(log10(max(max(time)))));
set(gca, 'fontsize', 16), axis([left right down up])
xlabel('Computer time (s)'), ylabel(sprintf('Relative error at t = %.3f', TF))
legend(labels, 'Location', 'NorthEast')

end

function out = isNanTest(u, dim)

B = u.blocks;
if ( dim == 1 )
    out = any(cell2mat(cellfun(@(C) isnan(C), B, 'UniformOutput', 0)));
elseif ( dim == 2 )
    out = any(cell2mat(cellfun(@(C) isnan(C.cols), B, 'UniformOutput', 0)));
    out = out || ...
        any(cell2mat(cellfun(@(C) isnan(C.rows), B, 'UniformOutput', 0)));
elseif ( dim == 3 )
    out = any(cell2mat(cellfun(@(C) isnan(C.cols), B, 'UniformOutput', 0)));
    out = out || ...
        any(cell2mat(cellfun(@(C) isnan(C.rows), B, 'UniformOutput', 0)));
    out = out || ...
        any(cell2mat(cellfun(@(C) isnan(C.tubes), B, 'UniformOutput', 0)));
end

end


function [idx, iter, obj, H] = symnmf_cluster(X, k, options)
%SYMNMF_CLUSTER
% [idx, iter, obj, H] = symnmf_cluster(X, k, options)
%
% This function performs graph clustering on a data matrix.
%
% Input:
% X - NxP data matrix (N observations with dimension P),
%     where each row is one observation,
%     and each column is one feature/variable.
%     (If an NxN similarity matrix is available,
%      please call 'symnmf_anls' directly.)
% k - Specifying the number of clusters.
% options - A structure of optional parameters for
%           clustering (see details below).
%
% Default configurations:
%   options.graph_type = 'sparse';
%   options.similarity_type = 'gaussian';
%   options.graph_objfun = 'ncut';
%   (no options.kk given)
%   options.nn = 7;
%   options.tol = 1e-3;
%   options.maxiter = 10000;
%   options.rep = 1;
%   (no options.Hinit given)
%   options.computeobj = true;
%   options.alg = 'anls';
%
% Output:
% idx - Nx1 vector, containing the clustering assignment
%       of each observation.
% iter - Number of iterations actually used
% obj - Final value of the objective function
%           f(H) = ||A - HH'||_F^2
% H - Final result of low-rank matrix H
%
% Available options:
% 1. options.graph_type
%    = 'full': to construct a fully-connected graph
%    = 'sparse': to construct a sparse graph where
%                each observation is connected to
%                its KK nearest neighbors. (DEFAULT)
%
%    (Note: Only undirected graph is supported,
%           so the resulting similarity matrix
%           is symmetric.)
%
% 2. options.similarity_type
%    = 'gaussian': to use self-tuning Gaussian similarity,
%                  proposed in [Zelnik-Manor and Perona,
%                  2004] (see more details in functions
%                  'scale_dist3' and 'scale_dist3_knn') (DEFAULT)
%    = 'inner_product': to use inner product similarity
%                       (good choice for text data)
%
% 3. options.graph_objfun
%    = 'ncut': to use normalized cut (DEFAULT)
%    = 'rcut': to use ratio cut
%
% 4. options.kk (DEFAULT is unset, i.e. depending on N)
%    specifies the number of nearest neighbors WHEN
%    options.graph_type = 'sparse'.
%
%    The default value depends on N:
%    option.kk = floor(log2(N)) + 1;
%
% 5. options.nn (DEFAULT is 7)
%    specifies the nn-th neighbor, which is used in
%    the self-tuning Gaussian similarity WHEN
%    options.similarity_type = 'gaussian'.
%
% 6. options.tol (DEFAULT is 1e-3)
%    controls the termination of SymNMF algorithm.
%
% 7. options.maxiter (DEFAULT is 10000)
%    limits the number of iteration allowed in each run.
%
% 8. options.rep (DEFAULT is 1)
%    sets the nubmer of runs of SymNMF algorithm.
%
%    The return values 'idx', 'iter', 'obj', and 'H' are
%    corresponding to a single run, which is the run that
%    yields the lowest objective function value.
%
%    Using multiple runs of SymNMF may help avoid local minima.
%
% 9. options.Hinit (DEFAULT is unset, i.e. random initialization)
%    sets the initialization of matrix H in each run.
%
%    options.Hinit is a 3-dim array. The 3rd dimension may imply
%    the choice of options.rep, and each 2-dim array in the 1st
%    and 2nd dimensions is a NxK nonnegative matrix.
%
%    Random initialization is recommended. If options.Hinit has to
%    be a fixed set of matrices for testing, the instructions on
%    how to initialize H should be followed. Please consult the
%    usage of 'symnmf_newton'.
%
% 10. options.computeobj (DEFAULT is true)
%     specifies whether to compute the objective value
%     f(H) at the final solution H.
%
% 11. options.alg
%     = 'anls' to use ANLS algorithm (DEFAULT)
%     = 'newton' to use Newton-like algorithm
%
% This function is used for experiments in the following paper:
%     Da Kuang, Chris Ding, Haesun Park,
%     Symmetric Nonnegative Matrix Factorization for Graph Clustering,
%     The 12th SIAM International Conference on Data Mining (SDM '12), pp. 106--117.
% Please cite this paper if you find this code useful.
%

[n, p] = size(X);

if ~exist('options', 'var')
    graph_type = 'sparse';
    similarity_type = 'gaussian';
    graph_objfun = 'ncut';
    kk = floor(log2(n)) + 1;
    nn = 7;
    tol = 1e-3;
    maxiter = 10000;
    rep = 1;
    Hinit = [];
    computeobj = true;
    alg = 'anls';
else
    if isfield(options, 'graph_type')
        graph_type_names = {'full', 'sparse'};
        j = find(strcmpi(options.graph_type, graph_type_names));
        if ~isempty(j)
            graph_type = graph_type_names{j};
        else
            error('Invalid options.graph_type value!');
        end
    else
        graph_type = 'sparse';
    end
    if isfield(options, 'similarity_type')
        similarity_type_names = {'gaussian', 'inner_product'};
        j = find(strcmpi(options.similarity_type, similarity_type_names));
        if ~isempty(j)
            similarity_type = similarity_type_names{j};
        else
            error('Invalid options.similarity_type value!');
        end
    else
        similarity_type = 'gaussian';
    end
    if isfield(options, 'graph_objfun')
        graph_objfun_names = {'ncut', 'rcut'};
        j = find(strcmpi(options.graph_objfun, graph_objfun_names));
        if ~isempty(j)
            graph_objfun = graph_objfun_names{j};
        else
            error('Invalid options.graph_objfun value!');
        end
    else
        graph_objfun = 'ncut';
    end
    if isfield(options, 'kk')
        if ~isempty(options.kk) & isnumeric(options.kk) & options.kk < n
            kk = options.kk;
        else
            error('options.kk must be an integer less than N!');
        end
    else
        kk = floor(log2(n)) + 1;
    end
    if isfield(options, 'nn')
        if ~isempty(options.nn) & isnumeric(options.nn) & options.nn < n
            nn = options.nn;
        else
            error('options.nn must be an integer less than N!');
        end
    else
        nn = 7;
    end
    if isfield(options, 'tol')
        if ~isempty(options.tol) & isnumeric(options.tol) & options.tol > 0 & options.tol < 1
            tol = options.tol;
        else
            error('options.tol must be a real number and 0 < options.tol < 1!');
        end
    else
        tol = 1e-3;
    end
    if isfield(options, 'maxiter')
        if ~isempty(options.maxiter) & isnumeric(options.maxiter) & options.maxiter > 0
            maxiter = options.maxiter;
        else
            error('options.maxiter must be a positive integer!');
        end
    else
        maxiter = 10000;
    end
    if isfield(options, 'rep')
        if ~isempty(options.rep) & isnumeric(options.rep) & options.rep > 0
            rep = options.rep;
        else
            error('options.rep must be a positive integer!');
        end
        if isfield(options, 'Hinit') & size(options.Hinit, 3) ~= rep
            error('The third dimension of options.Hinit must match options.rep!')
        end
    else
        if isfield(options, 'Hinit') & ~isempty(options.Hinit)
            rep = size(options.Hinit, 3);
        else
            rep = 1;
        end
    end
    if isfield(options, 'Hinit')
        if ~isempty(options.Hinit) & isnumeric(options.Hinit) & size(options.Hinit, 1) == n & size(options.Hinit, 2) == k
            Hinit = options.Hinit;
        else
            error('The size of each initialization of H must be Nxk!');
        end
    else
        Hinit = [];
    end
    if isfield(options, 'computeobj')
        if ~isempty(options.computeobj) & (isnumeric(options.computeobj) | isboolean(options.computeobj))
            computeobj = options.computeobj;
        else
            error('options.computeobj must be boolean or a real number!');
        end
    else
        computeobj = true;
    end
    if computeobj == false & rep > 1
        error('options.computeobj must be true if options.rep > 1!');
    end
    if isfield(options, 'alg')
        alg_names = {'anls', 'newton'};
        j = find(strcmpi(options.alg, alg_names));
        if ~isempty(j)
            alg = alg_names{j};
        else
            error('Invalid options.alg value!');
        end
    else
        alg = 'anls';
    end
end

D = dist2(X, X);
if strcmp(graph_type, 'full') & strcmp(similarity_type, 'gaussian')
    A = scale_dist3(D, nn);
elseif strcmp(graph_type, 'full') & strcmp(similarity_type, 'inner_product')
    A = X * X';
elseif strcmp(graph_type, 'sparse') & strcmp(similarity_type, 'gaussian')
    A = scale_dist3_knn(D, nn, kk, true);
else % graph_type == 'sparse' & similarity_type == 'inner_product'
    Xnorm = X';
    d = 1./sqrt(sum(Xnorm.^2));
    Xnorm = bsxfun(@times, Xnorm, d);
    A = inner_product_knn(D, Xnorm, knn, true);
    clear Xnorm, d;
end
clear D;

if strcmp(graph_objfun, 'ncut')
    dd = 1 ./ sum(A);
    dd = sqrt(dd);
    A = bsxfun(@times, A, dd);
    A = A';
    A = bsxfun(@times, A, dd);
    clear dd;
end
A = (A + A') / 2;

params.maxiter = maxiter;
params.tol = tol;

obj_best = Inf;
for i = 1 : rep
    if isempty(Hinit)
        if strcmp(alg, 'newton')
            [sol, infos] = symm_newton(A, k, params);
        else % strcmp(alg, 'anls')
            [sol, infos] = symm_anls(A, k, params);
        end
    else
        params.Hinit = Hinit(:, :, i);
        if strcmp(alg, 'newton')
            [sol, infos] = symm_newton(A, k, params);
        else % strcmp(alg, 'anls')
            [sol, infos] = symm_anls(A, k, params);
        end
    end
    H = sol.H;
    [max_val, idx] = max(H, [], 2);
    if infos.cost(end) < obj_best
        idx_best = idx;
        iter_best = infos.iter(end);
        obj_best = infos.cost(end);
        H_best = H;
    end
end

idx = idx_best;
iter = iter_best;
obj = obj_best;
H = H_best;


function [ V ] = T2V( T )
%T2V converts 4x4 transformation matrix into a 1x6 vector
%   Inputs -
%   T - a standard 4x4 transformation matrix
%
%   Outputs -
%   V - 1x6 vector of form [x,y,z,rx,ry,rz] where x,y,z is the translation
%   and rx,ry,rz is an angle-axis representation of the angle where the
%   unit vector representing the axis has been multipled by the angle of
%   rotation about it

validateattributes(T, {'numeric'},{'size',[4,4]});

T = double(T);

V(1:3) = T(1:3,4);
V(4:6) = R2V(T(1:3,1:3));

end


function [score]=crps(ygibbs,yo)

% compute first the dimension of the gibbs sampler draws (normally, 'It' iterations minus 'Bu' discarded burn iterations)
It_Bu=size(ygibbs,1);

% compute the first summation term
sum1=sum(abs(ygibbs-repmat(yo,It_Bu,1)));

% compute the second summation term
 temp=abs(repmat(ygibbs,1,It_Bu)-repmat(ygibbs',It_Bu,1));
 sum2=sum(temp(:));

% eventually compute the score
score=(1/It_Bu)*sum1-(1/(2*It_Bu^2))*sum2;


function [Z_hat, E_hat] = exact_alm_lrr_l21v2(D, A, lambda, tol, maxIter,display)

% Aug 2013
% This matlab code implements the Exact ALM algorithm for
% min_{Z,E}  |Z|_* + lambda |E|_2,1  s.t.  D = AZ + E
%
% D - m x n matrix of observations/data (required input)
% A - m x k matrix of the dictionary (required input)

% lambda - weight on sparse error term in the cost function
%
% tol - tolerance for stopping criterion.
%     - DEFAULT 1e-7 if omitted or -1.
%
% maxIter - maximum number of iterations
%         - DEFAULT 1000, if omitted or -1.
%
[m n] = size(D);
k = size(A,2);

if nargin < 4 || isempty(tol)
    tol = 1e-7;
end

if nargin < 5 || isempty(maxIter)
    maxIter = 1000;
end

if nargin<6 || isempty(display)
    display = false;
end

maxIter_primal = 10000;
% initialize
Y = sign(D);
norm_two = norm(Y,2);
norm_inf = norm( Y(:), inf) / lambda;
dual_norm = max(norm_two, norm_inf);
Y = Y / dual_norm;

W = zeros(k,n);

Z_hat = zeros(k,n);
E_hat = zeros(m,n);
%parameters
dnorm = norm(D, 'fro');
tolProj1 = 1e-6 * dnorm;

anorm = norm(A,2);
tolProj2 = 1e-6 * dnorm/anorm;

mu = .5/norm_two; % this one can be tuned
rho = 6;          % this one can be tuned

%pre-computation
if m>=k
    inv_ata = inv(eye(k) + A'*A);
else
    inv_ata = eye(k) - A'/(eye(m)+A*A')*A;
end

iter = 0;
while iter < maxIter
    iter = iter + 1;

    % solve the primal problem by alternative projection
    primal_iter = 0;

    while primal_iter < maxIter_primal
        primal_iter = primal_iter + 1;
        temp_Z = Z_hat;
        temp_E = E_hat;

        %update J
        temp = temp_Z + W/mu;

        [U,S,V] = svd(temp, 'econ'); % stable
        %[U,S,V] = svdecon(temp); % fastest, but EIG must not contain NaN or Inf.

        diagS = diag(S);
        svp = length(find(diagS > 1/mu));
        diagS = max(0,diagS - 1/mu);

        if svp < 0.5 %svp = 0
            svp = 1;
        end
        J_hat = U(:,1:svp)*diag(diagS(1:svp))*V(:,1:svp)';

        % update Z
        temp = J_hat + A'*(D - temp_E) + (A'*Y-W)/mu;
        Z_hat = inv_ata*temp;

        %update E
        temp = D - A*Z_hat + Y/mu;
        E_hat =  solve_l1l2(temp, lambda/mu);

        if norm(E_hat - temp_E, 'fro') < tolProj1 && norm(Z_hat - temp_Z)<tolProj2
            break;
        end
    end

    H1 = D - A*Z_hat - E_hat;
    H2 = Z_hat - J_hat;
    Y = Y + mu*H1;
    W = W + mu*H2;
    mu = rho * mu;

    %% stop Criterion
    stopCriterion = max(norm(H1, 'fro')/dnorm, norm(H2,'fro')/dnorm*anorm);
    if display
        disp(['LRR: Iteration' num2str(iter) '(' num2str(primal_iter) '), mu ' num2str(mu) ', |E|_2,0 ' num2str(sum(sum(E_hat.^2,1)>0))...
        ', stopCriterion ' num2str(stopCriterion)]);
    end

    if stopCriterion < tol
        break;
    end

end

end


function f = diff( f, varargin )
%DIFF   Derivative of a diskfun in Cartesian coordinates.
%
%   F = DIFF( F ) computes the first derivative of F with respect to x.
%
%   F = DIFF( F, DIM )  computes the first derivative of F. If DIM = 1, the
%   derivative is taken in the x-direction. If DIM = 2, the derivative
%   is taken in the y-direction.
%
%   F = DIFF( F, DIM, K) computes the kth derivatives of F in the variable
%   given by DIM.
%
% See also DISKFUN/LAPLACIAN, DISKFUN/DIFFX, DISKFUN/DIFFY

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

if ( isempty(f) )
    f = diskfun;
    return
end

% Parse user inputs:
if ( nargin == 1 )
    dim = 1;
    K = 1;
elseif ( nargin == 2 )
    dim = varargin{1};
    K = 1;
else
    dim = varargin{1};
    K = varargin{2};
end

if ( dim ~= 1 && dim ~= 2  )
    error('DISKFUN:DIFF:DIM', 'Unrecognized coordinate dimension.');
end

if ( abs( K - round(K) ) > eps )
    error('DISKFUN:DIFF:DIFFORDER', 'Fractional derivatives not allowed.')
end
K = round( K );

% Implement higher derivatives as repeated (iterated) differentiation
for j=1:K
    f = onediff(f, dim);
end

end
% TODO: This code will not work for complex valued diskfuns, if we ever
% allow them.

function f = onediff(f, dim)

% Make sure f has as short a length as possible.
f=simplify(f);

% We are going to work at the tech level to make things faster.
[C, ~, R] = cdr( f );

% Do everything with even length columns since then no special
% modifications are required for dividing by rho series expansion.
parity = mod(length(C),2);
n = length(C)+parity;
m = length(R);

% The variable coefficients in the definitions of the derivatives means
% that the length of the columns and rows will increase by one wave number
% after taking the derivatives with respect to x and y.
m = m+2;
n = n+2;

% Matrices for multiplying by sin(theta), cos(theta) and 1/r in coefficient space.
Msinm = .5i*spdiags(ones(m,1)*[-1,1],[-1 1],m,m);
Mcosm = .5*spdiags(ones(m,1)*[1,1],[-1 1],m,m);
Mn = ultraS.multmat(n, [0;1], 0);

% Work at the tech level to make things faster.
ctechs = C.funs{1}.onefun;
rtechs = R.funs{1}.onefun;

% Alias to get the extra zeros in place.
ctechs.coeffs = ctechs.alias(ctechs.coeffs, n);
rtechs.coeffs = rtechs.alias(rtechs.coeffs, m);

% Compute the derivatives.
dCdr = diff(ctechs);
dRdth = diff(rtechs)/pi;

% 1/r*col
rinv = Mn \ ctechs.coeffs;

if (dim==1) %d/dx

    %CDR for -1/r.*sin(th).*d/dth
    C1 = rinv;
    R1 = -Msinm*dRdth.coeffs;

    %CDR for cos(th).*d/dr
    C2 = dCdr.coeffs;
    R2 = Mcosm*rtechs.coeffs;

else  %d/dy

    %CDR for 1/r.*cos(th).*d/dth
    C1 = rinv;
    R1 = Mcosm*dRdth.coeffs;

    %CDR for sin(th).*d/dr
    C2 = dCdr.coeffs;
    R2 = Msinm*rtechs.coeffs;
end

% Put pieces back together
f1 = f;
c1techs = chebtech2({' ',C1});
f1.cols.funs{1}.onefun = c1techs;
r1techs = real(trigtech({'',R1}));
f1.rows.funs{1}.onefun = r1techs;

% Parity changes
temp = f1.idxPlus;
f1.idxPlus = f1.idxMinus;
f1.idxMinus = temp;

f2 = f;
c2techs = chebtech2({'',C2});
f2.cols.funs{1}.onefun = c2techs;
r2techs = real(trigtech({'',R2}));
f2.rows.funs{1}.onefun = r2techs;

% Parity changes
temp = f2.idxPlus;
f2.idxPlus = f2.idxMinus;
f2.idxMinus = temp;

% Compression plus may not preserve the expansion properties we want.
% So we sample each piece add them together and construct a diskfun.
% TODO: Fix this so everything is done in coefficient space, like this
% f = f1 + f2;

% Constructor requires even m
m = m + mod(m, 2);

f = diskfun(sample(f1,m,n/2+1)+sample(f2,m,n/2+1));

end


function [S,Li] = eeg_interp_sph_spline(V,X,Y,Z,Npoints)

% eeg_interp_sph_spline - Spherical Spline Laplacian of Potential
%
% Useage: [S,Li] = eeg_lap_sph_spline(voltage,X,Y,Z,Npoints)
%
% where:    'voltage' is an EEG/ERP measurement at time t from
%           electrode positions (X,Y,Z) on a scalp surface.  All
%           input arrays are the same size, assumed (Nelec x 1).
%           The origin of X,Y,Z is assumed (0,0,0).
%
%           S  => the x,y,z points on a hemisphere
%           Li => spherical spline Laplacian of voltages
%
%           S is generated by 'elec_sphere_points' with
%           Rpoints=Npoints and Epoints=16.
%
% Notes:    This function calculates the spherical spline Laplacian
%           of Perrin et al (1989).  Electroenceph. & Clin.
%             Neurophysiology, 72: 184-187.
%

% $Revision: 1.1 $ $Date: 2009-04-28 22:13:52 $

% Licence:  GNU GPL, no implied or express warranties
% History:  08/01  Darren.Weber_at_radiology.ucsf.edu, with
%                  mathematical and matlab advice from
%                  Dr. Murk Bottema (Flinders University of SA)
%
%           08/01  Needs testing & verification!
%                  With large electrode arrays, the result should be
%                  regularized (not sure why).
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Check for correct size & orientation of X,Y,Z,V
    [x1,x2] = size(X);
    [y1,y2] = size(Y);
    [z1,z2] = size(Z);
    [v1,v2] = size(V);
    if ~and(isequal(x1,y1,z1,v1),isequal(x2,y2,z2,v2))
        error('ERROR: all X, Y, Z, V must be size (Nx1)');
    end
    if x1 < x2, X = X'; [x1,x2] = size(X); end
    if y1 < y2, Y = Y'; [y1,y2] = size(Y); end
    if z1 < z2, Z = Z'; [z1,z2] = size(Z); end
    if v1 < v2, V = V'; [v1,v2] = size(V); end
    if ~(isequal(x2,y2,z2,v2,1))
        error('ERROR: all X, Y, Z, V must be size (Nx1)');
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Computations derived from Perrin et al. (1989)

    % G * C = V,  where:
    %
    % G  (NxN)    spherical spline
    % C  (Nx1)    spline coefficients
    % V  (Nx1)    voltage potentials at (X,Y,Z) electrode locations
    %
    % solve for C = V * Sp' (C = V\Sp; (see "help slash"))

    % First calc G, where G is a function of the cosine of
    % the angle (theta) between electrode point vectors
    A   = [ X Y Z ];
    COS = cosines(A,A);
    G   = spheric_spline(COS);
    C   = spline_coefficients(G,V); % spline coefficients (Co,C1,...,Cn)
    Co  = C(1);
    Ci  = C(2:end);

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Obtain interpolated potentials at S (eq.1, Perrin et al., 1989)
    % V(s) = Co + sum( Ci * g(x) )

    % get spherical electrode radius
    [r,x,y,z] = elec_sphere_fit(X,Y,Z,0,0,0,0);
    % Generate spherical points for interpolation
    [x,y,z]   = elec_sphere_points(16,Npoints,r);
    S         = [x y z];

    fprintf('...Spherical Interpolation Progress:\n');
    rows = 80;  % progress indicator

    for p = 1:length(S)

        B = S(p,:);
        Cos = cosines(A,B);         %(1xN)

        Gx  = spheric_spline(Cos);  %(1xN)

        CiGx = Ci .* Gx';           %(1xN)

        Vi(p,1) = Co + sum(CiGx);

        fprintf('.');  % progress indicator
        if([p] == rows ) fprintf('\n'); rows = rows + 80; end
    end

return

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Solve eq. 3 Perrin et al. (1989)
% g(COS) = 1/4pi * sum[n=1:inf] (( (2*n+1)/(n^m * (n+1)^m) ) * Pn(COS));

function [Gx] = spheric_spline(Cosine)

    m = 4;    N = 7;    % gives accuracy of 10^-6

    P = legendre(N,Cosine);
    %P = LEGENDRE(N,X) computes the associated Legendre functions
    %of degree N and order m = 0, 1, ..., N, evaluated for each element
    %of X.
    %In general, P has one more dimension than X.
    %Each element P(m+1,i,j,k,...) contains the associated Legendre
    %function of degree N and order m evaluated at X(i,j,k,...).

    ndim = ndims(P);
    switch ndim
    case 2, P = P(2:N+1,:);
    case 3, P = P(2:N+1,:,:);
    case 4, P = P(2:N+1,:,:,:);
    otherwise
    end

    k = (1/4 * pi);

    for n = 1:(N),  Series(n,1) = (2*n + 1) / (n^(m-1) * (n+1)^(m-1));  end

    if min(size(Cosine)) == 1,    Gx      = k * ( Series' * P );
    else
        for i = 1:length(Cosine), Gx(i,:) = k * ( Series' * P(:,:,i) );
        end
    end
return

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Solve eq. 2 Perrin et al. (1989)
function [C] = spline_coefficients(Gx,V)

    % add ones to first row & column of Gx
    tmp =  ones(size(V));       Gx   = [tmp Gx];
    tmp = [ones(size(V))' 1]';  Gx   = [tmp Gx']';

    Gx(1,1) = 0;    % according to Murk
    %Gx(1,1) = 1;    % according to EMSE, Greenblatt

    CoV = [0 V']';

    C = Gx\CoV;
return

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Cos] = cosines(A,B)

    for     a = 1:size(A,1),  Aa = A(a,:); A_len = norm(Aa);
        for b = 1:size(B,1),  Bb = B(b,:); B_len = norm(Bb);

            Cos(a,b) = dot(Aa,Bb) / (A_len * B_len);
        end
    end
return


function [A,B,flag] = oprACAv(opr,k,X,Xnrm,Y,Ynrm,tol)
%+========================================================================+
%|                                                                        |
%|            OPENOPR - LIBRARY FOR SPECIFIC OPERATORS IN BEM             |
%|           openOpr is part of the GYPSILAB toolbox for Matlab           |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2019.                             |
%| 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       : oprACAv.m                                     |
%|    #    |   VERSION    : 0.61                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 05.09.2019                                    |
%| ( === ) |   SYNOPSIS   : Adaptative Cross Approximation, partial       |
%|  `---'  |                & total pivoting                              |
%+========================================================================+

% Matrix dimensions
Nx = size(X,1);
Ny = size(Y,1);

% Initialize indices
Ir = (1:Nx)';
Ic = (1:Ny)';

% Partial pivoting with handle function
row = @(i) oprGreenKernel(opr,k,X(i,:),Xnrm(i,:),Y,Ynrm);
col = @(j) oprGreenKernel(opr,k,X,Xnrm,Y(j,:),Ynrm(j,:));

% First row (row index of maximum reference value)
i   = 1;
B   = row(i);
dim = size(B,3);

% First pivot (maximum of the first row)
[~,j] = max(max(abs(B),[],3));
delta = B(j,:,:);

% Securitys
delta(abs(delta)<1e-12) = 1e-12;

% First column
A = col(j)./delta;

% Unused indices1
Ir = [Ir(1:i-1);Ir(i+1:end)];
Ic = [Ic(1:j-1);Ic(j+1:end)];

% Frobenius norm for the initial tensor product
Rn2 = zeros(dim,1);
err = 0;
for l = 1:dim
    An2    = A(:,1,l)'*A(:,1,l);
    Bn2    = B(:,1,l)'*B(:,1,l);
    Rn2(l) = An2*Bn2;
    err    = max(err,sqrt(An2)*sqrt(Bn2)/sqrt(Rn2(l)));
end

% Iterative construction using frobenius norm
n = 1;
while (err > tol)
    % New pivot by maximum
    [~,i] = max(max(abs(A(Ir,n,:)),[],3));
    new   = row(Ir(i));
    for l = 1:dim
        new(:,:,l) = new(:,:,l) - B(:,:,l)*A(Ir(i),1:n,l).';
    end
    [~,j] = max(max(abs(new(Ic,:,:)),[],3));
    delta = new(Ic(j),:,:);

    % New pivot by minimum
    [~,iMin] = min(min(abs(A(Ir,n,:)),[],3));
    newMin   = row(Ir(iMin));
    for l = 1:dim
        newMin(:,:,l) = newMin(:,:,l) - B(:,:,l)*A(Ir(iMin),:,l).';
    end
    [~,jMin] = max(max(abs(newMin(Ic,:,:)),[],3));
    deltaMin = newMin(Ic(jMin),:,:);

    % Best pivot
    if (norm(deltaMin(:)) > norm(delta(:)))
        i     = iMin;
        new   = newMin;
        j     = jMin;
        delta = deltaMin;
    end

    % Security
    delta(abs(delta)<1e-12) = 1e-12;

    % Update row
    B(:,n+1,:) = new;

    % New column
    new = col(Ic(j));
    for l = 1:dim
        new(:,:,l) = (new(:,:,l) - A(:,:,l)*B(Ic(j),1:n,l).')./delta(l);    %B(:,:,l)*A(Ir(i),:,l).';
    end
    A(:,n+1,:) = new;

    % Update row and column indices
    Ir = [Ir(1:i-1);Ir(i+1:end)];
    Ic = [Ic(1:j-1);Ic(j+1:end)];

    % Incrementation
    n = n + 1;

    % Relative Frobenius error by block
    err = 0;
    for l = 1:dim
        An2    = A(:,n,l)'*A(:,n,l);
        Bn2    = B(:,n,l)'*B(:,n,l);
        u      = B(:,n,l)'*B(:,1:n-1,l);
        v      = A(:,n,l)'*A(:,1:n-1,l);
        AB     = v*u.';
        Rn2(l) = Rn2(l) + 2*real(AB) + An2*Bn2;
        err    = max(err,sqrt(An2)*sqrt(Bn2)/sqrt(Rn2(l)));
    end

    % Compression failed
    if (n*(Nx+Ny) > Nx*Ny)
        A    = [];
        B    = [];
        flag = 0;
        return
    end
%     sqrt(An2)*sqrt(Bn2)/sqrt(Rn2)
%     norm(A*B.' - A(:,1:n-1)*B(:,1:n-1).','fro')/norm(A*B.','fro')
%     norm(A(:,n)*B(:,n).','fro')/norm(A*B.','fro')
%     norm(ref,'inf')/nrm
%     '================================'
end

% B transposition lead to  A * B
B    = permute(B,[2 1,3]);
flag = 1;
end


function [y,err]=golaycodec(x,enc,ext)
% GOLAYCODEC - encode/decode a binary array using the Golay code with error correction.
%
% golaycodec encodes or decodes a binary array using the Golay code.
% Words of length 12 are encoded as codewords of length 23 (or 24 for the
% extended Golay code).  The Golay code is tolerant of up to 3 errors,
% i.e. for any word of length 23 there is a unique codeword that differs
% in at most 3 positions.
%
% The Golay code can be constructed as a polynomial code using one of the
% degree 11 factors of z^23+1 (mod 2).  Here we use
%   g = 1 + z^2 + z^4 + z^5 + z^6 + z^10 + z^11
% For more details see Wikipedia etc.
%
% Usage:
%   y = golaycodec(x)
%   [y,err] = golaycodec(x)
%   ... = golaycodec(x,enc)
%   ... = golaycodec(x,enc,ext)
%
% Inputs:
%   x can be an Mx12, Mx23 or Mx24 binary (logical) array or an Mx1 integer array.
%     If x is Mx1, the binary representation of each integer is used.
%   enc is a logical; if true x is to be encoded, otherwise x is to be decoded.
%     If not specified, enc is inferred from the size of x or set to true.
%   ext is a logical; if true use the extended Golay code (length 24),
%     otherwise use the non-extended Golay code (length 23).
%     If not specified, enc is inferred from the size of x or set to false.
%     NOTE: the algorithm has so far only been implemented for ext=false
%
% Outputs:
%   y is an array consisting of the coded or decoded rows of x
%     If x is Mx12 and ext=false then y is Mx23
%     If x is Mx12 and ext=true then y is Mx24
%     If x is Mx23 or Mx24 then y is Mx12
%     If x is Mx1 then y is Mx1
%   err (only available if decoding is done, e.g enc=false) is an array
%     with the errors that differ each word in x from its closest codeword)
%
% Example: encode some messages, add transmission errors and decode
%   n=10;
%   x=zeros(n,12);for i=1:n,x(i,ceil(12*rand(1,8)))=1;end; % random message
%   y=golaycodec(x);
%   err=zeros(n,23);for i=1:n,err(i,ceil(23*rand(1,3)))=1;end; % 3 random errors per message
%   y1=xor(y,err); % add transmission error
%   [x1,err1]=golaycodec(y1); % decode: should have x1==x and err1==err
%

% Author: Ben Petschel 18/3/2009
%
% Change history:
%  18/3/2009 - created
%  30/4/2009 - updated to implement extended Golay codes
%

encdef = true; % default value of "enc" if not specified for integer input
extdef = false; % default value of "ext" if not specified

% input checking: x
nx=size(x,2);
if any(~isfinite(x(:))) || any(~isreal(x(:)))
  error('golaycodec: input array must be finite real values');
end;
if nx==1
  if any(x~=round(x))
    error('golaycodec: column input must be integers');
  end;
  if any(x<0)
    % check later that integers are small enough
    error('golaycodec: column input must be non-negative integers');
  end;
  intout = true;

elseif nx>1
  if any((x(:)~=0)&(x(:)~=1))
    error('golaycodec: entries of row or matrix input must be 0 or 1');
  end;
  if ~any(nx==[12,23,24]),
    error('golaycodec: number of columns of input must be 1, 12, 23 or 24');
  end;
  intout = false; % binary output

else
  error('golaycodec: empty input');
end; % if nx==1 elseif ... else ... end;

% input checking: enc, ext must be booleans
if nargin>=2,
  if isnumeric(enc),
    enc = enc>0;
  end;
  if ~islogical(enc) || ~isscalar(enc)
    error('golaycodec: enc must be 1x1 logical value');
  end;
end;
if nargin>=3,
  if isnumeric(ext),
    ext = ext>0;
  end;
  if ~islogical(ext) || ~isscalar(ext)
    error('golaycodec: enc must be 1x1 logical value');
  end;
end;

% input checking: consistency of nx with enc and ext if specified
switch nx
  case 1
    if nargin == 1,
      enc = encdef; % set to default value (usually true)
    end;
    if nargin <= 2,
      ext = extdef; % set to default value (usually false)
    end;
  case 12
    if nargin == 1,
      enc = true; % encode 12-digit word
    elseif ~enc,
      error('golaycodec: cannot decode 12-digit words');
    end;
    if nargin <=2,
      ext = extdef; % set to default value (usually false)
    end;
  case 23
    if nargin == 1,
      enc = false; % decode 23-digit word
    elseif enc,
      error('golaycodec: cannot encode 23-digit words');
    end;
    if nargin <=2,
      ext = false; % not from the extended code
    elseif ext,
      error('golaycodec: 23 digit words are not in the extended code');
    end;
  case 24
    if nargin == 1,
      enc = false; % decode 24-digit word
    elseif enc,
      error('golaycodec: cannot encode 24-digit words');
    end;
    if nargin <=2,
      ext = true; % use the extended code
    elseif ~ext,
      error('golaycodec: 24-digit words are not in the non-extended code');
    end;
  otherwise
    error('golaycodec: input must have 1, 12, 23 or 24 columns');
end; % switch nx

% for integer inputs, check that the integers are not too large
if intout
  if enc
    if any(x(:)>=2^12),
      error('golaycodec: integers for coding must be less than 2^12');
    end;
  elseif any(x(:)>=2^23),
    error('golaycodec: integers for decoding must be less than 2^23');
  end;
end;

% output checking: nargout>1 only for decoding
if (nargout>2) || ((nargout>1) && enc)
  error('golaycodec: can only specify two outputs when decoding');
end;

if nx==1,
  % convert integers to the appropriate length
  if enc
    nx=12;
  elseif ext
    % decode extended code
    nx=24;
  else
    % decode non-extended code
    nx=23;
  end;
  x = dec2logi(x,nx);
end; % if nx==1

% retain generating polynomial, parity check and error parity table
persistent g h errtab

if isempty(g)
  % generating polynomial: g = 1+x^2+x^4+x^5+x^6+x^10+x^11
  g=zeros(1,12);g([0,2,4,5,6,10,11]+1)=1;
end;

if isempty(h)
  % parity check polynomial is the other factor of x^23-1 (mod 2)
  % h = (1+x)*(1+x+x^5+x^6+x^7+x^9+x^11)
  h=mod(deconv([1,zeros(1,22),1],g),2);
end;

if isempty(errtab),
  % set up the error table for decoding: determine all errors of up to 3 bits
  % the error will be determined by simple table lookup of the parity check
  % the table is a sparse array indexed by the integer representation of the parity bits
  errtab = geterrtab(h,3,23);
end;

% now encode or decode the array
if enc
  y = circprod(x,g,23);
  if ext
    % if using extended code, append a checksum bit (row sums mod 2)
    y = [y,mod(sum(y,2),2)];
  end;
else
  if ext
    % drop the checksum bit if using extended code
    c = x(:,24);
    x = x(:,1:23);
  end;
  % check parity
  p = circprod(x,h,23);
  % look up parity in error table
  err = dec2logi(errtab(logi2dec(p)+1),23);
  y = zeros(size(x,1),12);
  for i=1:size(x,1),
    y(i,:) = mod(deconv(x(i,:)-err(i,:),g),2);
  end;
  if ext
    % if using extended code, append the checksum bit error
    err = [err,mod(c-sum(y,2),2)];
  end;
end;

% convert back to integers, if necessary
if intout,
  y = logi2dec(y);
  if nargout == 2,
    err = logi2dec(err);
  end;
end;

end % main function golaycodec

function t=geterrtab(h,k,n)
% produces lookup table of parity checks of all errors with weight <=k on n digits
% y is a sparse matrix: index is 1 + (h*err converted to integer)
% and y(i) is (err) converted to binary
%
% h must be row vector of length <= n
% also 0 <= k <= n.

m=length(h);
if (2^n-1)>intmax,
  error('golaycodec:geterrtab: too many digits to convert to integer');
end;
if m>n,
  error('golaycodec:geterrtab: parity check polynomial is too long');
end;

t = sparse(1,1,0,2^n,1); % initialize with 0*h+1 -> 0

for i=1:k,
  ind = nchoosek(1:n,i); % all possible combinations of i error digits
  nck = size(ind,1);
  errbin = zeros(nck,n); % error in binary format
  errbin(sub2ind([nck,n],repmat((1:nck)',i,1),ind(:)))=1; % in row j place 1's in columns ind(j,:)
  errint = logi2dec(errbin); % error in integer format
  errh = circprod(errbin,h,n); % do parity check using h
  t(logi2dec(errh)+1)=errint;
end;

end % helper function geterrtab(...)

function z=circprod(x,h,n)
% does parity check of rows of x using polynomial h
m=size(x,1);

% do circular convolution by fourier transform (fft acts on columns)
% (could also use conv and wrap the tail elements, but conv isn't vectorized)
z = mod(round(ifft(fft(x',n).*repmat(fft(h',n),1,m))'),2);

end % helper function paritycheck

function y=logi2dec(x)
% converts each row of logical array from binary to a decimal number
n=size(x,2);
y=x*(2.^(n-1:-1:0))';

end % helper function logi2dec

function y=dec2logi(x,n)
% converts vector of integers to array of logicals (binary representation)
y=rem(floor(x(:)*2.^(1-n:0)),2); % see dec2bin.m (base matlab) for details

end % helper function dec2logi


function cout = intermutual(cin1, cin2, n)

%tstoolbox/@core/intermutual
%   Syntax:
%     * intermutual(cin1,cin2,n)
%
%   Input Arguments:
%     * cin1,cin2 - core objects
%
%   Calculates the mutual information of cin1 and cin2.
%
% Copyright 1997-2001 DPI Goettingen, License http://www.physik3.gwdg.de/tstool/gpl.txt

N = dlens(cin1,1);
cout = zeros(n,1);
epsilon = 1e-9;
points1 = data(cin1);
points1 = points1 - min(points1);
points2 = data(cin2);
points2 = points2 - min(points2);
ma1 = max(points1);
ma2 = max(points2);
increment = 1/N;

for i=1:n
	partitionen = 2^i;
	pointsA = 1+floor(points1 / (ma1/(partitionen-epsilon)));
	pointsB = 1+floor(points2 / (ma2/(partitionen-epsilon)));
	% Aus den Werten sind jetzt fertige Indizes fuer die Partititonen geworden

	histA = sparse(pointsA, ones(N,1), increment);
	histB = sparse(ones(N,1), pointsB, increment);
	histAB = sparse(pointsA, pointsB, increment);
	[ind1, ind2, value] = find(histAB);
	amf = sum(value .* log2(value ./ (histA(ind1) .* histB(ind2)')));	% '

	cout(i) = amf;
end

%cout = core(amf);


function SE = functionComputeSE_DL_poweralloc(rho,signal,interference,prelogFactor)
%Compute the SE in Theorem 4.6 using the formulation in (7.1) for a given
%power allocation scheme.
%
%INPUT:
%rho          = K x L matrix where element (k,j) is the downlink transmit
%               power allocated to UE k in cell j
%signal       = K x L matrix where element (k,j,n) is a_jk in (7.2)
%interference = K x L x K x L matrix where (l,i,jk,n) is b_lijk in (7.3)
%prelogFactor = Prelog factor
%
%OUTPUT:
%SE = K x L matrix where element (k,j) is the downlink SE of UE k in cell j
%     using the power allocation given as input
%
%
%This Matlab function was developed to generate simulation results to:
%
%Emil Bjornson, Jakob Hoydis and Luca Sanguinetti (2017),
%"Massive MIMO Networks: Spectral, Energy, and Hardware Efficiency",
%Foundations and Trends in Signal Processing: Vol. 11, No. 3-4,
%pp. 154-655. DOI: 10.1561/2000000093.
%
%For further information, visit: https://www.massivemimobook.com
%
%This is version 1.0 (Last edited: 2017-11-04)
%
%License: This code is licensed under the GPLv2 license. If you in any way
%use this code for research that results in publications, please cite our
%monograph as described above.

%Extract number of UEs
K = size(signal,1);

%Extract number of cells
L = size(signal,2);

%Prepare to save results
SE = zeros(K,L);

%% Go through all cells
for j = 1:L

    %Go through all UEs in cell j
    for k = 1:K

        %Compute the SE in Theorem 4.6 using the formulation in (7.1)
        SE(k,j) = prelogFactor*log2(1+(rho(k,j)*signal(k,j)) / (sum(sum(rho.*interference(:,:,k,j))) + 1));

    end

end


function gmst=utc2gmst(t,ut1_utc)

ep2000=[2000 1 1 12 0 0];

tut=timeadd(t,ut1_utc);
[ut,tut0]=time2sec(tut);
t1=timediff(tut0,epoch2time(ep2000))/86400.0/36525.0;
t2=t1^2; t3=t1^2;
gmst0=24110.54841+8640184.812866*t1+0.093104*t2-6.2E-6*t3;
gmst=gmst0+1.002737909350795*ut;

if gmst<0
    gmst=-mod(abs(gmst),86400.0)*pi/43200.0;
else
    gmst=mod(abs(gmst),86400.0)*pi/43200.0;
end

return


function [ node_xy, element_node, element_neighbor ] = example2_q4_mesh ( ...
  node_num, element_num )

%*****************************************************************************80
%
%% EXAMPLE2_Q4_MESH sets up example #2 Q4 mesh.
%
%  Discussion:
%
%    The region is a semicircle.  This example includes degenerate elements
%    (the first layer of elements is touching the origin, and so has a side
%    of length zero).  The elements are not parallelograms.  And the elements
%    vary in size.
%
%    Because of the treatment of node 1, algorithms for counting boundary
%    edges may become "confused".
%
%    The appropriate values of NODE_NUM and ELEMENT_NUM can be found by
%    calling EXAMPLE1_Q4_MESH_SIZE first.
%
%   29---30---31---32---33---34---35---36---37
%    | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 |
%   20---21---22---23---24---25---26---27---28
%    | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 |
%   11---12---13---14---15---16---17---18---19
%    |  9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 |
%    2----3----4----5----6----7----8----9---10
%    |  1 |  2 |  3 |  4 |  5 |  6 |  7 |  8 |
%    1----1----1----1----1----1----1----1----1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 February 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, integer ELEMENT_NUM, the number of elements.
%
%    Output, real NODE_XY(2,NODE_NUM), the coordinates of the
%    nodes.
%
%    Output, integer ELEMENT_NODE(4,ELEMENT_NUM), the nodes
%    that make up the elements.
%
%    Output, integer ELEMENT_NEIGHBOR(4,ELEMENT_NUM), the
%    element neighbors on each side.  Negative values indicate edges that
%    lie on the exterior.
%
  k = 1;
  node_xy(1,k) = 0.0;
  node_xy(2,k) = 0.0;

  for row = 1 : 4
    r = row;
    for col = 0 : 8
      a = ( 8 - col ) * pi / 8.0;
      k = k + 1;
      node_xy(1,k) = r * cos ( a );
      node_xy(2,k) = r * sin ( a );
    end
  end

  element = 0;
  for row = 0 : 3
    for col = 0 : 7
      element = element + 1;
      if ( row == 0 )
        element_node(1,element) = 1;
        element_node(2,element) = 1;
        element_node(3,element) = col + 3;
        element_node(4,element) = col + 2;
      else
        element_node(1,element) = element_node(4,element-8);
        element_node(2,element) = element_node(3,element-8);
        element_node(3,element) = element_node(2,element) + 9;
        element_node(4,element) = element_node(1,element) + 9;
      end
    end
  end

  element = 0;
  for row = 0 : 3
    for col = 0 : 7
      element = element + 1;
      if ( row == 0 )
        element_neighbor(1,element) = -1;
      else
        element_neighbor(1,element) = element - 8;
      end
      if ( col == 7 )
        element_neighbor(2,element) = -1;
      else
        element_neighbor(2,element) = element + 1;
      end
      if ( row == 3 )
        element_neighbor(3,element) = - 1;
      else
        element_neighbor(3,element) = element + 8;
      end
      if ( col == 0 )
        element_neighbor(4,element) = - 1;
      else
        element_neighbor(4,element) = element - 1;
      end
    end
  end

  return
end


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
% function [fh,ph,th] = checkDerivative(fctn,x0);
%
% checks the implementation of a derivative by comparing the function
% with the Taylor-poly
%
%   \| f(x0 + h ) - TP_p(x0,h) \|   !=   O( h^{p+1} )
%
% Input:
%  fctn    function handle
%  x0      expanding point
%
% Output:
%  fh      figure handle to graphical output
%  ph      plot handle
%  th      text handle
% call checkDerivative for a minimal example.
%==============================================================================

function varargout = checkDerivative(fctn,x0,varargin)

if nargin == 0, % help and minimal example
  help(mfilename);
  fctn = @xSquare;
  x0   = 1;
  checkDerivative(fctn,x0);
  return;
end;

fig = [];
for k=1:2:length(varargin),     % overwrites default parameter
  eval([varargin{k},'=varargin{',int2str(k+1),'};']);
end;

FAIRmessage(mfilename,'.')
fprintf('test derivative of function <%s>\n',func2str(fctn));

fprintf('T0 = |f0 - ft|, T1 = |f0+h*f0'' - ft|\n');
[f0,df] = feval(fctn,x0);
if ~isnumeric(df),
  try
    [f0,para,df] = feval(fctn,x0);
  catch
    if isfield(df,'Q'), df = df.Q;  end;
  end;
end;

h = logspace(-1,-10,10);
v = randn(size(x0));
if isnumeric(df),
  dvf = df*v;
elseif isa(df,'function_handle'),
  dvf = df(v);
else
  keyboard;
end;

for j=1:length(h),
  ft = feval(fctn,x0+h(j)*v);      % function value
  T0(j) = norm(f0-ft);             % TaylorPoly 0
  T1(j) = norm(f0+h(j)*dvf - ft);  % TaylorPoly 1
  fprintf('h=%12.4e     T0=%12.4e    T1=%12.4e\n',h(j),T0(j),T1(j));
end;

fh = FAIRfigure(fig);
ph = loglog(h,[T0;T1]); set(ph(2),'linestyle','--')
th = title(sprintf('%s: |f-f(h)|,|f+h*dvf -f(h)| vs. h',mfilename));

if nargout>0,
  varargout = {fh,ph,th};
end;
FAIRmessage('.');
%------------------------------------------------------------------------------
function [y,dy] = xSquare(x)
if nargin == 0, return; end;
y = x.^2; dy = reshape(2*x,1,[]);
%==============================================================================


function [ n_data, h, a, t ] = owen_values ( n_data )

%*****************************************************************************80
%
%% OWEN_VALUES returns some values of Owen's T function.
%
%  Discussion:
%
%    Owen's T function is useful for computation of the bivariate normal
%    distribution and the distribution of a skewed normal distribution.
%
%    Although it was originally formulated in terms of the bivariate
%    normal function, the function can be defined more directly as
%
%      T(H,A) = 1 / ( 2 * pi ) *
%        Integral ( 0 <= X <= A ) e^(H^2*(1+X^2)/2) / (1+X^2) dX
%
%    In Mathematica, the function can be evaluated by:
%
%      fx = 1/(2*Pi) * Integrate [ E^(-h^2*(1+x^2)/2)/(1+x^2), {x,0,a} ]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 December 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real H, a parameter.
%
%    Output, real A, the upper limit of the integral.
%
%    Output, real T, the value of the function.
%
  n_max = 22;

  a_vec = [ ...
    0.5000000000000000E+00, ...
    0.1000000000000000E+01, ...
    0.2000000000000000E+01, ...
    0.3000000000000000E+01, ...
    0.5000000000000000E+00, ...
    0.1000000000000000E+01, ...
    0.2000000000000000E+01, ...
    0.3000000000000000E+01, ...
    0.5000000000000000E+00, ...
    0.1000000000000000E+01, ...
    0.2000000000000000E+01, ...
    0.3000000000000000E+01, ...
    0.5000000000000000E+00, ...
    0.1000000000000000E+01, ...
    0.2000000000000000E+01, ...
    0.3000000000000000E+01, ...
    0.5000000000000000E+00, ...
    0.1000000000000000E+01, ...
    0.2000000000000000E+01, ...
    0.3000000000000000E+01, ...
    0.1000000000000000E+02, ...
    0.1000000000000000E+03 ];

  h_vec = [ ...
    0.1000000000000000E+01, ...
    0.1000000000000000E+01, ...
    0.1000000000000000E+01, ...
    0.1000000000000000E+01, ...
    0.5000000000000000E+00, ...
    0.5000000000000000E+00, ...
    0.5000000000000000E+00, ...
    0.5000000000000000E+00, ...
    0.2500000000000000E+00, ...
    0.2500000000000000E+00, ...
    0.2500000000000000E+00, ...
    0.2500000000000000E+00, ...
    0.1250000000000000E+00, ...
    0.1250000000000000E+00, ...
    0.1250000000000000E+00, ...
    0.1250000000000000E+00, ...
    0.7812500000000000E-02, ...
    0.7812500000000000E-02, ...
    0.7812500000000000E-02, ...
    0.7812500000000000E-02, ...
    0.7812500000000000E-02, ...
    0.7812500000000000E-02 ];

  t_vec = [ ...
    0.4306469112078537E-01, ...
    0.6674188216570097E-01, ...
    0.7846818699308410E-01, ...
    0.7929950474887259E-01, ...
    0.6448860284750376E-01, ...
    0.1066710629614485E+00, ...
    0.1415806036539784E+00, ...
    0.1510840430760184E+00, ...
    0.7134663382271778E-01, ...
    0.1201285306350883E+00, ...
    0.1666128410939293E+00, ...
    0.1847501847929859E+00, ...
    0.7317273327500385E-01, ...
    0.1237630544953746E+00, ...
    0.1737438887583106E+00, ...
    0.1951190307092811E+00, ...
    0.7378938035365546E-01, ...
    0.1249951430754052E+00, ...
    0.1761984774738108E+00, ...
    0.1987772386442824E+00, ...
    0.2340886964802671E+00, ...
    0.2479460829231492E+00 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    h = 0.0;
    a = 0.0;
    t = 0.0;
  else
    h = h_vec(n_data);
    a = a_vec(n_data);
    t = t_vec(n_data);
  end

  return
end


 function X = nufft2_bilinear(omega, x, K1, K2, n_shift)
%function X = nufft2_bilinear(omega, x, K1, K2, n_shift)
%	in:
%		omega	[M 2]		frequencies in radians
%		x	[N1 N2]		image dimensions
%		J			# of neighbors used
%		K1,K2			FFT sizes (should be > N1,N2)
%		n_shift [2]		n = 0-n_shift to N-1-n_shift
%	out:
%		X	[M 1]	DTFT2 at omega, approximated by bilinear interp
%
%	Like fft(), this expects the signals to be x(0,0), ...
%	Use n_shift = [N1/2 N2/2] for x(-N1/2,-N2/2), ...
%
% Copyright 2001-9-20, Jeff Fessler, University of Michigan

warn('This function is obsolete.  Use nufft_init with the "linear" option')

% if no arguments, then run a simple test
if nargin < 1
	N1 = 4;
	N2 = 8;
	n_shift = [2.7 3.3];
	x = [[1:N1]'*ones(1,3), ones(N1,N2-3)]; % test signal
%	x = zeros(N1,N2); x(1,1) = 1;
	if 0	% test with uniform frequency locations
		o1 = 2 * pi * [0:(N1-1)]' / N1;
		o2 = 2 * pi * [0:(N2-1)]' / N2;
		[o1, o2] = ndgrid(o1, o2);
		omega = [o1(:) o2(:)];
	else	% nonuniform frequencies
		o1 = [0 7.2 2.6 3.3]';
		o2 = [0 4.2 -1 5.5]';
		omega = [o1(:) o2(:)];
	end
	Xd = dtft2(x, omega, n_shift);
	Xb = nufft2_bilinear(omega, x, 2*N1, 2*N2, n_shift);
%	Xb = reshape(Xb, N1, N2)
%	disp([Xd Xb Xb-Xd])
	help(mfilename)
	disp(sprintf('max %% difference = %g', max_percent_diff(Xd,Xb)))
	return
end

if ~isvar('n_shift') || isempty(n_shift), n_shift = [0 0]; end
if ~isvar('useloop') || isempty(useloop), useloop = 0; end

M = size(omega,1);
[N1 N2] = size(x);

koff1 = bilinear_offset(omega(:,1), K1);	% [M 1] nearest
koff2 = bilinear_offset(omega(:,2), K2);	% [M 1] nearest

% indices into oversampled FFT components
k1 = mod(outer_sum(koff1, [0 1]), K1) + 1;	% [M 2] {1,...,K1}
k2 = mod(outer_sum(koff2, [0 1]), K2) + 1;	% [M 2] {1,...,K2}

% 1D interpolation coefficient vectors
[u1l u1r] = interp_coef(omega(:,1), koff1, K1, N1);	% [M 1]
[u2l u2r] = interp_coef(omega(:,2), koff2, K2, N2);	% [M 1]

% precorrect for bilinear interpolation filtering
if 0
	warning 'applying precorrection'
	[i1, i2] = ndgrid([0:(N1-1)]-n_shift(1), [0:(N2-1)]-n_shift(2));
	tmp = (nufft_sinc(i1 / K1) .* nufft_sinc(i2 / K2)).^2;
	printm('sinc correction min=%g', min(tmp(:)))
	x = x ./ tmp;
end

% FFT and bilinear interpolation
% can't use interp2 here because we want periodic end conditions!
% oh, maybe i could with a little padding of ends...
Xk = fft2(x, K1, K2);
kk11 = k1(:,1) + (k2(:,1) - 1) * K1;
kk21 = k1(:,2) + (k2(:,1) - 1) * K1;
kk12 = k1(:,1) + (k2(:,2) - 1) * K1;
kk22 = k1(:,2) + (k2(:,2) - 1) * K1;
t1 = u1l .* Xk(kk11) + u1r .* Xk(kk21);
t2 = u1l .* Xk(kk12) + u1r .* Xk(kk22);
X = u2l .* t1 + u2r .* t2;

% apply phase shift
phase = exp(i * (omega * n_shift(:)));	% [M 1]
X = X .* phase;

% index from 0 to K-1 (will modulo later)
function koff = bilinear_offset(om, K)
gam = 2*pi/K;
koff = floor(om / gam);

% make 1D interpolation coefficient vectors
function [ul, ur] = interp_coef(om, koff, K, N)
gam = 2*pi/K;
ul = 1 - (om/gam - koff);
ur = 1 - ul;
ul = ul .* kphase(om - koff * gam, N);
ur = ur .* kphase(om - (koff+1) * gam, N);

% natural phase function. trick: force it to be 2pi periodic
function ph = kphase(om, N)
ph = exp(-i * mod0(om,2*pi) * (N-1)/2);


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
%
% function B = getElasticMatrixStg(omega,m,mu,lambda)
%
% Builds the elasticity matrix B for a domain defined by omega
% and a staggered grid discretization defined by m:
%
%     | a\nabla       0        0 |
% B = |       0 a\nabla        0 |
%     |       0       0  a\nabla |
%     |           b \div         |,
%
% where \nabla and \div are weighted by a=\sqrt(mu) and b=\sqrt(\lambda+\mu),
% respectively; defaults: \mu = 1, \lambda = 0.
% Note: the matrices are simple, it is size that matters.
%
% =============================================================================

function B = getElasticMatrixStg(omega,m,mu,lambda)
if nargin == 0,
  help(mfilename);
  runMinimalExample;
  B = 'endOfMinimalExample';
  return;
end;

                                        % set defautls
if ~exist('mu','var'),     mu     = [];  end;
if ~exist('lambda','var'), lambda = [];  end;
if isempty(mu),
  warning('mu has not yet been set! set mu=1');
  mu = 1;
end;
if isempty(lambda),
  warning('lambda has not yet been set! lambda=0');
  lambda = 0;
end;

dim = length(omega)/2;
h   = (omega(2:2:end)-omega(1:2:end))./m; % voxel size for integration
a   = sqrt(mu);
b   = sqrt(mu+lambda);

% for the dimension ms(i) of the i-th staggered grid, the i-th component
% of of m=[m(1),m(2),m(3)] has to be increased by one; the i-th staggered
% grid contains ns(i) points

ms  = @(i)   m + ((1:dim)==i);
ns  = @(i)   prod(ms(i));
% Example: m=(2,3), ms(1) = (3,3); ms(2)=(2,4), ns(1) = 9, ns(2) = 8.

% the dimension of \partial^{h,i}_k is p(i,k)-by-ns(i),
% where q(i)=prod(ms(i)) is the length of the i-th staggered grid
% and p(i,k) is the product of a modified m:
%   m(i) has to be increase by one and
%   m(k) has to be decrase  by one
p = @(i,k) prod(m + ((1:dim)==i) - ((1:dim==k)));

% Example:
% p(1,1) = 2*3         = 6,   p(1,2) = (2+1)*(3-1) = 6, ns(1) = 9
% p(2,1) = (2-1)*(3+1) = 4,   p(2,2) = 2*3         = 6, ns(2) = 8

% E is a q-by-q identity matrix, where q=m(k)+1 for i==k and q=m(k) (i~=k)
E = @(i,k) speye(m(k)+(i==k));

% Example:
% E(1,1) = speye(m(1)+1) = speye(3)
% E(1,2) = speye(m(2))   = speye(3)
% E(2,1) = speye(m(1))   = speye(2)
% E(2,2) = speye(m(2)+1) = speye(4)

% D is the 1D derivative matrix of size p-by-(p+1)
%        |-1 1     |
% D = |  .  .   |/h(k), where p=m(k) (i==k) and p=m(k)-1 (i~=k)
%        |     -1 1|

D = @(i,k) spdiags(ones(m(k)-(i~=k),1)*[-1,1],...
  [0,1],m(k)-(i~=k),m(k)+(i==k))/h(k);

% Example:
% D(1,1) is m(1)-by-(m(1)+1) = 2-by-3
% D(1,2) is (m(2)-1)-by-m(2) = 2-by-3
% D(2,1) is (m(1)-1)-by-m(1) = 1-by-2
% D(2,2) is m(2)-by-(m(2)+1) = 3-by-4

switch dim
  case 2
    % build the 2D elasticity operator
    %
    %      | a\nabla 0      |   |a D11  0     |
    %  B = |                | = |a D12  0     |
    %      | 0      a\nabla |   |0      a D21 |
    %      |                |   |0      a D22 |
    %      | b Div1 b Div2  |   |b D11  b D22 |

    D11 = kron(E(1,2),D(1,1));  D12 = kron(D(1,2),E(1,1));
    D21 = kron(E(2,2),D(2,1));  D22 = kron(D(2,2),E(2,1));

    % Example:
    % D11 = 3-by-3 \otimes 2-by-3 = 6-by-9
    % D12 = 2-by-3 \otimes 3-by-3 = 6-by-9
    % D21 = 4-by-4 \otimes 1-by-2 = 4-by-8
    % D22 = 3-by-4 \otimes 2-by-2 = 6-by-8

    B = [  a*D11,sparse(p(1,1),ns(2));
           a*D12,sparse(p(1,2),ns(2));
           sparse(p(2,1),ns(1)),a*D21;
           sparse(p(2,2),ns(1)),a*D22;
           b*D11,b*D22];
    % Example: B = 28-by-17
  case 3
    % build the 3D elasticity operator
    %
    %      |a D11  0      0    |
    %  B = |a D12  0      0    |
    %      |a D13  0      0    |
    %      |0      a D21  0    |
    %      |0      a D22  0    |
    %      |0      a D23  0    |
    %      |0      0      a D31|
    %      |0      0      a D32|
    %      |0      0      a D33|
    %      |b D11  b D22  b D33|

    % Example: m=[2,3,4] ns = (36,32,30)
    D11 = kron(kron(E(1,3),E(1,2)),D(1,1));
    D12 = kron(kron(E(1,3),D(1,2)),E(1,1));
    D13 = kron(kron(D(1,3),E(1,2)),E(1,1));
    D21 = kron(kron(E(2,3),E(2,2)),D(2,1));
    D22 = kron(kron(E(2,3),D(2,2)),E(2,1));
    D23 = kron(kron(D(2,3),E(2,2)),E(2,1));
    D31 = kron(kron(E(3,3),E(3,2)),D(3,1));
    D32 = kron(kron(E(3,3),D(3,2)),E(3,1));
    D33 = kron(kron(D(3,3),E(3,2)),E(3,1));

    % Example:
    % D11 = kron(kron(4-by-4,3-by-3),2-by-3) = 24-by-36
    % D12 = kron(kron(4-by-4,2-by-3),3-by-3) = 24-by-36
    % D13 = kron(kron(3-by-4,3-by-3),3-by-3) = 27-by-36
    % D21 = kron(kron(4-by-4,4-by-4),1-by-2) = 16-by-32
    % D22 = kron(kron(4-by-4,3-by-4),2-by-2) = 24-by-32
    % D23 = kron(kron(3-by-4,4-by-4),2-by-2) = 24-by-32
    % D31 = kron(kron(5-by-5,3-by-3),1-by-2) = 15-by-30
    % D32 = kron(kron(5-by-5,2-by-3),2-by-2) = 20-by-30
    % D32 = kron(kron(4-by-5,3-by-3),2-by-2) = 24-by-30

    B = [
      a * D11, sparse(p(1,1),ns(2)), sparse(p(1,1),ns(3)); ...
      a * D12, sparse(p(1,2),ns(2)), sparse(p(1,2),ns(3)); ...
      a * D13, sparse(p(1,3),ns(2)), sparse(p(1,3),ns(3)); ...
      sparse(p(2,1),ns(1)), a * D21, sparse(p(2,1),ns(3)); ...
      sparse(p(2,2),ns(1)), a * D22, sparse(p(2,2),ns(3)); ...
      sparse(p(2,3),ns(1)), a * D23, sparse(p(2,3),ns(3)); ...
      sparse(p(3,1),ns(1)), sparse(p(3,1),ns(2)), a * D31; ...
      sparse(p(3,2),ns(1)), sparse(p(3,2),ns(2)), a * D32; ...
      sparse(p(3,3),ns(1)), sparse(p(3,3),ns(2)), a * D33; ...
      b * D11,              b * D22,              b * D33
      ];
    % Example:
    % B = [75-by-36    0      0
    %         0     64-by-32  0
    %         0        0     59-by-30
    %      24-by-36 24-by-32 24-by-30 ]
    %   = 228-by-98

  otherwise,  error('nyi');
end;
%------------------------------------------------------------------------------
function runMinimalExample

omega = [0,1,0,2,0,3]; m = [4,5,6];
B2 = getElasticMatrixStg(omega(1:4),m(1:2),1,0);
B3 = getElasticMatrixStg(omega,m,1,0);
figure(1); clf;
subplot(1,2,1); spy(B2); title(sprintf('%s: B2',mfilename));
subplot(1,2,2); spy(B3); title(sprintf('%s: B3',mfilename));
%==============================================================================


function varargout = tucker(f)
%TUCKER   SLICE-TUCKER expansion of a CHEBFUN3 object.
%   [CORE, COLS, ROWS, TUBES] = TUCKER(F) returns the core tensor CORE and
%   the three factor quasimatrices COLS, ROWS, and TUBES in the low-rank
%   representation of a CHEBFUN3 object F. The factor quasimatrices are of
%   size Inf-by-length(F, 1), Inf-by-length(F, 2) and Inf-by-length(F, 3),
%   respectively and we have
%
%   F(x,y,z) = CORE x_1 COLS(x,:,:) x_2 ROWS(:,y,:) x_3 TUBES(:,:,z).
%
%   CORE = TUCKER(F) returns the core tensor used in the construction of F.
%
% See also CHEBFUN2/CDR.

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

if ( isempty(f) )
    varargout = cell(1, nargout);
    return
end

% Get the low rank representation for f.
fCore = f.core;
fCols = f.cols;
fRows = f.rows;
fTubes = f.tubes;

% Output:
if ( nargout <= 1 )
    varargout = {fCore};
else
    % ST decomposition
    varargout = {fCore, fCols, fRows, fTubes};
end

end


function c=projkern(c,p2,p3,p4,p5);
%PROJKERN  Projection onto generating kernel space
%   Usage:  cout=projkern(cin,a);
%           cout=projkern(cin,g,a);
%           cout=projkern(cin,ga,gs,a);
%
%   Input parameters:
%         cin   : Input coefficients
%         g     : analysis/synthesis window
%         ga    : analysis window
%         gs    : synthesis window
%         a     : Length of time shift.
%   Output parameters:
%         cout  : Output coefficients
%
%   `cout=projkern(cin,a)` projects a set of Gabor coefficients *c* onto the
%   space of possible Gabor coefficients. This means that *cin* and *cout*
%   synthesize to the same signal. A tight window generated from a Gaussian
%   will be used for both analysis and synthesis.
%
%   The rationale for this function is a follows: Because the coefficient
%   space of a Gabor frame is larger than the signal space (since the frame
%   is redundant) then there are many coefficients that correspond to the
%   same signal.
%
%   Therefore, you might desire to work with the coefficients *cin*, but you
%   are in reality working with *cout*.
%
%   `cout=projkern(cin,g,a)` does the same, using the window *g* for analysis
%   and synthesis.
%
%   `cout=projkern(cin,ga,gs,a)` does the same, but for different analysis
%   *ga* and synthesis *gs* windows.
%
%   See also: dgt, idgt

complainif_argnonotinrange(nargin,2,4,mfilename);

M=size(c,1);
N=size(c,2);

if nargin==2
  a=p2;
  L=a*N;
  ga=gabtight(a,M,L);
  gs=ga;
end;

if nargin==3;
  ga=p2;
  gs=p2;
  a=p3;
  L=a*N;
end;

if nargin==4;
  ga=p2;
  gs=p3;
  a=p4;
  L=a*N;
end;

assert_squarelat(a,M,1,'PROJKERN');

c=dgt(idgt(c,gs,a),ga,a,M);


function merged_sample = merge_samples(sample1, sample2, w1, w2, sample_merge_type)
% Merge sample1 and sample2 using weights w1 and w2. The type of merging is
% decided by sample_merge_type.
% The sample_merge_type can be
% 1) Merge: The output is the weighted sum of the input samples
% 2) Replace: The output is the first sample. ie w2 is assumed to be 0

% Normalise the weights so that they sum to one
alpha1 = w1/(w1+w2);
alpha2 = 1 - alpha1;

% Build the merged sample
if strcmpi(sample_merge_type, 'replace')
    merged_sample = sample1;
elseif strcmpi(sample_merge_type, 'merge')
    num_feature_blocks = numel(sample1);

    merged_sample = cell(1, 1, num_feature_blocks);
    for k = 1:num_feature_blocks
        merged_sample{k} = alpha1*sample1{k} + alpha2*sample2{k};
    end
else
    error('Invalid sample merge type');
end


function x = rot2quat (R, tol)
% Use:  q = rot2quat(R)
%
% Computes the unit quaternion corresponding to the 3x3 rotation matrix R
% tol controls the tolerance to check is R == identity(3)
% Note: quaternion is q = [qw qx qy qz], where qw is the scalar part of q
%
% From Luca Carlone's
% https://bitbucket.org/lucacarlone/pgo3d-duality-opencode/annotate/ebb6e1b8cebaad7f2aaf581b1d0c0bad737faebb/lib/rot2quat.m?at=master&fileviewer=file-view-default

if nargin < 2
  tol =  1.0e-5;
end

if isrot(R, tol) == 0
  s(4) = .5 * sqrt( 1 + R(1,1) + R(2,2) + R(3,3) );
  if norm(s(4)) <= tol
    %disp ('rotation = 180 degrees')
    [u,teta]=rot2uth(R);
    s(1)=u(1);
    s(2)=u(2);
    s(3)=u(3);
  elseif norm(s(4)-1) <= tol
    % disp ('rotation = 0 degrees')
    s(1) = 0;
    s(2) = 0;
    s(3) = 0;
  else
    s(1) = (R(3,2) - R(2,3))/(4 * s(4));
    s(2) = (R(1,3) - R(3,1))/(4 * s(4));
    s(3) = (R(2,1) - R(1,2))/(4 * s(4));
  end
  x(1) = s(4);
  x(2) = s(1);
  x(3) = s(2);
  x(4) = s(3);
else
  disp ('Error in input matrix')
  x=[1 0 0 0]';
end


function warped_img = mdlt_warping(height, width, img, Hmdlt, offset, X, Y)

warped_img = zeros(height,width,3);
[imgh,imgw,~] = size(img);

for xidx= 1:width
    for yidx = 1:height

        x_grididx = min(find(X >= xidx,1));
        y_grididx = min(find(Y >= yidx,1));

        grididx = (x_grididx-1) * size(Y,2) + y_grididx;
        h = reshape(Hmdlt(grididx,:),3,3);

        x = xidx - offset(1) + 1;
        y = yidx - offset(2) + 1;

        % [_x;_y;1] = H * [x;y;1]
        posz =  h(3,1) * x + h(3,2) * y + h(3,3) ;
        posx = (h(1,1) * x + h(1,2) * y + h(1,3)) ./ posz;
        posy = (h(2,1) * x + h(2,2) * y + h(2,3)) ./ posz;

        if (posx>=1)&&(posx<=imgw)&&(posy>=1)&&(posy<=imgh)
%             warped_img(yidx,xidx,:) = img(posy,posx,:);

            pix = [posy posx];
            float_Y=pix(1)-floor(pix(1));
            float_X=pix(2)-floor(pix(2));

            pix_up_left=[floor(pix(1)) floor(pix(2))];
            pix_up_right=[floor(pix(1)) ceil(pix(2))];
            pix_down_left=[ceil(pix(1)) floor(pix(2))];
            pix_down_right=[ceil(pix(1)) ceil(pix(2))];

            value_up_left   = (1-float_X) * (1-float_Y);
            value_up_right  =     float_X * (1-float_Y);
            value_down_left = (1-float_X) * float_Y;
            value_down_right=     float_X * float_Y;

            warped_img(yidx,xidx,:) = ...
               value_up_left*img(   pix_up_left(1),   pix_up_left(2),:)+ ...
              value_up_right*img(  pix_up_right(1),  pix_up_right(2),:)+ ...
             value_down_left*img( pix_down_left(1), pix_down_left(2),:)+ ...
            value_down_right*img(pix_down_right(1),pix_down_right(2),:);

        else
            warped_img(yidx,xidx,:) = [0,0,0];
        end
    end
end

end


function [out] = recharge_1(p1,S,Smax,flux)
%recharge_1

% Copyright (C) 2019, 2021 Wouter J.M. Knoben, Luca Trotter
% This file is part of the Modular Assessment of Rainfall-Runoff Models
% Toolbox (MARRMoT).
% MARRMoT is a free software (GNU GPL v3) and distributed WITHOUT ANY
% WARRANTY. See <https://www.gnu.org/licenses/> for details.

% Flux function
% ------------------
% Description:  Recharge as scaled fraction of incoming flux
% Constraints:  -
% @(Inputs):    p1   - fraction of flux that is recharge [-]
%               S    - current storage [mm]
%               Smax - maximum contributing storage [mm]
%               flux - incoming flux [mm/d]

out = p1*S/Smax*flux;

end