function yval = basis_matrix_tmp ( left, n, mbasis, ndata, tdata, ydata, tval )

%*****************************************************************************80
%
%% BASIS_MATRIX_TMP computes Q = T * MBASIS * P
%
%  Discussion:
%
%    YDATA is a vector of data values, most frequently the values of some
%    function sampled at uniformly spaced points.  MBASIS is the basis
%    matrix for a particular kind of spline.  T is a vector of the
%    powers of the normalized difference between TVAL and the left
%    endpoint of the interval.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 February 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer LEFT, indicats that TVAL is in the interval
%    [ TDATA(LEFT), TDATA(LEFT+1) ], or that this is the "nearest"
%    interval to TVAL.
%    For TVAL < TDATA(1), use LEFT = 1.
%    For TDATA(NDATA) < TVAL, use LEFT = NDATA - 1.
%
%    Input, integer N, the order of the basis matrix.
%
%    Input, real MBASIS(N,N), the basis matrix.
%
%    Input, integer NDATA, the dimension of the vectors TDATA and YDATA.
%
%    Input, real TDATA(NDATA), the abscissa values.  This routine
%    assumes that the TDATA values are uniformly spaced, with an
%    increment of 1.0.
%
%    Input, real YDATA(NDATA), the data values to be interpolated or
%    approximated.
%
%    Input, real TVAL, the value of T at which the spline is to be
%    evaluated.
%
%    Output, real YVAL, the value of the spline at TVAL.
%
  if ( left == 1 )
    arg = 0.5E+00 * ( tval - tdata(left) );
    first = left;
  elseif ( left < ndata - 1 )
    arg = tval - tdata(left);
    first = left - 1;
  elseif ( left == ndata - 1 )
    arg = 0.5E+00 * ( 1.0E+00 + tval - tdata(left) );
    first = left - 1;
  end
