function m = medianw( x, w, dim )
% Fast weighted median.
%
% Computes the weighted median of a set of samples.
%  http://en.wikipedia.org/wiki/Weighted_median
% "A weighted median of a sample is the 50% weighted percentile."
% For matrices computes median along each column (or dimension dim).
% If all weights are equal to 1 gives identical results to median.
%
% USAGE
%  m = medianw( x, w, [dim] )
%
% INPUTS
%  x      - vector or array of samples
%  w      - vector or array of weights
%  dim    - dimension along which to compute median
%
% OUTPUTS
%  m      - weighted median value of x
%
% EXAMPLE - simple toy example
%  x=[1 2 3]; w=[1 1 5]; medianw(x,w)
%
% EXAMPLE - comparison to median
%  n=randi(100); m=randi(100);
%  x=rand(n,m); w=ones(n,m);
%  m1=median(x); m2=medianw(x,w);
%  assert(isequal(m1,m2))
%
% See also median
%
% Piotr's Image&Video Toolbox      Version 3.24
% Copyright 2013 Piotr Dollar.  [pdollar-at-caltech.edu]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the Simplified BSD License [see external/bsd.txt]

if(nargin<3), dim=find(size(x)~=1,1); end
d=dim; nd=ndims(x); n=numel(x);

if( n==1 || size(x,d)==1 )
  m=x;
elseif( length(x)==n )
  [x,o]=sort(x); w=w(o); w=cumsum(w);
  w=w/w(end); [~,j]=min(w<=.5);
  if(j==1 || w(j-1)~=.5), m=x(j);
  else m=(x(j-1)+x(j))/2; end
else
  if(d>1), p=[d 1:d-1 d+1:nd]; x=permute(x,p); w=permute(w,p); end
  [x,o]=sort(x); w=w(o); w=cumsum(w); is={':'}; is=is(ones(1,nd-1));
  w=bsxfun(@rdivide,w,w(end,is{:})); [~,j]=min(w<=.5);
  s=size(x); s=reshape(((1:n/s(1))-1)*s(1),size(j));
  j0=max(1,j-1); j0=j0+s; j=j+s;
  same=w(j0)~=.5; j0(same)=j(same); m=(x(j0)+x(j))/2;
  if(d>1), p=[2:d 1 d+1:nd]; m=permute(m,p); end
end

end


function [ n_data, x, bip ] = airy_bi_prime_values ( n_data )

%*****************************************************************************80
%
%% AIRY_BI_PRIME_VALUES returns some values of the Airy function Bi'(x).
%
%  Discussion:
%
%    The Airy functions Ai(X) and Bi(X) are a pair of linearly independent
%    solutions of the differential equation:
%
%      W'' - X * W = 0
%
%    In Mathematica, the function can be evaluated by:
%
%      AiryBiPrime[x]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 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.
%
%  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 X, the argument of the function.
%
%    Output, real BIP, the derivative of the Airy BI function.
%
  n_max = 11;

  bip_vec = [ ...
     0.4482883573538264E+00, ...
     0.4515126311496465E+00, ...
     0.4617892843621509E+00, ...
     0.4800490287524480E+00, ...
     0.5072816760506224E+00, ...
     0.5445725641405923E+00, ...
     0.5931444786342857E+00, ...
     0.6544059191721400E+00, ...
     0.7300069016152518E+00, ...
     0.8219038903072090E+00, ...
     0.9324359333927756E+00 ];

  x_vec = [ ...
     0.0E+00, ...
     0.1E+00, ...
     0.2E+00, ...
     0.3E+00, ...
     0.4E+00, ...
     0.5E+00, ...
     0.6E+00, ...
     0.7E+00, ...
     0.8E+00, ...
     0.9E+00, ...
     1.0E+00 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    x = 0.0;
    bip = 0.0;
  else
    x = x_vec(n_data);
    bip = bip_vec(n_data);
  end

  return
end


function[varargout]=cellgrid(varargin)
%CELLGRID  Interpolate a cell array of numeric arrays onto a regular grid.
%
%   [TO,Y]=CELLGRID(T,X,DT) where T is a cell array of time arrays, and
%   X is of the same size as T, linearly interpolates the elements of X
%   within T at times TO, which are regularly spaced with interval DT.
%
%   DT may either be a scalar, or an array of the same length as T and X.
%
%   CELLGRID does not modify bad data points, such as those marked with
%   NaNs.  See CELLFILL to interpolate over bad data points.
%
%   As an example, with T and X given by
%
%       T{1} = [1 2 3  5  6]';   T{2}=[3 7  9 10]';
%       X{1} = [2 4 6 10 12]';   X{2}=[5 9 11 12]';
%
%   [TO,Y]=CELLGRID(T,X,1) will return
%
%       TO{1} = [1 2 3 4  5  6]';   TO{2} = [3 4 5 6 7 8  9  10]';
%       Y{1}  = [2 4 6 8 10 12]';   Y{2}  = [5 6 7 8 9 10 11 12]';
%
%   By default, CELLGRID uses INTERP1 with the 'pchip' method of
%   interpolation.  CELLGRID(...,STR) instead uses the method specified by
%   STR, e.g. STR='linear'.  See INTERP1 for details.
%
%   [TO,Y1,Y2,...YN]=CELLGRID(T,X1,X2,...XN,DT) with multiple input
%   arguments also works provided the XN are all the same size.
%
%   CELLGRID(X1,X2,...XN); with no output arguments overwrites the
%   original input variables.
%   __________________________________________________________________
%
%   Specifying interpolated times
%
%   [TO,Y]=CELLGRID(T,X,DT,A,B) will set TO to TO=A:DT:B, where A and B
%   are either scalars or an arrays of the same size as X.
%
%   The default behavior is equivalent to choosing A and B as the first and
%   last elements of TO, that is, to setting A=CELLMIN(T) and B=CELLMAX(T).
%   __________________________________________________________________
%
%   Parallelization
%
%   CELLGRID(...,'parallel') parallelizes the computation using a PARFOR
%   loop over the various input variables.  This requires that Matlab's
%   Parallel Computing Toolbox be installed.
%   __________________________________________________________________
%
%   'cellgrid --t' runs a test.
%
%   Usage: [to,y]=cellgrid(t,x,dt);
%          [to,y1,y2,y3]=cellgrid(t,x1,x2,x3,dt);
%          [to,y1,y2,y3]=cellgrid(t,x1,x2,x3,dt,a,b);
%          cellgrid(t,x1,x2,x3,dt);
%   __________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2015--2019 J.M. Lilly --- type 'help jlab_license' for details

if strcmp(varargin{1}, '--t')
    cellgrid_test,return
end

str='pchip';
cores='serial';

for i=1:2
    if ischar(varargin{end})
        if strcmpi(varargin{end}(1:3),'ser')||strcmpi(varargin{end}(1:3),'par')
            cores=varargin{end};
        else
            str=varargin{end};
        end
        varargin=varargin(1:end-1);
    end
end

if strcmpi(cores(1:3),'par')
    if exist('parpool')~=2
        disp('Sorry, parallel algorithm requires the Parallel Computing Toolbox.')
        disp('Defaulting to the serial algorithm.')
        str='serial';
    end
end

t=varargin{1};

if ~iscell(varargin{end-2})&&~ischar(varargin{end-2})
    a=varargin{end-1};
    b=varargin{end};
    if length(a)==1
        a=a+zeros(size(t));
    end
    if length(b)==1
        b=b+zeros(size(t));
    end
    varargin=varargin(1:end-2);
else
    a=cellmin(t);
    b=cellmax(t);
end

dt=varargin{end};

varargin=varargin(1:end-1);

%Size check
for i=2:length(varargin)
    if size(varargin{i})~=size(varargin{1});
        error('All input arguments must be the same size.')
    end
end

if length(dt)==1
    dt=dt+0*a;
end

for i=1:length(t)
   to{i,1}=[a(i):dt(i):b(i)]';
end

if strcmpi(cores(1:3),'par')
    disp('CELLGRID employing parallel algorithm.')
    parfor j=2:length(varargin)
        for i=1:length(t)
            if length(t{i})>1
                bool=~isnan(t{i})&~isnan(varargin{j}{i});
                if length(find(bool))~=0
                    args{j}{i,1}=interp1(t{i}(bool),varargin{j}{i}(bool),to{i},str);
                else
                    args{j}{i,1}=nan*to{i};
                end
            else
                args{j}{i,1}=varargin{j}{i};
            end
        end
    end
else
    for j=2:length(varargin)
        for i=1:length(t)
            if length(t{i})>1
                bool=~isnan(t{i})&~isnan(varargin{j}{i});
                if length(find(bool))>=2
                    %i,j,length(find(bool))
                    args{j}{i,1}=interp1(t{i}(bool),varargin{j}{i}(bool),to{i},str);
                else
                    args{j}{i,1}=nan*to{i};
                end
            else
                args{j}{i,1}=varargin{j}{i};
            end
        end
    end
end

varargout=args;
if ~isempty(varargout{2})
    for i=1:length(to)
        if isempty(varargout{2}{i})
            to{i}=[];
        end
    end
else
    to=[];
end
varargout{1}=to;

eval(to_overwrite(length(varargin)))

function[]=cellgrid_test

t{1}=[1 2 3  5  6]';
t{2}=[3 7  9 10]';
x{1}=[2 4 6 10 12]';
x{2}=[5 9 11 12]';

to{1}=[1 2 3 4  5  6]';
to{2}=[3 4 5 6 7 8  9  10]';
y{1}=[2 4 6 8 10 12]';
y{2}=[5 6 7 8 9 10 11 12]';

cellgrid(t,x,1);

reporttest('CELLGRID',aresame(x,y)&&aresame(to,t))


function [flowval cut R F] = max_flow(A,u,v,varargin)
% MAX_FLOW Compute the max flow on A from u to v.
%
% flowval=max_flow(A,u,v) computes the maximum flow on the network defined by
% the adjacency structure A, with source u and sink v.
%
% [flowval cut R F] = max_flow(A,u,v) returns the maximum flow in the
% network A with source u and sink v as well as additional information.
% For each vertex on the source side of the mincut, mincut(i) = 1,
% for each vertex on the sink side, mincut(i) = -1.
% R is the residual graph.  R(i,j) is the amount of unused capacity
% on edge (i,j).  F is the flow graph, F(i,j) is the amount of used
% capacity on edge (i,j).  F, A, and R satisfy the relationship A = F + R.
%
% The optional parameter algname specifies the algorithm used to compute
% the maximum flow.  For reference, the push relabel method is likely the
% best general purpose algorithm.  The Edmunds-Karp algorithm
%
% ... = max_flow(A,...) takes a set of
% key-value pairs or an options structure.  See set_matlab_bgl_options
% for the standard options.
%   options.algname: the max flow/min cut algorithm
%     [{'push_relabel'} | 'edmunds_karp' | 'kolmogorov']
%   options.fix_diag: remove any diagonal entries [0 | {1}]
%
% Note: the values on A are interpreted as integers, please round them
% yourself to get the best interpretation.  The code uses the floor of
% the values in A.
%
% Example:
%    load('graphs/max_flow_example.mat')
%    max_flow(A,1,8)

% David Gleich
% Copyright, Stanford University, 2006-2008

%% History
%  2006-04-16: Initial version
%  2006-05-31: Added full2sparse check
%  2007-07-08: Added additional algname
%    Fixed transpose option to implement the pretranspose
%    Fixed documentation bug
%  2007-07-09: Added non-negative edge capacities check
%  2008-09-23: Fixed "check" changing the input (Bug #273796)
%  2008-10-07: Changed options parsing
%    Added fix_diag option
%%

[trans check full2sparse] = get_matlab_bgl_options(varargin{:});
if full2sparse && ~issparse(A), A = sparse(A); end

options = struct('algname', 'push_relabel','fix_diag',1);
options = merge_options(options, varargin{:});

% no negative capacities and no diagonal entries allowed
if options.fix_diag, A = A - diag(diag(A)); end
if check, check_matlab_bgl(A,struct('noneg',1,'nodiag',1)); end

% max_flow will transpose the data inside

% but ~trans means the input is already transposed, so pre-transpose
if ~trans, A = A'; end

n = size(A,1);

if nargout == 2
    [flowval cut] = max_flow_mex(A,u,v,lower(options.algname));
elseif nargout >= 3
    [flowval cut ri rj rv] = max_flow_mex(A,u,v,lower(options.algname));

    % If anyone needs this operation to be more efficient, send me email,
    % and I can make max_flow_mex return this more efficiently.
    R = sparse(ri,rj,rv,n,n);
    if ~trans
        R = R';
    end
else
    flowval = max_flow_mex(A,u,v,lower(options.algname));
end

if nargout >= 4
    F = A - R;
end


function [E, Emin, Emax, Es] = grbm_energy (x, W, vbias, hbias, sigmas)

n_samples = size(x,1);

Wh = ones(n_samples,1)*hbias' + bsxfun(@rdivide, x, sigmas.^2) * W;

Es = bsxfun(@minus,x,vbias');
Es = sum(bsxfun(@rdivide, Es.^2, sigmas.^2),2)/2;

Wh_plus = max(Wh, 0);
Es = Es - sum(log(exp(-Wh_plus)+exp(Wh-Wh_plus))+Wh_plus,2);
Es = Es';

Emin = min(Es);
Emax = max(Es);
E = mean(Es);

end


function [pro, res] = transferRT0toCR3(node,elem,fixedface)
% Reference: https://arxiv.org/abs/2102.03396
%
%
% See also transferRT0toCR

% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

%% Data Structure and Basis for RT0
[elem2face,face,dofSign] = dof3face(elem);
Dlambda = gradbasis3(node,elem);
NF = size(face,1);
locFace = [2 3 4; 1 4 3; 1 2 4; 1 3 2];

%% Assembling
T = sparse(3*NF,NF);
for i = 1:4
    for j = 1:4
        ii = double(elem2face(:,i));
		jj = double(elem2face(:,j));
        j1 = locFace(j,1); j2 = locFace(j,2);  j3 = locFace(j,3);
        % [j1,j2] is the edge opposite to vertex j.
%         sT = (1/2*(i~=j1)*Clambda(:,:,j2) - 1/2*(i~=j2)*Clambda(:,:,j1))...
%              .*repmat(double(dofSign(:,j)),1,2);      % phi_j evaluates at m_i.
        sT = -(Dlambda(:,:,j1) + Dlambda(:,:,j2) + Dlambda(:,:,j3))...
             .*repmat(double(dofSign(:,j)),1,3)/3;      % phi_j evaluates at c_t.
        sT = 1/2*sT;
        T = T + sparse([ii NF+ii 2*NF+ii], [jj jj jj], sT(:), 3*NF, NF);
    end
end
pro = T;
pro([fixedface;fixedface;fixedface],:) = 2*pro([fixedface;fixedface;fixedface],:);
res = pro';
end


function [f,Ph]=haralick_n(qs,nL,q, nanFlag)
% Haralick textures measurements

Ph = coocurrance_alldir_mod(q);
% last row and column corresponds to outside ROI!
if nanFlag
    Ph=Ph(1:end-1,1:end-1);
    nL=nL-1;
end

%compute features
R=sum(Ph(:));
Ph=Ph/R;
% Energy (1)
f(1)=sum(sum(Ph.^2));
% Contrast (2)
f(2)=0.0;
for n=0:nL-1
   temp=0;
   for i=1:nL
      for j=1:nL
         if (abs(i-j) == n)
            temp=temp+Ph(i,j);
         end
      end
   end
   f(2)=f(2)+n^2*temp;
end
% Correlation
%using symmetry Ph'=Ph!
Px=sum(Ph);
Py=sum(Ph');
vec=[1:nL];
ux=sum(Px .*vec);
uy=sum(Py .*vec);

varx=sum(Px .* vec.^2)-ux^2;
sigx=sqrt(varx);
vary=sum(Py .* vec.^2)-uy^2;
sigy=sqrt(vary);
u=vec*Ph(i,j)*vec';
f(3)=(u-ux*uy)/(sigx*sigy);

%Entropy (3)
res = 1e-10; % realmin
f(4)=-sum(sum(Ph.*log2(Ph+res))); % log????

% variance
f(5)=0;
for i=1:nL
   for j=1:nL
      f(5)=f(5)+(i-u)^2*Ph(i,j);
   end
end
% P(x+y)
f(6)=0;  % sum of entropy...
for k=2:2*nL
   temp=0.0;
   for i=1:nL
      for j=1:nL
         if ((i+j) == k)
            temp=temp+Ph(i,j);
         end
      end
   end
   Pxpy(k)=temp;
   f(6)=f(6)-temp*log2(temp+res);
end

% inverse different moment..
f(7)=0;
f(8)=0;  %Homogeneity (4)
for i=1:nL
   for j=1:nL
      temp1=1/(1+(i-j)^2)*Ph(i,j);
      temp2=1/(1+abs(i-j))*Ph(i,j);
      f(7)=f(7)+temp1;
      f(8)=f(8)+temp2;
   end
end

return


function theta=orderpoints(A, center, axis)
X0_in=center(1);
Y0_in=center(2);
x_axis=axis(:,1);
y_axis=axis(:,2);
centerlized=A-repmat([X0_in, Y0_in], size(A,1),1);
% length=sqrt(sum(centerlized.^2,2));
% cosx=centerlized*x_axis./length;
% cosy=centerlized*y_axis./length;
%
% for i=1:size(A,1)
%     if (cosy(i)>0)
%         theta(i)=acos(cosx(i));
%     else
%         theta(i)=2*pi-acos(cosx(i));
%
%     end
% end

x1=centerlized(:,1);
y1=centerlized(:,2);
x2=repmat(x_axis(1), numel(x1),1);
y2=repmat(x_axis(2), numel(x1),1);
theta = mod(atan2(x1.*y2-x2.*y1,x1.*x2+y1.*y2),2*pi)';


function [ vX, mX ] = SolveLsLInfProx( mA, vB, paramLambda, numIterations )
% ----------------------------------------------------------------------------------------------- %
%[ vX, mX ] = SolveLsL1Prox( mA, vB, lambdaFctr, numIterations )
% Solve L Infinity Regularized Least Squares Using Proximal Gradient (PGM) Method.
% Input:
%   - mA                -   Input Matirx.
%                           The model matrix.
%                           Structure: Matrix (m X n).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
%   - vB                -   input Vector.
%                           The model known data.
%                           Structure: Vector (m X 1).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
%   - paramLambda       -   Parameter Lambda.
%                           The L Infinity Regularization parameter.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range: (0, inf).
%   - numIterations     -   Number of Iterations.
%                           Number of iterations of the algorithm.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range {1, 2, ...}.
% Output:
%   - vX                -   Output Vector.
%                           Structure: Vector (n X 1).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
% References
%   1.  Wikipedia PGM - https://en.wikipedia.org/wiki/Proximal_gradient_method.
% Remarks:
%   1.  Using vanilla PGM.
% Known Issues:
%   1.  A
% TODO:
%   1.  B
% Release Notes:
%   -   1.0.000     23/08/2017
%       *   First realease version.
% ----------------------------------------------------------------------------------------------- %

mAA = mA.' * mA;
vAb = mA.' * vB;
vX  = pinv(mA) * vB; %<! Dealing with "Fat Matrix"

stopThr = 1e-6;

stepSize = 1 / (2 * (norm(mA, 2) ^ 2));
% stepSize = 1 / sum(mA(:) .^ 2); %<! Faster to calculate, conservative (Hence slower)

mX = zeros([size(vX, 1), numIterations]);
mX(:, 1) = vX;

for ii = 2:numIterations

    vG = (mAA * vX) - vAb;
    vV = vX - (stepSize * vG);
    vX = vV - (stepSize * paramLambda * ProjectL1Ball(vV / (paramLambda * stepSize), 1, stopThr));

    mX(:, ii) = vX;

end

end


% Impulse response invariant discretization of fractional second
% order filter.
%
% irid_fsof function is prepared to compute a discrete-time finite
% dimensional (z) transfer function to approximate a continuous-time
% fractional second order low-pass filter (LPF) [1/(s^2 + a*s + b)]^r, where "s" is
% the Laplace transform variable; "r" is a real number in the range of
% (0,1); a and b are the time constant of LPF [1/(s^2 + a*s + b)]^r, where a, b >= 0.
%
% The proposed approximation keeps the impulse response "invariant"
%
% IN:
%       a, b: the time constant of (the first order) LPF
%             (a and b are arbitrary positive real numbers)
%       r: the fractional order in (0,1)
%       Ts: the sampling period
%       norder: the finite order of the approximate z-transfer function
%       (the orders of denominator and numerator z-polynomial are the same)
% OUT:
%       sr: returns the LTI object that approximates the [1/(s^2 + a*s + b)]^r
%           in the sense of invariant impulse response.
% TEST CODE
% [sr]=irid_fsof(0.01,3,2,.8,5);
%
% Reference:
%       (1) Yan Li, Hu Sheng and YangQuan Chen.
%           "Analytical Impulse Response of A Fractional Second Order Filter
%           and Its Impulse Response Invariant Discretization".
%       (2) YangQuan Chen.
%           "Impulse-invariant discretization of fractional order low-pass filters".
%           Sept. 2008. CSOIS AFC (Applied Fractional Calculus) Seminar.
%           http://fractionalcalculus.googlepages.com/
% --------------------------------------------------------------------
% Yan Li, Ph.D,
% School of Control Science and Engineering,
% Shandong University, Jinan, Shandong 250061, P. R. China.
% --------------------------------------------------------------------
%
% --------------------------------------------------------------------
% YangQuan Chen, Ph.D, Associate Professor and Graduate Coordinator
% Department of Electrical and Computer Engineering,
% Director, Center for Self-Organizing and Intelligent Systems (CSOIS)
% Utah State University, 4120 Old Main Hill, Logan, UT 84322-4120, USA
% E: yqchen@ece.usu.edu or yqchen@ieee.org, T/F: 1(435)797-0148/3054;
% W: http://www.csois.usu.edu or http://yangquan.chen.googlepages.com
% --------------------------------------------------------------------
%
%--------------------------------------------------------------------
% Hu Sheng, Ph.D,
% Department of Electrical and Computer Engineering,
% Center for Self-Organizing and Intelligent Systems (CSOIS)
% Utah State University, 4120 Old Main Hill, Logan, UT 84322-4120, USA
% --------------------------------------------------------------------
% Note: All the codes were finished under the direction of Dr. YangQuan Chen.
%       The codes were writen by Dr. Yan Li and Hu Sheng,
%
% See also irid_fod.m
%          at http://www.mathworks.com/matlabcentral/files/21342/irid_fod.m
%          irid_doi.m
%          at
%          http://www.mathworks.com/matlabcentral/fileexchange/
%          26380-impulse-response-invariant-discretization-of-distributed-order-integrator

%*************************************************************************
function [sr]=irid_fsof(Ts,a,b,r,norder)

if nargin<5; norder=5; end
if a < 0 | b < 0 , sprintf('%s','a and b constant has to be positive'), return, end
if Ts < 0 , sprintf('%s','Sampling period has to be positive'), return, end
if r>=1 | r<= 0, sprintf('%s','The fractional order should be in (0,1)'), return, end
if norder<2, sprintf('%s','The order of the approximate transfer function has to be greater than 1'), return, end

close all;
wmax0=2*pi/Ts/2;          % rad./sec. Nyquist frequency
wmax=floor(1+ log10(wmax0) );
wmin=wmax-5;
w=logspace(wmin,wmax,1000);
j=sqrt(-1);
L=10/Ts;                  % decides the number of points of the impulse response function h(n)
t=[1:L]*Ts; y=[]; ht=[]; y1=[]; y2=[];

if a^2-4*b<0              % Case 1: a^2-4*b<0
    for k=1:length(t)
        y1(k)=quad(@(tau)realconvolution((-a)/2,sqrt(-a^2+4*b)/2,r,tau,t(k)),0,t(k));
        y2(k)=quad(@(tau)imconvolution((-a)/2,sqrt(-a^2+4*b)/2,r,tau,t(k)),0,t(k));
    end
    ht=y1+y2;
elseif a^2-4*b==0         % Case2: a^2-4*b==0
       ht = exp(-sqrt(b).*t).*t.^(2*r-1)/gamma(2*r);
else                      % Case 3: a^2-4*b>0
    for k=1:length(t)
       ht(k)=quadgk(@(x)integration2(x,a,b,r,t(k)),0,t(k));
    end
    s=(-a+sqrt(abs(a^2-4*b)))/2;
    ht= (exp(s.*t)/gamma(r)/gamma(r)).*ht;
end
h = [ht.*Ts];              % approcimation
q=norder;p=norder;
[B,A]=stmcb((h),q,p);
sprintf('Impulse response invariant Discrete approximated transfer function:')
sr=tf(B,A,Ts)
hht=impulse(sr,t);         % approcimated impulse response
                           % frequency response
srfr=(1./((j*w).^2 +a*j*w+b)).^(r);
srfr1=freqresp(sr,w);      % approcimated frequency response

figure;
subplot(3,1,1)             % comparision of impulse response
plot(t,ht,'b');
hold on;
plot(t,hht./Ts,'r-.')
axis([Ts,Ts.*L,-0.5,1]);
xlabel('Time');ylabel('Impulse response');
grid on;
legend(['impulse response of 1/(s^2 + ',num2str(a), '* s +',num2str(b),' )^{',num2str(abs(r)),'}'],'approximated impulse response');

subplot(3,1,2)            % comparision of magnitude
semilogx(w,20*log10(abs(srfr)),'b');hold on;
semilogx(w,20*log10(abs(reshape(srfr1, 1000, 1))),'r-.');
legend(['mag. Bode of 1/(s^2 + ',num2str(a), '* s +',num2str(b),' )^{',num2str(abs(r)),'}'],'approximated mag. Bode');
xlabel('Frequency (Hz)');ylabel('Magnitude (dB)');grid on;

subplot(3,1,3)            % comparision of phase
semilogx(w,(180/pi) * (angle(srfr)),'b');hold on;
semilogx(w,(180/pi) * (angle(reshape(srfr1, 1000, 1))),'r-.');
grid on
xlabel('Frequency (Hz)');ylabel('Phase (degrees)');
legend(['phase Bode of 1/(s^2 + ',num2str(a), '* s +',num2str(b),' )^{',num2str(abs(r)),'}'],'approximated phase Bode')

end


function fem2d_pack_test01 ( )

%*****************************************************************************80
%
%% TEST01 tests BANDWIDTH_MESH.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST01\n' );
  fprintf ( 1, '  BANDWIDTH_MESH computes the geometric bandwidth:\n' );
  fprintf ( 1, '  of a finite element mesh.\n' );

  nelemx = 2;
  nelemy = 6;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  NELEMX = %d\n', nelemx );
  fprintf ( 1, '  NELEMY = %d\n', nelemy );

  element_order = 6;
  element_num = grid_element_num ( 'T6', nelemx, nelemy );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  ELEMENT_ORDER = %d\n', element_order );
  fprintf ( 1, '  ELEMENT_NUM   = %d\n', element_num );

  element_node = grid_t6_element ( nelemx, nelemy );

  grid_print ( element_order, element_num, element_node );

  [ ml, mu, m ] = bandwidth_mesh ( element_order, element_num, ...
    element_node );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Lower bandwidth ML = %d\n', ml );
  fprintf ( 1, '  Upper bandwidth MU = %d\n', mu );
  fprintf ( 1, '  Total bandwidth M  = %d\n', m );

  return
end


%DEMO_HURDLE Demonstration of Logit Negative-binomial hurdle model
%            using Gaussian process prior
%
%  Description
%    Hurdle models can be used to model excess number of zeros
%    compared to usual Poisson and negative binomial count models.
%    Hurdle models assume a two-stage process, where the first
%    process determines whether the count is larger than zero, and
%    the second process determines the non-zero count. Both
%    processes are given a zero mean Gaussian process prior. The
%    two stage model formulation makes it possible to make
%    inference for the two latent processes separately.
%
%    In this demo we construct logit negative binomial hurdle model
%    by using logit and zero truncated negative binomial models.
%    The posterior inference is made with parallel-EP
%    approximation.
%
%    See also  DEMO_SPATIAL2, DEMO_CLASSIFIC1
%
% Copyright (c) 2008-2010 Jarno Vanhatalo
% Copyright (c) 2010-2011 Aki Vehtari
% Copyright (c) 2011 Jaakko Riihimäki

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

% load the data
S = which('demo_spatial1');
data = load(strrep(S,'demo_spatial1.m','demodata/spatial1.txt'));

x = data(:,1:2);
ye = data(:,3);
y = data(:,4);

x0=x;
x=bsxfun(@rdivide,bsxfun(@minus,x0,mean(x0)),std(x0));

% for zero-process
yz=double(y>0)*2-1;
% for count-process
ci=find(y>0);
yc=y(ci);
xc=x(ci,:);
yec=ye(ci,:);

% Create the covariance functions
pl = prior_t('s2',10);
pm = prior_sqrtunif();
cf = gpcf_matern32('lengthScale', 5, 'magnSigma2', 0.05, ...
                   'lengthScale_prior', pl, 'magnSigma2_prior', pm);

% Create the zero part
likz=lik_probit();
gpz=gp_set('lik',likz,'cf',cf,'jitterSigma2',1e-6,'latent_method','EP','latent_opt',struct('parallel','on'));
% Create the count part
likc=lik_negbinztr();
gpc=gp_set('lik',likc,'cf',cf,'jitterSigma2',1e-6,'latent_method','EP','latent_opt',struct('parallel','on'));
% Note that although both parts use 'cf', they have separate hyperparameters

% Set the options for the quasi-Newton optimization
opt=optimset('TolFun',1e-2,'TolX',1e-2,'Display','iter');
gpz=gp_optim(gpz,x,yz,'opt',opt,'optimf',@fminlbfgs);
gpc=gp_optim(gpc,xc,yc,'z',yec,'opt',opt,'optimf',@fminlbfgs);

% make prediction to the data points
[Efz, Varfz] = gp_pred(gpz, x, yz, x);
[Efc, Varfc] = gp_pred(gpc, xc, yc, xc, 'z', yec);

% Define help parameters for plotting
xii=sub2ind([60 35],x0(:,2),x0(:,1));
[X1,X2]=meshgrid(1:35,1:60);

% Plot the figures
figure
subplot(1,2,1)
G=NaN(size(X1));
G(xii)=Efz(:);
pcolor(X1,X2,G),shading flat
colorbar
axis equal
axis([0 35 0 60])
title('Posterior mean of latent zero process')

subplot(1,2,2)
G=NaN(size(X1));
G(xii(ci))=Efc(:);
pcolor(X1,X2,G),shading flat
colorbar
axis equal
axis([0 35 0 60])
title('Posterior mean of latent count process')


function [lb,ub,redundant,psstruct,infeasible] = milppresolve(A,b,lb,ub,integer_variables,binary_variables,changed_bounds);
%MILPPRESOLVE Internal function for presolving MILPs

% Simple bound pre-processing (paper by Savelsbergh)
% No code optimization at all

if nargin < 5
    integer_variables = [];
end
if nargin < 6
    binary_variables = [];
end
if nargin < 7
    changed_bounds = [];
end

ubnew = ub;
lbnew = lb;
goon = 1;

AL0A  = (A>0).*A;
AG0A  = (A<0).*A;

At = A';
use_indicies=ones(length(b),1);
used = full(any(A(:,find(changed_bounds)),2));
isbinary  = ismembc(1:length(lb),binary_variables);
isinteger = ismembc(1:length(lb),binary_variables);

goon = all(lb<=ub);
infeasible = ~goon;
if ~infeasible

    bi_up = AL0A*ub+AG0A*lb;
    bi_dn = AL0A*lb+AG0A*ub;

    while goon

        bminusbdn=b-bi_dn;
        for i = find(use_indicies & used)'

            [ii,jj,kk] = find(At(:,i));

            Cp = ii(kk>0);
            if ~isempty(Cp)
                new1=lbnew(Cp)+bminusbdn(i)./At(Cp,i);
                ubnew(Cp)=min(ubnew(Cp),new1);
            end

            Cm = ii(kk<0);
            if ~isempty(Cm)
                new2=ubnew(Cm)+bminusbdn(i)./At(Cm,i);
                lbnew(Cm)=max(lbnew(Cm),new2);
            end

            if any(lbnew>ubnew)
                infeasible = 1;
                break
            end

        end

        if ~infeasible

            lbnew(integer_variables) = round(lbnew(integer_variables)+0.49999);
            ubnew(integer_variables) = round(ubnew(integer_variables)-0.49999);

            lbnew(binary_variables) = round(lbnew(binary_variables)+0.49999);
            ubnew(binary_variables) = round(ubnew(binary_variables)-0.49999);

            goon = (~all((lb==lbnew) & (ub==ubnew))) & all(lbnew<=ubnew);

            used = (lb~=lbnew) | (ub~=ubnew);
            used = full(any(A(:,find(used)),2));

            lb = lbnew;
            ub = ubnew;

            bi_up = AL0A*ub+AG0A*lb;
            bi_dn = AL0A*lb+AG0A*ub;
            redundant = find(bi_up<=b);

            use_indicies = use_indicies & bi_up>b;
        else
            goon = 0;
        end
    end
    redundant = find(bi_up<=b);
else
    redundant=[];
end
psstruct.AL0A = AL0A;
psstruct.AG0A = AG0A;


%% Display Start message
disp('Running PSK SER Simulation set')
%% Plot theoretical curves
SNRs = -4:24;
[h_fig, h_lines] = PSK_SER_Curves(SNRs);
%% Run Monte Carlo simulations

PSK_SER = zeros(5,length(SNRs));
hold on
simLines = semilogy(SNRs, PSK_SER,'*');

tic
[PSK_BER, PSK_SER] = PSK_Simulate(SNRs, simLines);
toc
%% Plot simulation results
% Simulation results are plotted inside the PSK_Simulate function
%% Add Legend
% Do MATLAB graphics magic to create concise legend
% See "Controling Legends" in MATLAB doc

% group lines together
simGrp = hggroup('DisplayName','Simulation');
theoGrp = hggroup('DisplayName','Theoretical');
set(simLines,'Parent',simGrp)
set(h_lines,'Parent',theoGrp)
set(get(get(simGrp,'Annotation'),'LegendInformation'),...
    'IconDisplayStyle','on'); % Include this hggroup in the legend
set(get(get(theoGrp,'Annotation'),'LegendInformation'),...
    'IconDisplayStyle','on'); % Include this hggroup in the legend
legend show

disp('Note: Fig. 5-2-10 in Proakis 3rd Edition is labeled incorrectly.')
disp('The y-axis in that figure should be labeled from 10^-5 at the bottom')
disp('rather than 10^-6.  All other labels should be corrected accordingly.')
disp('This figure has been corrected in the 4th Edition.')


function [ L b info ] = ldml_learn( X, Y, k, it, verbose, A0 )
% [ L b info ] = ldml_learn( X, Y, k, it, verbose, A0 )
%
%  * Mandatory inputs:
% X : (m x d) data matrix (m data points with d dimensions)
% Y : (m x 1) class labels
%
%  * To learn a kernelized metric, provide the kernel Gram matrix instead of the data matrix X.
%
%  * Optional inputs:
% k : number of dimensions of projection space (default=d)
% it : number of iterations (default=100)
% verbose : boolean for verbosity (default=false)
% A0 : parameter initialization vector (default=random)
%
%  * Outputs:
% L : (d x k) projection matrix
% b : (scalar) distance threshold as learnt by LDML (may or may not be optimal depending on application)
% info : struct with information about the optimization, with following fields
%     .fA : sequence of function values during gradient descent
%     .it : effective number of iterations
%     .A  : parameter vector that you can re-use for initialization (cf "A0" in input section)
%
% Matthieu Guillaumin, INRIA.

[ n d ] = size(X);
m = numel(Y);
I=(1:n);

if n ~= m,
   error('ldml','Second input must have a number of elements equal to the number of rows of the first input');
end

YY=sparse(1:n,Y(:),true);
c=size(YY,2);

% Set default values
opt=2;
if nargin<opt+1,    k=d;                    end
if nargin<opt+2,    it=100;                 end
if nargin<opt+3,    verbose=false;          end

if verbose, fprintf('Learning (%d x %d) LDML with %d instances in %d bags with %d labels\n',d,k,n,m,c); end

% Set initial projection as a random orthogonal matrix. TODO: several random initialization.
if nargin<opt+4,
    A0 = orth(rand(d,k));
    A0 = A0(:);
    A0(end+1)=rand;
end

H=vec(hist_count(I));
if m>size(H,1),
    H(size(H,1)+1:m)=0;
end

% Perform gradient descent
[ A fA it ] = minimize( A0, 'mildml_fg', it, verbose, double(X), double(I)-1, double(sparse(YY)), double(H), n, d, k, m, c);
info.A=A;info.fA=fA;info.it=it;

% Get projection matrix and bias from learned parameter vector
b = A(end);
L = reshape(A(1:end-1),d,k);


function mu = kseeds(X, k)
% Perform kmeans++ seeding
% Input:
%   X: d x n data matrix
%   k: number of seeds
% Output:
%   mu: d x k seeds
% Written by Mo Chen (sth4nth@gmail.com).
n = size(X,2);
D = inf(1,n);
mu = X(:,ceil(n*rand));
for i = 2:k
    D = min(D,sum((X-mu(:,i-1)).^2,1));
    mu(:,i) = X(:,randp(D));
end


function klu_demo
% KLU demo
%
% Example:
%   klu_demo
%
% See also klu, btf

% Copyright 2004-2009 Timothy A. Davis, Univ. of Florida

load west0479
A = west0479 ;

n = size (A,1) ;
b = rand (n,1) ;

clf
subplot (2,2,1) ;
spy (A)
title ('west0479') ;

subplot (2,2,2) ;
[p, q, r] = btf (A) ;
drawbtf (A, p, q, r) ;
title ('BTF form') ;

[x,info,c] = klu (A, '\', b) ;
matlab_condest = condest (A) ;
matlab_cond = cond (full (A)) ;
fprintf ('MATLAB condest: %g KLU condest: %g cond: %g\n', ...
    matlab_condest, c, matlab_cond) ;

fprintf ('\nKLU with scaling, AMD ordering and condition number estimate:\n') ;
[LU,info] = klu (A, struct ('ordering',0, 'scale', 1)) ;
x = klu (LU, '\', b) ;
resid = norm (A*x-b,1) / norm (A,1) ;
rgrowth = full (min (max (abs ((LU.R \ A (LU.p,LU.q)) - LU.F)) ./ ...
    max (abs (LU.U)))) ;
fprintf ('resid: %g KLU condest: %g rgrowth: %g\n', resid, c, rgrowth) ;
disp (info) ;

subplot (2,2,3) ;
spy (LU.L + LU.U + LU.F) ;
title ('KLU+AMD factors') ;

fprintf ('\nKLU with COLAMD ordering\n') ;
[LU,info] = klu (A, struct ('ordering',1)) ;
x = klu (LU, '\', b) ;
resid = norm (A*x-b,1) / norm (A,1) ;
fprintf ('resid: %g\n', resid) ;
disp (info) ;

subplot (2,2,4) ;
spy (LU.L + LU.U + LU.F) ;
title ('KLU+COLAMD factors') ;

fprintf ('\nKLU with natural ordering (lots of fillin)\n') ;
[x,info] = klu (A, '\', b, struct ('ordering',2)) ;
resid = norm (A*x-b,1) / norm (A,1) ;
fprintf ('resid: %g\n', resid) ;
disp (info) ;

try

    fprintf ('\nKLU with CHOLMOD(A''*A) ordering\n') ;
    [x,info] = klu (A, '\', b, struct ('ordering',3)) ;
    resid = norm (A*x-b,1) / norm (A,1) ;
    fprintf ('resid: %g\n', resid) ;
    disp (info) ;

    fprintf ('\nKLU with CHOLMOD(A+A'') ordering\n') ;
    [x,info] = klu (A, '\', b, struct ('ordering',4)) ;
    resid = norm (A*x-b,1) / norm (A,1) ;
    fprintf ('resid: %g\n', resid) ;
    disp (info) ;

catch
    fprintf ('KLU test with CHOLMOD skipped (CHOLMOD not installed)\n') ;
end


function dudt = burgers(t,u,epsilon,D1,D2)

dudt = epsilon*D2*u - u.*(D1*u);


function x = AngNormalize(x)
%ANGNORMALIZE  Reduce angle to range [-180, 180)
%
%   X = ANGNORMALIZE(X) reduces angles in [-540, 540) to the range
%   [-180, 180).  X can be any shape.

  x(x >= 180) = x(x >= 180) - 360;
  x(x < -180) = x(x < -180) + 360;
end


function [ATilde,P,R,C]=equilibrateMat(A,algorithm,maxIter,RelTol,AbsTol)
%%EQUILIBRATEMAT Equilibrate a general (not necessarily symmetric) real or
%                complex matrix. This finds a permutation matrix and two
%                diagonal matrices such that the equilibrated matrix,
%                ATilde=P*R*A*C has the infinity-norm of each row and
%                column equal to 1.
%
%INPUTS: A An nXn real or complex matrix. Unless algorithm=1, in which case
%          A must be real and all positive.
% algorithm An optional parameter selecting the algorithm to use for
%          equilibration. Possible values are:
%          0 (The default if omitted or an empty matrix is passed) Use the
%            algorithm involving bipartite matching, as described in
%            Section 2.1.2 of [1], which is one of the algorithmic variants
%            given in [2].
%          1 This is the heuristic ALG3 in [1]. The algorithm should work
%            with all-positive, real matrices. It does not work with
%            complex matrices. Also, the algorithm usually does not work
%            with matrices containing negative elements. This algorithm
%            always produces P=eye(n,n).
%  maxIter If algorithm=1, this is the maximum number of iterations to
%          perform. The default if omitted or an empty matrix is passed is
%          500. This is not used if algorithm=0.
% RelTol, AbsTol The absolute and relative convergence criteria for
%         algorithm 1. The defaults if omitted or empty matrices are passed
%         are 1e-10 and 1e-13.
%
%OUTPUTS: ATilde The nXn equlibrated matrix.
%              P The nNn permutation matrix.
%              R The left-multiplying diagonal matrix.
%              C The right-multiplying diagonal matrix.
%
%EXAMPLE:
%Here, we create a symmetric matrix and equilibrate it. The equilibration
%makes the largest magnitude values in each row and column equal 1.
%However, it also changes the condition number (usually for the better), as
%we shall show:
% %A random complex matrix.
% A=10*rand(10,10)+10*1j*rand(10,10);
% ATilde=equilibrateMat(A);
%
% %The original maximum absolute value elements in the columns and rows.
% max(ATilde,[],1)
% max(ATilde,[],2)
%
% %After equilibration, the maximum mangitude elements in the columns and
% % %rows are all 1.
% max(ATilde,[],1)
% max(ATilde,[],2)
%
% %The condition number often but not always improves.
% cond(A,Inf)
% cond(ATilde,Inf)
%
%REFERENCES:
%[1] P. Liu, "An exploration of matrix equilibration," University of
%    British Columbia. CPSC 517, Tech. Rep., 30 Sep. 2015. [Online].
%    Available: http://dx.doi.org/10.14288/1.0103601
%[2] I. S. Duff and J. Koster, "On algorithms for permuting large entries
%    to the diagonal of a sparse matrix," SIAM Journal on Matrix Analysis
%    and Applications, vol. 22, no. 4, pp. 973-996, 2001.
%
%September 2019 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<5||isempty(AbsTol))
    AbsTol=1e-13;
end

if(nargin<4||isempty(RelTol))
    RelTol=1e-10;
end

if(nargin<3||isempty(maxIter))
    maxIter=500;
end

if(nargin<2||isempty(algorithm))
    algorithm=0;
end

switch(algorithm)
    case 0%The algorithm of [2].
        n=size(A,1);
        ATrans=-log(A);

        %In the current version of the Tracker Component Library, the compiled
        %version of assign2D does not support complex numbers in the cost matrix.
        %Thus, if the mex file is in the path, we have to remove it from the path
        %and use the m file. We can add the compiled version back to the path after
        %calling assign2D.
        [filepath,~,ext]=fileparts(which('assign2D'));

        if(~strcmp(ext,'m'))
            %If it isn't the m file that will be used, we have to remove the
            %filepath from the current path.
            rmpath(filepath);
        else
            filepath=[];
        end

        %Note that u and v are reversed; the definition in [1] is different from
        %that used in assign2D. ATrans will be complex if A had any negative terms
        %or if A were complex to begin with. However, the complex values are
        %correctly handled in the m function implementation of assign2D.
        [col4row,~,~,v,u]=assign2D(ATrans,false);

        %If a compiled implementation of assign2D had to be removed from the
        %current path.
        if(~isempty(filepath))
            addpath(filepath)
        end

        if(isempty(col4row))
            %The equilibration problem is infeasible.
            P=[];
            R=[];
            C=[];
            return
        end

        %We have to obtain the dual variables for the original problem, not the
        %transformed one that is sovled in assign2D.
        ADelta=min(ATrans(:));
        %If A is all positive, the problem wasn't transformed.
        if(ADelta>0)
            ADelta=0;
        end
        u=u+ADelta;

        %Construct the permutation matrix.
        P=zeros(n,n);
        for k=1:n
            P(k,col4row(k))=1;
        end

        R=diag(exp(u));
        C=diag(exp(v));

        %R and C should be real if A is real. This deals with any possible finite
        %precision issues.
        if(isreal(A))
            R=real(R);
            C=real(C);
        end

        ATilde=P*R*A*C;
    case 1%ALG3 in [1].
        if(~isreal(A))
            error('This algorithm only works with real matrices.')
        end

        if(any(A(:)<0))
            warning('This algorithm often does not satisfy the desired norm criterion when A contains negative elements.')
        end

        n=size(A,1);

        R=eye(n,n);
        C=eye(n,n);
        ATilde=A;

        D=eye(n,n);
        for curIter=1:maxIter
            ATildeOld=ATilde;
            for i=1:n
                alphaR=1/sqrt(norm(ATilde(i,:),Inf));
                alphaC=1/sqrt(norm(ATilde(:,i),Inf));

                Dr=D;
                Dr(i,i)=alphaR;
                Dc=D;
                Dc(i,i)=alphaC;

                R=R*Dr;
                C=C*Dc;
                ATilde=Dr*ATilde*Dc;
            end

            diffMag=abs(ATilde-ATildeOld);
            if(all((diffMag(:)<=RelTol*abs(ATilde(:)))|(diffMag(:)<=AbsTol)))
               return;
            end
        end

        P=eye(n,n);

    otherwise
        error('Unknown algorithm specified.')
end
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function [AC,Rank,fS] = UpdateConvergenceArchive(AC,Population,NC,Z,Xic,sigma_niche,Problem)
% Update the Convergence Archive

%------------------------------- Copyright --------------------------------
% Copyright 2017-2018 Yiping Liu
% Please contact {yiping0liu@gmail.com} if you have any problem.
%--------------------------------------------------------------------------

    Population = [Population,AC];
    PopObj = Population.objs;
    PopDec = Population.decs;
    N = size(PopObj,1);
    Rank   = inf(1,N);      % Rank of each solution
    nrank  = 1;             % Current rank

    %% Normalize
    PopObj = PopObj - repmat(Z,N,1);
    PopDec = (PopDec - repmat(Problem.lower,N,1))./repmat(Problem.upper-Problem.lower,N,1);

    %% Convergence indicator
    fS = mean(PopObj');

    %% Calculate distance between every two solutions in the IC decsion subspace
    d = pdist2(PopDec(:,Xic),PopDec(:,Xic),'chebychev');

    %% Rank
    Choose = false(1,N);
    Q = true(1,N);
    Q1 = false(1,N);
    while sum(Choose) < NC
        if sum(Q) == 0
           Q = Q1;
           Q1 = false(1,N);
           nrank = nrank+1;
        end
        % Choose x with min FS
        temp1 = fS == min(fS(Q));
        xmin = find(and(temp1,Q));
        xmin = xmin(1);
        Rank(xmin) = nrank;
        Choose(xmin) = true;
        Q(xmin) = false;
        % Delete solution near x_min
        temp3=d(xmin,:);
        temp2=temp3<sigma_niche;
        Delete = and(temp2,Q);
        Q(Delete) = false;
        Q1(Delete) = true;
    end
    AC = Population(Choose);
    Rank = Rank(Choose);
    fS = fS(Choose);
end


% % multichannel superdirective beamformer
% % Ziteng Wang @ 201811
% % reference: Berkun R, Cohen I, Benesty J. A tunable beamformer for robust superdirective beamforming[C]//Acoustic Signal Enhancement (IWAENC), 2016 IEEE International Workshop on. IEEE, 2016: 1-5.
%

clear all
close all
warning('off')
addpath('..\STFT\')
addpath('..\Simulation\')
addpath('..\Simulation\RIR-Generator\')

%% simulation start
flatStart = 1;
prefix = '';   % for saving file

speechDir = '..\Simulation\Data\';
speechFile = 'fajw0_sa1.wav';
saveDir = 'GeneratedData\';
if ~exist(saveDir)
    mkdir(saveDir)
end
speech = audioread([speechDir speechFile]);

% configuration
cfg = [];
cfg.fs = 16000;                     % sampling rate
cfg.room = [6 5 3];                 % room dimension (m)
cfg.T60 = 0.3;                      % reverberation time (s)

cfg.Nch = 6;
cfg.micCenter = [2 3 1.5];          % array center (m)
cfg.micCoordinate =[0.0425,0.0,0.0;
        0.02125,0.03680608,0.0;
        -0.02125,0.03680608,0.0;
        -0.0425,0.0,0.0;
        -0.02125,-0.03680608,0.0;
        0.02125,-0.03680608,0.0;];  % microphone array coordinates

cfg.az = 180;
cfg.el = 0;
cfg.dist = 2;

cfg.SNR = 10;
if cfg.SNR ~= inf               % inf means no noise
    cfg.noiseType = 'diffuse';    % choice {'white' 'diffuse' 'recorded'}
    if strcmp(cfg.noiseType, 'recorded')
        % check first the noise is longer than speech!
        cfg.noiseFile = '';
    end
end

cfg.SIR = inf;
if cfg.SIR ~= inf
    % check the interference is longer than speech!
    cfg.interfFile = '';
    cfg.azITF = 180;
    cfg.elITF = 0;
    cfg.distITF = 2;
end

% setup room and collect data
setup_room
setup_noise

%% processing start
% the following parts are specific to the algorithm
Nfft = 1024;

Y = stft_multi_2(y, Nfft);
[Nframe, Nbin, Nch] = size(Y);

%% superdirective beamformer

%%% diagloading value !
diagLoad = 0.01;

% diffuse noise coherence
SDcov = ones(Nch, Nch, Nbin);

% in matlab: sinc(x) = sin(pi*x) / (pi*x)
for bin = 1:Nbin
    for chi=1:Nch-1
        for chj=chi+1:Nch
            SDcov(chi,chj,bin) = sinc(2*(bin-1)/Nfft*fs*norm(micPose(chi,:)-micPose(chj,:))/c);
            SDcov(chj,chi,bin) = SDcov(chi,chj,bin);
        end
    end
end

% free field steering vector
steerVec = zeros(Nch, Nbin);
for bin = 1:Nbin
    steerVec(:,bin)= exp(-2*1i*pi*(bin-1)/Nfft*fs*TDOA);
end

% filter coefficients:
% h = PhiN^-1 * d / d^H * PhiN^-1 * d
PhiNinv = zeros(Nch, Nch, Nbin);
for bin = 1:Nbin
    PhiNinv(:,:,bin) = inv(SDcov(:,:,bin) + diagLoad*eye(Nch));
end
tmp = squeeze(sum(bsxfun(@times, PhiNinv, permute(steerVec, [3,1,2])), 2));
hSuperDirective = bsxfun(@rdivide, tmp, sum(bsxfun(@times, conj(steerVec), tmp),1));

% apply the filter
Xest = sum(bsxfun(@times, conj(permute(hSuperDirective, [3,2,1])), Y), 3);
xest = istft_multi_2(Xest, length(speech));

%% record and plot
audiowrite([saveDir prefix 'SuperDirective' postfix '.wav'], xest, fs);

% plot the beampattern
dist = 2;
el = 0;
for az=1:360
    sourcePose = dist * [cos(el/180*pi)*cos(az/180*pi) cos(el/180*pi)*sin(az/180*pi) sin(el/180*pi)] + micCenter;
    TDOA = sqrt(sum((bsxfun(@minus, sourcePose, micPose)).^2, 2))/c;
    for bin=1:Nbin
        azVec = exp(-2*1i*pi*(bin-1)/Nfft*fs*TDOA);
        beamPattern(az,bin) = abs(sum(conj(hSuperDirective(:,bin)) .* azVec));
        beamPattern(az,bin) = max(20*log10(beamPattern(az,bin)), -30); % in dB
    end
end
figure;imagesc(beamPattern); axis xy; colorbar
title(['BeamPattern of SD steered towards azimuth ' num2str(cfg.az)]);
xlabel('frequency index'); ylabel('azimuth angle');

% white noise gain
for bin=1:Nbin
    whiteNoiseGain(bin) = 10*log10(1/(hSuperDirective(:,bin)'*hSuperDirective(:,bin)));
end
figure; plot(whiteNoiseGain)
title(['WNG of SD with diagnal loading (' num2str(diagLoad) ')']);
xlabel('frequency index'); ylabel('WNG in dB');


function [pc,Pf,Pb] = rc2pcv(rc,R0)

%function [pc,Pf,Pb] = rc2pcv(rc,R0)
% Transforms forward reflection coefficients rcb into
% normalized correlation matrices pc.

%S. de Waele, March 2003.

s = kingsize(rc);
order = s(3)-1;
dim = s(1); I = eye(dim);
%if nargin == 1, R0 = I; end

pc = zeros(s);  pc(:,:,1)  = I;

Pf   = zeros(s); Pf(:,:,1)   = R0;
Pb   = zeros(s); Pb(:,:,1)  = R0;

for p = 1:order,
   TsqrtPf = Tsqrt(Pf(:,:,p)); %square root M defined by: M=Tsqrt(M)*Tsqrt(M)'
   TsqrtPb= Tsqrt(Pb(:,:,p));
   %partial correlation
   pc(:,:,p+1) = -inv(TsqrtPf)*rc(:,:,p+1)*TsqrtPb;
   %residual matrices
   Pf(:,:,p+1)  = (I-TsqrtPf *pc(:,:,p+1) *pc(:,:,p+1)'*inv(TsqrtPf ))*Pf(:,:,p);
   Pb(:,:,p+1) = (I-TsqrtPb*pc(:,:,p+1)'*pc(:,:,p+1) *inv(TsqrtPb))*Pb(:,:,p);
end %for p = 2:order,


function [data] = normalise(data,u,std)

[N]=size(data,2);

data=(data-u*ones(1,N)) ./ (std*ones(1,N));
data=data';


classdef FilterP0 < handle

   properties (Access = private)
       levelSet
       levelSet0
       dens0
   end

   methods (Access = public)

       function obj = FilterP0(ls,d)
           obj.levelSet = ls;
           obj.computeElementalLevelSet(d);
           obj.computeDensityP0();
       end

       function d = getDensity(obj)
           d = obj.dens0;
       end

   end

   methods (Access = private)

       function computeElementalLevelSet(obj,d)
            shape = d.shape;
            conec = d.conec;
            quadr = d.quadr;
            ngaus = quadr.ngaus;
            nelem = size(conec,1);
            nnode = size(shape,1);

            phiP0 = zeros(nelem,ngaus);
            phi   = obj.levelSet;
            for igaus = 1:ngaus
                for inode = 1:nnode
                    nodes = conec(:,inode);
                    phiN = phi(nodes);
                    phiP0(:,igaus) = phiP0(:,igaus) + shape(inode,igaus)*phiN;
                end
            end
            obj.levelSet0 = phiP0;
       end

        function computeDensityP0(obj)
            ls = obj.levelSet0;
            obj.dens0 = obj.computeDensity(ls);
        end

   end

   methods (Access = private, Static)

        function dens = computeDensity(phi)
            dens = 1 - heaviside(phi);
        end

   end

end


function isobars

% eeg_topo_murks_contour - illustrate isocontours of a surface

clear; close all;

s = 40;
w = 1/150;
cvec = 10:60:300;

p = -s:s;
[x,y] = meshgrid(p);
f = exp(-w*(x.^2 + y.^2));
f = max(f,0);
f = sqrt(f);

surf(x,y,f); shading interp; hold on; rotate3D

V = (x + 10).^2 + (y + 10).^2 + f.^2;

for k = 1:length(cvec)
  c = cvec(k);
  Vc = (V > c);

%  Vcdil = dilate(Vc); Vcdil = double(Vcdil);

  se = [0 1 0 ; 1 1 1 ; 0 1 0];
%  se = ones(1);
  Vcdil = conv2(Vc,se,'same');
  Vcdil = Vcdil > 0;

  Vc = Vcdil - Vc;
  Vcvec = find(Vc);

  plot3(x(Vcvec),y(Vcvec),f(Vcvec),'w.')

end

hold off


function [ labels ] = kernelkmeans_classifier(XGrid,X,centroid,eigens,kernel,kpar)
%KMEANS_CLASSIFIER K-means classifier
%
%  input ------------------------------------------------------------------
%
%       o X          : (N x D), data to classifiy
%
%       o centroids  : (L x D), L centroids
%
%       o dist       : string, distance metric
%
%   output-----------------------------------------------------------------
%
%       o labels     : (N x 1), class labels
%
%

KnGrid         = gram(XGrid,X,kernel,kpar(1),kpar(2));
KnGrid         = KnGrid * eigens;
HGrid          = KnGrid ./ repmat(sqrt(sum(KnGrid.^2, 2)), 1, size(eigens,2));

D           = ml_distfunc(HGrid, centroid, 'sqeuclidean');
[~, labels] = min(D, [], 2);

end


function jac = jacexpfit(p, data)
  n=data;
  m=max(size(p));

  for i=1:n
    jac(i, 1:m)=[exp(-p(2)*i), -p(1)*i*exp(-p(2)*i), 1.0];
  end


function [m,c]=cep2pow(u,v,mode)
%CEP2POW convert cepstral means and variances to the power domain
% Inputs:
%    u: vector giving the cepstral means with u(1) the 0'th cepstral coefficient
%    v: cepstral covariance matrix or else a vector containing the diagonal elements
% mode: 'c'  pow=exp(irdct(cep))    [default]
%       'f'  pow=exp(rsfft(cep)/n)  [fft length even]
%       'fo' pow=exp(rsfft(cep)/n)  [fft length odd]
%       'i'  pow=exp(cep)           [ no transformation ]
%
% Outputs:
%    m: row vector giving means in the power domain
%    c: covariance matrix in the power domain

%      Copyright (C) Mike Brookes 1998
%      Version: $Id: cep2pow.m 713 2011-10-16 14:45:43Z 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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargin<3 mode='c'; end
if min(size(v))==1
   v=diag(v);
end
u=u(:)';    % force u to be a row vector
if any(mode=='f')
   n=2*length(u)-2;
   if any(mode=='o')
      n=n+1;
   end
   p=rsfft(u',n)/n;
   q=rsfft(rsfft(v,n)',n)/n^2;
elseif any(mode=='i')
    p=u';
    q=v';
else
   p=irdct(u');
   q=irdct(irdct(v)');
end
m=exp(p+0.5*diag(q))';
c=(m'*m).*(exp(q)-1);


function [ idx, tri ] = nearest_neighbor ( varargin )

%NEAREST_NEIGHBOR    find nearest neighbors
%   IDX = NEAREST_NEIGHBOR(X) finds the nearest neighbor by Euclidean
%   distance to each point (column) in X from X. X is a matrix with points
%   as columns. IDX is a vector of indices into X, such that X(:, IDX) are
%   the nearest neighbors to X. e.g. the nearest neighbor to X(:, 2) is
%   X(:, IDX(2))
%
%   IDX = NEAREST_NEIGHBOR(P, X) finds the nearest neighbor by Euclidean
%   distance to each point in P from X. P and X are both matrices with the
%   same number of rows, and points are the columns of the matrices. Output
%   is a vector of indices into X such that X(:, IDX) are the nearest
%   neighbors to P
%
%   IDX = NEAREST_NEIGHBOR(I, X) where I is a logical vector or vector of
%   indices, and X has at least two rows, finds the nearest neighbor in X
%   to each of the points X(:, I).
%   I must be a row vector to distinguish it from a single point.
%   If X has only one row, the first input is treated as a set of 1D points
%   rather than a vector of indices
%
%   IDX = NEAREST_NEIGHBOR(..., Property, Value)
%   Calls NEAREST_NEIGHBOR with the indicated parameters set. Property
%   names can be supplied as just the first letters of the property name if
%   this is unambiguous, e.g. NEAREST_NEIGHBOR(..., 'num', 5) is equivalent
%   to NEAREST_NEIGHBOR(..., 'NumberOfNeighbors', 5). Properties are case
%   insensitive, and are as follows:
%      Property:                         Value:
%      ---------                         ------
%         NumberOfNeighbors             natural number, default 1
%            NEAREST_NEIGHBOR(..., 'NumberOfNeighbors', K) finds the closest
%            K points in ascending order to each point, rather than the
%            closest point. If Radius is specified and there are not
%            sufficient numbers, fewer than K neighbors may be returned
%
%         Radius                         positive, default +inf
%            NEAREST_NEIGHBOR(..., 'Radius', R) finds neighbors within
%            radius R. If NumberOfNeighbors is not set, it will find all
%            neighbors within R, otherwise it will find at most
%            NumberOfNeighbors. The IDX matrix is padded with zeros if not
%            all points have the same number of neighbors returned. Note
%            that specifying a radius means that the Delaunay method will
%            not be used.
%
%         DelaunayMode                   {'on', 'off', |'auto'|}
%            DelaunayMode being set to 'on' means NEAREST_NEIGHBOR uses the
%            a Delaunay triangulation with dsearchn to find the points, if
%            possible. Setting it to 'auto' means NEAREST_NEIGHBOR decides
%            whether to use the triangulation, based on efficiency. Note
%            that the Delaunay triangulation will not be used if a radius
%            is specified.
%
%         Triangulation                  Valid triangulation produced by
%                                        delaunay or delaunayn
%            If a triangulation is supplied, NEAREST_NEIGHBOR will attempt
%            to use it (in conjunction with dsearchn) to find the
%            neighbors.
%
%   [IDX, TRI] = NEAREST_NEIGHBOR( ... )
%   If the Delaunay Triangulation is used, TRI is the triangulation of X'.
%   Otherwise, TRI is an empty matrix
%
%   Example:
%
%     % Find the nearest neighbor in X to each column of X
%     x = rand(2, 10);
%     idx = nearest_neighbor(x);
%
%     % Find the nearest neighbors to each point in p
%     p = rand(2, 5);
%     x = rand(2, 20);
%     idx = nearest_neighbor(p, x)
%
%     % Find the five nearest neighbors to points x(:, [1 6 20]) in x
%     x = rand(4, 1000)
%     idx = nearest_neighbor([1 6 20], x, 'NumberOfNeighbors', 5)
%
%     % Find all neighbors within radius of 0.1 of the points in p
%     p = rand(2, 10);
%     x = rand(2, 100);
%     idx = nearest_neighbor(p, x, 'r', 0.1)
%
%     % Find at most 10 nearest neighbors to point p from x within a
%     % radius of 0.2
%     p = rand(1, 2);
%     x = rand(2, 30);
%     idx = nearest_neighbor(p, x, 'n', 10, 'r', 0.2)
%
%
%   See also DELAUNAYN, DSEARCHN, TSEARCH

%TODO    Allow other metrics than Euclidean distance
%TODO    Implement the Delaunay mode for multiple neighbors

% Copyright 2006 Richard Brown. This code may be freely used and
% distributed, so long as it maintains this copyright line
error(nargchk(1, Inf, nargin, 'struct'));

% Default parameters
userParams.NumberOfNeighbors = []    ; % Finds one
userParams.DelaunayMode       = 'auto'; % {'on', 'off', |'auto'|}
userParams.Triangulation      = []    ;
userParams.Radius             = inf   ;

% Parse inputs
[P, X, fIndexed, userParams] = parseinputs(userParams, varargin{:});

% Special case uses Delaunay triangulation for speed.

% Determine whether to use Delaunay - set fDelaunay true or false
nX  = size(X, 2);
nP  = size(P, 2);
dim = size(X, 1);

switch lower(userParams.DelaunayMode)
    case 'on'
        %TODO Delaunay can't currently be used for finding more than one
        %neighbor
        fDelaunay = userParams.NumberOfNeighbors == 1 && ...
            size(X, 2) > size(X, 1)                    && ...
            ~fIndexed                                  && ...
            userParams.Radius == inf;
    case 'off'
        fDelaunay = false;
    case 'auto'
        fDelaunay = userParams.NumberOfNeighbors == 1 && ...
            ~fIndexed                                  && ...
            size(X, 2) > size(X, 1)                    && ...
            userParams.Radius == inf                   && ...
            ( ~isempty(userParams.Triangulation) || delaunaytest(nX, nP, dim) );
end

% Try doing Delaunay, if fDelaunay.
fDone = false;
if fDelaunay
    tri = userParams.Triangulation;
    if isempty(tri)
        try
            tri   = delaunayn(X');
        catch
            msgId = 'Nearest_Neighbor:DelaunayFail';
            msg = ['Unable to compute delaunay triangulation, not using it. ',...
                'Set the DelaunayMode parameter to ''off'''];
            warning(msgId, msg);
        end
    end
    if ~isempty(tri)
        try
            idx = dsearchn(X', tri, P')';
            fDone = true;
        catch
            warning('Nearest_Neighbor:DSearchFail', ...
                'dsearchn failed on triangulation, not using Delaunay');
        end
    end
else % if fDelaunay
    tri = [];
end

% If it didn't use Delaunay triangulation, find the neighbors directly by
% finding minimum distances
if ~fDone
    idx = zeros(userParams.NumberOfNeighbors, size(P, 2));

    % Loop through the set of points P, finding the neighbors
    Y = zeros(size(X));
    for iPoint = 1:size(P, 2)
        x = P(:, iPoint);

        % This is the faster than using repmat based techniques such as
        % Y = X - repmat(x, 1, size(X, 2))
        for i = 1:size(Y, 1)
            Y(i, :) = X(i, :) - x(i);
        end

        % Find the closest points, and remove matches beneath a radius
        dSq = sum(abs(Y).^2, 1);
        iRad = find(dSq < userParams.Radius^2);
        if ~fIndexed
            iSorted = iRad(minn(dSq(iRad), userParams.NumberOfNeighbors));
        else
            iSorted = iRad(minn(dSq(iRad), userParams.NumberOfNeighbors + 1));
            iSorted = iSorted(2:end);
        end

        % Remove any bad ones
        idx(1:length(iSorted), iPoint) = iSorted';
    end
    %while ~isempty(idx) && isequal(idx(end, :), zeros(1, size(idx, 2)))
    %    idx(end, :) = [];
    %end
    idx( all(idx == 0, 2), :) = [];
end % if ~fDone
if isvector(idx)
    idx = idx(:)';
end
end % nearest_neighbor

%DELAUNAYTEST   Work out whether the combination of dimensions makes
%fastest to use a Delaunay triangulation in conjunction with dsearchn.
%These parameters have been determined empirically on a Pentium M 1.6G /
%WinXP / 512MB / Matlab R14SP3 platform. Their precision is not
%particularly important
function tf = delaunaytest(nx, np, dim)
switch dim
    case 2
        tf = np > min(1.5 * nx, 400);
    case 3
        tf = np > min(4 * nx  , 1200);
    case 4
        tf = np > min(40 * nx , 5000);

        % if the dimension is higher than 4, it is almost invariably better not
        % to try to use the Delaunay triangulation
    otherwise
        tf = false;
end % switch
end % delaunaytest

%MINN   find the n most negative elements in x, and return their indices
%  in ascending order
function I = minn(x, n)

% Make sure n is no larger than length(x)
n = min(n, length(x));

% Sort the first n
[xsn, I] = sort(x(1:n));

% Go through the rest of the entries, and insert them into the sorted block
% if they are negative enough
for i = (n+1):length(x)
    j = n;
    while j > 0 && x(i) < xsn(j)
        j = j - 1;
    end

    if j < n
        % x(i) should go into the (j+1) position
        xsn = [xsn(1:j), x(i), xsn((j+1):(n-1))];
        I   = [I(1:j), i, I((j+1):(n-1))];
    end
end

end %minn

%PARSEINPUTS    Support function for nearest_neighbor
function [P, X, fIndexed, userParams] = parseinputs(userParams, varargin)
if length(varargin) == 1 || ~isnumeric(varargin{2})
    P           = varargin{1};
    X           = varargin{1};
    fIndexed    = true;
    varargin(1) = [];
else
    P             = varargin{1};
    X             = varargin{2};
    varargin(1:2) = [];

    % Check the dimensions of X and P
    if size(X, 1) ~= 1
        % Check to see whether P is in fact a vector of indices
        if size(P, 1) == 1
            try
                P = X(:, P);
            catch
                error('Nearest_Neighbor:InvalidIndexVector', ...
                    'Unable to index matrix using index vector');
            end
            fIndexed = true;
        else
            fIndexed = false;
        end % if size(P, 1) == 1
    else % if size(X, 1) ~= 1
        fIndexed = false;
    end

    if ~fIndexed && size(P, 1) ~= size(X, 1)
        error('Nearest_Neighbor:DimensionMismatch', ...
            'No. of rows of input arrays doesn''t match');
    end
end
% Parse the Property/Value pairs
if rem(length(varargin), 2) ~= 0
    error('Nearest_Neighbor:propertyValueNotPair', ...
        'Additional arguments must take the form of Property/Value pairs');
end

propertyNames = {'numberofneighbors', 'delaunaymode', 'triangulation', ...
    'radius'};
while length(varargin) ~= 0
    property = varargin{1};
    value    = varargin{2};

    % If the property has been supplied in a shortened form, lengthen it
    iProperty = find(strncmpi(property, propertyNames, length(property)));
    if isempty(iProperty)
        error('Nearest_Neighbor:InvalidProperty', 'Invalid Property');
    elseif length(iProperty) > 1
        error('Nearest_Neighbor:AmbiguousProperty', ...
            'Supplied shortened property name is ambiguous');
    end
    property = propertyNames{iProperty};

    switch property
        case 'numberofneighbors'
            if rem(value, 1) ~= 0 || ...
                    value > length(X) - double(fIndexed) || ...
                    value < 1
                error('Nearest_Neighbor:InvalidNumberOfNeighbors', ...
                    'Number of Neighbors must be an integer, and smaller than the no. of points in X');
            end
            userParams.NumberOfNeighbors = value;

        case 'delaunaymode'
            fOn = strcmpi(value, 'on');
            if strcmpi(value, 'off')
                userParams.DelaunayMode = 'off';
            elseif fOn || strcmpi(value, 'auto')
                if userParams.NumberOfNeighbors ~= 1
                    if fOn
                        warning('Nearest_Neighbor:TooMuchForDelaunay', ...
                            'Delaunay Triangulation method works only for one neighbor');
                    end
                    userParams.DelaunayMode = 'off';
                elseif size(X, 2) < size(X, 1) + 1
                    if fOn
                        warning('Nearest_Neighbor:TooFewDelaunayPoints', ...
                            'Insufficient points to compute Delaunay triangulation');
                    end
                    userParams.DelaunayMode = 'off';

                elseif size(X, 1) == 1
                    if fOn
                        warning('Nearest_Neighbor:DelaunayDimensionOne', ...
                            'Cannot compute Delaunay triangulation for 1D input');
                    end
                    userParams.DelaunayMode = 'off';
                else
                    userParams.DelaunayMode = value;
                end
            else
                warning('Nearest_Neighbor:InvalidOption', ...
                    'Invalid Option');
            end % if strcmpi(value, 'off')

        case 'radius'
            if isscalar(value) && isnumeric(value) && isreal(value) && value > 0
                userParams.Radius = value;
                if isempty(userParams.NumberOfNeighbors)
                    userParams.NumberOfNeighbors = size(X, 2) - double(fIndexed);
                end
            else
                error('Nearest_Neighbor:InvalidRadius', ...
                    'Radius must be a positive real number');
            end

        case 'triangulation'
            if isnumeric(value) && size(value, 2) == size(X, 1) + 1 && ...
                    all(ismember(1:size(X, 2), value))
                userParams.Triangulation = value;
            else
                error('Nearest_Neighbor:InvalidTriangulation', ...
                    'Triangulation not a valid Delaunay Triangulation');
            end
    end % switch property

    varargin(1:2) = [];
end % while
if isempty(userParams.NumberOfNeighbors)
    userParams.NumberOfNeighbors = 1;
end
end %parseinputs


function [y,mm]=v_momfilt(x,r,w,m)
%V_MOMFILT calculates moments of a signal using a sliding window Y=(X,R,W,M)
%
% Inputs: x    is the input signal
%         r    is a list of moments to calculate
%              (+ve = relative to mean, -ve = relative to zero)
%         w    is the window (or just the length of a Hamming window)
%              Note: If the window is asymmetric, you should be aware that it gets
%              flipped in the convolution process
%         m    is the sample of w to use as the centre [default=ceil(length(w)/2+0.5)]
%
%         mm   the actual value of m used. Output point y(i) is based on x(i+m-w:i+m-1).
%
% Example:
%  To calculate a running kurtosis using a Hamming window of length 30:
%             y=v_momfilt(x,[2 4],30); k=y(:,2)./y(:,1).^2

%	   Copyright (C) Mike Brookes 2007
%      Version: $Id: v_momfilt.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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if nargin < 3
   w=hamming(length(x));
elseif  length(w)==1
   w=hamming(w);
end
lw=length(w);
w=w(:);                 % force to a column vector
if nargin < 4
   m=(1+lw)/2;
end
m=max(round(m),1);
mm=m;
r=round(r(:))';             % force integer row vector of moments

lx=prod(size(x));
lxx=lx+m-1;
xx=zeros(lxx,1);
xx(1:lx)=x(:);              % extend with zeros so filter() works correctly

cw=cumsum(w);
sw=cw(end);
y0=repmat(sw,lxx,1);
lxw=min(lxx,lw);
y0(1:lxw)=cw(1:lxw);
y0(lx+1:lx+m-1)=y0(lx+1:lx+m-1)-cw(1:m-1);      % equivalent to y0=filter(w,1,xx^0);
yd=y0(m:end);
yd(abs(yd)<eps)=1;

nr=length(r);
wlx=ones(lx,1);
wlxx=ones(lxx,1);
y=zeros(lx,nr);
mr=max(abs(r));                         % max moment to calculate
mk=zeros(1,mr);                       % list of moments
mk(-r(r<0))=1;                         % choose the moments we need to calculate
maxr=max(r);
if maxr>1
    mk(1:maxr)=1;
end
ml=find(mk>0);
lml=length(ml);
if lml
    mlx=mk;
    mlx(ml)=1:lml;                        % mapping from moment into ml
    wlml=ones(1,lml);
    xm=filter(w,1,xx(:,wlml).^ml(wlxx,:));    % calculate all the moments
    xm=xm(m:end,:)./yd(:,wlml);                             % remove the useless start values and normalize
end
fr=find(r<0);
if length(fr)
    y(:,fr)=xm(:,mlx(-r(fr)));    % zero-centred moments
end
fr=find(r==0);                              % 0'th moment
if length(fr)
    y(:,fr)=1;
end
fr=find(r==1);                              % 1'st moment about mean
if length(fr)
    y(:,fr)=0;
end
fr=find(r==2);                              % 2'nd moment about mean
if length(fr)
    yfr=xm(:,2)-xm(:,1).^2;
    y(:,fr)=yfr(:,ones(1,length(fr)));
end
if maxr>2
    mon=[1 -1];
    bc=[1 -2 1];
    am=zeros(lx,maxr);
    am(:,1)=xm(:,1);                            % copy the mean across
    ml=2:maxr;
    wlml=ones(1,length(ml));
    am(:,2:end)=xm(:,wlml).^ml(ones(lx,1),:);              % calculate powers of the mean
    for i=3:maxr
        bc=conv(bc,mon);                        % calculate binomial coefficients
        fr=find(r==i);
        if length(fr)
            yfr=xm(:,i)+sum(xm(:,i-1:-1:2).*am(:,1:i-2).*bc(wlx,2:i-1),2)+am(:,i)*(bc(i)+bc(i+1));
            y(:,fr)=yfr(:,ones(1,length(fr)));
        end
    end
end


%BITWISE_AND  Calculates the per-element bit-wise conjunction of two arrays or an array and a scalar
%
%     dst = cv.bitwise_and(src1, src2)
%     dst = cv.bitwise_and(src1, src2, 'OptionName', optionValue, ...)
%
% ## Input
% * __src1__ first input array or a scalar.
% * __src2__ second input array or a scalar. In case both are array, they must
%   have the same size and type.
%
% ## Output
% * __dst__ output array that has the same size and type as the input arrays.
%
% ## Options
% * __Mask__ optional operation mask, 8-bit single channel array, that
%   specifies elements of the output array to be changed. Not set by default.
% * __Dest__ Used to initialize the output `dst` when a mask is used. Not set
%   by default.
%
% Computes bitwise conjunction of the two arrays (`dst = src1 & src2`).
%
% The function calculates the per-element bit-wise logical conjunction for:
%
% * Two arrays when `src1` and `src2` have the same size:
%
%       dst(I) = src1(I) AND src2(I) if mask(I) != 0
%
% * An array and a scalar when `src2` is constructed from Scalar or has the
%   same number of elements as `size(src1,3)`:
%
%       dst(I) = src1(I) AND src2 if mask(I) != 0
%
% * A scalar and an array when `src1` is constructed from Scalar or has the
%   same number of elements as `size(src2,3)`:
%
%       dst(I) = src1 AND src2(I) if mask(I) != 0
%
% In case of floating-point arrays, their machine-specific bit representations
% (usually IEEE754-compliant) are used for the operation. In case of
% multi-channel arrays, each channel is processed independently. In the second
% and third cases above, the scalar is first converted to the array type.
%
% See also: cv.bitwise_or, cv.bitwise_xor, cv.bitwise_not, bitand
%


function x = r83s_cg ( n, a, b, x )

%*****************************************************************************80
%
%% R83S_CG uses the conjugate gradient method on an R83S system.
%
%  Discussion:
%
%    The R83S storage format is used for a tridiagonal scalar matrix.
%    The vector A(3) contains the subdiagonal, diagonal, and superdiagonal
%    values that occur on every row.
%
%    The matrix A must be a positive definite symmetric band matrix.
%
%    The method is designed to reach the solution after N computational
%    steps.  However, roundoff may introduce unacceptably large errors for
%    some problems.  In such a case, calling the routine again, using
%    the computed solution as the new starting estimate, should improve
%    the results.
%
%  Example:
%
%    Here is how an R83S matrix of order 5, stored as (A1,A2,A3), would
%    be interpreted:
%
%      A2  A3   0   0   0
%      A1  A2  A3   0   0
%       0  A1  A2  A3   0
%       0   0  A1  A2  A3
%       0   0   0  A1  A2
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Frank Beckman,
%    The Solution of Linear Equations by the Conjugate Gradient Method,
%    in Mathematical Methods for Digital Computers,
%    edited by John Ralston, Herbert Wilf,
%    Wiley, 1967,
%    ISBN: 0471706892,
%    LC: QA76.5.R3.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    N must be positive.
%
%    Input, real A(3), the matrix.
%
%    Input, real B(N), the right hand side vector.
%
%    Input/output, real X(N).
%    On input, an estimate for the solution, which may be 0.
%    On output, the approximate solution vector.
%
  b = b(:);
  x = x(:);
%
%  Initialize
%    AP = A * x,
%    R  = b - A * x,
%    P  = b - A * x.
%
  ap = r83s_mv ( n, n, a, x );

  r(1:n,1) = b(1:n,1) - ap(1:n,1);
  p(1:n,1) = b(1:n,1) - ap(1:n,1);
%
%  Do the N steps of the conjugate gradient method.
%
  for it = 1 : n
%
%  Compute the matrix*vector product AP=A*P.
%
    ap = r83s_mv ( n, n, a, p );
%
%  Compute the dot products
%    PAP = P*AP,
%    PR  = P*R
%  Set
%    ALPHA = PR / PAP.
%
    pap = p' * ap;
    pr = p' * r;

    if ( pap == 0.0 )
      return
    end

    alpha = pr / pap;
%
%  Set
%    X = X + ALPHA * P
%    R = R - ALPHA * AP.
%
    x(1:n,1) = x(1:n,1) + alpha * p(1:n,1);
    r(1:n,1) = r(1:n,1) - alpha * ap(1:n,1);
%
%  Compute the vector dot product
%    RAP = R*AP
%  Set
%    BETA = - RAP / PAP.
%
    rap = r' * ap;

    beta = - rap / pap;
%
%  Update the perturbation vector
%    P = R + BETA * P.
%
    p(1:n,1) = r(1:n,1) + beta * p(1:n,1);

  end

  return
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Q = INVERSEKINEMATIC_KUKA_KR16_2(robot, T)
%   Solves the inverse kinematic problem for the KUKA KR16 2 robot
%   where:
%   robot stores the robot parameters.
%   T is an homogeneous transform that specifies the position/orientation
%   of the end effector.
%
%   A call to Q=INVERSEKINEMATIC_KUKA_KR16_2 returns 8 possible solutions, thus,
%   Q is a 6x8 matrix where each column stores 6 feasible joint values.
%
%
%   Example code:
%
%   robot=load_robot('kuka', 'KUKA_KR16_2');
%   q = [0 0 0 0 0 0];
%   T = directkinematic(robot, q);
%   %Call the inversekinematic for this robot
%   qinv = inversekinematic(robot, T);
%   check that all of them are feasible solutions!
%   and every Ti equals T
%   for i=1:8,
%        Ti = directkinematic(robot, qinv(:,i))
%   end
%	See also DIRECTKINEMATIC.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function q = inversekinematic_kuka_kr16_2(robot, T)

%initialize q,
%eight possible solutions are generally feasible
q=zeros(6,8);

% %Evaluate the parameters
% theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
L6=abs(d(6));

%T= [ nx ox ax Px;
%     ny oy ay Py;
%     nz oz az Pz];
Px=T(1,4);
Py=T(2,4);
Pz=T(3,4);

%Compute the position of the wrist, being W the Z component of the end effector's system
W = T(1:3,3);

% Pm: wrist position
Pm = [Px Py Pz]' - L6*W;

%first joint, two possible solutions admited:
% if q(1) is a solution, then q(1) + pi is also a solution
q1=atan2(Pm(2), Pm(1));

%solve for q2
q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm);
%the other possible solution is q1 + pi
q2_2=solve_for_theta2(robot, [q1+pi 0 0 0 0 0 0], Pm);

%solve for q3
q3_1=solve_for_theta3(robot, [q1 0 0 0 0 0 0], Pm);
%solver for q3 for both cases
q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm);

%the next matrix doubles each column. For each two columns, two different
%configurations for theta4, theta5 and theta6 will be computed. These
%configurations are generally referred as wrist up and wrist down solution
q = [q1         q1         q1        q1       q1+pi   q1+pi   q1+pi   q1+pi;
     q2_1(1)    q2_1(1)    q2_1(2)   q2_1(2)  q2_2(1) q2_2(1) q2_2(2) q2_2(2);
     q3_1(1)    q3_1(1)    q3_1(2)   q3_1(2)  q3_2(1) q3_2(1) q3_2(2) q3_2(2);
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0];

%leave only the real part of the solutions
q=real(q);

%Note that in this robot, the joint q3 has a non-simmetrical range. In this
%case, the joint ranges from 60 deg to -219 deg, thus, the typical normalizing
%step is avoided in this angle (the next line is commented). When solving
%for the orientation, the solutions are normalized to the [-pi, pi] range
%only for the theta4, theta5 and theta6 joints.

%normalize q to [-pi, pi]
q(1,:) = normalize(q(1,:));
q(2,:) = normalize(q(2,:));
% solve for the last three joints
% for any of the possible combinations (theta1, theta2, theta3)
for i=1:2:size(q,2),
    qtemp = solve_spherical_wrist(robot, q(:,i), T, 1,'geometric'); %wrist up
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i)=qtemp;

    qtemp = solve_spherical_wrist(robot, q(:,i), T, -1, 'geometric'); %wrist up
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i+1)=qtemp;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for second joint theta2, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q2 = solve_for_theta2(robot, q, Pm)

%Evaluate the parameters
theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
alpha = eval(robot.DH.alpha);

%See geometry
L2=abs(a(2));
L3=abs(d(4));
A2 = abs(a(3));

%See geometry of the robot
%compute L4
L4 = sqrt(A2^2 + L3^2);

%The inverse kinematic problem can be solved as in the IRB 140 (for example)

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

beta = atan2(-p1(2), p1(1));
gamma = real(acos((L2^2+r^2-L4^2)/(2*r*L2)));

%return two possible solutions
%elbow up and elbow down
%the order here is important and is coordinated with the function
%solve_for_theta3
q2(1) = pi/2 - beta - gamma; %elbow up
q2(2) = pi/2 - beta + gamma; %elbow down

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for third joint theta3, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q3 = solve_for_theta3(robot, q, Pm)

%Evaluate the parameters
theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
alpha = eval(robot.DH.alpha);

%See geometry
L2=abs(a(2));
L3=abs(d(4));

A2 =(a(3));

%See geometry of the robot
%compute L4
L4 = sqrt(A2^2 + L3^2);

%the angle phi is fixed
phi=acos((A2^2+L4^2-L3^2)/(2*A2*L4));

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

beta = real(acos((L2^2 + L4^2 - r^2)/(2*L2*L4)));

%return two possible solutions
%elbow up and elbow down solutions
%the order here is important
q3(1) = pi - phi - beta;
q3(2) = pi - phi + beta;


% Copyright 2019 Jonas Koenemann, Moritz Diehl, University of Freiburg
% Copyright 2015-2018 Jonas Koennemanm, Giovanni Licitra
% Redistribution is permitted under the 3-Clause BSD License terms. Please
% ensure the above copyright notice is visible in any derived work.
%
function [solution,times,problem] = vanderpol

  problem = ocl.Problem(10, @varsfun, @daefun, @pathcosts, 'N', 30);

  % intial state bounds
  problem.setInitialBounds('x',     0);
  problem.setInitialBounds('y',     1);

  % Get and set initial guess
  initialGuess = problem.getInitialGuess();
  initialGuess.states.x.set(-0.2);

  % Run solver to obtain solution
  [solution,times] = problem.solve(initialGuess);

  % plot solution
  figure
  hold on
  plot(times.states.value,solution.states.x.value,'-.','LineWidth',2)
  plot(times.states.value,solution.states.y.value,'--k','LineWidth',2)
  stairs(times.controls.value,solution.controls.F.value,'r','LineWidth',2)
  xlabel('time')
  legend({'x','y','u'})

  snapnow;
end

function varsfun(svh)
  % Scalar x:  -0.25 <= x <= inf
  % Scalar y: unbounded
  svh.addState('x', 'lb', -0.25, 'ub', inf);
  svh.addState('y');

  % Scalar u: -1 <= F <= 1
  svh.addControl('F', 'lb', -1, 'ub', 1);
end

function daefun(daeh,x,~,u,~)
  daeh.setODE('x', (1-x.y^2)*x.x - x.y + u.F);
  daeh.setODE('y', x.x);
end

function pathcosts(ch,x,~,u,~)
  ch.add( x.x^2 );
  ch.add( x.y^2 );
  ch.add( u.F^2 );
end


function pass = test_mldivide(pref)

if ( nargin == 0 )
    pref = chebfunpref();
end

% Generate a few random points to use as test values.
seedRNG(6178);
xr = 2 * rand(100, 1) - 1;

T = restrict(chebpoly(0:3), [-1 -0.5 0 0.5 1]);
L = restrict(legpoly(0:3), [-1 0 1]);

%% Scalar A:
pass(1) = normest(2\T - .5*T) < 10*eps;

%% Numeric A:
B = T.';
A = (1:4).';
x = A\B;
x0 = feval(x, 0);
x0_true = -1/15;
pass(2) = abs(x0 - x0_true) < 10*eps;

A = eye(4);
X = A\B;
X0 = feval(X, 0);
pass(3) = norm(X0 - [1 0 -1 0].') < 10*eps;

%% Transposed A:
A = T.';
B = (1:4).';
X = A\B;
X0 = feval(X, 0);
X0_true = -2.625;
pass(4) = abs(X0 - X0_true) < 100*eps;

%% Else

X = T\L;
C = diag([1 1 .75 .625]);
C(1, 3) = .25;
C(2, 4) = .375;
pass(5) = norm(X - C, inf) < 10*eps;

%% Test a bug from #437

x = chebfun('x');
A = [];
for j = 0:6
   xj = -1 + j/4;
   A = [ A , max(0, 1-4*abs(x-xj)) ];
end
u = A\x;
expected = [...
  -0.999851249504164
  -0.750297500991670
  -0.498958746529156
  -0.253867512891710
   0.014428798095994
   0.196152320507735
   0.700961919873066];
err = norm(u - expected, inf);
pass(6) = err < 1e3*eps;

%% Test on SINGFUN:

% scalar * [1 x INF] = [1 x INF] => scalar\row SINGFUN:
f = chebfun(@(x)sin(20*x)./(x+1), 'exps', [-1 0], 'splitting', 'on');
f = f.';
g = 3\f;
op = @(x) sin(20*x)./(3*(x+1));
g_vals = feval(g, xr);
g_exact = op(xr).';
err = g_vals - g_exact;
pass(7) = norm(err, inf) < 1e5*vscale(g)*eps;

% [1 x INF] * [INF x 1] = scalar => row SINGFUN\scalar:
f = chebfun(@(x)(sin(100*x).^2+1)./((x+1).^0.25), 'exps', [-0.25 0], 'splitting', 'on');
f = f.';
g = f\3;
err = f*g - 3;
pass(8) = abs(err) < 10*vscale(g)*eps;

% [INF x 1] * SCALAR = [INF x 1] => column SINGFUN\column SINGFUN:

f = chebfun(@(x)3*(x.^2+3)./(x+1).^0.4, 'exps', [-0.4 0], 'splitting', 'on');
g = chebfun(@(x)(x.^2+3)./(x+1).^0.4, 'exps', [-0.4 0], 'splitting', 'on');
h = f\g;
err = h - 1/3;
pass(9) = norm(err, inf) < vscale(f)*eps;

% [TODO]: Revive the following test.

%% Test for function defined on unbounded domain:

% % Set the domain:
% dom = [-Inf 3*pi];
% domCheck = [-1e6 3*pi];
%
% % Generate a few random points to use as test values:
% x = diff(domCheck) * rand(100, 1) + domCheck(1);
%
% % A*X = B, where both A and B are UNBNDFUNs ==> X = A\B
%
% opA = @(x) [exp(x) x.*exp(x) (1-exp(x))./x];
% A = chebfun(opA, dom);
% opB = @(x) [(2*x+1).*exp(x) exp(x) 2*(1-exp(x))./x];
% B = chebfun(opB, dom);
% X = A\B;
% res = A*X - B;
% err = feval(res, x);
% pass(10) = norm(err(:), inf) < max([eps*get(A,'vscale') ...
%     eps*get(B,'vscale')]);

end


%% Double Pendulum
% Clear work space and initaliza figure
clear; clc; close all; %figure;

%% Initial and Constant Values

% Initial values for tuned state (1):
% gamma0    = pi/12;
% dtgamma0  = pi;
% alpha0    = 0;
% dtalpha0  = 0;

% Initial values for tuned state (2):
% gamma0    = 0;
% dtgamma0  = 0;
% alpha0    = pi/6;
% dtalpha0  = pi/36;

% Initial values for tuned state (3):
% gamma0    = 0;
% dtgamma0  = 0;
% alpha0    = pi/24;
% dtalpha0  = pi/2;

% Initial values for chaotic solution:
% gamma0    = 0;
% dtgamma0  = 0;
% alpha0    = pi/2;
% dtalpha0  = 5;

% Initial values for chaotic solution2:
gamma0    = 0;
dtgamma0  = 0;
alpha0    = pi/2;
dtalpha0  = 5.025;

% Constant Values
beta0     = pi/2;
lambda    = 2.74; %rad/s
omega     = 5.48; %rad/s
psi       = 0.96;
eta       = 0.24;

fps       = 10;
%movie     = true;

%% Using solver ode45

duration  = 100;
ivp       = [gamma0; dtgamma0; alpha0; dtalpha0; beta0; lambda; omega; psi; eta];
[t,x]     = ode45(@dpendulum,[0 duration], ivp);

%options = odeset('RelTol',1e-4,'AbsTol',[1e-4 1e-4 1e-5]);
%[t,x]=ode45(@dpendulum,[0 duration], ivp, options);

%% Plot

nframes=duration*fps;
%t = linspace(0,duration,nframes);
%x = deval(sol,t);

gamma    = x(:,1);
dtgamma  = x(:,2);
alpha    = x(:,3);
dtalpha  = x(:,4);
% we assume:
l1=1; % b distance
l2=2; % c distance

%% Plot alpha vs Gamma
figure;
plot (t,x(:,3),'-.',t,x(:,1),'-')

%% Plot d_alpha vs d_Gamma
figure;
plot (t,x(:,4),'-.',t,x(:,2),'-')


function s = ClenshawL(c,x)
% Clenshaw algorithmus for evaluating Legendre series
%
% Description
%
% $$ s = \sum_n=0^N c_n P_n(x)$$
%
% Input
%  c - coefficients
%  x - evaluation nodes
%
% Output
%  s - Legendre series
%

% init
c= [c(:);0;0];
dn = zeros(size(x));
d1 = zeros(size(x));
d2 = zeros(size(x));

% use backward tree term recurence
for l = length(c)-2:-1:1

  d1 = d2 + (2*l+1)/(l+1) * x .* dn;
  d2 = c(l) - l/(l+1) * dn;
  dn = d1;

end

s = d2 + x .* d1;

end


function [kls,xx1,xx2] = vbmc_kldiv(vp1,vp2,Ns,gaussflag)
%VBMC_KLDIV Kullback-Leibler divergence between two variational posteriors.
%   KLS = VBMC_KLDIV(VP1,VP2) returns an estimate of the (asymmetric)
%   Kullback-Leibler (KL) divergence between two variational posterior
%   distributions VP1 and VP2. KLS is a 2-element vector whose first element
%   is KL(VP1||VP2) and the second element is KL(VP2||VP1). The symmetrized
%   KL divergence can be computed as mean(KLS).
%
%   KLS = VBMC_KLDIV(VP1,VP2,NS) uses NS random draws to estimate each
%   KL divergence (default NS=1e5).
%
%   KLS = VBMC_KLDIV(VP1,VP2,NS,GAUSSFLAG) computes the "Gaussianized"
%   KL-divergence if GAUSSFLAG=1, that is the KL divergence between two
%   multivariate normal distibutions with the same moments as the variational
%   posteriors given as inputs. Otherwise, the standard KL-divergence is
%   returned for GAUSSFLAG=0 (default).
%
%   [KLS,XX1,XX2] = VBMC_KLDIV(...) returns NS samples from the variational
%   posteriors VP1 and VP2 as, respectively, NS-by-D matrices XX1 and XX2,
%   where D is the dimensionality of the problem.
%
%   If GAUSSFLAG is 1, VP1 and/or VP2 can be N-by-D matrices of samples
%   from variational posteriors (they do not need have the same number
%   of samples).
%
%   See also VBMC, VBMC_MTV, VBMC_PDF, VBMC_RND, VBMC_DIAGNOSTICS.

if nargin < 3 || isempty(Ns); Ns = 1e5; end
if nargin < 4 || isempty(gaussflag); gaussflag = false; end

% This was removed because the comparison *has* to be in original space,
% given that the transform might change for distinct variational posteriors
% if nargin < 5 || isempty(origflag); origflag = true; end
origflag = true;

kls = NaN(1,2);

if ~gaussflag && (~vbmc_isavp(vp1) || ~vbmc_isavp(vp2))
    error('vbmc_kldiv:WrongInputs', ...
        'Unless the KL divergence is Gaussianized, VP1 and VP2 need to be variational posteriors.');
end

%try
    if gaussflag
        if Ns == 0  % Analytical calculation
            if origflag
                error('vbmc_kldiv:NoAnalyticalMoments', ...
                    'Analytical moments are available only for the transformed space.')
            end
            [q1mu,q1sigma] = vbmc_moments(vp1,0);
            [q2mu,q2sigma] = vbmc_moments(vp2,0);
            xx1 = []; xx2 = [];
        else        % Numerical moments
            if vbmc_isavp(vp1)
                [q1mu,q1sigma] = vbmc_moments(vp1,origflag,Ns);
            else
                q1mu = mean(vp1,1);
                q1sigma = cov(vp1);
            end
            if vbmc_isavp(vp2)
                [q2mu,q2sigma] = vbmc_moments(vp2,origflag,Ns);
            else
                q2mu = mean(vp2,1);
                q2sigma = cov(vp2);
            end
        end
        [kls(1),kls(2)] = mvnkl(q1mu,q1sigma,q2mu,q2sigma);

    else
        MINP = realmin;

        xx1 = vbmc_rnd(vp1,Ns,origflag,1);
        q1 = vbmc_pdf(vp1,xx1,origflag);
        q2 = vbmc_pdf(vp2,xx1,origflag);
        q1(q1 == 0 | ~isfinite(q1)) = 1;    % Ignore these points
        q2(q2 == 0 | ~isfinite(q2)) = MINP;
        kls(1) = -mean(log(q2) - log(q1));

        xx2 = vbmc_rnd(vp2,Ns,origflag,1);
        q1 = vbmc_pdf(vp1,xx2,origflag);
        q2 = vbmc_pdf(vp2,xx2,origflag);
        q1(q1 == 0 | ~isfinite(q1)) = MINP;
        q2(q2 == 0 | ~isfinite(q2)) = 1;    % Ignore these points
        kls(2) = -mean(log(q1) - log(q2));

    end

    kls = max(kls,0); % Correct for numerical errors

%catch

    % Could not compute KL divs

%end


function Stock_PCA()
clc; clear all; clearvars
addpath('functions_IO');

%
% Download all necessary data
%
finviz_dir = '/Users/ansonwong/Desktop/MLF_data/finviz'; % finviz file will be finviz.csv
chart_dir = '/Users/ansonwong/Desktop/MLF_data/charts'; % charts will be XXXX.txt
%datestr_use = datestr(now,'mmm_dd_yyyy');

need_download_finviz = 0; % downloads finviz.csv and places it in finviz_dir
need_download_chart = 0; % downloads all charts into chart_dir based on finviz.csv
need_convert_finviz = 0; % create a finviz.mat that is based off of finviz.csv data (numbers/NaN only)

% Download finviz and chart data (as well as converting finviz data to a
% convenient format)
if(need_download_finviz == 1)
  download_finviz_data(finviz_dir);
end
if(need_download_chart == 1)
  download_chart_data(chart_dir,finviz_dir)
end
if(need_convert_finviz == 1)
  data2num_finviz(finviz_dir)
end

%
% Analyze data now
%

% Load all data
% Now we have 'FHEAD', 'FHEAD_names', 'name_cell', 'data_label', 'data'
finviz2_filename = strcat(finviz_dir,'/finviz.mat');
M = load(finviz2_filename,'-mat');

% Set new data variables
X = M.data'; % n=7091 x d=62
XS = M.name_cell; % n=7091 x 4
FH = M.FHEAD; % structure array with indices for X
FH_name = M.FHEAD_names; % cell array of string n=7091 x 1
clear M
[n,d]=size(X);

%{
    'MarketCap'
    'PE'
    'PEForward'
    'PEG'
    'PS'
    'PB'
    'PCash'
    'PFreeCash'
    'DividendYield'
    'PayoutRatio'
    'EPS'
    'EPSGrowthThisYear'
    'EPSGrowthNextYear'
    'EPSGrowthPast5Years'
    'EPSGrowthNext5Years'
    'SalesGrowth5Years'
    'EPSGrowthLastQuarter'
    'SalesGrowthLastQuarter'
    'SharesOutstanding'
    'Float'
    'InsiderOwnership'
    'InsiderTransactions'
    'InstitutionalOwnership'
    'InstitutionalTransactions'
    'FloatShort'
    'ShortRatio'
    'ReturnOnAssets'
    'ReturnOnEquity'
    'ReturnOnInvestment'
    'CurrentRatio'
    'QuickRatio'
    'LTDebtEquity'
    'TotalDebtEquity'
    'GrossMargin'
    'OperatingMargin'
    'ProfitMargin'
    'PerformanceWeek'
    'PerformanceMonth'
    'PerformanceQuarter'
    'PerformanceHalfYear'
    'PerformanceYear'
    'PerformanceYTD'
    'Beta'
    'AverageTrueRange'
    'VolatilityWeek'
    'VolatilityMonth'
    'SMA20day'
    'SMA50day'
    'SMA200day'
    'High50day'
    'Low50day'
    'High52week'
    'Low52week'
    'RSI14'
    'ChangeFromOpen'
    'Gap'
    'AnalystRecommendation'
    'AverageVolume'
    'RelativeVolume'
    'Price'
    'ChangePercentage'
    'Volume'
    'TargetPrice'
%}

%{
corrcoef(X(:,FH.ReturnOnAssets),X(:,FH.ReturnOnAssets))

%}
% Find the NaN value fractions
if(0)
  fprintf('[n=%d,d=%d]\n',n,d);
  FID = fopen('stats_pca.txt','w');
  for j=1:d
    n_nonnan = sum(~isnan(X(:,j)));
    fprintf('%d) %s (%.0f%% good)\n',j,FH_name{j},100*n_nonnan/n);
    fprintf(FID,'%d) %s (%.0f%% good)\n',j,FH_name{j},100*n_nonnan/n);
  end
  fclose(FID);
end

%
% Find a subset of X
%
%index_Xsubset = find(X(:,FH.MarketCap)>100);
index_Xsubset = find(X(:,FH.AverageVolume)>1000);
fprintf('index_subset = %d\n',length(index_Xsubset));
X = X(index_Xsubset,:);

%plot_2var(1,FH.ReturnOnAssets,FH.ChangePercentage,X,FH_name);

% Plot between 2 variables
% 1) Gap, ChangePercentage
% 2)
%plot_2var(1,FH.ReturnOnAssets,FH.ChangePercentage,X,FH_name);
%plot_2var(1,FH.ReturnOnAssets,FH.ChangePercentage,X,FH_name);
%plot_2var(1,FH.ReturnOnEquity,FH.ChangePercentage,X,FH_name);
%plot_2var(1,FH.ReturnOnEquity,FH.ChangePercentage,X,FH_name);

%
% Use features and reduce to non-NaN, X_nonnan
%
j_list = ...
  [FH.FloatShort,...
   FH.ChangePercentage];

size(X)
[X,indices_nan] = collect_nonnan(X,j_list);
size(X)

rho = corr(X,'type','Pearson')
figure(2)
clf(2)
plot(X(:,1),X(:,2),'.');

%[X,indices_nan] = collect_nonnan(X,1:d);

%
% Normalize Xtilde_nonnan = X_nonnan
%
X = standardizeCols(X);

%
% Perform PCA
%
[coeff,score,latent,tsquared,explained] = pca(X);

coeff

for i=1:length(explained)
  fprintf('PC %d = %.1f%%\n',i,explained(i));
end

%{
indices_nan = zeros(n,1);
for j=length(j_list),
  indices_nan = indices_nan + isnan(X(:,j_list(j)));
end
indices_nan = indices_nan > 0;
X(indices_nan,:) = []; % remove tickers without marketcap info

size(X)
sum(~isnan(X))
%}

clearvars
rmpath('functions_IO');
end

% Given a j_list, reduces the list so that we have no nans
function [X_new,indices_nan] = collect_nonnan(X,j_list)
[n,d]=size(X);

% Find ticker indices that are nan in j_list
indices_nan = zeros(n,1);
for j=1:length(j_list),
  indices_nan = indices_nan + isnan(X(:,j_list(j)));
end
indices_nan = indices_nan > 0;
indices_nonnan = ~indices_nan;

% Make new X
X_new = X(indices_nonnan,:);
vec_setempty = 1:d;
vec_setempty(j_list) = [];
X_new(:,vec_setempty) = [];

% Make sure every value of X_new is non-nan
if( any( diff(sum(~isnan(X_new))) ) )
  error('collect_nonnan did not work')
end

end

function plot_2var(fignum,jx,jy,X,FH_name)
index_nonnan = find(~isnan(X(:,jx)) & ~isnan(X(:,jy)));
xplot = X(index_nonnan,jx); yplot = X(index_nonnan,jy);
figure(fignum)
clf(fignum)
plot(xplot,yplot,'.')
hold on
xlabel(FH_name{jx});
ylabel(FH_name{jy});
hold off
end


function p00_jac_test ( problem_num )

%*****************************************************************************80
%
%% P00_JAC_TEST compares the jacobian to a finite difference estimate.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 September 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer PROBLEM_NUM, the number of problems.
%
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'P00_JAC_TEST\n' );
  fprintf ( 1, '  Find the maximum relative difference between the\n' );
  fprintf ( 1, '  jacobian and a finite difference estimate.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   Problem    Option      Diff               I         J\n' );

  for problem = 1 : problem_num

    option_num = p00_option_num ( problem );

    fprintf ( 1, '\n' );

    for option = 1 : option_num

      nvar = p00_nvar ( problem, option );

      x0 = p00_start ( problem, option, nvar );

      for i = 1 : nvar
        [ dx, seed ] = r8_uniform_01 ( seed );
        x0(i) = x0(i) + 0.01 * dx;
      end

      [ max_adif, max_adif_i, max_adif_j, max_rdif, max_rdif_i, max_rdif_j ] = ...
        p00_jac_check ( problem, option, nvar, x0 );

      fprintf ( 1, '  %8d  %8d  %14e  %8d  %8d\n', ...
        problem, option, max_rdif, max_rdif_i, max_rdif_j );

    end
  end

  return
end


function ccii_1_tabulate ( )

%*****************************************************************************80
%
%% CCII_1_TABULATE tabulates CCII_1 quadrature rules for the Hermite integral.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 May 2014
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CCII_1_TABULATE\n' );
  fprintf ( 1, '  Tabulate CCII_1 quadrature rules for the Hermite integral.\n' );
  fprintf ( 1, '  Density function rho(x) = 1.\n' );
  fprintf ( 1, '  Region: -oo < x < +oo.\n' );
  fprintf ( 1, '  Exactness: NONE.\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Print the first 5 rules:\n' );
  fprintf ( 1, '   I          X(I)            W(I)\n' );
  fprintf ( 1, '\n' );

  ell = 1.0;

  for n = 1 : 5

    [ x, w ] = ccii_1 ( n, ell );

    fprintf ( 1, '\n' );
    for i = 1 : n
      fprintf ( 1, '  %2d  %14.6f  %14.6f\n', i, x(i), w(i));
    end
    fprintf ( 1, ' Sum                  %14.6f  %14.6f\n', sum ( w(1:n) ) );

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Print the first 5 nested rules:\n' );
  fprintf ( 1, '   I          X(I)            W(I)\n' );
  fprintf ( 1, '\n' );

  ell = 1.0;

  for j = 1 : 5

    n = 2^j - 1;

    [ x, w ] = ccii_1 ( n, ell );

    fprintf ( 1, '\n' );
    for i = 1 : n
      fprintf ( 1, '  %2d  %14.6f  %14.6f\n', i, x(i), w(i) );
    end
    fprintf ( 1, ' Sum                  %14.6f\n', sum ( w(1:n) ) );

  end

  return
end