%
%  TVEC(I) = ARG**(N-I).
%
  tvec(n,1) = 1.0E+00;
  for i = n-1 : -1 : 1
    tvec(i,1) = arg * tvec(i+1,1);
  end

  yval = 0.0E+00;
  for j = 1 : n
    yval = yval + ( tvec(1:n,1)' * mbasis(1:n,j) ) * ydata(first - 1 + j);
  end

  return
end


function [r,c,V] = findnearest(srchvalue,srcharray,bias)

% Usage:
% Find the nearest numerical value in an array to a search value
% All occurances are returned as array subscripts
%
% Output:
%
% For 2D matrix subscripts (r,c) use:
%
%       [r,c] = findnearest(srchvalue,srcharray,gt_or_lt)
%
%
% To also output the found value (V) use:
%
%       [r,c,V] = findnearest(srchvalue,srcharray,gt_or_lt)
%
%
% For single subscript (i) use:
%
%         i   = findnearest(srchvalue,srcharray,gt_or_lt)
%
%
% Inputs:
%
%    srchvalue = a numerical search value
%    srcharray = the array to be searched
%    bias      = 0 (default) for no bias
%                -1 to bias the output to lower values
%                 1 to bias the search to higher values
%                (in the latter cases if no values are found
%                 an empty array is ouput)
%
%
% By Tom Benson (2002)
% University College London
% t.benson@ucl.ac.uk

if nargin<2
    error('Need two inputs: Search value and search array')
elseif nargin<3
    bias = 0;
end

% find the differences
srcharray = srcharray-srchvalue;

if bias == -1   % only choose values <= to the search value

    srcharray(srcharray>0) =inf;

elseif bias == 1  % only choose values >= to the search value

    srcharray(srcharray<0) =inf;

end

% give the correct output
if nargout==1 | nargout==0

    if all(isinf(srcharray(:)))
        r = [];
    else
        r = find(abs(srcharray)==min(abs(srcharray(:))));
    end

elseif nargout>1
    if all(isinf(srcharray(:)))
        r = [];c=[];
    else
        [r,c] = find(abs(srcharray)==min(abs(srcharray(:))));
    end

    if nargout==3
        V = srcharray(r,c)+srchvalue;
    end
end


function V = construct_V_matrix(measurements)
%function V = construct_V_matrix(measurements)
%
% This function constructs and returns the translational data matrix V
% defined in equation (16) of the paper

% Copyright (C) 2016 by David M. Rosen

D = length(measurements.t{1}); % D = dimension of SE(d)
N = max(max(measurements.edges));  % N = number of nodes in the pose graph
M = size(measurements.edges,1); % M = number of edges in the pose graph

% The number of nonzero elements in V; there are D nonzero elements for
% each translational measurement, plus D*N slots to store the sum of the
% observations emanating from each node

NNZ = D*M + D*N;

rows = zeros(1, NNZ);
cols = zeros(1, NNZ);
vals = zeros(1, NNZ);

for e = 1:M

    %Extract measurement data
    i = measurements.edges(e, 1);  %The node that this edge *leaves*
    j = measurements.edges(e, 2);  %The node that this edge *enters*

    tij = measurements.t{e};
    tau_ij = measurements.tau{e};

    %Set V_ji = -tau_ij * t_ij'

    cmin = D*(e-1) + 1;
    cmax = D*(e-1) + D;

    rows(cmin:cmax) = j*ones(1, D);
    cols(cmin:cmax) = D*(i-1) + 1 : D*(i-1) + D;
    vals(cmin:cmax) = -tau_ij * tij';

    %Add this observation to the weighted sum of measurements emanating
    %from node i

    cmin = D*M + D*(i - 1) + 1;
    cmax = D*M + D*(i - 1) + D;
    vals(cmin : cmax) = vals(cmin : cmax) + tau_ij*tij';
end

% Fill in the indices for the running sums

for i = 1:N
    cmin = D*M + D*(i-1) + 1;
    cmax = D*M + D*(i-1) + D;

    rows(cmin:cmax) = i*ones(1,D);
    cols(cmin:cmax) = [D*(i-1) + 1 : D*(i - 1) + D];
end

V = sparse(rows, cols, vals, N, D*N);


function [ n_data, a, x, fx ] = poisson_cdf_values ( n_data )

%*****************************************************************************80
%
%% POISSON_CDF_VALUES returns some values of the Poisson CDF.
%
%  Discussion:
%
%    CDF(X)(A) is the probability of at most X successes in unit time,
%    given that the expected mean number of successes is A.
%
%    In Mathematica, the function can be evaluated by:
%
%      Needs["Statistics`DiscreteDistributions`]
%      dist = PoissonDistribution [ a ]
%      CDF [ dist, x ]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%    Daniel Zwillinger,
%    CRC Standard Mathematical Tables and Formulae,
%    30th Edition, CRC Press, 1996, pages 653-658.
%
%  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 A, the parameter of the function.
%
%    Output, integer X, the argument of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 21;

  a_vec = [ ...
     0.02E+00, ...
     0.10E+00, ...
     0.10E+00, ...
     0.50E+00, ...
     0.50E+00, ...
     0.50E+00, ...
     1.00E+00, ...
     1.00E+00, ...
     1.00E+00, ...
     1.00E+00, ...
     2.00E+00, ...
     2.00E+00, ...
     2.00E+00, ...
     2.00E+00, ...
     5.00E+00, ...
     5.00E+00, ...
     5.00E+00, ...
     5.00E+00, ...
     5.00E+00, ...
     5.00E+00, ...
     5.00E+00 ];

  fx_vec = [ ...
     0.9801986733067553E+00, ...
     0.9048374180359596E+00, ...
     0.9953211598395555E+00, ...
     0.6065306597126334E+00, ...
     0.9097959895689501E+00, ...
     0.9856123220330293E+00, ...
     0.3678794411714423E+00, ...
     0.7357588823428846E+00, ...
     0.9196986029286058E+00, ...
     0.9810118431238462E+00, ...
     0.1353352832366127E+00, ...
     0.4060058497098381E+00, ...
     0.6766764161830635E+00, ...
     0.8571234604985470E+00, ...
     0.6737946999085467E-02, ...
     0.4042768199451280E-01, ...
     0.1246520194830811E+00, ...
     0.2650259152973617E+00, ...
     0.4404932850652124E+00, ...
     0.6159606548330631E+00, ...
     0.7621834629729387E+00 ];

  x_vec = [ ...
     0, 0, 1, 0, ...
     1, 2, 0, 1, ...
     2, 3, 0, 1, ...
     2, 3, 0, 1, ...
     2, 3, 4, 5, ...
     6 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    a = 0.0;
    x = 0;
    fx = 0.0;
  else
    a = a_vec(n_data);
    x = x_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


function uI = faceinterpolate(u,node,elem,elemType)
%% FACEINTERPOLATE interpolate to face elements (RT0 or BDM1).
%
% uI = faceinterpolate(u,node,elem,elemType) interpolates a given function u
% into the lowesr order RT0 or BDM1 finite element spaces. The coefficient
% is given by the line integral int_e u*n ds. The input elemType can be 'RT0'
% or 'BDM1'.
%
% Example
%
%    maxIt = 5;
%    node = [1,0; 1,1; 0,1; -1,1; -1,0; -1,-1; 0,-1; 0,0]; % nodes
%    elem = [1,2,8; 3,8,2; 8,3,5; 4,5,3; 7,8,6; 5,6,8];    % elements
%    bdEdge = setboundary(node,elem,'Dirichlet');
%    pde = mixBCdata;
%    err = zeros(maxIt,2); N = zeros(maxIt,1);
%    for i =1:maxIt
%      [node,elem,bdEdge] = uniformrefine(node,elem,bdEdge);
%      [u,sigma,M] = PoissonRT0(node,elem,pde,bdEdge);
%      sigmaI = faceinterpolate(pde.Du,node,elem,'RT0');
%      err(i,1) = getL2errorRT0(node,elem,pde.Du,sigmaI,pde.d);
%      err(i,2)=sqrt((sigma-sigmaI)'*M*(sigma-sigmaI));
%      N(i) = size(u,1);
%    end
%     figure;
%     showrate2(N,err(:,1),2,'r-+','||Du - \sigma_I||',...
%               N,err(:,2),2,'b-+','||\sigma^{RT_0} - \sigma_I||');
%
% See also edgeinterpolate, edgeinterpolate1, edgeinterpolate2
%
% Created by Ming Wang at Mar 29, 2011, M-lint modified at May 14, 2011.
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

if ~exist('elemType','var'), elemType = 'RT0'; end
%% edge
if size(elem,2) == 2 % the input elem is edge
    edge = elem;
else
    [tempvar,edge] = dofedge(elem);
end
NE = size(edge,1);
edgeVec = node(edge(:,2),:) - node(edge(:,1),:);
nVec = zeros(NE,2);
nVec(:,1) = edgeVec(:,2);
nVec(:,2) = -edgeVec(:,1);

%% dof for RT0
[lambda,weight] = quadpts1(4);
nQuad = size(lambda,1);
uI = zeros(NE,1);
for i = 1:nQuad
    pxy = lambda(i,1)*node(edge(:,1),:)+lambda(i,2)*node(edge(:,2),:);
    flux = u(pxy);
    uI = uI + weight(i)*dot(flux,nVec,2);
end

%% dof for BDM1
if strcmp(elemType,'BDM1')
   uI(NE+1:2*NE) = zeros(NE,1);
   for i = 1:nQuad
        pxy = lambda(i,1)*node(edge(:,1),:)+lambda(i,2)*node(edge(:,2),:);
        flux = u(pxy);
        uI(NE+1:2*NE) = uI(NE+1:2*NE)+ ...
                     weight(i)*3*(lambda(i,1)-lambda(i,2))*dot(flux,nVec,2);
   end
end


function [ff,f]=v_lpccc2ff(cc,np,nc,c0)
%V_LPCCC2FF Convert complex cepstrum to complex spectrum FF=(CC,NP,NC)
%
%  Inputs: cc(nf,n)     Complex ceptral coefficients excluding c(0), one frame per row
%          np           Size of output spectrum is np+1 [n]
%                       Alternatively, a vector of output frequencies in the range 0 to 0.5
%          nc           Number of cepstral coefficients to use [np or, if np is a vector, n]
%                       Set nc=-1 to use n coefficients
%          c0(nf,1)     Cepstral coefficient c(0) [0]
%
% Outputs: ff(nf,np+2)  Complex spectrum from DC to Nyquist
%          f(1,np+2)    Normalized frequencies (0 to 0.5)
%
% The "complex cepstral coefficients", cc(n), are the inverse discrete-time Fourier transform
% of the log of the complex-valued spectrum. The cc(n) are real-valued and, for n<0, cc(n)=0.
% The "real cepstral coeffcients", rc(n), are the inverse discrete-time Fourier transform
% of the log of the magnitude spectrum; rc(0)=cc(0) and rc(n)=0.5*cc(n) for n~=0.
% For highest speed, choose np+1 to be a power of 2.

%      Copyright (C) Mike Brookes 2014
%      Version: $Id: v_lpccc2ff.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[nf,mc]=size(cc);
if nargin<2 || ~numel(np)
    if nargout
        np=mc;
    else
        np=128;
    end
end
if nargin>=3 && numel(nc)==1 && nc==-1 nc=mc; end
if nargin<4 || ~numel(c0) c0=zeros(nf,1); end
if numel(np)>1 || np(1)<1
    if nargin<3 || ~numel(nc) nc=mc; end
    f=np(:)';
    if nc==mc
        ff=exp([c0 cc]*exp(-2i*pi*(0:mc)'*f));
    else
        ff=exp([c0 lpccc2cc(cc,nc)]*exp(-2i*pi*(0:nc)'*f));
    end
else
    if nargin<3 || ~numel(nc) nc=np; end
    if nc==mc
        ff=exp(v_rfft([c0 cc].',2*np).');
    else
        ff=exp(v_rfft([c0 v_lpccc2cc(cc,nc)].',2*np).');
    end
    f=linspace(0,0.5,np+1);
end
if ~nargout
    subplot(2,1,2);
    plot(f,unwrap(angle(ff.')));
    xlabel('Normalized frequency f/f_s');
    ylabel('Phase (rad)');
    subplot(2,1,1);
    plot(f,db(abs(ff.')));
    xlabel('Normalized frequency f/f_s');
    ylabel('Gain (dB)');
end


%% Test Problems for CLP
clc
clear

%% CLP Options Function
clc
clpset

a = clpset

%% LP1 [-31.4]
clc
f = -[6 5]';
A = sparse([1,4; 6,4; 2, -5]);
rl = -Inf(3,1);
ru = [16;28;6];
lb = [0;0];
ub = [10;10];

opts = [];
opts.display = 1;
opts.maxiter = 15;
opts.algorithm = 'automatic';
opts.numThreads = 4;
[x,ff,e,i,lambda] = clp([],f,A,rl,ru,lb,ub,opts);

%% LP1 with redundant row [-31.4]
clc
f = -[6 5]';
A = sparse([1,4; 2 8; 6,4; 2, -5]);
rl = -Inf(4,1);
ru = [16;32;28;6];
lb = [0;0];
ub = [10;10];

opts = [];
opts.display = 1;
opts.maxiter = 15;
opts.algorithm = 3;
% opts.writeprob = 'prob.dat-s';
% opts.writesol = 'sol.dat-s';
[x,ff,e,i,lambda] = clp([],f,A,rl,ru,lb,ub,opts)

%% LP2 [2]
clc
f = [8,1]';
A = sparse([-1,-2;1,-4;3,-1;1,5;-1,1;-1,0;0,-1]);
rl = -Inf(7,1);
ru = [-4,2,21,39,3,0,0]';

[x,ff,e,i,lambda] = clp([],f,A,rl,ru)

%% LP3 [-3.75]
clc
f = -[-1, 2]';
A = sparse([2, 1;-4, 4]);
rl = -Inf(2,1);
ru = [5, 5]';

[x,ff,e,i,lambda] = clp([],f,A,rl,ru)

%% LP4 [-97.5]
clc
f = -[1 2 3]';
A = sparse([-1,1,1; 1,-3,1]);
b = [20,30]';
Aeq = sparse([1 1 1]);
A = [A;Aeq;-Aeq];
beq = 40;
ru = [b;beq;-beq];
rl = -Inf(size(ru));
lb = [0 0 0]';
ub =[40 inf inf]';

[x,ff,e,i,lambda] = clp([],f,A,rl,ru,lb,ub,opts)

%% LP5 [-30, no linear constraints]
clc
f = -[1, 2]';
lb = [0;0];
ub = [10;10];

[x,ff,e,i,lambda] = clp([],f,[],[],[],lb,ub,opts)

%% LP6 infeasible
clc
%Objective & Constraints
f = -[6 5]';
A = sparse([1,4; 6,4; 2, -5]);
rl = -Inf(3,1);
ru = [16;28;-30];
lb=[0;0];
ub=[10;10];

opts = [];
opts.display = 1;
opts.maxiter = 150;
opts.algorithm =5;

[x,ff,e,i,lambda] = clp([],f,A,rl,ru,lb,ub,opts)

%% LP7 unbounded
clc
f = -[1, 2]';
lb = [0;0];
ub = [10;Inf];

opts = [];
opts.display = 1;
opts.maxiter = 150;
opts.algorithm = 0;

[x,ff,e,i,lambda] = clp([],f,[],[],[],lb,ub,opts)

%% LARGE LP
clc
clear clp
prob = coinRead('maros-r7.mps');

opts = [];
opts.display = 1;
opts.maxiter = 10000;
opts.doPresolve = 1;
opts.algorithm = 5;
opts.numThreads = 4;

[~,ff,e,i,lambda] = clp(prob.H,prob.f,prob.A,prob.rl,prob.ru,prob.lb,prob.ub,opts)

%% AMPL Problem LP
clc
clear clp
prob = coinRead('prod.mps');

opts = [];
opts.display = 1;
opts.maxiter = 10000;
opts.algorithm = 0;
opts.doPresolve = 1;
opts.numThreads = 1;

[~,ff,e,i,lambda] = clp(prob.H,prob.f,prob.A,prob.rl,prob.ru,prob.lb,prob.ub,opts)

%% LP No Presolve Dual
clc
f = -[-1, 2]';
A = sparse([2, 1;-4, 4]);
rl = -Inf(2,1);
ru = [5, 5]';

opts = [];
opts.display = 1;
opts.maxiter = 15;
opts.algorithm = 0;
opts.doPresolve = 0;
opts.numThreads = 1;
[x,ff,e,i,lambda] = clp([],f,A,rl,ru,[],[],opts);

%% LP Empty after Presolve Abc
clc
f = -[-1, 2]';
A = sparse([2, 1;-4, 4]);
rl = -Inf(2,1);
ru = [5, 5]';

opts = [];
opts.display = 1;
opts.algorithm = 0;
opts.numThreads = 2;
[x,ff,e,i,lambda] = clp([],f,A,rl,ru,[],[],opts);

%% QP1 -2.83333333301227;
clc
H = speye(3);
f = -[2 3 1]';
A = sparse([1 1 1;3 -2 -3; 1 -3 2]);
b = [1;1;1];

opts = [];
opts.display = 2;
opts.algorithm = 5;
opts.numThreads = 1;

% prob=optiprob('qp',H,f,'ineq',A,b);
% coinWrite(prob,'testqp.mps')

[x,fval,e,i,l] = clp(H,f,A,-Inf(size(b)),b,[],[],opts)

%% QP2 -8.22222220552525
clc
H = (sparse([1 -1; -1 2]));
f = -[2 6]';
A = sparse([1 1; -1 2; 2 1]);
b = [2; 2; 3];
lb = [0;0];

opts = [];
opts.display = 1;
opts.algorithm = 3;
opts.objbias = 10;

[x,fval,e,i,l] = clp(tril(H),f,A,-Inf(size(b)),b,[],[],opts)

%% QP3 -6.41379310344827
H = tril(sparse([1 -1; -1 2]));
f = -[2 6]';
A = sparse([1 1; -1 2; 2 1]);
b = [2; 2; 3];
Aeq = sparse([1 1.5]);
beq = 2;
A = [A;Aeq]; rl = [zeros(size(b));beq]; ru = [b;beq];
lb = [0;0];
ub = [10;10];

opts = [];
opts.display = 2;
[x,fval,e,i,l] = clp(H,f,A,rl,ru,lb,ub,opts)


function pass = test_cumsum(pref)

if ( nargin == 0 )
    pref = chebfunpref;
end
tol = 100*pref.cheb3Prefs.chebfun3eps;

% Check cumsum on cube domain
[x, y, z] = cheb.xyz;

pass(1) = norm(cumsum(x) - .5*(x.^2-1)) < tol;
pass(2) = norm(cumsum(x, 1) - .5*(x.^2-1)) < tol;
pass(3) = norm(cumsum(x, 2) - x.*(y+1)) < tol;
pass(4) = norm(cumsum(x, 3) - x.*(z+1)) < tol;

pass(5) = norm(cumsum(y) - y.*(x+1)) < tol;
pass(6) = norm(cumsum(y, 1) - y.*(x+1)) < tol;
pass(7) = norm(cumsum(y, 2) - .5*(y.^2-1)) < tol;
pass(8) = norm(cumsum(y, 3) - y.*(z+1)) < tol;

pass(9) = norm(cumsum(z) - z.*(x+1)) < tol;
pass(10) = norm(cumsum(z, 1) - z.*(x+1)) < tol;
pass(11) = norm(cumsum(z, 2) - z.*(y+1)) < tol;
pass(12) = norm(cumsum(z, 3) - .5*(z.^2-1)) < tol;

% Check cumsum on rectangular box domain
x = chebfun3(@(x,y,z) x, [-1.1  2 -0.2 3 5 6]);
y = chebfun3(@(x,y,z) y, [-1.1  2 -0.2 3 5 6]);
z = chebfun3(@(x,y,z) z, [-1.1  2 -0.2 3 5 6]);

pass(13) = norm(cumsum(x) - .5*(x.^2-(-1.1)^2)) < tol;
pass(14) = norm(cumsum(x, 1) - .5*(x.^2-(-1.1)^2)) < tol;
pass(15) = norm(cumsum(x, 2) - x.*(y+0.2)) < tol;
pass(16) = norm(cumsum(x, 3) - x.*(z-5)) < tol;

pass(17) = norm(cumsum(y) - y.*(x+1.1)) < tol;
pass(18) = norm(cumsum(y, 1) - y.*(x+1.1)) < tol;
pass(19) = norm(cumsum(y, 2) - .5*(y.^2-(-0.2)^2)) < tol;
pass(20) = norm(cumsum(y, 3) - y.*(z-5)) < tol;

pass(21) = norm(cumsum(z) - z.*(x+1.1)) < tol;
pass(22) = norm(cumsum(z, 1) - z.*(x+1.1)) < tol;
pass(23) = norm(cumsum(z, 2) - z.*(y+0.2)) < tol;
pass(24) = norm(cumsum(z, 3) - .5*(z.^2-5^2)) < tol;

% Check that a triple cumsum is a cumsum3
f = sin((x-.1).*(y+.1).*(z+.1));
pass(25) = norm(cumsum(cumsum(cumsum(f), 2), 3) - cumsum3(f)) < tol;
pass(26) = norm(cumsum(cumsum(cumsum(f, 1), 2), 3) - cumsum3(f)) < tol;

% Look in one direction and make sure we get the right thing:
f = chebfun(@(x) exp(x));
f3 = chebfun3(@(x,y,z) exp(x));
g = cumsum(f);
g3 = cumsum(f3);
g3x = g3(:,0,.3);
pass(27) = norm(g3x-g) < tol;

end


function SD = SD_evaluation(F)
[m,n]=size(F);
u=mean(mean(F));
SD=sqrt(sum(sum((F-u).^2))/(m*n));
end


function [fL] = bruteloglike(vValues,time_as)
    % bruteloglike calculates the log likelihood function for the modeled aftershock sequence and the maximum likelihood estimate for k, c and p
    %
    % [fL] = bruteloglike(vValues,time_as);
    % -------------------------------------------------------------
    % Reference: Ogata, Estimation of the parameters in the modified Omori formula
    % for aftershock sequences by  the maximum likelihood procedure, J. Phys. Earth, 1983
    % (Formula 6)
    %
    % J. Woessner
    % updated: 29.07.03

    p = vValues(1);
    c = vValues(2);
    k = vValues(3);

    % Setting start end end time
    fTstart = min(time_as);
    fTend = max(time_as);

    if p ~= 1
        fAcp = ((fTend+c).^(1-p)-(fTstart+c).^(1-p))./(1-p);
        %cumnr_model = k/(p-1)*(c^(1-p)-(c+time_as(i)).^(1-p)); % integrated form of MOL
    else
        fAcp = log(fTend+c)-log(fTstart+c);
        %cumnr_model = k*log(time_as(i)/c+1); % integrated form of MOL
    end
    % rms = (sum((i-cumnr_model).^2)/length(i))^0.5; % RMS between observed data and MOL
    % Log likelihood
    nNumEvents = length(time_as);
    fL = -(nNumEvents*log(k)-p*sum(log(time_as+c))-k*fAcp);


function [ r, s, area ] = node_reference_t10 ( )

%*****************************************************************************80
%
%% NODE_REFERENCE_T10 returns the basis nodes for a 10 node triangle.
%
%  Reference Element T10:
%
%    |
%    1  10
%    |  |\
%    |  | \
%    |  8  9
%    |  |   \
%    S  |    \
%    |  5  6  7
%    |  |      \
%    |  |       \
%    0  1--2--3--4
%    |
%    +--0----R---1-->
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real R(10), S(10), the coordinates of the basis nodes.
%
%    Output, real AREA, the area of the element.
%
  r(1) = 0.0;
  s(1) = 0.0;

  r(2) = 1.0 / 3.0;
  s(2) = 0.0;

  r(3) = 2.0 / 3.0;
  s(3) = 0.0;

  r(4) = 1.0;
  s(4) = 0.0;

  r(5) = 0.0;
  s(5) = 1.0 / 3.0;

  r(6) = 1.0 / 3.0;
  s(6) = 1.0 / 3.0;

  r(7) = 2.0 / 3.0;
  s(7) = 1.0 / 3.0;

  r(8) = 0.0;
  s(8) = 2.0 / 3.0;

  r(9) = 1.0 / 3.0;
  s(9) = 2.0 / 3.0;

  r(10) = 0.0;
  s(10) = 1.0;

  area = 0.5;

  return
end


function cout=comp_col2diag(cin);
%COMP_COL2DIAG  transforms columns to diagonals (in a special way)
%
%  This function transforms the first column to the main diagonal. The
%  second column to the first side-diagonal below the main diagonal and so
%  on.
%
%  This way fits well the connection of matrix and spreading function, see
%  spreadfun.
%
%  This function is its own inverse.

%   AUTHOR : Peter L. Søndergaard.
%   TESTING: OK
%   REFERENCE: OK

L=size(cin,1);
cout=zeros(L,assert_classname(cin));

jj=(0:L-1).';
for ii=0:L-1
  cout(ii+1,:)=cin(ii+1,mod(ii-jj,L)+1);
end;


function prob_test079 ( )

%*****************************************************************************80
%
%% TEST079 tests GENLOGISTIC_CDF, GENLOGISTIC_CDF_INV, GENLOGISTIC_PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 April 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST079\n' );
  fprintf ( 1, '  For the Generalized Logistic PDF:\n' );
  fprintf ( 1, '  GENLOGISTIC_PDF evaluates the PDF.\n' );
  fprintf ( 1, '  GENLOGISTIC_CDF evaluates the CDF;\n' );
  fprintf ( 1, '  GENLOGISTIC_CDF_INV inverts the CDF.\n' );

  a = 1.0;
  b = 2.0;
  c = 3.0;

  check = genlogistic_check ( a, b, c );

  if ( ~check );
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TEST079 - Fatal error!\n' );
    fprintf ( 1, '  The parameters are not legal.\n' );
    return
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  PDF parameter A =             %14f\n', a );
  fprintf ( 1, '  PDF parameter B =             %14f\n', b );
  fprintf ( 1, '  PDF parameter C =             %14f\n', c );

  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, '       X            PDF           CDF            CDF_INV\n' );
  fprintf ( 1, '\n' );

  for i = 1 : 10

    [ x, seed ] = genlogistic_sample ( a, b, c, seed );

    pdf = genlogistic_pdf ( x, a, b, c );

    cdf = genlogistic_cdf ( x, a, b, c );

    x2 = genlogistic_cdf_inv ( cdf, a, b, c );

    fprintf ( 1, ' %14f  %14f  %14f  %14f\n', x, pdf, cdf, x2 );

  end

  return
end


function Test_trustregions

    % We attempt to compute an intrinsic mean of subspaces, that is, of
    % points on the Grassmann manifold. Let there be m subspaces of
    % dimension p embedded in R^n. We generate random data for our tests:
    % X is a random nxpxm matrix such that each slice of size nxp is an
    % orthonormal basis of a subspace.
    n = 50;
    p = 3;
    m = 100;
    Gr_multi = grassmannfactory(n, p, m);
    X = Gr_multi.rand();

    % Our search space is the Grassmann manifold: we want to locate one
    % point on the Grassmannian that "averages" the m given points.
    Gr = grassmannfactory(n, p);

    % The cost is the sum of squared Riemannian distances to the data
    % points.
    function f = cost(x)
        f = 0;
        for i = 1 : m
            xi = X(:, :, i);
            f = f + Gr.dist(x, xi)^2;
        end
        f = f/(2*m);
    end

    % The gradient is based on the Riemannian logarithmic map at the
    % current point, which gives tangent vectors at x pointing towards the
    % data points.
    function g = grad(x)
        g = Gr.zerovec(x);
        for i = 1 : m
            xi = X(:, :, i);
            g = g - Gr.log(x, xi);
        end
        g = g / m;
    end

    % Setup the problem structure, with the manifold M and the cost and its
    % gradient. Notice that we do not provide a Hessian, because it's
    % rather tricky to compute.
    problem.M = Gr;
    problem.cost = @cost;
    problem.grad = @grad;
%     problem.hess = @(x, xdot) Gr.zerovec(x) ;% * Gr.norm(x, xdot);

    % For peace of mind, check that the gradient is correct.
    % checkgradient(problem);
    % pause;

    % As a simple minded initial guess, choose one of the data points.
    x0 = X(:, :, 1);

    % Setup some options for the trustregions algorihm
    options.tolgradnorm = 1e-16;
    options.maxtime = 30;
    options.maxiter = 200;
    options.verbosity = 2;
    options.debug = 0;
    options.rho_regularization = 1e3;

    % We did not specify a Hessian, but use trustregions anyway. Hence, the
    % Hessian will be approximated, and we should be warned about it. To
    % disable the warning, you may execute this command:
    warning('off', 'manopt:getHessian:approx');

    [x, cost_x, info] = trustregions(problem, x0, options); %#ok<ASGLU>

    xdata = [info.time];
    ydata = [info.gradnorm];
    semilogy(xdata, ydata, '.-');
    xlabel('Time [s]');
    ylabel('Gradient norm');
    hold on;
    radius_change = [0 sign(diff([info.Delta]))];
    text(xdata(radius_change > 0), ydata(radius_change > 0)/2, '+');
    text(xdata(radius_change < 0), ydata(radius_change < 0)/2, '-');
    text(xdata, ydata*2, num2str([info.numinner]'));
    hold off;

    % info
    % keyboard;

end


function taulist = InverseDynamics(thetalist, dthetalist, ddthetalist, ...
                                   g, Ftip, Mlist, Glist, Slist)
% *** CHAPTER 8: DYNAMICS OF OPEN CHAINS ***
% Takes thetalist: n-vector of joint variables,
%       dthetalist: n-vector of joint rates,
%       ddthetalist: n-vector of joint accelerations,
%       g: Gravity vector g,
%       Ftip: Spatial force applied by the end-effector expressed in frame
%             {n+1},
%       Mlist: List of link frames {i} relative to {i-1} at the home
%              position,
%       Glist: Spatial inertia matrices Gi of the links,
%       Slist: Screw axes Si of the joints in a space frame, in the format
%              of a matrix with the screw axes as the columns.
% Returns taulist: The n-vector of required joint forces/torques.
% This function uses forward-backward Newton-Euler iterations to solve the
% equation:
% taulist = Mlist(thetalist) * ddthetalist + c(thetalist, dthetalist) ...
%           + g(thetalist) + Jtr(thetalist) * Ftip
% Example Input (3 Link Robot):
%
% clear; clc;
% thetalist = [0.1; 0.1; 0.1];
% dthetalist = [0.1; 0.2; 0.3];
% ddthetalist = [2; 1.5; 1];
% g = [0; 0; -9.8];
% Ftip = [1; 1; 1; 1; 1; 1];
% M01 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.089159]; [0, 0, 0, 1]];
% M12 = [[0, 0, 1, 0.28]; [0, 1, 0, 0.13585]; [-1, 0 ,0, 0]; [0, 0, 0, 1]];
% M23 = [[1, 0, 0, 0]; [0, 1, 0, -0.1197]; [0, 0, 1, 0.395]; [0, 0, 0, 1]];
% M34 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.14225]; [0, 0, 0, 1]];
% G1 = diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]);
% G2 = diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]);
% G3 = diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]);
% Glist = cat(3, G1, G2, G3);
% Mlist = cat(3, M01, M12, M23, M34);
% Slist = [[1; 0; 1;      0; 1;     0], ...
%        [0; 1; 0; -0.089; 0;     0], ...
%        [0; 1; 0; -0.089; 0; 0.425]];
% taulist = InverseDynamics(thetalist, dthetalist, ddthetalist, g, ...
%                         Ftip, Mlist, Glist, Slist)
%
% Output:
% taulist =
%   74.6962
%  -33.0677
%   -3.2306

n = size(thetalist, 1);
Mi = eye(4);
Ai = zeros(6, n);
AdTi = zeros(6, 6, n + 1);
Vi = zeros(6, n + 1);
Vdi = zeros(6, n + 1);
Vdi(4: 6, 1) = -g;
AdTi(:, :, n + 1) = Adjoint(TransInv(Mlist(:, :, n + 1)));
Fi = Ftip;
taulist = zeros(n, 1);
for i=1: n
    Mi = Mi * Mlist(:, :, i);
    Ai(:, i) = Adjoint(TransInv(Mi)) * Slist(:, i);
    AdTi(:, :, i) = Adjoint(MatrixExp6(VecTose3(Ai(:, i) ...
                    * -thetalist(i))) * TransInv(Mlist(:, :, i)));
    Vi(:, i + 1) = AdTi(:, :, i) * Vi(:, i) + Ai(:, i) * dthetalist(i);
    Vdi(:, i + 1) = AdTi(:, :, i) * Vdi(:, i) ...
                    + Ai(:, i) * ddthetalist(i) ...
                    + ad(Vi(:, i + 1)) * Ai(:, i) * dthetalist(i);
end
for i = n: -1: 1
    Fi = AdTi(:, :, i + 1)' * Fi + Glist(:, :, i) * Vdi(:, i + 1) ...
         - ad(Vi(:, i + 1))' * (Glist(:, :, i) * Vi(:, i + 1));
    taulist(i) = Fi' * Ai(:, i);
end
end


%% RATE OF CONVERGENCE OF ADAPTIVE FINITE ELEMENT METHOD USING WG ELEMENT
%
% This example is to show the rate of convergence of the lowest order
% finite element approximation of the second order elliptic equation.
%
% # Lshape problem.
% # Kellogg problem.

clear all; close all;
%% Kellogg problem
[node,elem] = squaremesh([-1 1 -1 1], 0.5);
bdFlag = setboundary(node,elem,'Dirichlet');
pde = Kelloggdata;
option.L0 = 1;
option.maxIt = 100;
option.maxN = 1e4;
option.theta = 0.2;
option.plotflag = 1;
err = afemPoisson(node,elem,pde,bdFlag,option);
figure;
showrate2(err.N,err.H1,40,'k-*','||Du-Du_h||',err.N,err.eta,40,'-+','eta');
latexerrtable(err.N,[err.H1 err.eta])

%% Lshape problem
[node,elem] = squaremesh([-1,1,-1,1],1);
[node,elem] = delmesh(node,elem,'x>0 & y<0');
bdFlag = setboundary(node,elem,'Dirichlet');
pde = Lshapedata;
format shorte
option.L0 = 1;
option.maxIt = 25;
option.printlevel = 1;
% option.elemType = 'WG';
option.plotflag = 1;
err = afemPoisson(node,elem,pde,bdFlag,option);


function [C, S] = fcs(x)
%FCS Computes Fresnel integrals.
%[C, S] = fcs(x) returns Fresnel integrals C ans S for argument x,
% x must be double and real.
% F = fcs(x) returns complex F = C+i*S
%
% Algorithm:
% This function uses an improved method for computing Fresnel integrals
% with an error of less then 1x10-9, described in:
%
% Klaus D. Mielenz, Computation of Fresnel Integrals. II
% J. Res. Natl. Inst. Stand. Technol. 105, 589 (2000), pp 589-590
%
% Copyright (c) 2002 by Peter L. Volegov (volegov@unm.edu)
% All Rights Reserved.

error('You need to compile fcs.c to a mex file for your platform.');


% FORMAT:
%
% sampval = ms2sample(timems, fs, rounding, offset)
%
% INPUTS:
%
% timesam     - time value in samples
% fs          - sample rate
%
% Optional inputs:
%
% rounding    - 1 means round the result {default}, 0 means do not round
% offset      - offset time in milliseconds (you can use sample2ms recursively here). Default 0
%
% OUTPUT:
%
% sampval     - time in milliseconds
%
% EXAMPLES:
%
% 1) For a time serie recorded from 0 to 5 secs, at fs=500 sps, get the time in ms for the sample # 337.
%
% msval = sample2ms(337, 500)
%
% msval =
%
%    674
%
% 2) For a time serie recorded from -1 to 5 secs, at fs=500 sps, get the absolute time in ms for the sample # 337.
%
% msval = sample2ms(337, 500, 0, sample2ms(500,500))
%
% msval =
%
%    1674
%
%
% Author: Javier Lopez-Calderon
% Center for Mind and Brain
% University of California, Davis,
% Davis, CA
% 2013

function msval = sample2ms(timesam, fs, rounding, offset)
if nargin<1
        help sample2ms
        return
end
if nargin<4
        offset = 0;
end
if nargin<3
        rounding = 1;
end
if nargin<2
        error('Two inputs are requiered at least.')
end
msval = offset + 1000*timesam/fs;
if rounding
        msval = round(msval);
end


%test script fgg_2D_experiment.m for the 2D NFFT based on Fast Gaussian
%Gridding.
%
%NOTE: In order for this FGG_2D to work, the C
%file "FGG_Convolution2D.c" must be compiled into a Matlab executable
%(cmex) with the following command: mex FGG_Convolution2D.c
%
%Code by (send correspondence to):
%Matthew Ferrara, Research Mathematician
%AFRL Sensors Directorate Innovative Algorithms Branch (AFRL/RYAT)
%Matthew.Ferrara@wpafb.af.mil

clear all;
close all;
%clc
%path(path, './nfftde');%NFFT mex files from Potts, et al.

% make an "image"
%Note for even lengths, the Nfft routine defines the image-space origin
% at pixel location [Nx/2 + 1, Ny/2 + 1].
% Convention:  x counts down rows, y counts L to R columns.
N=16;%even length assumed below...tedious to generalize...
z = zeros(N,N);%
%Make a smiley face:
z(N/2+3,N/2-1 : N/2+1 ) = 1;
z(N/2+2,N/2-2 ) = 1;
z(N/2+2,N/2+2 ) = 1;
z(N/2-1,N/2-1) = 1;
z(N/2+1,N/2) = 1;
z(N/2-1,N/2+1) = 1;
%imagesc(z)

N=[N,N];
img=double(z);
% Now, let's compute a matlab DFT in d dimensions using the "nfft" command.
% Note, use fftshifts to match indexing convention as used above in Pott's
% nfft.
data=fftshift(ifftn(ifftshift(img)));
DFTout = fftshift(fftn(ifftshift(data),N));
z=z(:);

% We need knots on [-1/2, 1-1/Nx]x[-1/2, 1-1/Ny] as fundamental period.
% make square grid of knots for exact comparison to fft
tmpx = linspace(-1/2,1/2 -1/N(1), N(1));% tmpx(end)=[];
tmpy = linspace(-1/2,1/2 -1/N(2), N(2));% tmpy(end)=[];

%this creates N+1 points, then discards point at +0.5.
%store my K knots as a d-by-K matrix (d=2 dimension here)
%...following four lines could be cleverly vectorized, avoiding loop.
[Y,X]=meshgrid(tmpy,tmpx);

knots=[X(:),Y(:)];

Nx=N(1);
Ny=N(2);
%set the desired number of digits of accuracy desired from the NUFFT
%routine:
Desired_accuracy = 6;%6=single precision, 12=double precision.
tic
MattOut_Gauss=FGG_2d_type1(data(:),knots,[Nx,Ny],Desired_accuracy);
imagesc(abs(MattOut_Gauss))
colorbar
title('(Type-I Fast Gaussian Gridding) NFFT output')
disp(['NUFFT evaluated in ',num2str(toc),' seconds'])
MattOut_t2=iFGG_2d_type2(MattOut_Gauss,knots,Desired_accuracy);
MattOut_Gauss=FGG_2d_type1(MattOut_t2(:),knots,[Nx,Ny],Desired_accuracy);
figure
imagesc(abs(MattOut_Gauss))
colorbar
title('(Type-I Fast Gaussian Gridding) NFFT output from Type-II-generated data')
norm(MattOut_t2-z(:))

figure
imagesc(abs(img-MattOut_Gauss))
colorbar
title('Error between DFT and NFFT')
Mean_Error=mean(abs(MattOut_Gauss(:)-DFTout(:)))


% "example_nufft1_reverse.m"
% This m-file is an example of applying the NUFFT method "in reverse,"
% meaning that you have a nonuniformly-sampled signal spectrum in hand
% and you want to try to "compute its inverse Fourier transform"
% to get uniform set of signal samples.  In the nomenclature of the 1999
% SIAM J Sci. Comput. paper by Nguyen and Liu, this is "Problem 1."
% (It is somewhat related to MRI reconstruction by gridding.)
% Before doing this, you should ask yourself if that is *really*
% what you want to do.  In most inverse problems, I think it is NOT
% what we should do, as argued in the T-SP paper on this method.
% Instead, we should solve the inverse problem *iteratively*, using
% the NUFFT method (and its adjoint) each iteration.
% Furthermore, the min-max optimality of the NUFFT method was only
% established for the "forward direction:" going from uniform signal
% samples to nonuniform frequency samples.
% (You can replace time and frequency in the above discussion.)
% Anyway, let's try it here and see how it works...

%
% synthesize some spectral data that is nonuniformly spaced
%
No = 2^7;				% number of frequency samples
rng(0)
om = 2*pi*sort(rand(No,1)-0.5);		% random frequency samples on [-pi,pi ]
%om = 2*pi*[-No/2:No/2-1]'/No;		% test with uniform samples

% a spectrum with periodic components, hopefully visible in other domain
X = inline('2 + 2*sin(om*10) + 4*cos(15*om) + 4*cos(20*om)', 'om');
Xm = X(om);

%
% go to the time domain the "exact" (slow) DTFT way.
% this implements essentially equation (1) in Nguyen and Liu (1999)
%
N1 = 2^6;		% # of time samples
n = [0:N1-1]'-N1/2;	% time sample locations
xd = dtft2_adj(Xm, [om 0*om], N1, 1, [N1/2 0]);

%
% now do it the fast NUFFT way
%
if 1 || ~isvar('st')	% create NUFFT structure
	J = 5;		% interpolation neighborhood
	K1 = N1*2;	% two-times oversampling
	st = nufft_init(om, N1, J, K1, N1/2, 'minmax:kb');
end

xn = nufft_adj(Xm, st);	% call ADJOINT to go "in reverse"

%
% compare slow exact to fast NUFFT
%
figure(1), clf, pl=220;
subplot(pl+1)
oo = [-200:200]'/400*2*pi;
plot(om, Xm, '.', oo, X(oo), '-')
xlabel \omega, ylabel X(\omega), title 'Spectrum and samples'

subplot(pl+2)
plot(n, real(xd), 'c.-', n, imag(xd), 'y.-')
xlabel n, ylabel x[n], title 'Exact "nonuniform FT"'

subplot(pl+3)
plot(n, real(xn), 'c.-', n, imag(xn), 'y.-')
xlabel n, ylabel x[n], title 'Fast NUFFT adjoint'

subplot(pl+4)
plot(n, real(xn-xd), 'c.-', n, imag(xn-xd), 'y.-')
xlabel n, ylabel x[n], title 'Approximation Error (very small!)'

%
% ok fine, so if you look at figure 1 you see that
% the NUFFT gives a great approximation to the "exact" formula
% but is the result *really* what you wanted?
% It seems that ideally the result should be 4 spikes with
% no background junk.  If we solved "Problem 5" in Nguyen and Liu,
% then we should get that!  This is how we have approached the
% MRI reconstruction problem, as described in ../mri.
%
% For iterative 2D version, see ../example/mri_example.m

return

%
% here is a different strategy: interpolate the nonuniform data
% onto a uniform grid, then simply take the inverse FFT.
% this didn't work so well because it is a lousy gridding method.
% todo: need to put a better gridding method here!
%
ok = [-N1/2:(N1/2-1)]'/N1*2*pi;
Xg = interp1(om, Xm, ok, 'linear', 'extrap');
xg = fftshift(ifft(fftshift(Xg)));

%xg = xg ./ nufft_sinc(n/N1).^2; % post-compensate?

figure(2), clf, pl=220;
subplot(pl+1)
plot(n, real(xg), 'c.-', n, imag(xg), 'y.-')
xlabel n, ylabel x[n], title 'Gridding "reconstruction"'

Xg = dtft1(xg, om, N1/2);
Xd = dtft1(xd/No, om, N1/2);

subplot(pl+2)
plot(om, real(Xg), '.', om, real(Xm), '-')
xlabel \omega, ylabel X(\omega), title ''

subplot(pl+3)
plot(om, real(Xd), '.', om, real(Xm), '-')
xlabel \omega, ylabel X(\omega), title ''


function K = covNNone(hyp, x, z, i)

% Neural network covariance function with a single parameter for the distance
% measure. The covariance function is parameterized as:
%
% k(x^p,x^q) = sf2 * asin(x^p'*P*x^q / sqrt[(1+x^p'*P*x^p)*(1+x^q'*P*x^q)])
%
% where the x^p and x^q vectors on the right hand side have an added extra bias
% entry with unit value. P is ell^-2 times the unit matrix and sf2 controls the
% signal variance. The hyperparameters are:
%
% hyp = [ log(ell)
%         log(sqrt(sf2) ]
%
% Copyright (c) by Carl Edward Rasmussen and Hannes Nickisch, 2010-09-10.
%
% See also COVFUNCTIONS.M.

if nargin<2, K = '2'; return; end                  % report number of parameters
if nargin<3, z = []; end                                   % make sure, z exists
xeqz = isempty(z); dg = strcmp(z,'diag');                       % determine mode

n = size(x,1);
ell2 = exp(2*hyp(1));
sf2 = exp(2*hyp(2));

sx = 1 + sum(x.*x,2);
if dg                                                               % vector kxx
  K = sx./(sx+ell2);
else
  if xeqz                                                 % symmetric matrix Kxx
    S = 1 + x*x';
    K = S./(sqrt(ell2+sx)*sqrt(ell2+sx)');
  else                                                   % cross covariances Kxz
    S = 1 + x*z'; sz = 1 + sum(z.*z,2);
    K = S./(sqrt(ell2+sx)*sqrt(ell2+sz)');
  end
end

if nargin<4                                                        % covariances
  K = sf2*asin(K);
else                                                               % derivatives
  if i==1                                                          % lengthscale
    if dg
      V = K;
    else
      vx = sx./(ell2+sx);
      if xeqz
        V = repmat(vx/2,1,n) + repmat(vx'/2,n,1);
      else
        vz = sz./(ell2+sz); nz = size(z,1);
        V = repmat(vx/2,1,nz) + repmat(vz'/2,n,1);
      end
    end
    K = -2*sf2*(K-K.*V)./sqrt(1-K.*K);
  elseif i==2                                                        % magnitude
    K = 2*sf2*asin(K);
  else
    error('Unknown hyperparameter')
  end
end


  function y = downsample3(x, m)
%|function y = downsample3(x, m)
%| downsample by averaging by integer factors
%| m can be a scalar (same factor for both dimensions)
%| or a 3-vector
%| in
%|	x	[nx ny nz]
%|	m	[1] or [1 3]
%| out
%|	y	[nx/m ny/m nz/m]
%| function modified to 3D space by: Taka Masuda

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

if ndims(x) == 2
	warn('2d case')
	if numel(m) == 1, m = [m m 1]; end
	if numel(m) ~= 3, fail 'm not length 3?', end
	y = downsample2(x, m(1:2));
return
end

if ndims(x) ~= 3
	fail('not 3d')
end

if numel(m) == 1
	m = m * ones(ndims(x),1);
end
if numel(m) ~= ndims(x), error 'bad m', end

% downsample along each dimension
y = downsample1(x, m(1));
y = downsample1(permute(y, [2 1 3]), m(2));
y = downsample1(permute(y, [3 2 1]), m(3)); % [3 1 2] order
y = permute(y, [2 3 1]);

% downsample1hide()
% 1d down sampling of each column
function y = downsample1hide(x, m)
'ok'
if m == 1, y = x; return; end
n1 = floor(size(x,1) / m);
n2 = size(x,2);
n3 = size(x,3);
y = zeros(n1,n2,n3);
for ii=0:m-1
	y = y + x(ii+[1:m:m*n1],:,:);
	ticker(mfilename, ii+1, m)
end
y = y / m;

% downsample3_test
function downsample3_test
x = [6 5 2];
x = reshape(2*[1:prod(x)], x);
down = 2;
y = downsample3(x, down);
if down == 1
	jf_equal(y, x)
else
	jf_equal(y, [39 63; 43 67; 47 71])
end

if has_aspire
	filex = [test_dir filesep 'testx.fld'];
	filey = [test_dir filesep 'testy.fld'];
	fld_write(filex, x)
	delete(filey)
	com = ['op sample3 mean ' filey ' ' filex ' %d %d %d'];
	com = sprintf(com, down, down, down);
	os_run(com)
	z = fld_read(filey);
	if ~isequal(y, z), error 'aspire/matlab mismatch', end
end

if 0 % big
	x = zeros(2.^[7 8 9]);
	cpu etic
	y = downsample3(x, 2);
	cpu etoc 'downsample3 time:'
end


function b = r8cc_vxm ( m, n, nz_num, colptr, rowind, a, x )

%*****************************************************************************80
%
%% R8CC_VXM multiplies a vector times a R8CC matrix.
%
%  Discussion:
%
%    The R8CC format is the double precision sparse compressed column
%    format.  Associated with this format, we have an M by N matrix
%    with NZ_NUM nonzero entries.  We construct the column pointer
%    vector COL of length N+1, such that entries of column J will be
%    stored in positions COL(J) through COL(J+1)-1.  This indexing
%    refers to both the ROW and A vectors, which store the row indices
%    and the values of the nonzero entries.  The entries of the
%    ROW vector corresponding to each column are assumed to be
%    ascending sorted.
%
%    The R8CC format is equivalent to the MATLAB "sparse" format,
%    and the Harwell Boeing "real unsymmetric assembled" (RUA) format.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Iain Duff, Roger Grimes, John Lewis,
%    User's Guide for the Harwell-Boeing Sparse Matrix Collection,
%    October 1992
%
%  Parameters:
%
%    Input, integer M, the number of rows of the matrix.
%
%    Input, integer N, the number of columns of the matrix.
%
%    Input, integer NZ_NUM, the number of nonzero elements in A.
%
%    Input, integer COLPTR(N+1), points to the first element of each column.
%
%    Input, integer ROWIND(NZ_NUM), contains the row indices of the elements.
%
%    Input, real A(NZ_NUM), the matrix.
%
%    Input, real X(M), the vector to be multiplied.
%
%    Output, real B(N), the product A'*X.
%
  b(1:n) = 0.0;

  for j = 1 : n
    for k = colptr(j) : colptr(j+1) - 1
      i = rowind(k);
      b(j) = b(j) + a(k) * x(i);
    end
  end

  return
end


function X_rec = recoverData(Z, U, K)
%RECOVERDATA Recovers an approximation of the original data when using the
%projected data
%   X_rec = RECOVERDATA(Z, U, K) recovers an approximation the
%   original data that has been reduced to K dimensions. It returns the
%   approximate reconstruction in X_rec.
%

% You need to return the following variables correctly.
X_rec = zeros(size(Z, 1), size(U, 1));

% ====================== YOUR CODE HERE ======================
% Instructions: Compute the approximation of the data by projecting back
%               onto the original space using the top K eigenvectors in U.
%
%               For the i-th example Z(i,:), the (approximate)
%               recovered data for dimension j is given as follows:
%                    v = Z(i, :)';
%                    recovered_j = v' * U(j, 1:K)';
%
%               Notice that U(j, 1:K) is a row vector.
%

% =============================================================

end


% Fig. 6.12   Feedback Control of Dynamic Systems, 5e
%             Franklin, Powell, Emami
%

clear all;
close all;

num1=[10 10];
den1=[1 10];
w=logspace(-2,3,100);
[m1,p1]=bode(num1,den1,w);
num2=[10 -10];
[m2,p2]=bode(num2,den1,w);
figure(1)
loglog(w,m1,w,m2);
xlabel('\omega (rad/sec)');
ylabel('Magnitude');
title('Fig. 6.12 Bode Plot for a NMP System (a) magnitude');
grid;
pause;
figure(2)
semilogx(w,p1,w,p2);
grid;
xlabel('\omega (rad/sec)');
ylabel('Phase (deg)');
title('Fig 6.12 (b) phase');


function dImgLow = fDownSample( dImgHigh, out_size )

out_x_sz = out_size(1); out_y_sz = out_size(2); out_z_sz = out_size(3);
[in_x_sz,in_y_sz, in_z_sz, nCha, nTime] = size( dImgHigh );

% build grid vectors for the up/down sampling
% ============================================
% if the input is even & output is odd-> use floor for all
% if the output is even & input is odd -> use ceil for all
% other cases - don't care
% for downsampling -> the opposite
if (~mod( in_x_sz,2 ) & (out_x_sz>in_x_sz)) | (mod( in_x_sz,2 ) & (out_x_sz<in_x_sz))
    x_output_space  = max(floor((out_x_sz-in_x_sz)/2),0) + [1:min(in_x_sz,out_x_sz)];
    x_input_space   = max(floor((in_x_sz-out_x_sz)/2),0) + [1:min(in_x_sz,out_x_sz)];
else
    x_output_space  = max(ceil((out_x_sz-in_x_sz)/2),0) + [1:min(in_x_sz,out_x_sz)];
    x_input_space   = max(ceil((in_x_sz-out_x_sz)/2),0) + [1:min(in_x_sz,out_x_sz)];
end
if (~mod( in_y_sz,2 ) & (out_y_sz>in_y_sz)) | (mod( in_y_sz,2 ) & (out_y_sz<in_y_sz))
   y_output_space  = max(floor((out_y_sz-in_y_sz)/2),0) + [1:min(in_y_sz,out_y_sz)];
   y_input_space   = max(floor((in_y_sz-out_y_sz)/2),0) + [1:min(in_y_sz,out_y_sz)];
else
   y_output_space  = max(ceil((out_y_sz-in_y_sz)/2),0) + [1:min(in_y_sz,out_y_sz)];
   y_input_space   = max(ceil((in_y_sz-out_y_sz)/2),0) + [1:min(in_y_sz,out_y_sz)];
end
if (~mod( in_z_sz,2 ) & (out_z_sz>in_z_sz)) | (mod( in_z_sz,2 ) & (out_z_sz<in_z_sz))
   z_output_space  = max(floor((out_z_sz-in_z_sz)/2),0) + [1:min(in_z_sz,out_z_sz)];
   z_input_space   = max(floor((in_z_sz-out_z_sz)/2),0) + [1:min(in_z_sz,out_z_sz)];
else
   z_output_space  = max(ceil((out_z_sz-in_z_sz)/2),0) + [1:min(in_z_sz,out_z_sz)];
   z_input_space   = max(ceil((in_z_sz-out_z_sz)/2),0) + [1:min(in_z_sz,out_z_sz)];
end

dImgLow = zeros( out_x_sz, out_y_sz, out_z_sz, size(dImgHigh,4), size(dImgHigh,5) );
dImgLow(x_output_space,y_output_space, z_output_space, :, : ) = dImgHigh(x_input_space,y_input_space, z_input_space, :, :);

end


function [llh] = tapas_dlinear_hier_llh(data, theta, ptheta)
%% Likelihood of the nodes of a model with diagonal precision matrix.
% Note that this is the likelihood of several parameters theta 1 to n with mean
% mu and precision matrix diag(pe).
%

%
% aponteeduardo@gmail.com
% copyright (C) 2016
%

y = data.y;
u = data.u;

theta = theta.y;

[np, nc] = size(y);

llh = zeros(np, nc);

ln2pi = log(2 * pi);

for j = 1:nc
    % Mean
    mu = theta{j}.mu;
    % Precision
    pe = theta{j}.pe;
    lpe = log(pe);

   for i = 1:np
        r = y{i, j} - mu;
        llh(i, j) = sum(- 0.5 * ln2pi + 0.5 * lpe - 0.5 * pe .* r .* r);
    end
end

end


function [x,y] = combinatorialDependentRows(A)

[m,n]=size(A);
% A is a binarized [F R] matrix (each entry of A is 0 or 1)
% if this program is feasible, then there exist 2 combinatorially dependent subsets of rows of A

%cvx_solver gurobi
cvx_solver mosek

cvx_begin sdp quiet

    variable x(m) binary;
    variable y(m) binary;

    % x,y are nonempty and disjoint subsets of rows of A
    x+y<=1;
    sum(x)>=1;
    sum(y)>=1;

    % trick to test for same support (idea: m > largest possible sum)
    m*x'*A>=y'*A;
    m*y'*A>=x'*A;

cvx_end

disp(find(x))
disp(find(y))

end


% This script runs program rundivcrl
% for nine coordinate systems and puts
% the output in rundivcrl.tst
if exist('rundivcrl.tst','file')==2
  delete rundivcrl.tst
end
syms x y z
vxyz=[x*y,y*z,z*x];
diary rundivcrl.tst
% rundivcrl(vxyz,cordname,tn,noprint) call list
rundivcrl(vxyz,@cylin,[10*rand,rand*2*pi,rand*10]);
rundivcrl(vxyz,@sphr,[10*rand,rand*pi,rand*2*pi]);
rundivcrl(vxyz,@cone,[2*rand,rand*pi/2,rand*2*pi]);
rundivcrl(vxyz,@elipcyl,[2*rand,rand*2*pi,2*rand]);
rundivcrl(vxyz,@elipsod,[rand,1+rand,2+rand]);
rundivcrl(vxyz,@notort,[2*rand,rand*pi,rand*2*pi]);
rundivcrl(vxyz,@oblate,[2*rand,rand*pi,rand*2*pi]);
rundivcrl(vxyz,@parab,[2*rand,2*rand,2*rand]);
rundivcrl(vxyz,@toroid,[rand*2,rand*2*pi,rand*2*pi]);
diary off


function determ = cheby_van2_determinant ( n )

%*****************************************************************************80
%
%% CHEBY_VAN2_DETERMINANT returns the determinant of the CHEBY_VAN2 matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 November 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Output, real DETERM, the determinant.
%
  if ( n <= 0 )
    determ = 0.0;
  elseif ( n == 1 )
    determ = 1.0;
  else
    determ = r8_mop ( floor ( n / 2 ) ) * sqrt ( 2.0 )^( 4 - n );
  end

  return
end


% CSparse: a Concise Sparse matrix Package.
%
%   Matrices used in CSparse must in general be either sparse and real,
%   or dense vectors.  Ordering methods can accept any sparse matrix.
%
%   cs_add       - sparse matrix addition.
%   cs_amd       - approximate minimum degree ordering.
%   cs_chol      - sparse Cholesky factorization.
%   cs_cholsol   - solve A*x=b using a sparse Cholesky factorization.
%   cs_counts    - column counts for sparse Cholesky factor L.
%   cs_dmperm    - maximum matching or Dulmage-Mendelsohn permutation.
%   cs_dmsol     - x=A\b using the coarse Dulmage-Mendelsohn decomposition.
%   cs_dmspy     - plot the Dulmage-Mendelsohn decomposition of a matrix.
%   cs_droptol   - remove small entries from a sparse matrix.
%   cs_esep      - find an edge separator of a symmetric matrix A
%   cs_etree     - elimination tree of A or A'*A.
%   cs_gaxpy     - sparse matrix times vector.
%   cs_lsolve    - solve a sparse lower triangular system L*x=b.
%   cs_ltsolve   - solve a sparse upper triangular system L'*x=b.
%   cs_lu        - sparse LU factorization, with fill-reducing ordering.
%   cs_lusol     - solve Ax=b using LU factorization.
%   cs_make      - compiles CSparse for use in MATLAB.
%   cs_multiply  - sparse matrix multiply.
%   cs_nd        - generalized nested dissection ordering.
%   cs_nsep      - find a node separator of a symmetric matrix A.
%   cs_permute   - permute a sparse matrix.
%   cs_print     - print the contents of a sparse matrix.
%   cs_qr        - sparse QR factorization.
%   cs_qleft     - apply Householder vectors on the left.
%   cs_qright    - apply Householder vectors on the right.
%   cs_qrsol     - solve a sparse least-squares problem.
%   cs_randperm  - random permutation.
%   cs_sep       - convert an edge separator into a node separator.
%   cs_scc       - strongly-connected components of a square sparse matrix.
%   cs_scc2      - cs_scc, or connected components of a bipartite graph.
%   cs_sparse    - convert a triplet form into a sparse matrix.
%   cs_sqr       - symbolic sparse QR factorization.
%   cs_symperm   - symmetric permutation of a symmetric matrix.
%   cs_transpose - transpose a real sparse matrix.
%   cs_updown    - rank-1 update/downdate of a sparse Cholesky factorization.
%   cs_usolve    - solve a sparse upper triangular system U*x=b.
%   cs_utsolve   - solve a sparse lower triangular system U'*x=b.
%   cspy         - plot a matrix in color.
%   ccspy        - plot the connected components of a matrix.

% Example:
%   help cs_add

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

% helper function:
%   cs_must_compile - return 1 if source code f must be compiled, 0 otherwise


% produces synthetic Omori aftershock sequence using given epicentres
% Omori parameters can be chosen arbitraily
%
% Samuel Neukomm, 27.2.2004

[filename,pathname] = uigetfile('*.mat','Load earthquake sequence');
do = ['load ' pathname filename]; eval(do)

lon = a.Longitude; lat = a.Latitude; mag = a.Magnitude; dep = a.Depth;
[m_main, main] = max(a.Magnitude);
if size(a,2) == 9
    date_matlab = datenum(a.Date.Year,a.Date.Month,a.Date.Day,a.Date.Hour,a.Date.Minute,zeros(size(a,1),1));
else
    date_matlab = datenum(a.Date.Year,a.Date.Month, a.Date.Day,a.Date.Hour,a.Date.Minute,a(:,10));
end

date_main = date_matlab(main);
time_aftershock = date_matlab-date_main;

l = time_aftershock(:) > 0;
t_aftershock = time_aftershock(l);
eqcatalogue = a.subset(l);

ncum = (1:length(t_aftershock))';

prompt = {'p value:','c value:','k value:'};
def = {'0.9','0.05','1000'};
dlgTitle = 'Choose Omori parameters';
lineNo = 1;
answer = inputdlg(prompt,dlgTitle,lineNo,def);

pv = str2double(answer{1});
cv = str2double(answer{2});
kv = str2double(answer{3});

tasnew = (cv^(1-pv)-ncum*(pv-1)/kv).^(1/(1-pv))-cv;

date_mat_new = tasnew + date_main;
[yr,mon,day,hr,mn,sec] = datevec(date_mat_new);

datum = [yr mon day hr mn sec];
yr = decyear(datum);

if size(a,2) == 9
    a = [a(main,:) 0; eqcatalogue(:,1) eqcatalogue(:,2) yr mon day eqcatalogue(:,6) eqcatalogue(:,7) hr mn sec];
else
    a = [a(main,1:10); eqcatalogue(:,1) eqcatalogue(:,2) yr mon day eqcatalogue(:,6) eqcatalogue(:,7) hr mn sec];
end

[filename, pathname] = uiputfile('*.mat', 'Save synthetic catalog as');

try
    save(fullfile(pathname, filename),'a','pv','cv','kv');
catch
    disp('failed to save'); %complain
end


function [x,P]=ukf(fstate,x,P,hmeas,z,Q,R)
% UKF   Unscented Kalman Filter for nonlinear dynamic systems
% [x, P] = ukf(f,x,P,h,z,Q,R) returns state estimate, x and state covariance, P
% for nonlinear dynamic system (for simplicity, noises are assumed as additive):
%           x_k+1 = f(x_k) + w_k
%           z_k   = h(x_k) + v_k
% where w ~ N(0,Q) meaning w is gaussian noise with covariance Q
%       v ~ N(0,R) meaning v is gaussian noise with covariance R
% Inputs:   f: function handle for f(x)
%           x: "a priori" state estimate
%           P: "a priori" estimated state covariance
%           h: fanction handle for h(x)
%           z: current measurement
%           Q: process noise covariance
%           R: measurement noise covariance
% Output:   x: "a posteriori" state estimate
%           P: "a posteriori" state covariance
%
% Example:
%{
n=3;      %number of state
q=0.1;    %std of process
r=0.1;    %std of measurement
Q=q^2*eye(n); % covariance of process
R=r^2;        % covariance of measurement
f=@(x)[x(2);x(3);0.05*x(1)*(x(2)+x(3))];  % nonlinear state equations
h=@(x)x(1);                               % measurement equation
s=[0;0;1];                                % initial state
x=s+q*randn(3,1); %initial state          % initial state with noise
P = eye(n);                               % initial state covraiance
N=20;                                     % total dynamic steps
xV = zeros(n,N);          %estmate        % allocate memory
sV = zeros(n,N);          %actual
zV = zeros(1,N);
for k=1:N
  z = h(s) + r*randn;                     % measurments
  sV(:,k)= s;                             % save actual state
  zV(k)  = z;                             % save measurment
  [x, P] = ukf(f,x,P,h,z,Q,R);            % ekf
  xV(:,k) = x;                            % save estimate
  s = f(s) + q*randn(3,1);                % update process
end
for k=1:3                                 % plot results
  subplot(3,1,k)
  plot(1:N, sV(k,:), '-', 1:N, xV(k,:), '--')
end
%}
% Reference: Julier, SJ. and Uhlmann, J.K., Unscented Filtering and
% Nonlinear Estimation, Proceedings of the IEEE, Vol. 92, No. 3,
% pp.401-422, 2004.
%
% By Yi Cao at Cranfield University, 04/01/2008
%
L=numel(x);                                 %numer of states
m=numel(z);                                 %numer of measurements
alpha=1e-3;                                 %default, tunable
ki=0;                                       %default, tunable
beta=2;                                     %default, tunable
lambda=alpha^2*(L+ki)-L;                    %scaling factor
c=L+lambda;                                 %scaling factor
Wm=[lambda/c 0.5/c+zeros(1,2*L)];           %weights for means
Wc=Wm;
Wc(1)=Wc(1)+(1-alpha^2+beta);               %weights for covariance
c=sqrt(c);
X=sigmas(x,P,c);                            %sigma points around x
[x1,X1,P1,X2]=ut(fstate,X,Wm,Wc,L,Q);          %unscented transformation of process
% X1=sigmas(x1,P1,c);                         %sigma points around x1
% X2=X1-x1(:,ones(1,size(X1,2)));             %deviation of X1
[z1,Z1,P2,Z2]=ut(hmeas,X1,Wm,Wc,m,R);       %unscented transformation of measurments
P12=X2*diag(Wc)*Z2';                        %transformed cross-covariance
K=P12*inv(P2);
x=x1+K*(z-z1);                              %state update
P=P1-K*P12';                                %covariance update

function [y,Y,P,Y1]=ut(f,X,Wm,Wc,n,R)
%Unscented Transformation
%Input:
%        f: nonlinear map
%        X: sigma points
%       Wm: weights for mean
%       Wc: weights for covraiance
%        n: numer of outputs of f
%        R: additive covariance
%Output:
%        y: transformed mean
%        Y: transformed smapling points
%        P: transformed covariance
%       Y1: transformed deviations

L=size(X,2);
y=zeros(n,1);
Y=zeros(n,L);
for k=1:L
    Y(:,k)=f(X(:,k));
    y=y+Wm(k)*Y(:,k);
end
Y1=Y-y(:,ones(1,L));
P=Y1*diag(Wc)*Y1'+R;

function X=sigmas(x,P,c)
%Sigma points around reference point
%Inputs:
%       x: reference point
%       P: covariance
%       c: coefficient
%Output:
%       X: Sigma points

A = c*chol(P)';
Y = x(:,ones(1,numel(x)));
X = [x Y+A Y-A];


function matlab_time = unixtime2mat(unix_time);
% unixtime2mat  Converts unix time stamps (seconds since Jan 1, 1970) to
%               Matlab serial date number (decimal days since Jan 1 0000).
%
%               USAGE:
%                      unixtime2mat(unix_time)
%
%
%               The function may not handle leap years or leap seconds
%               appropriately.
%
%               Val Schmidt
%               Center for Coastal and Ocean Mapping
%               2007

unix_epoch = datenum(1970,1,1,0,0,0);
matlab_time = unix_time./86400 + unix_epoch;


%WAVE_BASES  1D Wavelet functions Morlet, Paul, or DOG
%
%  [DAUGHTER,FOURIER_FACTOR,COI,DOFMIN] = ...
%      wave_bases(MOTHER,K,SCALE,PARAM);
%
%   Computes the wavelet function as a function of Fourier frequency,
%   used for the wavelet transform in Fourier space.
%   (This program is called automatically by WAVELET)
%
% INPUTS:
%
%    MOTHER = a string, equal to 'MORLET' or 'PAUL' or 'DOG'
%    K = a vector, the Fourier frequencies at which to calculate the wavelet
%    SCALE = a number, the wavelet scale
%    PARAM = the nondimensional parameter for the wavelet function
%
% OUTPUTS:
%
%    DAUGHTER = a vector, the wavelet function
%    FOURIER_FACTOR = the ratio of Fourier period to scale
%    COI = a number, the cone-of-influence size at the scale
%    DOFMIN = a number, degrees of freedom for each point in the wavelet power
%             (either 2 for Morlet and Paul, or 1 for the DOG)
%
%----------------------------------------------------------------------------
%   Copyright (C) 1995-1998, Christopher Torrence and Gilbert P. Compo
%   University of Colorado, Program in Atmospheric and Oceanic Sciences.
%   This software may be used, copied, or redistributed as long as it is not
%   sold and this copyright notice is reproduced on each copy made.  This
%   routine is provided as is without any express or implied warranties
%   whatsoever.
%----------------------------------------------------------------------------
function [daughter,fourier_factor,coi,dofmin] = ...
	wave_bases(mother,k,scale,param);

mother = upper(mother);
n = length(k);

if (strcmp(mother,'MORLET'))  %-----------------------------------  Morlet
	if (param == -1), param = 6.;, end
	k0 = param;
	expnt = -(scale.*k - k0).^2/2.*(k > 0.);
	norm = sqrt(scale*k(2))*(pi^(-0.25))*sqrt(n);    % total energy=N   [Eqn(7)]
	daughter = norm*exp(expnt);
	daughter = daughter.*(k > 0.);     % Heaviside step function
	fourier_factor = (4*pi)/(k0 + sqrt(2 + k0^2)); % Scale-->Fourier [Sec.3h]
	coi = fourier_factor/sqrt(2);                  % Cone-of-influence [Sec.3g]
	dofmin = 2;                                    % Degrees of freedom
elseif (strcmp(mother,'PAUL'))  %--------------------------------  Paul
	if (param == -1), param = 4.;, end
	m = param;
	expnt = -(scale.*k).*(k > 0.);
	norm = sqrt(scale*k(2))*(2^m/sqrt(m*prod(2:(2*m-1))))*sqrt(n);
	daughter = norm*((scale.*k).^m).*exp(expnt);
	daughter = daughter.*(k > 0.);     % Heaviside step function
	fourier_factor = 4*pi/(2*m+1);
	coi = fourier_factor*sqrt(2);
	dofmin = 2;
elseif (strcmp(mother,'DOG'))  %--------------------------------  DOG
	if (param == -1), param = 2.;, end
	m = param;
	expnt = -(scale.*k).^2 ./ 2.0;
	norm = sqrt(scale*k(2)/gamma(m+0.5))*sqrt(n);
	daughter = -norm*(i^m)*((scale.*k).^m).*exp(expnt);
	fourier_factor = 2*pi*sqrt(2./(2*m+1));
	coi = fourier_factor/sqrt(2);
	dofmin = 1;
else
	error('Mother must be one of MORLET,PAUL,DOG')
end

return


function pass = test_times( pref )

% Grab some preferences
if ( nargin == 0 )
    pref = chebfunpref();
end
tol = 1e2*pref.techPrefs.chebfuneps;

f = ballfun(@(x,y,z)x.*y);
Z = ballfun(@(x,y,z)0);

% Example 1:
F = ballfunv(f,Z,Z);
G = ballfunv(Z,f,Z);
H = times(F,G);
Hexact = ballfunv(Z,Z,Z);
pass(1) = norm(H - Hexact) < tol;

% Example 2:
F = ballfunv(f,Z,Z);
G = ballfunv(-f,Z,Z);
H = times(F,G);
Hexact = ballfunv(-power(f,2),Z,Z);
pass(2) = norm(H - Hexact) < tol;

% Example 3:
F = ballfunv(f,2*f,3*f);
G = ballfunv(-2*f,f,-f);
H = times(F,G);
Hexact = ballfunv(-2*power(f,2),2*power(f,2),-3*power(f,2));
pass(3) = norm(H - Hexact) < tol;

% Example 4:
% Multiply ballfunv by ballfun
V = ballfunv(@(x,y,z)x,@(x,y,z)y,@(x,y,z)z);
f = ballfun(@(x,y,z)cos(y));
exact = ballfunv(@(x,y,z)x.*cos(y),@(x,y,z)y.*cos(y),@(x,y,z)z.*cos(y));
pass(3) = norm(V*f-exact) < tol;
pass(4) = norm(f*V-exact) < tol;

if (nargout > 0)
    pass = all(pass(:));
end
end


function result = SimulateSSCOMP_3Spaces(D, K, Ng, theta, SNR, shuffle)
% Simulates SSC-OMP algorithm
% D - Ambient space dimension
% K - Subspace dimension
% Ng - Number of vectors per subspace
% theta - angle between disjoint subspaces
% shuffle - whether to shuffle the vectors or not.

    if nargin < 6
        shuffle = false;
    end
    if nargin < 5
        SNR = Inf;
    end
    % number of subspaces = number of clusters
    ns = 3;
    % Let us form the subspaces
    [A, B, C] = spx.la.spaces.three_disjoint_spaces_at_angle(K, deg2rad(theta));
    % Put them together
    X = [A B C];
    % Put them to bigger dimension
    X = spx.la.spaces.k_dim_to_n_dim(X, D);
    % Perform a random orthonormal transformation
    O = orth(randn(D));
    X = O * X;
    % Split them again
    A = X(:, 1:K);
    B = X(:, K + (1:K));
    C = X(:, 2*K + (1:K));
    % coefficients for Ng vectors chosen randomly in subspace A
    coeffs_a = randn(K,Ng);
    % avoid small values
    coeffs_a = 2*sign(coeffs_a) + coeffs_a;
    % coefficients for Ng vectors chosen randomly in subspace B
    coeffs_b = randn(K,Ng);
    % coefficients for Ng vectors chosen randomly in subspace C
    coeffs_c = randn(K,Ng);
    % avoid small values
    coeffs_c = 2*sign(coeffs_c) + coeffs_c;
    % Actual vectors from the three subspaces
    XA = A * coeffs_a;
    XB = B * coeffs_b;
    XC = C * coeffs_c;
    % Prepare the overall set of signals
    X = [XA XB XC];
    % ground through clustering data
    true_labels = [1*ones(Ng,1) ; 2*ones(Ng,1) ; 3*ones(Ng,1)];
    % All signals are expected to  have a K-sparse representation
    if shuffle
        % We also shuffle the signals
        shuffled_indices = randperm(3*Ng);
        X = X(:, shuffled_indices);
        true_labels = true_labels(shuffled_indices);
    end
    if ~isinf(SNR)
        % We need to add some noise in the signals.
        noises = spx.data.noise.Basic.createNoise(X, SNR);
        % Preserve original data
        X0 = X;
        % Add noise
        X = X0 + noises;
    end
    % Keep data for reference.
    result.num_subspaces = 3;
    result.theta = theta;
    result.subspace_dimension = K;
    result.ambient_dimension = D;
    result.num_signals_per_subspace = Ng;
    result.num_total_signals = size(X, 2);
    result.true_labels = true_labels;
    result.X = X;

    tstart = tic;
    % Application of Sparse subspace clustering
    solver = spx.cluster.ssc.SSC_OMP(X, K, ns);
    solver.Quiet = true;
    ssc_result = solver.solve();
    elapsed_time = toc(tstart);
    result.elapsed_time = elapsed_time;
    result.C = solver.Representation;
    % fprintf('Sparse subspace clustering time spent: %.2f seconds\n', elapsed_time);

    cluster_labels = ssc_result.Labels;
    %combined_labels = [true_labels cluster_labels]';
    result.cluster_labels = cluster_labels;

    % Time to compare the clustering
    comparer = spx.cluster.ClusterComparison(true_labels, cluster_labels);
    result.comparison = comparer.fMeasure();
    % fprintf('Sparse subspace clustering results:\n');
    % comparer.printF1MeasureResult(result.comparison);
end


function [node,elem,HB] = cubemesh(box,h)
%% CUBEMESH a uniform mesh of a cube
%
% [node,elem,HB] = cubemesh([x0,x1,y0,y1,z0,z1],h) enerates a uniform mesh
% of the cube [x0,x1]*[y0,y1]*[z0,z1] with mesh size h.
%
% Example
%
%  [node,elem,HB] = cubemesh([-1,1,-1,1,-1,1],0.5);
%  showmesh3(node,elem);
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

x0 = box(1); x1 = box(2);
y0 = box(3); y1 = box(4);
z0 = box(5); z1 = box(6);
node = [x0,y0,z0; x1,y0,z0; x1,y1,z0; x0,y1,z0; ...
        x0,y0,z1; x1,y0,z1; x1,y1,z1; x0,y1,z1];
elem = [1 2 3 7; 1 4 3 7; 1 5 6 7; 1 5 8 7; 1 2 6 7; 1 4 8 7];
n = ceil(log2(abs(x1-x0)/h));
for k = 1:n
    [node,elem] = uniformrefine3(node,elem);
end
% Set this as an initial grid
N0 = size(node,1);
HB(1:N0,1:3) = repmat((1:N0)',1,3);
HB(1:N0,4) = 0;


function [hess, storedb] = getHessian(problem, x, d, storedb)
% Computes the Hessian of the cost function at x along d.
%
% function [hess, storedb] = getHessian(problem, x, d, storedb)
%
% Returns the Hessian at x along d of the cost function described in the
% problem structure. The cache database storedb is passed along, possibly
% modified and returned in the process.
%
% If an exact Hessian is not provided, an approximate Hessian is returned
% if possible, without warning. If not possible, an exception will be
% thrown. To check whether an exact Hessian is available or not (typically
% to issue a warning if not), use canGetHessian.
%
% See also: getPrecon getApproxHessian canGetHessian

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Dec. 30, 2012.
% Contributors:
% Change log:

    if isfield(problem, 'hess')
    %% Compute the Hessian using hess.

		is_octave = exist('OCTAVE_VERSION', 'builtin');
		if ~is_octave
			narg = nargin(problem.hess);
		else
			narg = 3;
		end

        % Check whether the hess function wants to deal with the store
        % structure or not.
        switch narg
            case 2
                hess = problem.hess(x, d);
            case 3
                % Obtain, pass along, and save the store structure
                % associated to this point.
                store = getStore(problem, x, storedb);
                [hess store] = problem.hess(x, d, store);
                storedb = setStore(problem, x, storedb, store);
            otherwise
                up = MException('manopt:getHessian:badhess', ...
                    'hess should accept 2 or 3 inputs.');
                throw(up);
        end

    elseif isfield(problem, 'ehess') && canGetEuclideanGradient(problem)
    %% Compute the Hessian using ehess.

        % We will need the Euclidean gradient for the conversion from the
        % Euclidean Hessian to the Riemannian Hessian.
        [egrad, storedb] = getEuclideanGradient(problem, x, storedb);

		is_octave = exist('OCTAVE_VERSION', 'builtin');
		if ~is_octave
			narg = nargin(problem.ehess);
		else
			narg = 3;
		end

        % Check whether the ehess function wants to deal with the store
        % structure or not.
        switch narg
            case 2
                ehess = problem.ehess(x, d);
            case 3
                % Obtain, pass along, and save the store structure
                % associated to this point.
                store = getStore(problem, x, storedb);
                [ehess store] = problem.ehess(x, d, store);
                storedb = setStore(problem, x, storedb, store);
            otherwise
                up = MException('manopt:getHessian:badehess', ...
                    'ehess should accept 2 or 3 inputs.');
                throw(up);
        end

        % Convert to the Riemannian Hessian
        hess = problem.M.ehess2rhess(x, egrad, ehess, d);

    else
    %% Attempt the computation of an approximation of the Hessian.

        [hess, storedb] = getApproxHessian(problem, x, d, storedb);

    end

end


function [ fea, out ] = ex_axistressstrain1( varargin )
%EX_AXISTRESSSTRAIN1 Example for hollow cylider axisymmetric stress-strain.
%
%   [ FEA, OUT ] = EX_AXISTRESSSTRAIN1( VARARGIN ) Example to calculate displacements and stresses
%   in a hollow cylinder in axisymmetric/cylindrical coordinates.
%
%   Ref. 4.1.9 Long (generalized plane strain) cylinder subjected to internal and external pressure.
%   [1] Applied Mechanics of Solids, Allan F. Bower, 2012 (http://solidmechanics.org/).
%
%   Accepts the following property/value pairs.
%
%       Input       Value/{Default}        Description
%       -----------------------------------------------------------------------------------
%       a           scalar {1.5}           Cylinder inner radius
%       b           scalar {2}             Cylinder outer radius
%       l           scalar {3}             Cylinder length
%       pa          scalar {5e3}           Inner load force
%       pb          scalar {20e4}          Outer load force
%       E           scalar {200e9}         Modulus of elasticity
%       nu          scalar {0.3}           Poissons ratio
%       igrid       scalar 0/{1}           Cell type (0=quadrilaterals, 1=triangles)
%       hmax        scalar {0.1}           Max grid cell size
%       sfun        string {sflag2}        Shape function for displacements
%       iplot       scalar 0/{1}           Plot solution (=1)
%                                                                                .
%       Output      Value/(Size)           Description
%       -----------------------------------------------------------------------------------
%       fea         struct                 Problem definition struct
%       out         struct                 Output struct

% Copyright 2013-2022 Precise Simulation, Ltd.

cOptDef = { 'a',        0.9;
            'b',        2;
            'l',        3;
            'pa',       5e3;
            'pb',       20e4;
            'E',        200e9;
            'nu',       0.3;
            'igrid',    0;
            'hmax',     0.01;
            'sfun',     'sflag2';
            'iplot',    1;
            'tol',      5e-2;
            'fid',      1 };
[got,opt] = parseopt(cOptDef,varargin{:});
fid       = opt.fid;

% Geometry and grid.
a = opt.a;
b = opt.b;
l = opt.l;
fea.geom.objects = { gobj_rectangle( a, b, 0, l, 'R1' ) };
if ( opt.igrid==1 )
  fea.grid = gridgen( fea, 'hmax', opt.hmax, 'fid', fid );
else
  fea.grid = rectgrid( ceil((b-a)/opt.hmax), ceil(l/opt.hmax), [a b;0 l] );
  if( opt.igrid<0 )
    fea.grid = quad2tri( fea.grid );
  end
end
n_bdr = max(fea.grid.b(3,:));   % Number of boundaries.

% Axisymmetric stress-strain equation definitions.
fea.sdim = { 'r', 'z' };
fea = addphys( fea, @axistressstrain );
fea.phys.css.eqn.coef{1,end} = { opt.nu };
fea.phys.css.eqn.coef{2,end} = { opt.E  };
fea.phys.css.sfun            = { opt.sfun opt.sfun };   % Set shape functions.

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

bccoef = mat2cell( zeros(2,n_bdr), [1 1], ones(1,n_bdr) );
bccoef{1,2} = opt.pb*b;
bccoef{1,4} = opt.pa*a;
fea.phys.css.bdr.coef{1,end} = bccoef;

% Solve.
fea       = parsephys( fea );
fea       = parseprob( fea );
fea.sol.u = solvestat( fea, 'icub', 1+str2num(strrep(opt.sfun,'sflag','')), 'fid', fid );

% Postprocessing.
n = 20;
r = linspace(a,b,n);
z = l/2*ones(1,n);
pa = opt.pa; pb = -opt.pb; E = opt.E; nu = opt.nu;
% 4.1.9 http://solidmechanics.org/Text/Chapter4_1/Chapter4_1.php#Sect4_1_9
u_ref  = (1+nu)*a^2*b^2/E/(b^2-a^2)*( (pa-pb)./r' + (1-2*nu)*(pa*a^2 - pb*b^2)/a^2/b^2*r' );
sr_ref = (pa*a^2-pb*b^2)/(b^2-a^2) - a^2*b^2/(b^2-a^2)./(r').^2*(pa-pb);
st_ref = (pa*a^2-pb*b^2)/(b^2-a^2) + a^2*b^2/(b^2-a^2)./(r').^2*(pa-pb);
sz_ref = 2*nu*(pa*a^2-pb*b^2)/(b^2-a^2);
u  = evalexpr( fea.phys.css.eqn.vars{3,2}, [r;z], fea );
w  = evalexpr( fea.phys.css.eqn.vars{4,2}, [r;z], fea );
sr = evalexpr( fea.phys.css.eqn.vars{5,2}, [r;z], fea );
st = evalexpr( fea.phys.css.eqn.vars{6,2}, [r;z], fea );
sz = evalexpr( fea.phys.css.eqn.vars{7,2}, [r;z], fea );
if( opt.iplot>0 )
  subplot(1,2,1)
  postplot( fea, 'surfexpr', fea.phys.css.eqn.vars{3,2} )
  title('r-displacement')
  subplot(1,2,2), hold on
  plot(u_ref,r,'r-')
  plot(u,r,'b.')
  legend('exact solution','computed solution')
  xlabel('r')
  grid on
end

% Error checking.
out.erru  = norm( u_ref - u )/norm( u_ref );
out.errw  = norm( w );
out.errsr = norm( sr_ref - sr )/norm( sr_ref );
out.errst = norm( st_ref - st )/norm( st_ref );
out.errsz = norm( sz_ref - sz )/norm( sz_ref );
out.err   = [ out.erru, out.errw, out.errsr, out.errst, out.errsz ];
out.pass  = all(out.err < opt.tol);

if( nargout==0 )
  clear fea out
end


function varargout = transformVector3d(varargin)
%TRANSFORMVECTOR3D Transform a vector with a 3D affine transform.
%
%   V2 = transformVector3d(V1, TRANS);
%   Computes the vector obtained by transforming vector V1 with affine
%   transform TRANS.
%   V1 has the form [x1 y1 z1], and TRANS is a [3x3], [3x4], or [4x4]
%   matrix, with one of the forms:
%   [a b c]   ,   [a b c j] , or [a b c j]
%   [d e f]       [d e f k]      [d e f k]
%   [g h i]       [g h i l]      [g h i l]
%                                [0 0 0 1]
%
%   V2 = transformVector3d(V1, TRANS) also works when V1 is a [Nx3xMxEtc]
%   array of double. In this case, V2 has the same size as V1.
%
%   V2 = transformVector3d(X1, Y1, Z1, TRANS);
%   Specifies vectors coordinates in three arrays with same size.
%
%   [X2 Y2 Z2] = transformVector3d(...);
%   Returns the coordinates of the transformed vector separately.
%
%
%   See also
%   vectors3d, transforms3d, transformPoint3d

% ------
% Author: David Legland
% E-mail: david.legland@inrae.fr
% Created: 2008-11-25, from transformPoint3d
% Copyright 2008-2022 INRA - TPV URPOI - BIA IMASTE

if nargin~=2 && nargin~=4
    error('Invalid number of input arguments. Type ''help transformVector3d'' for details.');
end

% Extract only the linear part of the affine transform
trans = varargin{end};
trans(1:4,4) = [0; 0; 0; 1];

% Call transformPoint3d using equivalent output arguments
varargout = cell(1, max(1,nargout));
[varargout{:}] = transformPoint3d(varargin{1:end-1}, trans);


% Studies the recovery probability of different algorithms
% at a fixed sparsity level and varying number of signals.

close all;
clear all;
clc;
rng('default');
% Create the directory for storing images
[status_code,message,message_id] = mkdir('bin');
% Signal space
N = 256;
% Number of measurements
M = 48;
% Number of signals
Ss = 1:32;
% Sparsity level
K = 16;
num_trials = 100;
num_ss = length(Ss);
success_with_s.somp = zeros(num_ss, 1);
success_with_s.ra_omp = zeros(num_ss, 1);
success_with_s.ra_ormp = zeros(num_ss, 1);
success_with_s.cosamp_mmv = zeros(num_ss, 1);
success_with_s.ra_cosamp = zeros(num_ss, 1);

snr_threshold = 100;

for ns=1:num_ss
    % Current number of signals
    S = Ss(ns);
    num_successes.somp = 0;
    num_successes.ra_omp = 0;
    num_successes.ra_ormp = 0;
    num_successes.cosamp_mmv = 0;
    num_successes.ra_cosamp = 0;
    for nt=1:num_trials
        % Sensing matrix
        Phi = spx.dict.simple.gaussian_dict(M, N);
        % Sparse signal generator
        gen  = spx.data.synthetic.SparseSignalGenerator(N, K, S);
        % Gaussian distributed non-zero samples
        X = gen.gaussian;
        % Measurement vectors
        Y = Phi * X;

        % Create the solver for simultaneous orthogonal matching pursuit
        solver = spx.pursuit.joint.OrthogonalMatchingPursuit(Phi, K, 2);
        result = solver.solve(Y);
        % Solution vectors
        X_Rec = result.Z;
        % Comparison
        cs = spx.commons.SparseSignalsComparison(X, X_Rec, K);
        % Reconstruction SNR
        snr = cs.cum_signal_to_noise_ratio;
        success = snr > snr_threshold;
        num_successes.somp = num_successes.somp + success;

        % Create the solver for rank aware orthogonal matching pursuit
        solver = spx.pursuit.joint.RankAwareOMP(Phi, K);
        result = solver.solve(Y);
        % Solution vectors
        X_Rec = result.Z;
        % Comparison
        cs = spx.commons.SparseSignalsComparison(X, X_Rec, K);
        % Reconstruction SNR
        snr = cs.cum_signal_to_noise_ratio;
        success = snr > snr_threshold;
        num_successes.ra_omp = num_successes.ra_omp + success;

        % Create the solver for rank aware order recursive matching pursuit
        solver = spx.pursuit.joint.RankAwareORMP(Phi, K);
        result = solver.solve(Y);
        % Solution vectors
        X_Rec = result.Z;
        % Comparison
        cs = spx.commons.SparseSignalsComparison(X, X_Rec, K);
        % Reconstruction SNR
        snr = cs.cum_signal_to_noise_ratio;
        success = snr > snr_threshold;
        num_successes.ra_ormp = num_successes.ra_ormp + success;

        % Create the solver for CoSaMP MMV
        solver = spx.pursuit.joint.CoSaMP(Phi, K);
        result = solver.solve(Y);
        % Solution vectors
        X_Rec = result.Z;
        % Comparison
        cs = spx.commons.SparseSignalsComparison(X, X_Rec, K);
        % Reconstruction SNR
        snr = cs.cum_signal_to_noise_ratio;
        success = snr > snr_threshold;
        num_successes.cosamp_mmv = num_successes.cosamp_mmv + success;

        % Create the solver for Rank Aware CoSaMP MMV
        solver = spx.pursuit.joint.CoSaMP(Phi, K);
        solver.RankAwareResidual = true;
        result = solver.solve(Y);
        % Solution vectors
        X_Rec = result.Z;
        % Comparison
        cs = spx.commons.SparseSignalsComparison(X, X_Rec, K);
        % Reconstruction SNR
        snr = cs.cum_signal_to_noise_ratio;
        success = snr > snr_threshold;
        num_successes.ra_cosamp = num_successes.ra_cosamp + success;

        fprintf('S: %d, K=%d, trial=%d\n', S, K, nt);
    end
    success_with_s.somp(ns) = num_successes.somp / num_trials;
    success_with_s.ra_omp(ns) = num_successes.ra_omp / num_trials;
    success_with_s.ra_ormp(ns) = num_successes.ra_ormp / num_trials;
    success_with_s.cosamp_mmv(ns) = num_successes.cosamp_mmv / num_trials;
    success_with_s.ra_cosamp(ns) = num_successes.ra_cosamp / num_trials;
end

save ('bin/success_with_s_comparison.mat');


function y = lnCumGaussian(x)

% LNCUMGAUSSIAN log cumulative distribution for the normalised Gaussian.
% FORMAT
% DESC computes the logarithm of the cumulative Gaussian
% distribution.
% ARG X : input position.
% RETURN y : log probability of the value under the cumulative
% Gaussian.
%
% SEEALSO : erf, erfcx, cumGaussian, lnDiffCumGaussian, gaussOverDiffCumGaussian
%
% COPYRIGHT : Neil D. Lawrence, 2004, 2005, 2006

% NDLUTIL

index = find(x< 0);
if length(index)
  y(index) = -.5*x(index).*x(index) + log(.5) + log(erfcx(-sqrt(2)/2* ...
                                                    x(index)));
end
index = find(x>=0);
if length(index)
  y(index) = log(cumGaussian(x(index)));
end
y=reshape(y, size(x));


function value = hexagon_contains_point_2d ( v, p )

%*****************************************************************************80
%
%% HEXAGON_CONTAINS_POINT_2D finds if a point is inside a hexagon in 2D.
%
%  Discussion:
%
%    This test is only valid if the hexagon is convex.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 April 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real V(2,6), the vertics, in counterclockwise order.
%
%    Input, real P(2), the point to be tested.
%
%    Output, logical VALUE, is TRUE if X is in the hexagon.
%
  n = 6;
%
%  A point is inside a convex hexagon if and only if it is "inside"
%  each of the 6 halfplanes defined by lines through consecutive
%  vertices.
%
  for i = 1 : n

    j = mod ( i, n ) + 1;

    if (  v(1,i) * ( v(2,j) - p(2  ) ) ...
        + v(1,j) * ( p(2  ) - v(2,i) ) ...
        + p(1  ) * ( v(2,i) - v(2,j) ) < 0.0 )

      value = 0;
      return

    end

  end

  value = 1;

  return
end


classdef ContourFitting < handle
    %CONTOURFITTING  Contour Fitting algorithm using Fourier descriptors
    %
    % Contour fitting matches two contours `z_a` and `z_b` minimizing distance
    % `d(z_a, z_b) = sum_n (a_n - s * b_n * exp(j * (n * alpha + phi)))^2`
    % where `a_n` and `b_n` are Fourier descriptors of `z_a` and `z_b` and `s`
    % is a scaling factor and `phi` is angle rotation and `alpha` is starting
    % point factor adjustement.
    %
    % ## References:
    % [PersoonFu1977]:
    % > E Persoon and King-Sun Fu. "Shape discrimination using fourier
    % > descriptors". IEEE Transactions on Pattern Analysis and Machine
    % > Intelligence, 7(3):170-179, 1977.
    %
    % [BergerRaghunathan1998]:
    % > L Berger, V A Raghunathan, C Launay, D Ausserre, and Y Gallot.
    % > "Coalescence in 2 dimensions: experiments on thin copolymer films and
    % > numerical simulations". The European Physical Journal B - Condensed
    % > Matter and Complex Systems, 2(1):93-99, 1998.
    %
    % See also: cv.ContourFitting.ContourFitting, cv.matchShapes,
    %  cv.ShapeContextDistanceExtractor
    %

    properties (SetAccess = private)
        % Object ID
        id
    end

    properties (Dependent)
        % number of Fourier descriptors used in
        % cv.ContourFitting.estimateTransformation equal to number of contour
        % points after resampling.
        CtrSize
        % number of Fourier descriptors used for optimal curve matching in
        % cv.ContourFitting.estimateTransformation when using vector of points
        FDSize
    end

    %% ContourFitting
    methods
        function this = ContourFitting(varargin)
            %CONTOURFITTING  Create ContourFitting object
            %
            %     obj = cv.ContourFitting()
            %     obj = cv.ContourFitting('OptionName',optionValue, ...)
            %
            % ## Options
            % * __CtrSize__ number of contour points after resampling.
            %   default 1024
            % * __FDSize__ number of Fourier descriptors. default 16
            %
            % See also: cv.ContourFitting.estimateTransformation
            %
            this.id = ContourFitting_(0, 'new', varargin{:});
        end

        function delete(this)
            %DELETE  Destructor
            %
            %     obj.delete()
            %
            % See also: cv.ContourFitting
            %
            if isempty(this.id), return; end
            ContourFitting_(this.id, 'delete');
        end

        function [alphaPhiST, d] = estimateTransformation(this, src, ref, varargin)
            %ESTIMATETRANSFORMATION  Fits two closed curves using Fourier descriptors
            %
            %     [alphaPhiST, d] = obj.estimateTransformation(src, ref)
            %     [...] = obj.estimateTransformation(..., 'OptionName',optionValue, ...)
            %
            % ## Input
            % * __src__ Contour defining first shape (source), or Fourier
            %   descriptors if `FD` is true.
            % * __ref__ Contour defining second shape (target), or Fourier
            %   descriptors if `FD` is true.
            %
            % ## Output
            % * __alphaPhiST__ transformation as a 5-elements vector
            %   `[alpha, phi, s, Tx, Ty]`, where:
            %   * __alpha__ starting point factor adjustement
            %   * __phi__ angle rotation in radian
            %   * __s__ scaling factor
            %   * __Tx__, __Ty__ the translation
            % * __d__ distance between `src` and `ref` after matching.
            %
            % ## Options
            % * __FD__ If false then `src` and `ref` are contours, and
            %   if true `src` and `ref` are Fourier descriptors. default false
            %
            % When `FD` is false, it applies cv.ContourFitting.contourSampling
            % and cv.ContourFitting.fourierDescriptor to compute Fourier
            % descriptors.
            %
            % More details in [PersoonFu1977] and [BergerRaghunathan1998].
            %
            % See also: cv.ContourFitting.transformFD,
            %  cv.ContourFitting.contourSampling,
            %  cv.ContourFitting.fourierDescriptor
            %
            [alphaPhiST, d] = ContourFitting_(this.id, 'estimateTransformation', src, ref, varargin{:});
        end
    end

    %% Algorithm
    methods (Hidden)
        function clear(this)
            %CLEAR  Clears the algorithm state
            %
            %     obj.clear()
            %
            % See also: cv.ContourFitting.empty, cv.ContourFitting.load
            %
            ContourFitting_(this.id, 'clear');
        end

        function b = empty(this)
            %EMPTY  Checks if algorithm object is empty
            %
            %     b = obj.empty()
            %
            % ## Output
            % * __b__ Returns true if the algorithm object is empty
            %   (e.g. in the very beginning or after unsuccessful read).
            %
            % See also: cv.ContourFitting.clear, cv.ContourFitting.load
            %
            b = ContourFitting_(this.id, 'empty');
        end

        function save(this, filename)
            %SAVE  Saves the algorithm parameters to a file
            %
            %     obj.save(filename)
            %
            % ## Input
            % * __filename__ Name of the file to save to.
            %
            % This method stores the algorithm parameters in the specified
            % XML or YAML file.
            %
            % See also: cv.ContourFitting.load
            %
            ContourFitting_(this.id, 'save', filename);
        end

        function load(this, fname_or_str, varargin)
            %LOAD  Loads algorithm from a file or a string
            %
            %     obj.load(fname)
            %     obj.load(str, 'FromString',true)
            %     obj.load(..., 'OptionName',optionValue, ...)
            %
            % ## Input
            % * __fname__ Name of the file to read.
            % * __str__ String containing the serialized model you want to
            %   load.
            %
            % ## Options
            % * __ObjName__ The optional name of the node to read (if empty,
            %   the first top-level node will be used). default empty
            % * __FromString__ Logical flag to indicate whether the input is a
            %   filename or a string containing the serialized model.
            %   default false
            %
            % This method reads algorithm parameters from the specified XML or
            % YAML file (either from disk or serialized string). The previous
            % algorithm state is discarded.
            %
            % See also: cv.ContourFitting.save
            %
            ContourFitting_(this.id, 'load', fname_or_str, varargin{:});
        end

        function name = getDefaultName(this)
            %GETDEFAULTNAME  Returns the algorithm string identifier
            %
            %     name = obj.getDefaultName()
            %
            % ## Output
            % * __name__ This string is used as top level XML/YML node tag
            %   when the object is saved to a file or string.
            %
            % See also: cv.ContourFitting.save, cv.ContourFitting.load
            %
            name = ContourFitting_(this.id, 'getDefaultName');
        end
    end

    %% Getters/Setters
    methods
        function value = get.CtrSize(this)
            value = ContourFitting_(this.id, 'get', 'CtrSize');
        end
        function set.CtrSize(this, value)
            ContourFitting_(this.id, 'set', 'CtrSize', value);
        end

        function value = get.FDSize(this)
            value = ContourFitting_(this.id, 'get', 'FDSize');
        end
        function set.FDSize(this, value)
            ContourFitting_(this.id, 'set', 'FDSize', value);
        end
    end

    %% Static functions
    methods (Static)
        function out = contourSampling(src, numElt)
            %CONTOURSAMPLING  Contour sampling
            %
            %     out = cv.ContourFitting.contourSampling(src, numElt)
            %
            % ## Input
            % * __src__ input contour, vector of 2D points stored in numeric
            %   array Nx2/Nx1x2/1xNx2 or cell array of 2-element vectors
            %   `{[x,y], ...}`.
            % * __NumElt__ number of points in `out` contour.
            %
            % ## Output
            % * __out__ output contour with `numElt` points.
            %
            % See also: cv.findContours, cv.approxPolyDP
            %
            out = ContourFitting_(0, 'contourSampling', src, numElt);
        end

        function dst = fourierDescriptor(src, varargin)
            %FOURIERDESCRIPTOR  Fourier descriptors for planed closed curves
            %
            %     dst = cv.ContourFitting.fourierDescriptor(src)
            %     dst = cv.ContourFitting.fourierDescriptor(src, 'OptionName',optionValue, ...)
            %
            % ## Input
            % * __src__ input contour, vector of 2D points stored in numeric
            %   array Nx2/Nx1x2/1xNx2 or cell array of 2-element vectors
            %   `{[x,y], ...}`.
            %
            % ## Output
            % * __dst__ 2-channel array of type `single` and length `NumElt`.
            %
            % ## Options
            % * __NumElt__ number of rows in `dst` or cv.getOptimalDFTSize
            %   rows if `NumElt=-1`. default -1
            % * __NumFD__ number of FD to return in `dst`,
            %   `dst = [FD(1...NumFD/2) FD(NumFD/2-NumElt+1...:NumElt)]`.
            %   default -1 (return all of FD as is).
            %
            % For more details about this implementation, please see
            % [PersoonFu1977].
            %
            % See also: cv.dft
            %
            dst = ContourFitting_(0, 'fourierDescriptor', src, varargin{:});
        end

        function dst = transformFD(src, t, varargin)
            %TRANSFORMFD  Transform a contour
            %
            %     dst = cv.ContourFitting.transformFD(src, t)
            %     dst = cv.ContourFitting.transformFD(src, t, 'OptionName',optionValue, ...)
            %
            % ## Input
            % * __src__ contour, or Fourier descriptors if `FD` is true.
            % * __t__ 1x5 transform matrix given by
            %   cv.ContourFitting.estimateTransformation method.
            %
            % ## Output
            % * __dst__ 2-channel matrix of type `double` and `NumElt` rows.
            %
            % ## Options
            % * __FD__ if true `src` are Fourier descriptors, if false `src`
            %   is a contour. default true
            %
            % See also: cv.ContourFitting.estimateTransformation,
            %  cv.ContourFitting.contourSampling,
            %  cv.ContourFitting.fourierDescriptor
            %
            dst = ContourFitting_(0, 'transformFD', src, t, varargin{:});
        end
    end

end


function [ a, seed ] = wathen_gb ( nx, ny, n, seed )

%*****************************************************************************80
%
%% WATHEN_GB returns the Wathen matrix, using general banded (GB) storage.
%
%  Discussion:
%
%    The Wathen matrix is a finite element matrix which is sparse.
%
%    The entries of the matrix depend in part on a physical quantity
%    related to density.  That density is here assigned random values between
%    0 and 100.
%
%    The matrix order N is determined by the input quantities NX and NY,
%    which would usually be the number of elements in the X and Y directions.
%    The value of N is
%
%      N = 3*NX*NY + 2*NX + 2*NY + 1,
%
%    The matrix is the consistent mass matrix for a regular NX by NY grid
%    of 8 node serendipity elements.
%
%    The local element numbering is
%
%      3--2--1
%      |     |
%      4     8
%      |     |
%      5--6--7
%
%    Here is an illustration for NX = 3, NY = 2:
%
%     23-24-25-26-27-28-29
%      |     |     |     |
%     19    20    21    22
%      |     |     |     |
%     12-13-14-15-16-17-18
%      |     |     |     |
%      8     9    10    11
%      |     |     |     |
%      1--2--3--4--5--6--7
%
%    For this example, the total number of nodes is, as expected,
%
%      N = 3 * 3 * 2 + 2 * 2 + 2 * 3 + 1 = 29
%
%    The matrix is symmetric positive definite for any positive values of the
%    density RHO(X,Y).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 July 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Nicholas Higham,
%    Algorithm 694: A Collection of Test Matrices in MATLAB,
%    ACM Transactions on Mathematical Software,
%    Volume 17, Number 3, September 1991, pages 289-305.
%
%    Andrew Wathen,
%    Realistic eigenvalue bounds for the Galerkin mass matrix,
%    IMA Journal of Numerical Analysis,
%    Volume 7, Number 4, October 1987, pages 449-457.
%
%  Parameters:
%
%    Input, integer NX, NY, values which determine the size of the matrix.
%
%    Input, integer N, the number of variables.
%
%    Input/output, integer SEED, the random number seed.
%
%    Output, real A(9*NX+13,N), the matrix.
%
  if ( nargin < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'WATHEN_GB - Fatal error!\n' );
    fprintf ( 1, '  Not enough input.\n' );
    error ( 'WATHEN_GB - Fatal error!' );
  end

  if ( nargin < 2 )
    ny = nx;
    fprintf ( 1, '\n' );
    fprintf ( 1, '  NY was not supplied.  Setting NY = NX = %d.\n', ny );
  end

  if ( nargin < 3 )
    n = wathen_order ( nx, ny );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  N was not supplied.  N = %d\n', n );
  end

  if ( nargin < 4 )
    seed = 123456789;
  end

  ml = 3 * nx + 4;
  mu = 3 * nx + 4;

  a = zeros ( 2 * ml + mu + 1, n );

  em = [
     6.0, -6.0,  2.0, -8.0,  3.0, -8.0,  2.0, -6.0;
    -6.0, 32.0, -6.0, 20.0, -8.0, 16.0, -8.0, 20.0;
     2.0, -6.0,  6.0, -6.0,  2.0, -8.0,  3.0, -8.0;
    -8.0, 20.0, -6.0, 32.0, -6.0, 20.0, -8.0, 16.0;
     3.0, -8.0,  2.0, -6.0,  6.0, -6.0,  2.0, -8.0;
    -8.0, 16.0, -8.0, 20.0, -6.0, 32.0, -6.0, 20.0;
     2.0, -8.0,  3.0, -8.0,  2.0, -6.0,  6.0, -6.0;
    -6.0, 20.0, -8.0, 16.0, -8.0, 20.0, -6.0, 32.0 ]';

  node = zeros(8,1);

  for j = 1 : ny

    for i = 1 : nx
%
%  For the element (I,J), determine the indices of the 8 nodes.
%
      node(1) = 3 * j * nx + 2 * j + 2 * i + 1;
      node(2) = node(1) - 1;
      node(3) = node(1) - 2;

      node(4) = ( 3 * j - 1 ) * nx + 2 * j + i - 1;
      node(8) = node(4) + 1;

      node(5) = ( 3 * j - 3 ) * nx + 2 * j + 2 * i - 3;
      node(6) = node(5) + 1;
      node(7) = node(5) + 2;

      [ rho, seed ] = r8_uniform_01 ( seed );
      rho = 100.0 * rho;

      for krow = 1 : 8
        for kcol = 1 : 8
          ii = node(krow);
          jj = node(kcol);
          a(ii-jj+ml+mu+1,jj) = a(ii-jj+ml+mu+1,jj) + rho * em(krow,kcol);
        end
      end

    end
  end

  return
end