function features = values_to_features(values)
	% Convert a time-series obtained from a sample to a feature using
	% different kind of statistics over the values and the derivatives

	mean_val = nanmean(values, 2);
	variance = nanvar(values, 0, 2);
	% feature_skewness = skewness(values(~isnan(values)));
	% feature_kurtosis = kurtosis(values(~isnan(values)));

	abs_delta = abs(values(:, 2:end) - values(:, 1:end-1));
	mean_delta = nanmean(abs_delta, 2);
	var_delta = nanvar(abs_delta, 0, 2);

	maximum = max(values, [], 2);
	minimum = min(values, [], 2);

	features = [mean_val; variance; ...
	%           feature_skewness; feature_kurtosis; ...
                mean_delta; var_delta; maximum; minimum];
end


function val=calcGAE(xTrue,xEst,is3D)
%%CALCGAE Compute the scalar geometric average error (GAE). Unlike the
%         RMSE, it is not dominated by large individual terms.
%
%INPUTS: xTrue The truth data. This is either an xDimXNumSamples matrix or
%              an xDimXNXnumSamples matrix. The latter formulation is
%              useful when the MSE over multiple Monte Carlo runs of an
%              entire length-N track is desired. In the first formulation,
%              we take N=1. Alternatively, if the same true value is used
%              for all numSamples, then xTrue can just be an xDimXN matrix.
%              Alternatively, if xTrue is the same for all numSamples and
%              all N, then just an xDimX1 matrix can be passed. N and
%              numSamples are inferred from xEst.
%         xEst An xDimXnumSamples set of estimates or an xDimXNXnumSamples
%              set of estimates (if values at N times are desired).
%         is3D An optional indicating that xEst is 3D. This is only used if
%              xEst is a matrix. In such an instance, there is an ambiguity
%              whether xEst is truly 2D, so N=1, or whether numSamples=1
%              and xEst is 3D with the third dimension being 1. If this
%              parameter is omitted or an empty matrix is passed, it is
%              assumed that N=1.
%
%OUTPUTS: val The 1XN set of scalar GAE values.
%
%The GAE is given in Equation 3 in [1].
%
%EXAMPLE:
%For a Gaussian random vector, the root-trace of the covariance matrix is
%the RMSE. The RMSE is larger than the average Euclidean error, which is
%also larger than the  geometric average error
% R=[28,   4, 10;
%     4,  22, 16;
%     10, 16, 16];%The covariance matrix.
% xTrue=[10;-20;30];
% numRuns=100000;
% xEst=GaussianD.rand(numRuns,xTrue,R);
% valRMSE=rootTrace=sqrt(trace(R))
% valAEE=calcAEE(xTrue,xEst)
% valGAE=calcGAE(xTrue,xEst)
%
%REFERENCES:
%[1] X. R. Li and Z. Zhao, "Measures of performance for evaluation of
%    estimators and filters," in Proceedings of SPIE: Conference on Signal
%    and Data processing of Small Targets, vol. 4473, San Diego, CA, 29
%    Jul. 2001, pp. 530-541.
%
%February 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<3||isempty(is3D))
    is3D=false;
end

xDim=size(xEst,1);
if(ismatrix(xEst)&&is3D==false)
    N=1;
    numSamples=size(xEst,2);
    xEst=reshape(xEst,[xDim,1,numSamples]);

    if(size(xTrue,2)==1)
        %If the true values are the same for all samples.
        xTrue=repmat(xTrue,[1,1,numSamples]);
    else
        xTrue=reshape(xTrue,[xDim,1,numSamples]);
    end
else
    N=size(xEst,2);
    numSamples=size(xEst,3);

    if(ismatrix(xTrue))
        if(size(xTrue,2)==1)
            %If the true values are the same for all samples and for all N.
            xTrue=repmat(xTrue,[1,N,numSamples]);
        else
            %If the true values are the same for all samples.
            xTrue=repmat(xTrue,[1,1,numSamples]);
        end
    end
end

val=zeros(1,N);
for k=1:N
    val(k)=exp((1/(2*numSamples))*sum(log(sum((xEst(:,k,:)-xTrue(:,k,:)).^2,1))));
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 value = error_f ( x )

%*****************************************************************************80
%
%% ERROR_F computes the error function.
%
%  Discussion:
%
%    This function was renamed "ERROR_F" from "ERF", to avoid a conflict
%    with the name of a corresponding routine often, but not always,
%    supplied as part of the math support library.
%
%    The definition of the error function is:
%
%      ERF(X) = ( 2 / SQRT ( PI ) ) * Integral ( 0 <= T <= X ) EXP ( -T**2 ) dT
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 March 2007
%
%  Author:
%
%    FORTRAN77 original version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    David Kahaner, Cleve Moler, Steven Nash,
%    Numerical Methods and Software,
%    Prentice Hall, 1989,
%    ISBN: 0-13-627258-4,
%    LC: TA345.K34.
%
%  Parameters:
%
%    Input, real X, the argument of the error function.
%
%    Output, real VALUE, the value of the error function at X.
%
  persistent nterf;
  persistent sqeps;
  persistent xbig;

  erfcs = [ ...
    -0.049046121234691808, -0.14226120510371364, ...
     0.010035582187599796, -0.000576876469976748, ...
     0.000027419931252196, -0.000001104317550734, ...
     0.000000038488755420, -0.000000001180858253, ...
     0.000000000032334215, -0.000000000000799101, ...
     0.000000000000017990, -0.000000000000000371, ...
     0.000000000000000007 ];

  sqrtpi = 1.7724538509055160;
%
%  Initialize the Chebyshev series.
%
  if ( size ( nterf ) == 0 )
    nterf = inits ( erfcs, 13, 0.1 * eps );
    xbig = sqrt ( - log ( sqrtpi * eps ) );
    sqeps = sqrt ( 2.0 * eps );
  end

  y = abs ( x );

  if ( y <= sqeps )
    value = 2.0 * x / sqrtpi;
  elseif ( y <= 1.0 )
    value = x * ( 1.0 + csevl ( 2.0 * x * x - 1.0, erfcs, nterf ) );
  elseif ( y <= xbig )
    value = r8_sign ( x ) * ( 1.0 - error_fc ( y ) );
  else
    value = r8_sign ( x );
  end

  return
end


function hypersphere_properties_test01 ( )

%*****************************************************************************80
%
%% HYPERSPHERE_PROPERTIES_TEST01 tests the coordinate conversion routines.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 December 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'HYPERSPHERE_PROPERTIES_TEST01\n' );
  fprintf ( 1, '  Test the coordinate conversion routines:\n' );
  fprintf ( 1, '  CARTESIAN_TO_HYPERSPHERE: X       -> R,Theta\n' );
  fprintf ( 1, '  HYPERSPHERE_TO_CARTESIAN: R,Theta -> X.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Pick a random X, and compute X2 by converting X\n' );
  fprintf ( 1, '  to hypersphere and back.  Consider norm of difference.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  M    || X - X2 ||\n' );
  seed = 123456789;
  n = 1;
  for m = 1 : 5
    fprintf ( 1, '\n' );
    for test = 1 : 5
      [ x, seed ] = r8mat_uniform_01 ( m, n, seed );
      [ c, seed ] = r8vec_uniform_01 ( m, seed );
      [ r, theta ] = cartesian_to_hypersphere ( m, n, c, x );
      x2 = hypersphere_to_cartesian ( m, n, c, r, theta );
      err = norm ( x - x2 );
      fprintf ( 1, '  %d  %g\n', m, err );
    end
  end

  return
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  Gaussian Process Regression 2D Example  %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%             1) Load 2D Regression Datasets                 %%
%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Example of Gaussian Process Regression (2D)
%% Generate Random Gaussian distribution
clear all; close all; clc;
K = 90;
a =-50;
b = 50;
x = a + (b-a).*rand(K,1);
y = a + (b-a).*rand(K,1);

a    = 40;
b    = 100;
vars = a + (b-a).*rand(K,1);

Mu    = [x,y]';
Sigma = zeros(2,2,K);

for k=1:K
    Sigma(:,:,k) = eye(2,2) .* vars(k);
end

gmm_x.Priors    = ones(1,K)./K;
gmm_x.Mu        = Mu;
gmm_x.Sigma     = Sigma;

% Generate some training Data
X = ml_gmm_sample(500,gmm_x.Priors,gmm_x.Mu,gmm_x.Sigma )';
f = @(X)ml_gmm_pdf(X',gmm_x.Priors,gmm_x.Mu,gmm_x.Sigma );
y = f(X);
y = y(:);

% Plot Training Data

% Plot Real Function
options           = [];
options.title     = 'Training data';
options.surf_type = 'surf';

if exist('h1','var') && isvalid(h1), delete(h1);end
h1 = ml_plot_value_func(X,f,[1 2],options);hold on

% Plot Training Data
options.bFigure     = false;
options.surf_type   = 'scatter';
options.points_size = 20;
ml_plot_value_func(X,f,[1 2],options);hold on

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%                 2) "Train" GPR Model                       %%
%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Train GP (no real training, just give parameters)
epsilon         = 0.1;
dims            = 2;
model.X_train   = X;
model.y_train   = y;
rbf_var         = 25;
gp_f            = @(X)ml_gpr(X,[],model,epsilon,rbf_var);

% Plot Estimated Function
options           = [];
options.title     = 'Estimated y=f(x) from Gaussian Process Regression';
options.surf_type = 'surf';
if exist('h2','var') && isvalid(h2), delete(h2);end
h2 = ml_plot_value_func(X,gp_f,[1 2],options);hold on

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%           3) Test GPR Model on Train points                %%
%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Test GPR
options           = [];
options.bFigure   = true;
options.title     = 'Test data';
options.surf_type = 'pcolor';
dims              = [1,2];

if exist('h2','var') && isvalid(h2), delete(h2);end
h2 = gp_plot(model.X_train,gp_f,dims,options);
colorbar

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%           4) Grid Search for GPR with RBF Kernel           %%
%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% K-fold cross validation
Kfold = 20;

disp('Parameter grid search GP');

rbf_vars = [5,10,20,40,50,100,10000];

test  = cell(length(rbf_var),1);
train = cell(length(rbf_var),1);

for i=1:length(rbf_vars)
    disp(['[' num2str(i) '/' num2str(length(rbf_vars)) ']']);

    f                       = @(X,y,model)ml_gpr(X,y,model,epsilon,rbf_vars(i));
    [test_eval,train_eval]  = ml_kcv(X,y,Kfold,f,'regression');

    test{i}                 = test_eval;
    train{i}                = train_eval;
    disp(' ');
end

%% Get Statistics

[ stats ] = ml_get_cv_grid_states_regression(test,train);

%% Plot Statistics

options             = [];
options.title       = 'GPR k-CV';
options.metrics     = {'nrmse','r'};     % <- you can add many other metrics, see list in next cell box
options.para_name   = 'variance rbf';

if exist('handle','var'), delete(handle); end
[handle,handle_test,handle_train] = ml_plot_cv_grid_states_regression(stats,rbf_vars,options);

%% Full list of evaluation metrics for regression methods

options.metric = {'mse','nmse','rmse','nrmse','mae','mare','r','d','e','me','mre'};

%   '1'  - mean squared error                               (mse)
%   '2'  - normalised mean squared error                    (nmse)
%   '3'  - root mean squared error                          (rmse)
%   '4'  - normalised root mean squared error               (nrmse)
%   '5'  - mean absolute error                              (mae)
%   '6'  - mean  absolute relative error                    (mare)
%   '7'  - coefficient of correlation                       (r)
%   '8'  - coefficient of determination                     (d)
%   '9'  - coefficient of efficiency                        (e)
%  '10'  - maximum absolute error                           (me)
%  '11'  - maximum absolute relative error                  (mre)


function kern = gibbsKernParamInit(kern)

% GIBBSKERNPARAMINIT GIBBS kernel parameter initialisation.
% This is the non stationary kernel proposed by Mark Gibbs in his 1997
% thesis. It is similar to an RBF but has a length scale that varies
% with input location. This leads to an additional term in front of
% the kernel.
%
% Given
% r = sqrt((x_i - x_j)'*(x_i - x_j))
%
% we have
% k(x_i, x_j) = sigma2*Z*exp(-r^2/(l(x)*l(x) + l(x')*l(x')))
%
% where
% Z = sqrt(2*l(x)*l(x')/(l(x)*l(x) + l(x')*l(x'))
%
% The parameters are sigma2, the process variance (kern.variance),
% and the parameters of l(x) which is a function that can be specified by the user, by default an MLP is used.
%
% SEEALSO : mlpCreate, gibbsKernSetLengthScaleFunc
%
% FORMAT
% DESC initialises Mark Gibbs's non-stationary
%  kernel structure with some default parameters.
% ARG kern : the kernel structure which requires initialisation.
% RETURN kern : the kernel structure with the default parameters placed in.
%
% SEEALSO : kernCreate, kernParamInit
%
% COPYRIGHT : Neil D. Lawrence, 2006

% KERN

kern.variance = 1;
options = mlpOptions(5);
kern.lengthScaleFunc = modelCreate('mlp', kern.inputDimension, 1, options);
kern.lengthScaleTransform = optimiDefaultConstraint('positive');

kern.nParams = 1+kern.lengthScaleFunc.numParams;

kern.transforms.index = kern.nParams;
kern.transforms.type = optimiDefaultConstraint('positive');

kern.isStationary = false;


function [pos vel] = IntKalman(z)
%
%
persistent A H Q R
persistent x P
persistent firstRun

if isempty(firstRun)
  firstRun = 1;

  dt = 0.1;

  A = [ 1 dt;
        0 1  ];
  H = [0 1];

  Q = [ 1 0;
        0 3 ];
  R = 10;

  x = [ 0 20 ]';
  P = 5*eye(2);
end

xp = A*x;
Pp = A*P*A' + Q;

K = Pp*H'*inv(H*Pp*H' + R);

x = xp + K*(z - H*xp);
P = Pp - K*H*Pp;

pos = x(1);
vel = x(2);


function J=polAzEquiDistRefChangeCrossGrad(pAzPt,latLonRefOld,latLonRefNew,rE)
%%POLAZEQUIDISTREFCHANGECROSSGRAD Given a point in polar azimuthal
%      coordinates with respect to a reference position on a spherical
%      Earth, find the gradient of the point in a polar azimuthal
%      coordinate system with a difference reference point taken with
%      respect to the point in the original system. This is the gradient of
%      the polAzEquidistRefChange function.
%
%INPUTS: pAzPts A 2XN set of the [ground distance; heading] points, with
%              the heading given in radians East of North, to convert.
%              Alternatively, if heights are also given, this can be a 3XN
%              set of points with the height being the third dimension.
% latLonRefOld A 2X1 [latitude;longitude] reference point in radians about
%              which pAzPts was computed.
% latLonRefNew A 2X1 [latitude;longitude] reference point in radians with
%              respect to which the function to be differentied is defined.
%           rE The radius of the reference sphere. If this argument is
%              omitted or an empty matrix is passed, the value in
%              Constants.WGS84MeanRadius is used.
%
%OUTPUTS: J A 2X2XN matrix holding the gradient of the transformation
%           evaluated at each point in pAzPts (or a 3X3XN matrix if height
%           is also provided). If one says that a point in pAzPts=[p;az;h]
%           and a converted point is [p1;az1;h1], the gradients in 3D are
%           ordered:
%           [dp1dp,   dp1dAz,  dp1dh;
%            dAz1dp, dAz1dAz, dAz1dh;
%            dh1dp,   dh1dAz,  dh1dh];
%
%The gradient is the analytic gradient of the spherical Earth conversion
%given in the function polAzEquidistRefChange. Due to a singularity, the
%gradient is not valid if the target is exactly on the opposite side of the
%Earth from the first sensor.
%
%EXAMPLE:
%Given three random points, the accuracy of this function is compared to
%numeric differentiation. The relative error is on the order of what one
%might expect due to finite precision limitations.
% rE=Constants.WGS84MeanRadius;
% latLonRef1Old=[UniformD.rand(1,[-pi/2;pi/2]);
%                UniformD.rand(1,[-pi;pi])];
% latLonRefNew=[UniformD.rand(1,[-pi/2;pi/2]);
%               UniformD.rand(1,[-pi;pi])];
% latLonPt=[UniformD.rand(1,[-pi/2;pi/2]);
%           UniformD.rand(1,[-pi;pi])];
% pAzPt=ellips2PolarAzEquidistProj(latLonPt,latLonRef1Old,rE,0);
% J=polAzEquiDistRefChangeCrossGrad(pAzPt,latLonRef1Old,latLonRefNew,rE);
% f=@(x)polAzEquidistRefChange(x,latLonRef1Old,latLonRefNew,rE,0);
% JNumDiff=numDiff(pAzPt,f,2);
% RelErr=max(max(abs((J-JNumDiff)./JNumDiff)))
%
%December 2021 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<3||isempty(rE))
    rE=Constants.WGS84MeanRadius;
end

numPoints=size(pAzPt,2);
if(all(latLonRefOld==latLonRefNew))
    %For the special case of no change, force the result to be exact (avoid
    %finite precision issues).
    numDim=size(pAzPt,1);%2 or 3.
    J=repmat(eye(numDim,numDim),[1,1,numPoints]);
    return;
end

azStart=greatCircleAzimuth(latLonRefOld,latLonRefNew);
azOffset1=pi/2-azStart;
lambda0=greatCircleDistance(latLonRefOld,latLonRefNew,1);

if(size(pAzPt,1)==3)
    J=zeros(3,3,numPoints);
    J(3,3,:)=1;
else
    J=zeros(2,2,numPoints);
end

%Transform into the system with both sensors on the equator.
pAzPt(2,:)=pAzPt(2,:)+azOffset1;

p=pAzPt(1,:);
az=pAzPt(2,:);

cosPre=cos(p/rE);
sinPre=sin(p/rE);

cosLambda0=cos(lambda0);
sinLambda0=sin(lambda0);
sinAz=sin(az);
cosAz=cos(az);

denom2=1-(cosPre.*cosLambda0+sinPre.*sinAz.*sinLambda0).^2;
denom1=sqrt(denom2);

dp0dp=(cosLambda0.*sinPre-cosPre.*sinAz.*sinLambda0)./denom1;
dp0dAz=-rE.*sinPre.*cosAz.*sinLambda0./denom1;

dAz0dp=cosAz.*sinLambda0./(rE.*denom2);
dAz0dAz=sinPre.*(cosLambda0.*sinPre-cosPre.*sinAz.*sinLambda0)./denom2;

J(1,1,:)=dp0dp;
J(2,1,:)=dAz0dp;
J(1,2,:)=dp0dAz;
J(2,2,:)=dAz0dAz;

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.


% a=0.005, x^2
% a=0.0002, abs(x)^3
% a=0.0001, x^4

a = 0.005;
b = 0.00025;
xs = 0:50;
ys2 = zeros(length(xs), 1);
ys3 = zeros(length(xs), 1);
for i = 1:length(xs)
    ys2(i) = exp(-a*abs(xs(i))^2);
    ys3(i) = exp(-b*abs(xs(i))^3);
end
plot(xs, ys2);
hold on;
plot(xs, ys3);
hold off;


% Extract instantaneous phase from a signal
%
% This function extracts instantaneous phase of a signal based on Hilbert transform.
%
%  USAGE
%   phase_deg = general.extractPhase(eeg)
%   eeg             Vector that contains a signal
%   phase_deg       Vector that contains extracted phase. Vector values are in degrees.
%
function [phase_deg] = extractPhase(eeg)
%     phase_rad = phase(hilbert(eeg));
    phase_rad = angle(hilbert(eeg)); % faster than phase
    phase_deg = rad2deg(mod(phase_rad, 2*pi));
end


%DEMGPARD Demonstrate ARD using a Gaussian Process.
%
%	Description
%	The data consists of three input variables X1, X2 and X3, and one
%	target variable  T. The  target data is generated by computing
%	SIN(2*PI*X1) and adding Gaussian  noise, x2 is a copy of x1 with a
%	higher level of added noise, and x3 is sampled randomly from a
%	Gaussian distribution. A Gaussian Process, is trained by optimising
%	the hyperparameters  using the scaled conjugate gradient algorithm.
%	The final values of the hyperparameters show that the model
%	successfully identifies the importance of each input.
%
%	See also
%	DEMGP, GP, GPERR, GPFWD, GPGRAD, GPINIT, SCG
%

%	Copyright (c) Ian T Nabney (1996-2001)

clc;
randn('state', 1729);
rand('state', 1729);
disp('This demonstration illustrates the technique of automatic relevance')
disp('determination (ARD) using a Gaussian Process.')
disp(' ');
disp('First, we set up a synthetic data set involving three input variables:')
disp('x1 is sampled uniformly from the range (0,1) and has a low level of')
disp('added Gaussian noise, x2 is a copy of x1 with a higher level of added')
disp('noise, and x3 is sampled randomly from a Gaussian distribution. The')
disp('single target variable is given by t = sin(2*pi*x1) with additive')
disp('Gaussian noise. Thus x1 is very relevant for determining the target')
disp('value, x2 is of some relevance, while x3 should in principle be')
disp('irrelevant.')
disp(' ');
disp('Press any key to see a plot of t against x1.')
pause;

ndata = 100;
x1 = rand(ndata, 1);
x2 = x1 + 0.05*randn(ndata, 1);
x3 = 0.5 + 0.5*randn(ndata, 1);
x = [x1, x2, x3];
t = sin(2*pi*x1) + 0.1*randn(ndata, 1);

% Plot the data and the original function.
h = figure;
plotvals = linspace(0, 1, 200)';
plot(x1, t, 'ob')
hold on
xlabel('Input x1')
ylabel('Target')
axis([0 1 -1.5 1.5])
[fx, fy] = fplot('sin(2*pi*x)', [0 1]);
plot(fx, fy, '-g', 'LineWidth', 2);
legend('data', 'function');

disp(' ');
disp('Press any key to continue')
pause; clc;

disp('The Gaussian Process has a separate hyperparameter for each input.')
disp('The hyperparameters are trained by error minimisation using the scaled.')
disp('conjugate gradient optimiser.')
disp(' ');
disp('Press any key to create and train the model.')
disp(' ');
pause;

net = gp(3, 'sqexp');
% Initialise the parameters.
prior.pr_mean = 0;
prior.pr_var = 0.1;
net = gpinit(net, x, t, prior);

% Now train to find the hyperparameters.
options = foptions;
options(1) = 1;
options(14) = 30;

[net, options] = netopt(net, options, x, t, 'scg');

rel = exp(net.inweights);

fprintf(1, ...
  '\nFinal hyperparameters:\n\n  bias:\t\t%10.6f\n  noise:\t%10.6f\n', ...
  exp(net.bias), exp(net.noise));
fprintf(1, '  Vertical scale: %8.6f\n', exp(net.fpar(1)));
fprintf(1, '  Input 1:\t%10.6f\n  Input 2:\t%10.6f\n', ...
  rel(1), rel(2));
fprintf(1, '  Input 3:\t%10.6f\n\n', rel(3));
disp(' ');
disp('We see that the inverse lengthscale associated with')
disp('input x1 is large, that of x2 has an intermediate value and the variance')
disp('of weights associated with x3 is small.')
disp(' ');
disp('This implies that the Gaussian Process is giving greatest emphasis')
disp('to x1 and least emphasis to x3, with intermediate emphasis on')
disp('x2 in the covariance function.')
disp(' ')
disp('Since the target t is statistically independent of x3 we might')
disp('expect the weights associated with this input would go to')
disp('zero. However, for any finite data set there may be some chance')
disp('correlation between x3 and t, and so the corresponding hyperparameter remains')
disp('finite.')
disp('Press any key to continue.')
pause

disp('Finally, we plot the output of the Gaussian Process along the line')
disp('x1 = x2 = x3, together with the true underlying function.')
xt = linspace(0, 1, 50);
xtest = [xt', xt', xt'];

cn = gpcovar(net, x);
cninv = inv(cn);
[ytest, sigsq] = gpfwd(net, xtest, cninv);
sig = sqrt(sigsq);

figure(h); hold on;
plot(xt, ytest, '-k');
plot(xt, ytest+(2*sig), '-b', xt, ytest-(2*sig), '-b');
axis([0 1 -1.5 1.5]);
fplot('sin(2*pi*x)', [0 1], '--m');

disp(' ');
disp('Press any key to end.')
pause; clc; close(h); clear all


function g = sphf2cartf(f, lam, th, coord)
%SPHF2CARTF    Wrapper for evaluating a function defined in Cartesian
%
%  G = SPHF2CARTF(F, LAM, TH) evaluates the function handle F = F(x,y,z)
%  at x = cos(lam).*sin(th), y = sin(lam).*sin(th), z = cos(th). This is
%  the co-latitude spherical coordinate system.
%
%  G = SPHF2CARTF(F, LAM, TH, 0) same as SPHF2CARTF(F, LAM, TH).
%
%  G = SPHF2CARTF(F, LAM, TH, 1) evaluates F = F(x,y,z) at
%  at x = cos(lam).*cos(th), y = sin(lam).*cos(th), z = sin(th). This is
%  the latitude spherical coordinate system.

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

% For Developers, recall that:
%   coord - Type of spherical coordinate system:
%          coord = 0 (co-latitude) --> -pi <= lam < pi, 0 <= th <= pi
%          coord = 1 (latitude)    --> -pi <= lam < pi, -pi/2 <= th <= pi/2
% (Default above is co-latitude.)

% TODO: Create separate wrapper functions for latitude/co-latitude so that
% this if can be removed (thus improving performance.

if ( nargin == 3 )
    coord = 0;  % Default is to use co-latitude.
end

if ( coord == 0 )
    % Latitude: 0 <= theta < pi
    x = cos(lam).*sin(th);
    y = sin(lam).*sin(th);
    z = cos(th);
elseif ( coord == 1 )
    % Latitude: -pi/2 <= theta < pi/2
    x = cos(lam).*cos(th);
    y = sin(lam).*cos(th);
    z = sin(th);
else
    error('SPHEREFUN:sphf2cartf:CoordSysUnknown', ['Unknown coordinate '...
        'system for the sphere.']);
end

g = feval(f, x, y, z);

end


%% Dirichlet Mixture

clear classes

ds = prtDataGenOldFaithful;

mix = prtBrvMixture('components', repmat(prtBrvMvn,5,1), 'vbVerboseText', true, 'vbVerbosePlot', true, 'vbConvergenceThreshold', 1e-11);

[mixLearned, training] = mix.vbBatch(ds);

%% DP Mixture
clear classes
ds = prtDataGenOldFaithful;

mix = prtBrvDpMixture('components',repmat(prtBrvMvn,15,1), 'vbVerboseText',true, 'vbVerbosePlot', true, 'vbConvergenceThreshold',1e-10);
[mixLearned, training] = mix.vbBatch(ds);

%% Online Dirichlet Mixture

clear classes
%ds = prtDataGenOldFaithful;
ds = prtDataGenBimodal(5000);

mix = prtBrvMixture('components',repmat(prtBrvMvn,25,1),'vbOnlineLearningRateFunctionHandle',@(t)(32 + t).^(-0.6),'vbVerbosePlot',true,'vbOnlineBatchSize',25,'vbOnlineFullDataSetSize',ds.nObservations);
mixLearned = mix.vbOnline(ds.X);

%% Online DP Mixture

clear classes
ds = prtDataGenOldFaithful;
%ds = prtDataGenBimodal(5000);

mix = prtBrvDpMixture('components',repmat(prtBrvMvn,50,1),'vbOnlineLearningRateFunctionHandle',@(t)(32 + t).^(-0.8),'vbVerbosePlot',true,'vbOnlineBatchSize',25,'vbOnlineFullDataSetSize',ds.nObservations);
mixLearned = mix.vbOnline(ds.X);

%%

%% MCMC Dirichlet
clear classes
%ds = prtDataGenOldFaithful;
ds = prtDataGenBimodal;

mix = prtBrvMixture('components', repmat(prtBrvMvn,5,1), 'mcmcVerboseText', false, 'mcmcVerbosePlot', 100, 'mcmcTotalIterations', 5000);

[mixLearned, training] = mix.mcmc(ds);


function obj = setRotationMatrix(obj,r)
    % set the rotation matrix (R) of the frame w.r.t to the
    % reference frame
    %
    % Parameters:
    % r: the rotation matrix or Euler angles (in radian) @type
    % rowvec|matrix

    if isvector(r) && length(r) == 3
        %         rpy = rad2deg(r);
        obj.R = rotz(r(3)) * roty(r(2)) * rotx(r(1));
    elseif all(size(r)==[3,3])
        if isnumeric(r)
            assert(abs(det(r))-1 <= 1e-6,...
                'The determinant of the rotation matrix must equal 1.');
        end
        obj.R = r;
    end
    % remove small numbers generated from the rotation matrix
    if isnumeric(r)
        obj.R = roundn(obj.R,-6);
    end
    % update the homogeneous transformation matrix
    obj.computeHomogeneousTransform();
end

function R = rotx(alpha)
%rotz  rotate around X by ALPHA
%
%	R = rotx(alpha)
%
% See also: roty, rotz
% Author: Jake Reher: jreher@caltech.edu

R = [1 0 0; ...
     0 cos(alpha) -sin(alpha); ...
     0 sin(alpha)  cos(alpha)];

end

function R = roty(alpha)
%roty  rotate around Y by ALPHA
%
%	R = roty(alpha)
%
% See also: rotx, rotz
% Author: Jake Reher: jreher@caltech.edu

R = [cos(alpha)  0 sin(alpha); ...
             0   1          0; ...
    -sin(alpha)  0 cos(alpha)];

end

function R = rotz(alpha)
%rotz  rotate around Z by ALPHA
%
%	R = rotz(alpha)
%
% See also: ROTX, ROTY, ROT, POS.
% Author: Jake Reher: jreher@caltech.edus

R = [cos(alpha) -sin(alpha) 0; ...
     sin(alpha)  cos(alpha) 0; ...
              0           0 1];

end


function [pos, tri] = mesh_tetrahedron

% MESH_TETRAHEDRON returns the vertices and triangles of a tetrahedron.
%
% Use as
%   [pos, tri] = mesh_tetrahedron;
%
% See also MESH_ICOSAHEDRON, MESH_OCTAHEDRON, MESH_SPHERE

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

v1 = [ 0, 0, 1 ];
v2 = [  sqrt(8/9),          0, -1/3 ];
v3 = [ -sqrt(2/9),  sqrt(2/3), -1/3 ];
v4 = [ -sqrt(2/9), -sqrt(2/3), -1/3 ];

pos = [
  v1
  v2
  v3
  v4
  ];

tri = [
  1 2 3
  1 3 4
  1 4 2
  2 4 3
  ];

  % scale all vertices to the unit sphere
  pos = pos ./ repmat(sqrt(sum(pos.^2,2)), 1,3);


function p = sigmoid(x)
% Sigmoid function: s(x) = 1 / (1+exp(-x))

% This file is from pmtk3.googlecode.com

p = 1./(1+exp(-x));
end


function [repairedData, coefficients, grossErrors] = repair_corrupted_data(dictionary, corruptedData)
% repair_missing_data.m
%
%   Given a set of complete data vectors, and another set of vectors with
%   missing entries, use L1-minimization to repair the incomplete vectors.
%   This function required the CVX package for semidefinite programming.
%
%
% Inputs:
%   dictionary           - a matrix whose columns are data vectors that are
%                          used as overcomplete basis to represent other
%                          vectors. note that some or all of these columns
%                          may contain gross errors.
%   corruptedData        - a matrix whose columns are data vectors with
%                          some elements that may have gross errors. these
%                          vectors will be repaired using the dictionary.
%
% Outputs:
%   repairedData        - a matrix whose columns are repaired versions of
%                         the given incomplete data vectors
%   coefficients        - a matrix whose ith column is  a list of
%                         coefficients used to represent the ith vector in
%                         in corrupted data as a linear combination of
%                         vectors in the dictionary
%   grossErrors         - a matrix whose ij-th entry is true if the ij-th
%                         entry of corruptedData was corrupted by gross
%                         errors.
% Dependencies:
%   CVX package
%
% Nov. '07  Shankar Rao -- srrao@uiuc.edu

% Copyright 2007, University of Illinois. All rights reserved.

VERBOSE = false;
GROSS_ERROR_THRESHOLD = 0.5;

[dimensionCount, sampleCount] = size(dictionary);
corruptedSampleCount = size(corruptedData,2);

repairedData = corruptedData;
grossErrors = false(size(corruptedData));
coefficients = zeros(sampleCount+dimensionCount, corruptedSampleCount);
normalizedData = dictionary ./ repmat(sqrt(sum(dictionary.^2, 1)), dimensionCount, 1);
I = eye(dimensionCount);
X = [ normalizedData I];
for sampleIndex = 1:corruptedSampleCount
    y = corruptedData(:, sampleIndex);

    if VERBOSE,
        disp(sprintf('Repairing sample %d of %d', sampleIndex, corruptedSampleCount));
    end

    state = cvx_quiet(true);
    cvx_begin
        variable c(sampleCount+dimensionCount);
        minimize(norm(c,1));
            subject to
                X * c == y;
    cvx_end
    cvx_quiet(state);

    yhat = X(:, 1:sampleCount)*c(1:sampleCount);
    coefficients(:, sampleIndex) = c;
    errors = c(sampleCount+1:end);
    grossErrors(:, sampleIndex) = abs(errors) > GROSS_ERROR_THRESHOLD;
    repairedData(grossErrors(:, sampleIndex), sampleIndex) = yhat(grossErrors(:, sampleIndex));
end


function [f] = spm_mc_fx_3(x,v,P)
% equations of motion for the mountain car problem using basis functions
% problem
% FORMAT [f] = spm_mc_fx_3(x,v,P)
%
% x   - hidden states
% v   - exogenous inputs
% P.p - parameters for gradient function:     G(x(1),P.p)
% P.q - parameters for cost or loss-function: C(x(1),P.q)
%
% returns f = dx/dt = f  = [x(2);
%                           G - x(2)*C]*dt;
%
% where C determines divergence of flow x(2) at any position x(1).
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_mc_fx_3.m 3333 2009-08-25 16:12:44Z karl $

% gradient (G)
%--------------------------------------------------------------------------
G     = spm_DEM_basis(x.x(1),v,P.p);

% cost function (C)
%--------------------------------------------------------------------------
C     = spm_DEM_basis(x.x(1),v,P.q);

% flow
%--------------------------------------------------------------------------
dt    = 1/8;
f.x   = [x.x(2); G - x.x(2)*x.c]*dt;
f.c   = [-C - x.c]*dt;

% true scalar potential gradient (see spm_moutaincar_fx)
%--------------------------------------------------------------------------
% if x(1) < 0
%     G  = 2*x(1) + 1;
% else
%     xx = x(1)^2;
%     G  = (1 + 5*xx)^(-1/2) - 5*xx/(1 + 5*xx)^(3/2) + (x(1)/2)^4;
% end


function h = p08_h ( n, x )

%*****************************************************************************80
%
%% P08_H evaluates the Hessian for problem 8.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 October 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of variables.
%
%    Input, real X(N), the values of the variables.
%
%    Output, real H(N,N), the N by N Hessian matrix.
%
  ap = 0.00001;

  t1 = - 0.25 + sum ( x(1:n).^2 );

  d1 = 2.0 * ap;
  th = 4.0 * t1;

  for i = 1 : n
    h(i,i) = d1 + th + 8.0 * x(i)^2;
    for j = 1 : i - 1
      h(i,j) = 8.0 * x(i) * x(j);
    end
  end

  for i = 1 : n
    for j = i + 1 : n
      h(i,j) = h(j,i);
    end
  end

  return
end


function FPDF = TabulatedPDF (x, p)
% Return function handles to routines to calculate the area, mean, and
% second moment of a unit-variance, zero-mean, uniform probability
% density function.
%                 b
%   Farea(a,b) = Int p(x) dx
%                 a
%                 b
%   Fmean(a,b) = Int x p(x) dx
%                 a
%                b
%   Fvar(a,b) = Int x^2 p(x) dx
%                a
% where p(x) is a tabulated function. The PDF is assumed to be zero outside
% of the limits, and defined by linear interpolation between points. The
% tabulated function need not be initially normalized to unit area.
%
% x - Abscissa values (in increasing order)
% p - Vector of probability values at the corresponding abscissa values.
%     The PDF is assumed to be zero outside of [x(1), x(end)] and to be
%     linearly interpolated between abscissa points.

% global xpdf pdf

% Test for increasing x
if (any(diff(x) < 0))
  error('TabulatedPDF - Non-increasing abscissa values');
end
if (any(p < 0))
  error('TabulatedPDF - Negative probability value');
end
if (length(x) ~= length(p) || length(x) <= 1)
  error('TabulatedPDF - Invalid table length');
end

% Normalize the tabulated data
xpdf = x;
pdf = p;
A = Tarea(-Inf, Inf);
pdf = pdf / A;

FPDF = {@Tarea, @Tmean, @Tvar};

return

%----- ----- begin nested functions
function v = Tarea (a, b)

% global xpdf pdf

v = TabulatedPDFInt(@TLarea, a, b, xpdf, pdf);

return
end

% ----- ------
function v = Tmean (a, b)

% global xpdf pdf

v = TabulatedPDFInt(@TLmean, a, b, xpdf, pdf);

return
end

% ----- ------
function v = Tvar (a, b)

% global xpdf pdf

v = TabulatedPDFInt(@TLvar, a, b, xpdf, pdf);

return
end

% ----- -----
function v = TabulatedPDFInt (Fn, a, b, x, p)
% The abscissa values are assumed to be in increasing order

% Limit the evaluation interval and flip the limits if a > b
br = min(x(end), max(a, b));
ar = max(x(1), min(a, b));

if (br <= ar)
  v = 0;
  return
end

% Search for the intervals included in the integral
iL = max(find(x <= ar));
iU = min(find(x >= br));
if (iL == iU)
  error('TabulatedPDFInt - iL == iU');
end
x = x(iL:iU);       % Keep only intervals of interest
p = p(iL:iU);

% Lop off the end intervals
p(1) = Alin(ar, x(1:2), p(1:2));
x(1) = ar;
p(end) = Alin(br, x(end-1:end), p(end-1:end));
x(end) = br;

% Integrate
v = feval(Fn, x, p);

% Negate the integral if a > b
if (a > b)
  v = -v;
end

return
end

% ----- -----
function v = TLarea (x, p)

xl = x(1:end-1);
xu = x(2:end);
pl = p(1:end-1);
pu = p(2:end);
% syms p pl pu xl xu x
% p = pl+(x-xl)*(pu-pl)/(xu-xl)
% factor(int(p,xl,xu)
% ans = -1/2*(pl+pu)*(-xu+xl)
A = (xu-xl) .* (pl+pu);

v = 0.5 * sum(A);

return
end

% ----- ------
function v = TLmean (x, p)

xl = x(1:end-1);
xu = x(2:end);
pl = p(1:end-1);
pu = p(2:end);
% syms p pl pu xl xu x
% p = pl+(x-xl)*(pu-pl)/(xu-xl)
% factor(int(x*p,xl,xu)
% ans = -1/6*(-xu+xl)*(2*pl*xl+pu*xl+pl*xu+2*pu*xu)
A = (xu-xl) .* ((xl+xu) .* (pl+pu) + pl.*xl + pu.*xu);

v = sum(A) / 6;

return
end

% ----- -----
function v = TLvar (x, p)

xl = x(1:end-1);
xu = x(2:end);
pl = p(1:end-1);
pu = p(2:end);
% syms p pl pu xl xu x
% p = pl+(x-xl)*(pu-pl)/(xu-xl)
% factor(int(x^2*p,xl,xu)
% ans = -1/12*(-xu+xl)
%          *(3*pl*xl^2+pu*xl^2+2*xl*pu*xu+2*pl*xu*xl+pl*xu^2+3*pu*xu^2)
A =  (xu-xl) .* ((pu+pl) .* (xu+xl).^2 + 2*(pu.*xu.^2 + pl.*xl.^2));

v = sum(A) / 12;

return
end

% ----- -----
function pa = Alin(a, x, p)

slope = (p(2) - p(1)) / (x(2) - x(1));
pa = (a - x(1)) * slope + p(1);

return
end

% ---- end of the nested functions
end


function [inpC,p] = regCorrContrast(inp,Limit,pinit);
% regCorrContrast - removes mean and normalizes by the standard deviation,
%                both estimated by minimizing a robust error measure
%                between the sampled histogram and a mixture of 2 gaussians.
%
% [inpC,p] = regCorrContrast(inp, <pinit>);
%
% INPUT:
% - inp: set of original inplanes
% - pinit: initial parameters of the two best fitting Gaussians (optional)
%          (pinit = [mu1 sigma1^2 mu2 sigma2^2 p1 p2])
%
% Oscar Nestares - 5/99
%

if nargin<2
   Limit = 2;
end

% building the histogram
II = find(~isnan(inp));
[h x] = regHistogram(double(inp(II)), 256);

% normalizing the histogram
h = h/(sum(h)*mean(diff(x)));

% initial parameters: if they are not specified, chosoe the first
% gaussian with the actual mean and variance, and the second gaussian
% around 1 and with 1/10 of the actual variance, both with weights of 0.5
if nargin<3
   pinit = [mean(inp(II)) var(inp(II)) 1 var(inp(II))/10 0.5 0.5];
end

% minimizing the robust measure of the error
%p=fmins('regErrGaussRob', pinit, [], [], x, h);
p = fminsearch('regErrGaussRob', double(pinit), [], x, h);

% selecting the mean closer to 1
if abs(p(1)-1)>abs(p(3)-1)
   mu = p(3); sigma2 = p(4);
else
   mu = p(1); sigma2 = p(2);
end

% renormalizing
inpC = (inp - mu)/sqrt(sigma2);

% saturating for low and high values
%Limit = 4;  %%% This is a reasonable value, 2*std (now std=1)
Low = inpC < -Limit;
High = inpC > Limit;
inpC = inpC .*((~Low) & (~High)) + (-Limit)*Low + Limit*High;

return


function [s,time] = modulation(x,Ts,Nos,Fc)
% Ts : Sampling period
% Nos: Oversampling factor
% Fc : Carrier frequency
Nx=length(x);  offset = 0;
if nargin<5
    scale = 1;
    T=Ts/Nos; % Scale and Oversampling period for Baseband
else
    scale = sqrt(2);
    T=1/Fc/2/Nos; % Scale and Oversampling period for Passband
end
t_Ts = [0:T:Ts-T];
time = [0:T:Nx*Ts-T]; % One sampling interval and whole interval
tmp = 2*pi*Fc*t_Ts+offset;
len_Ts=length(t_Ts);
cos_wct = cos(tmp)*scale;
sin_wct = sin(tmp)*scale;
%s = zeros(N*len_Ts,1);
for n = 1:Nx
   s((n-1)*len_Ts+1:n*len_Ts) = real(x(n))*cos_wct-imag(x(n))*sin_wct;
end


function test_issue1985

% WALLTIME 00:10:00
% MEM 2gb

% DEPENDENCY filter_with_correction fir_filterdcpadded

nchan = 1;
fsample = 1000;
nsample = 2*fsample;

dat = zeros(nchan, nsample);
dat(1,fsample) = 1;

%%

fhp = 0.3;
flp = 30;
fbp = [fhp flp];

order = 30;
type = 'firws';

% use defaulls where possible
filt1 = ft_preproc_bandpassfilter(dat, fsample, fbp, order, type, 'onepass-zerophase');
filt2 = ft_preproc_bandpassfilter(dat, fsample, fbp, order, type, 'onepass-reverse-zerophase');
filt3 = ft_preproc_bandpassfilter(dat, fsample, fbp, order, type, 'onepass-minphase');

%%

figure; hold on
plot(dat * 0.1)
plot(filt1+0.01)
plot(filt2+0.02)
plot(filt3+0.03)
legend


function [y,yz,yzi] = autoGen_cst_swingFootHeight(q1,q2,q4,q5,l1,l2,l4,l5,stepLength,stepHeight)
%AUTOGEN_CST_SWINGFOOTHEIGHT
%    [Y,YZ,YZI] = AUTOGEN_CST_SWINGFOOTHEIGHT(Q1,Q2,Q4,Q5,L1,L2,L4,L5,STEPLENGTH,STEPHEIGHT)

%    This function was generated by the Symbolic Math Toolbox version 6.3.
%    25-Oct-2015 18:36:31

t3 = sin(q1);
t4 = l1.*t3;
t7 = sin(q2);
t8 = l2.*t7;
t9 = sin(q4);
t10 = l4.*t9;
t11 = sin(q5);
t12 = l5.*t11;
t2 = t4+t8-t10-t12;
t5 = 1.0./stepLength.^2;
t6 = cos(q1);
t13 = cos(q2);
t14 = cos(q4);
t15 = cos(q5);
y = -l1.*t6-l2.*t13+l4.*t14+l5.*t15-stepHeight.*(t2.^2.*t5-1.0);
if nargout > 1
    yz = [t4-l1.*stepHeight.*t2.*t5.*t6.*2.0;t8-l2.*stepHeight.*t2.*t5.*t13.*2.0;-t10+l4.*stepHeight.*t2.*t5.*t14.*2.0;-t12+l5.*stepHeight.*t2.*t5.*t15.*2.0];
end
if nargout > 2
    yzi = [2.0;3.0;5.0;6.0];
end


function filters = make_filters(radii, gtheta)

d = 2;

filters = cell(numel(radii), numel(gtheta));
for r = 1:numel(radii),
    for t = 1:numel(gtheta),

        ra = radii(r);
        rb = ra / 4;
        theta = gtheta(t);

        ra = max(1.5, ra);
        rb = max(1.5, rb);
        ira2 = 1 / ra^2;
        irb2 = 1 / rb^2;
        wr = floor(max(ra, rb));
        wd = 2*wr+1;
        sint = sin(theta);
        cost = cos(theta);

        % 1. compute linear filters for coefficients
        % (a) compute inverse of least-squares problem matrix
        filt = zeros(wd,wd,d+1);
        xx = zeros(2*d+1,1);
        for u = -wr:wr,
            for v = -wr:wr,
                ai = -u*sint + v*cost; % distance along major axis
                bi = u*cost + v*sint; % distance along minor axis
                if ai*ai*ira2 + bi*bi*irb2 > 1, continue; end % outside support
                xx = xx + cumprod([1;ai+zeros(2*d,1)]);
            end
        end
        A = zeros(d+1,d+1);
        for i = 1:d+1,
            A(:,i) = xx(i:i+d);
        end

        % (b) solve least-squares problem for delta function at each pixel
        for u = -wr:wr,
            for v = -wr:wr,
                ai = -u*sint + v*cost; % distance along major axis
                bi = u*cost + v*sint; % distance along minor axis
                if (ai*ai*ira2 + bi*bi*irb2) > 1, continue; end % outside support
                yy = cumprod([1;ai+zeros(d,1)]);
                filt(v+wr+1,u+wr+1,:) = A\yy;
            end
        end

        filters{r,t}=filt;
    end
end


function [s,d,S_k,S_si] = pinHoleSegment(k,si)

% PINHOLESEGMENT  Pin hole projection of a segment.
%   [S,D] = PINHOLESEGMENT(K,SI) projects into a pinhole camera with
%   intrinsic aprameters K the segment SI. It returns the projected segment
%   S and the non-observable depths D of the two endpoints.
%
%   SI is a 6-vector containing the two endpoints of the 3D segment.
%   S is a 4-vector conteining the two endpoints of the 2D segment.
%
%   SI can also be a 6-by-N matrix with N segments. In such case S is a
%   4-by-N matrix with N projected segments.
%
%   [S,D,S_k,S_si] = POINHOLESEGMENT(...) returns the Jacobians of S wrt K
%   and SI. It only works for single segments.
%
%   See also PINHOLE.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

p1 = si(1:3,:);
p2 = si(4:6,:);

if nargout <= 2

    [e1,d1] = pinHole(p1,k);
    [e2,d2] = pinHole(p2,k);
    s       = [e1;e2];
    d       = [d1;d2];

else % Jacobians

    if size(si,2) == 1

        [e1,d1,E1_p1,E1_k] = pinHole(p1,k);
        [e2,d2,E2_p2,E2_k] = pinHole(p2,k);
        s       = [e1;e2];
        d       = [d1;d2];

        S_k = [...
            E1_k
            E2_k];

        Z23 = zeros(2,3);

        S_si = [...
            E1_p1 Z23
            Z23   E2_p2];

    else

        error('Jacobians not available for multiple segments.')

    end

end

return

%% test
syms p1 p2 p3 q1 q2 q3 u0 v0 au av real
k  = [u0;v0;au;av];
si = [p1;p2;p3;q1;q2;q3];

[s,d,S_k,S_si] = pinHoleSegment(k,si);

simplify(S_k - jacobian(s,k))
simplify(S_si - jacobian(s,si))

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

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

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function value = p20_exact ( dim_num )

%*****************************************************************************80
%
%% P20_EXACT returns the exact integral for problem 20.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 June 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Output, real VALUE, the exact value of the integral.
%
  aval = 0.0;
  aval = p20_r8 ( 'G', 'A', aval );

  bval = 0.0;
  bval = p20_r8 ( 'G', 'B', bval );

  p = 0.0;
  p = p20_r8 ( 'G', 'P', p );

  value = 0.0;
  exponent = dim_num + p;

  minus_one = -1.0;
  for i = 0 : dim_num
    minus_one = - minus_one;
    value = value + minus_one * r8_choose ( dim_num, i ) ...
      * ( ( dim_num - i ) * bval + i * aval )^exponent;
  end

  for i = 1 : dim_num
    value = value / ( p + i );
  end

  return
end


function value = r4_li ( x )

%*****************************************************************************80
%
%% R4_LI evaluates the logarithmic integral for an R4 argument.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real VALUE, the logarithmic integral evaluated at X.
%
  persistent sqeps

  if ( isempty ( sqeps ) )
    sqeps = sqrt ( r4_mach ( 3 ) );
  end

  if ( x < 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R4_LI - Fatal error!\n' );
    fprintf ( 1, '  Function undefined for X <= 0.\n' );
    error ( 'R4_LI - Fatal error!' )
  end

  if ( x == 0.0 )
    value = 0.0;
    return
  end

  if ( x == 1.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R4_LI - Fatal error!\n' );
    fprintf ( 1, '  Function undefined for X = 1.\n' );
    error ( 'R4_LI - Fatal error!' )
  end

  if ( abs ( 1.0 - x ) < sqeps )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R4_LI - Warning!\n' );
    fprintf ( 1, '  Answer less than half precision.\n' );
    fprintf ( 1, '  X is too close to 1.\n' );
  end

  value = r4_ei ( log ( x ) );

  return
end


function [x,m,e] = RunningAverage(X,Y,varargin)

%RunningAverage - Compute running linear or angular average.
%
% Computes the running average of y=f(x). Variable y can be linear or circular
% (use radians). The error bars are standard errors of the mean for linear
% data, or 95% confidence intervals for circular data.
%
%  USAGE
%
%    [x,m,e] = RunningAverage(x,y,<options>)
%
%    x              x variable (e.g., time)
%    y              values at x
%    <options>      optional list of property-value pairs (see table below)
%
%    =========================================================================
%     Properties    Values
%    -------------------------------------------------------------------------
%     'window'      averaging window size (default (max-min)/10)
%     'overlap'     overlap between successive windows (default = 0.8*window)
%     'limits'      x limits to use instead of min and max
%     'type'        either 'linear' or 'circular' (default 'linear')
%    =========================================================================
%
%  OUTPUT
%
%    x              new x variable
%    m              running average
%    e              sem for linear variables, otherwise 95% confidence intervals

% Copyright (C) 2004-2011 by Michaël Zugaro
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 3 of the License, or
% (at your option) any later version.

% Default values
type = 'linear';
limits = [min(X) max(X)];
nBins = 10;
window = 0;
overlap = [];

if nargin < 2 | mod(length(varargin),2) ~= 0,
  error('Incorrect number of parameters (type ''help RunningAverage'' for details).');
end

% Parse parameter list
for i = 1:2:length(varargin),
	if ~ischar(varargin{i}),
		error(['Parameter ' num2str(i+2) ' is not a property (type ''help RunningAverage'' for details).']);
	end
	switch(lower(varargin{i})),
		case 'type',
			type = varargin{i+1};
			if ~isstring_FMAT(type,'linear','circular'),
				error('Incorrect value for property ''type'' (type ''help RunningAverage'' for details).');
			end

		case 'window',
			window = varargin{i+1};
			if ~isdscalar(window,'>0'),
				error('Incorrect value for property ''window'' (type ''help RunningAverage'' for details).');
			end

		case 'overlap',
			overlap = varargin{i+1};
			if ~isdscalar(overlap,'>=0'),
				error('Incorrect value for property ''overlap'' (type ''help RunningAverage'' for details).');
			end

		case 'limits',
			limits = varargin{i+1};
			if ~isdvector(limits,'#2','<'),
				error('Incorrect value for property ''limits'' (type ''help RunningAverage'' for details).');
			end

		otherwise,
			error(['Unknown property ''' num2str(varargin{i}) ''' (type ''help RunningAverage'' for details).']);

	end
end

x0 = limits(1);
x1 = limits(2);
if window == 0,
	window = (x1-x0)/nBins;
end
if isempty(overlap),
	overlap = 0.8*window;
end

% Loop through data
i = 1;
xi = x0+window/2;
while xi+window/2 <= x1,
	x(i,1) = xi;
	ok = InIntervals(X,xi+[-0.5 0.5]*window);
	if sum(ok) == 0,
		m(i,1) = NaN;
		e(i,:) = [NaN NaN];
	elseif strcmp(type,'circular'),
		[M,C] = CircularConfidenceIntervals(Y(ok));
		m(i,1) = M;
		e(i,:) = C;
	else
		m(i,1) = nanmean(Y(ok));
		n = sum(ok);
		s = nanstd(Y(ok))/sqrt(n);
		e(i,1) = m(i)-s;
		e(i,2) = m(i)+s;
	end
	xi = xi + window - overlap;
	i = i + 1;
end


function [prob,sol,fmin] = qp_prob(varargin)
%QP_PROB  Return an OPTI QP
%
%   prob = qp_prob(no) return a pre-built optiprob of a saved QP.
%
%   [prob,sol,fmin] = qp_prob(no) returns the optimum solution and
%   function eval at the optimum
%
%   no = qp_prob() returns the number of problems available for testing.

%   (C) 2011 Jonathan Currie (I2C2)

%Check if just returning no problems
if(nargin < 1)
    prob = 3; sol = []; fmin = [];
    return;
else
    no = varargin{1};
end

%Big switch yard
switch(no)
    case 1
        H = eye(3);
        f = -[2 3 1]';
        A = [1 1 1;3 -2 -3; 1 -3 2];
        b = [1;1;1];
        prob = optiprob('H',H,'f',f,'ineq',A,b);
        sol = [1/3;4/3;-2/3];
        fmin = -2.83333333301227;

    case 2
        H = [1 -1; -1 2];
        f = -[2 6]';
        A = [1 1; -1 2; 2 1];
        b = [2; 2; 3];
        lb = [0;0];
        prob = optiprob('H',H,'f',f,'ineq',A,b,'lb',lb);
        sol = [1/3;4/3];
        fmin = -8.22222220552525;

    case 3
        H = [1 -1; -1 2];
        f = -[2 6]';
        A = [1 1; -1 2; 2 1];
        b = [2; 2; 3];
        Aeq = [1 1.5];
        beq = 2;
        lb = [0;0];
        ub = [10;10];
        prob = optiprob('H',H,'f',f,'ineq',A,b,'eq',Aeq,beq,'bounds',lb,ub);
        sol = [0.34482763667623;1.10344824221585];
        fmin = -6.41379310344827;

    otherwise
        error('Problem not available or not implemented yet');
end


function seed = i4_seed_advance ( seed )

%*****************************************************************************80
%
%% I4_SEED_ADVANCE "advances" the seed.
%
%  Discussion:
%
%    This routine implements one step of the recursion
%
%      SEED = ( 16807 * SEED ) mod ( 2^31 - 1 )
%
%    This version of the routine does not check whether the input value of
%    SEED is zero.  If the input value is zero, the output value will be zero.
%
%    If we repeatedly use the output of SEED_ADVANCE as the next input,
%    and we start with SEED = 12345, then the first few iterates are:
%
%         Input      Output
%          SEED        SEED
%
%         12345   207482415
%     207482415  1790989824
%    1790989824  2035175616
%    2035175616    77048696
%      77048696    24794531
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 April 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Paul Bratley, Bennett Fox, Linus Schrage,
%    A Guide to Simulation,
%    Second Edition,
%    Springer, 1987,
%    ISBN: 0387964673,
%    LC: QA76.9.C65.B73.
%
%    Bennett Fox,
%    Algorithm 647:
%    Implementation and Relative Efficiency of Quasirandom
%    Sequence Generators,
%    ACM Transactions on Mathematical Software,
%    Volume 12, Number 4, December 1986, pages 362-376.
%
%    Pierre L'Ecuyer,
%    Random Number Generation,
%    in Handbook of Simulation,
%    edited by Jerry Banks,
%    Wiley, 1998,
%    ISBN: 0471134031,
%    LC: T57.62.H37.
%
%    Peter Lewis, Allen Goodman, James Miller,
%    A Pseudo-Random Number Generator for the System/360,
%    IBM Systems Journal,
%    Volume 8, Number 2, 1969, pages 136-143.
%
%  Parameters:
%
%    Input, integer SEED, the seed value.
%
%    Output, integer SEED, the "next" seed.
%
  i4_huge = 2147483647;

  seed = floor ( seed );

  seed = mod ( seed, i4_huge );

  if ( seed < 0 )
    seed = seed + i4_huge;
  end

  k = floor ( seed / 127773 );

  seed = 16807 * ( seed - k * 127773 ) - k * 2836;

  if ( seed < 0 )
    seed = seed + i4_huge;
  end

  return
end


%%
% Comparison of various dual norms (aka integral probability metrics).

SetAR = @(ar)set(gca, 'PlotBoxAspectRatio', [1 ar 1]);
rep = ['results/dual-norms/'];
[~,~] = mkdir(rep);

% random test
normalize = @(x)x/sum(x(:));
k = @(x,y)-abs(x-y);
nx = 3; ny = 5;
x = randn(nx,1);
y = randn(ny,1);
p = normalize(rand(nx,1));
q = normalize(rand(ny,1));
d = rkhs_norm(k,x,p,x,p);
d = rkhs_norm(k,x,p,y,q);

name = '1dirac-1dirac';
name = '1dirac-2dirac';

% delta_0 <-> delta_t
xmax = 3;
tlist = linspace(-xmax,xmax,257)';

switch name
    case '1dirac-1dirac'
        p = 1; q = 1;
        x = @(t)0;
        y = @(t)t;
    case '1dirac-2dirac'
        p = 1; q = [1;1]/2;
        x = @(t)0;
        y = @(t)[-t/2;t/2];
    otherwise
        error('Unknown');
end

D = []; lgd = {};
% energy distance
k = @(x,y)-abs(x-y);
f = @(t)sqrt( rkhs_norm(k, x(t),p, y(t),q) );
D(:,end+1) = arrayfun(f,tlist);
lgd{end+1} = 'Energy';
% Gaussian RKHS
sigma = .3;
k = @(x,y)exp( -(x-y).^2 / (2*sigma^2) );
f = @(t)sqrt( rkhs_norm(k, x(t),p, y(t),q) );
D(:,end+1) = arrayfun(f,tlist);
lgd{end+1} = 'Gauss';
% W1 distance
D(:,end+1) = abs(tlist);
lgd{end+1} = 'W_1';
% Flat distance
D(:,end+1) = min(abs(tlist),1);
lgd{end+1} = 'Flat';

%% display
clf;
plot(tlist, D, 'LineWidth', 2);
legend(lgd, 'Location', 'NorthWest');
axis tight;
SetAR(1/2);
set(gca, 'FontSize', 20);
saveas(gcf, [rep name '.eps'], 'epsc');


function cos_power_int_values_test ( )

%*****************************************************************************80
%
%% COS_POWER_INT_VALUES_TEST demonstrates the use of COS_POWER_INT_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 January 2015
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'COS_POWER_INT_VALUES_TEST:\n' );
  fprintf ( 1, '  COS_POWER_INT_VALUES returns values of \n' );
  fprintf ( 1, '  the N-th power of the cosine function.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      A      B      N      FX\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, a, b, n, fx ] = cos_power_int_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %8f  %8f  %6d  %24.16f\n', a, b, n, fx );

  end

  return
end


clear all
% ===== make testdata =====
% ti=0;
% tf=10;
% dt=.01;
% t=ti:dt:tf; % second
%
% a1=1;
% f1=5;
% y1=a1*cos(2*pi*f1*t)*rand(length(t));
% a2=5;
% f2=1;
% y2=a2*sin(2*pi*f2*t);
%
% testdata=y1+y2+10;

filename = 'cmy(finish).wav';
[data,Fs]=audioread(filename);

% wavplay(y,Fs)
testdata=data';
dt=1/Fs;
t=0:dt:length(testdata)/Fs-dt;

% ===== decomposition in time domain =====
st=100; % sifting time; 100-300
[IMF]=EMD(testdata,st);
% save('IMF.mat','IMF')
% load IMF.mat

%  for i=1:10;
%      plot(IMF(i,:));
%      title(['IMF' num2str(i)])
%      pause
%      close all
%  end

% set(gca,'xtick',[0:100:1000],'xticklabel',0:10);
% set(gca,'ytick',get(gca,'ytick'),'yticklabel',{'DC term','IMF5','IMF4','IMF3','IMF2','IMF1','test data'});
% xlabel('Time (second)');
%  pause
% ===== computation of instantaneous frequency and amplitude =====
[ipifs,ifs,amp]=imf2ipfa(IMF(1:end-1,:),dt); % the DC term should not be include

% ===== Hilbert spectrum =====
% x=round([t;t;t;t;t]'*100+1); % xlabel, time
[L,W]=size(ifs);

x=NaN(L,W);
for i=1:W;
    x(:,i)=t';
end
y=ifs;

x=x(:);
y=y(:);
z=amp(:);

% === x-axis (time) ===
dx=0.01;% time resolution 0.1 second
xi=0;
xf=t(end);

T=[xi:dx:0.5];

% === y-axis (frequency) ===
dy=10;% frequency resolution 100 Hz
yi=0;
yf=1500;
F=yi:dy:yf;

z(y>yf)=[];
x(y>yf)=[];
y(y>yf)=[]; % romove the data with frequency > 5000Hz

x=round(x/dx)+1;
y=round(y/dy)+1; % ylabel, frequency

x(z<0)=[]; % remove the bad data (negative amplitude)
y(z<0)=[];
z(z<0)=[];

% pause

% z(y>90)=[];
% x(y>90)=[];
% y(y>90)=[]; % romove the data with frequency > 9Hz

m=zeros(length(F),length(T));
c=m;
for i=1:length(z);
    m(y(i),x(i))=m(y(i),x(i))+z(i);
    c(y(i),x(i))=c(y(i),x(i))+1;
end

hsp=m./c;

close all
figure('position',[300 100 600 600])
axes('position',[.15 .64 .7 .3])
plot(t,testdata,'-k');
title('(a) Data');
set(gca,'fontsize',16)
grid on
axis tight

axes('position',[.15 .12 .7 .4])
pcolor(T,F,hsp);
shading flat
%[C,H]=contourf(T,F,hsp);
%set(H,'levellist',[0:.0001:.3])
caxis([0 .02])
cb=colorbar('position',[.88 .12 .03 .4]);
set(get(cb,'title'),'string','Amplitude')
xlabel('Time (second)');
ylabel('Frequency (Hz)');
ylim([0 1500])
title('(b) Hilbert spectrum');
set(gca,'fontsize',16)
set(gcf,'paperpositionmode','auto')
print -dpng -r300 Fig01


% Mathematics Q2876283
% https://math.stackexchange.com/questions/2876283
% Least Square with Optimization of Triangular Matrix
% References:
%   1.  aa
% Remarks:
%   1.  sa
% TODO:
% 	1.  ds
% Release Notes
% - 1.0.000     12/08/2018
%   *   First release.

%% General Parameters

subStreamNumberDefault = 79;

run('InitScript.m');

figureIdx           = 0; %<! Continue from Question 1
figureCounterSpec   = '%04d';

generateFigures = ON;

%% Simulation Parameters

numRowsA    = 6;
numColsA    = 20;

numRowsB    = 10;
numColsB    = numColsA;

stepSize        = 0.0025;
numIterations   = 500;

%% Generate Data

mA = randn([numRowsA, numColsA]);
mB = randn([numRowsB, numColsB]);

%% Projection onto Lower Triangular Matrices Set

mY = randn([numRowsA, numRowsA]);

cvx_begin('quiet')
    cvx_precision('best');
    variable mX(numRowsA, numRowsA) lower triangular
    minimize( norm(mX - mY, 'fro') )
cvx_end

disp([' ']);
disp(['CVX Solution Summary']);
disp(['The CVX Solver Status - ', cvx_status]);
disp(['The Optimal Value Is Given By - ', num2str(cvx_optval)]);
disp(['The Optimal Argument Is Given By - [ ', num2str(mX(:).'), ' ]']);
disp([' ']);

mX = tril(mY);

disp([' ']);
disp(['Projection Solution Summary']);
disp(['The Optimal Value Is Given By - ', num2str(norm(mX - mY, 'fro'))]);
disp(['The Optimal Argument Is Given By - [ ', num2str(mX(:).'), ' ]']);
disp([' ']);

%% Solution by CVX

cvx_begin('quiet')
    cvx_precision('best');
    variable mX(numRowsA, numRowsB) lower triangular;
    minimize( 0.5 * pow_pos(norm( mX * mB - mA, 'fro' ), 2) );
cvx_end

disp([' ']);
disp(['CVX Solution Summary']);
disp(['The CVX Solver Status - ', cvx_status]);
disp(['The Optimal Value Is Given By - ', num2str(cvx_optval)]);
disp(['The Optimal Argument Is Given By - [ ', num2str(mX(:).'), ' ]']);
disp([' ']);

%% Solution by Projected Gradient Descent

hObjFun = @(mX) (0.5 * (norm( (mX * mB) - mA, 'fro' ) ^ 2));
vObjVal = zeros([numIterations, 1]);

mBB = mB * mB.';
mAB = mA * mB.';

mX         = mAB * pinv(mBB); %<! Initialization by the Least Squares Solution
mX         = tril(mX);
vObjVal(1) = hObjFun(mX);

for ii = 2:numIterations

    mG = (mX * mBB) - mAB;
    mX = mX - (stepSize * mG);

    % Projection Step
    mX = tril(mX);

    vObjVal(ii) = hObjFun(mX);
end

disp([' ']);
disp(['Projected Gradient Descent Solution Summary']);
disp(['The Optimal Value Is Given By - ', num2str(vObjVal(numIterations))]);
disp(['The Optimal Argument Is Given By - [ ', num2str(mX(:).'), ' ]']);
disp([' ']);

figureIdx = figureIdx + 1;

hFigure     = figure('Position', figPosLarge);
hAxes       = axes();
hLineSeries = plot(1:numIterations, [vObjVal, cvx_optval * ones([numIterations, 1])]);
set(hLineSeries, 'LineWidth', lineWidthNormal);
set(hLineSeries(2), 'LineStyle', ':');
set(get(hAxes, 'Title'), 'String', {['Objective Function Value vs. Iteration']}, ...
    'FontSize', fontSizeTitle);
set(get(hAxes, 'XLabel'), 'String', 'Iteration Number', ...
    'FontSize', fontSizeAxis);
set(get(hAxes, 'YLabel'), 'String', 'Objective Function Value', ...
    'FontSize', fontSizeAxis);
set(hAxes, 'XLim', [1, numIterations]);
hLegend = ClickableLegend({['Projected Gradient Descent'], ['Optimal Value (CVX)']});
set(hAxes, 'LooseInset', [0.07, 0.07, 0.07, 0.07]);

if(generateFigures == ON)
    saveas(hFigure,['Figure', num2str(figureIdx, figureCounterSpec), '.png']);
end

%% Restore Defaults

% set(0, 'DefaultFigureWindowStyle', 'normal');
% set(0, 'DefaultAxesLooseInset', defaultLoosInset);


function value = index3_col ( i_min, i, i_max, j_min, j, j_max, ...
  k_min, k, k_max, index_min )

%*****************************************************************************80
%
%% INDEX3_COL indexes a 3D array by columns.
%
%  Discussion:
%
%    Entries of the array are indexed starting at entry (I_MIN,J_MIN,K_MIN),
%    and increasing the row index first, then the column index.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 April 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer I_MIN, I, I_MAX, for row indices,
%    the minimum, the index, and the maximum.
%
%    Input, integer J_MIN, J, J_MAX, for column indices,
%    the minimum, the index, and the maximum.
%
%    Input, integer K_MIN, K, K_MAX, for plane indices,
%    the minimum, the index, and the maximum.
%
%    Input, integer INDEX_MIN, the index of (I_MIN,J_MIN,K_MIN).
%    Typically, this is 0 or 1.
%
%    Output, integer VALUE, the index of element (I,J,K).
%
  value = index_min ...
             + ( i - i_min ) ...
             + ( j - j_min ) * ( i_max + 1 - i_min ) ...
             + ( k - k_min ) * ( j_max + 1 - j_min ) * ( i_max + 1 - i_min );

  return
end


function determ = carry_determinant ( n, alpha )

%*****************************************************************************80
%
%% CARRY_DETERMINANT returns the determinant of the CARRY matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, integer ALPHA, the numeric base being used in the addition.
%
%    Output, real DETERM, the determinant.
%
  power = ( n * ( n - 1 ) ) / 2;
  determ = 1.0 / alpha^power;

  return
end


function x = uniform_01_cdf_inv ( cdf )

%*****************************************************************************80
%
%% UNIFORM_01_CDF_INV inverts the Uniform 01 CDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real CDF, the value of the CDF.
%    0.0 <= CDF <= 1.0.
%
%    Output, real X, the corresponding argument.
%
  if ( cdf < 0.0 | 1.0 < cdf )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'UNIFORM_01_CDF_INV - Fatal error!\n' );
    fprintf ( 1, '  CDF < 0 or 1 < CDF.\n' );
    error ( 'UNIFORM_01_CDF_INV - Fatal error!' );
  end

  x = cdf;

  return
end


function[varargout] = vmean(varargin)
%VMEAN  Mean over non-NaN elements along a specified dimension.
%
%   Y=VMEAN(X,DIM) takes the mean of all non-NaN elements of X along
%   dimension DIM.
%
%   [Y,NUM]=VMEAN(X,DIM) also outputs the number of non-NaN data points
%   NUM, which has the same dimension as X.
%
%   [Y1,Y2,...YN]=VMEAN(X1,X2,...XN,DIM) or also works, where all the XN
%   are the same size.
%
%   VMEAN(X1,X2,...XN,DIM);  with no output arguments overwrites the
%   original input variables.
%   __________________________________________________________________
%
%   Weighted means
%
%   VMEAN can also form a weighted mean.
%
%   Y=VMEAN(X,DIM,W) forms the weighted mean of X using weights W, an array
%   of the same size as X.
%
%   In this case [Y,NUM]=VMEAN(X,DIM,W) returns total weight used in
%   forming the mean at each point, rather than the number of data points.
%
%   Thus VMEAN(X,DIM,W) is the same as VMEAN(X.*W,DIM)./VMEAN(W,DIM).
%
%   [Y1,Y2,...YN]=VMEAN(X1,X2,...XN,DIM,W) also works.
%   __________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2001--2015 J.M. Lilly --- type 'help jlab_license' for details

if strcmpi(varargin{1}, '--t')
  vmean_test;powermean_test;return
end

if ~isscalar(varargin{end})
    w=varargin{end};
    varargin=varargin(1:end-1);
else
    w=[];
end

dim=varargin{end};

for i=1:length(varargin)-1
  if isempty(w)
      [varargout{i},numi{i}]=vsum(varargin{i},dim);
      numi{i}(numi{i}==0)=nan;
      varargout{i}=varargout{i}./numi{i};
  else
      varargout{i}=vsum(varargin{i}.*w,dim);
      numi{i}=vsum(w,dim);
      varargout{i}=varargout{i}./numi{i};
  end
end

for i=length(varargin):nargout
  varargout{i}=numi{i-length(varargin)+1};
end

eval(to_overwrite(nargin-1))

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function[]=vmean_test
x1=[1 2 ; nan 4];
x2=[inf 6; nan 5];
ans1=[3/2 4]';
ans2=[6 5]';

vmean(x1,x2,2);
reporttest('VMEAN output overwrite', aresame(x1,ans1) && aresame(x2,ans2))

x1=[1 2 ; nan 4];
ans1=[3/2 4]';
ans2=[2 1]';

[y1,y2]=vmean(x1,2);
reporttest('VMEAN mean & num', aresame(y1,ans1) && aresame(y2,ans2))

function[varargout]=powermean(varargin)
%POWERMEAN  Power-weighted mean along a specified dimension.
%
%   FM=POWERMEAN(F,X,DIM) takes the mean of all finite elements of F along
%   dimension DIM, weighted by the squared magnitude of X.
%
%   The power-weighted mean of F is defined as
%
%        FM = SUM (ABS(X)^2.*F, DIM) / SUM(ABS(X)^2, DIM)
%
%   where X and F are arrays of the same size.
%
%   [FM1,FM2,...FMN]=POWERMEAN(F1,F2,...FN,X,DIM) also works.
%
%   POWERMEAN(F1,F2,...FN,X,DIM);  with no output arguments overwrites the
%   original input variables.
%
%   POWERMEAN with X a set of analytic signals is used to construct the
%   joint instantaneous moments.  For details on these quantities, see
%
%       Lilly and Olhede (2010),  Bivariate instantaneous frequency and
%           bandwidth.  IEEE Trans. Sig. Proc. 58 (2), 591--603.
%
%   See also VMEAN.
%
%   Usage: fm=powermean(f,x,dim);
%          [fm1,fm2]=powermean(f1,f2,x,dim);
%   __________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2009--2015 J.M. Lilly --- type 'help jlab_license' for details

if strcmpi(varargin{1}, '--t')
    powermean_test,return
end

dim=varargin{end};
x=varargin{end-1};

power=vsum(abs(x).^2,dim);
vswap(power,0,nan);
for i=1:length(varargin)-2
  varargout{i}=vsum(abs(x).^2.*varargin{i},dim)./power;
end

eval(to_overwrite(nargin-2))

function[]=powermean_test

bool(1)=aresame(powermean([1 2],[2 3],2),frac(4+2*9,13));
bool(2)=aresame(powermean([1 2],[2 3],2),vmean([1 2],2,[2 3].^2));

reporttest('VMEAN weighted mean',allall(bool))


function varargout = maxk(varargin)
% function res = MAXK(list, k)
%
% If LIST is a vector, MAXK returns in RES the K largest elements of LIST
%   RES is sorted in descending order
% [res loc] = MAXK(...)
%   Location of the largest elements: RES=LIST(LOC)
% If list is a matrix, MAXK operates along the first dimension
% Use MAXK(..., dim) to operate along the dimension dim
%     MAXK(..., dim, 'sorting', false) to disable the post-sorting step
%                                      (true by default)
%
% Author Bruno Luong <brunoluong@yahoo.com>
% Contributor: Matt Fig
% Last update: 07/April/2009
%              10/Jan/2010: possibility to disable post-sorting step

nout=cell(1,max(1,nargout));
[nout{:}] = minmaxk(@maxkmex, varargin{:});
varargout=nout;


function optionValueReturned = optionvalue(SpotPrice, StrikePrice, RiskFreeRate,TimeExpiry, Volatility,theOptionType,ButterflyRange)
% mcc -d compiled -B 'ccom:BSOptionModel,BSOptionModelClass,1' optionvalue.m webvizroutine.m
%CALCROUTINE Calculate the value of the option

OptionType = lower(theOptionType);

if (strcmpi(OptionType,'call') == 1) %Call option

     %Convert months to expiry to years to expiry
     TimeExpiry = TimeExpiry / 12;

     %Calculate the value of the option
     optionValueReturned = blsprice(SpotPrice, StrikePrice, RiskFreeRate, ...
          TimeExpiry, Volatility);

elseif (strcmpi(OptionType,'put') == 1) %Put option

     %Convert months to expiry to years to expiry
     TimeExpiry = TimeExpiry / 12;

     %Calculate the value of the put option
     optionValueReturned = blsprice(SpotPrice, StrikePrice, RiskFreeRate, ...
          TimeExpiry, Volatility);

elseif (strcmpi(OptionType,'straddle') == 1) %Straddle option

     %Convert months to expiry to years to expiry
     TimeExpiry = TimeExpiry / 12;

     %Compute the value of the straddle

     optionValueReturned = blsstrval(SpotPrice, StrikePrice, RiskFreeRate, ...
          TimeExpiry, Volatility);

elseif (strcmpi(OptionType,'butterfly') == 1) %Butterfly option

     %Convert months to expiry to years to expiry
     TimeExpiry = TimeExpiry / 12;

     %Compute the value of the butterfly
     optionValueReturned = blsbtyval(SpotPrice, StrikePrice, RiskFreeRate, ...
          TimeExpiry, Volatility, ButterflyRange);

end

%--------------------------------------------------------------------------

function StraddleValue = blsstrval(SpotPrice, StrikePrice, RiskFreeRate, ...
          TimeExpiry, Volatility)
%BLSSTRVAL Black Scholes value of a straddle option

%Calculate the value of both the call and put option
[CallValue, PutValue] = blsprice(SpotPrice, StrikePrice, RiskFreeRate, ...
     TimeExpiry, Volatility);

%Compute the value of the straddle
StraddleValue = CallValue + PutValue;

%end of BLSSTRVAL subroutine

function ButterflyValue = blsbtyval(SpotPrice, StrikePrice, RiskFreeRate, ...
          TimeExpiry, Volatility, ButterflyRange)
%BLSBTYVAL Black Scholes value of a butterfly option

%Set the different strike prices
LowStrike = StrikePrice .* (1 - ButterflyRange);
HighStrike = StrikePrice .* (1 + ButterflyRange);

%Value the long positions in the low and high struck calls
LowValue = blsprice(SpotPrice, LowStrike, RiskFreeRate, ...
     TimeExpiry, Volatility);
HighValue = blsprice(SpotPrice, HighStrike, RiskFreeRate, ...
     TimeExpiry, Volatility);

%Value the short position in the calls struck at the initial strike
%price
ShortValue = 2 .* -(blsprice(SpotPrice, StrikePrice, RiskFreeRate, ...
     TimeExpiry, Volatility));

%Calculate the total value of the butterfly
ButterflyValue = LowValue + HighValue + ShortValue;

%end of BLSBTYVAL subroutine


clear all; close all; clc;

%%
L=10; x3=-L:0.1:L; n=length(x3)-1; % define domain
x2=x3(1:n); k=(2*pi/(2*L))*[0:n/2-1 -n/2:-1]; % k-vector
ye=exp(-(x2.^2)); ye2=exp((x2.^2)/2); % define Gaussians
for j=0:9      % loop through 10 modes
  yd=real(ifft(((i*k).^j).*fft(ye))); % 2nd derivative
  mode=((-1)^(j))*(((2^j)*factorial(j)*sqrt(pi))^(-0.5))*ye2.*yd;
  y(:,j+1)=(mode).';  % store modes as columns
end

x=x2(n/2+1-40:n/2+1+40);  % keep only -4<x<4
yharm=y(n/2+1-40:n/2+1+40,:);

%%
n=length(x);
f=(exp(-(x-0.5).^2)+3*exp(-2*(x+1.5).^2))';

for j=1:10  % full reconstruction
  a(j,1)=trapz(x,f.*yharm(:,j));
end
f2=yharm*a;
Efull(1)=log(norm(f2-f)+1);  % reconstruction error

for j=1:10  % matrix M reconstruction
   for jj=1:j
       Area=trapz(x,yharm(:,j).*yharm(:,jj));
       M(j,jj)=Area;
       M(jj,j)=Area;
   end
end
Cfull=cond(M)   % get condition number

%% Test Random trials with P% of measurements
for thresh=1:5;
  for jloop=1:1000  % 1000 random trials
    n2=randsample(n,8*thresh);  % random sampling
    P=zeros(n,1); P(n2)=1;
    for j=1:10
        for jj=1:j   % compute M matrix
            Area=trapz(x,P.*(yharm(:,j).*yharm(:,jj)));
            M2(j,jj)=Area; M2(jj,j)=Area;
        end
    end
    for j=1:10  % reconstruction using gappy
        ftild(j,1)=trapz(x,P.*(f.*yharm(:,j)));
    end
    atild=M2\ftild;   % compute error
    f2=yharm*atild;   % compute reconstruction
    Err(jloop)=norm(f2-f); % L2 error
    con(jloop)=cond(M2);   % condition number
  end
  % mean and variance
  E(thresh)=mean(log(Err+1)); V(thresh)=(var(log(Err+1)));
  Ec(thresh)=mean(log(con)); Vc(thresh)=(var(log(con)));
end
E=[E Efull]; V=[V 0];
Ec=[Ec log(Cfull)]; Vc=[Vc 0];
subplot(2,1,1), bar(E), hold on, errorbar(E,V,'r.')
subplot(2,1,2), bar(Ec), hold on, errorbar(Ec,Vc,'r.')

%%  For 20% measurements, sort great from bad

Pmaster=[];
for jloop=1:200  % 200 random trials
n2=randsample(n,20);
P=zeros(n,1); P(n2)=1;

Pmaster=[Pmaster P];

for j=1:10
   for jj=1:j
       Area=trapz(x,P.*(yharm(:,j).*yharm(:,jj)));
       M2(j,jj)=Area; M2(jj,j)=Area;
   end
end
for j=1:10  % reconstruction using gappy
  ftild(j,1)=trapz(x,P.*(f.*yharm(:,j)));
end
atild=M2\ftild;   % compute error
f2=yharm*atild;
Er(jloop)=norm(f2-f);
co(jloop)=cond(M2);

end

%%
figure(5)
subplot(3,1,1), bar(log(co))
subplot(3,1,2), hist(log(Er+1),50)
subplot(3,1,3), hist(log(co),50)

figure(7)
pcolor(-Pmaster.'), colormap(hot)

[Cloc,jloc]=sort(co);
figure(8)
%bar(log(Cloc));

jbest=jloc(1:11); jworst=jloc(end-10:end);
Sbest(:,1:11)=Pmaster(:,jbest);
Sworst(:,1:11)=Pmaster(:,jworst);
csor=[100*Cloc(1:10) zeros(1,10) Cloc(end-9:end)];

subplot(3,1,1), pcolor(-Sbest.'); colormap(hot)
subplot(3,1,2), pcolor(-Sworst.'); colormap(hot)
subplot(3,1,3), bar(csor)

%%
figure(7), axis off
figure(8), subplot(3,1,1), axis off
figure(8), subplot(3,1,2), axis off
figure(8), subplot(3,1,3), set(gca,'Xticklabel',{},'Yticklabel',{},'Xlim',[0 31])
figure(5), subplot(3,1,1), set(gca,'Xticklabel',{},'Yticklabel',{},'Xlim',[0 200])
figure(5), subplot(3,1,2), set(gca,'Xticklabel',{},'Yticklabel',{})
figure(5), subplot(3,1,3), set(gca,'Xticklabel',{},'Yticklabel',{})
figure(3), subplot(2,1,1), set(gca,'Xticklabel',{},'Yticklabel',{})
figure(3), subplot(2,1,2), set(gca,'Xticklabel',{},'Yticklabel',{})

%%

clear q
clear q2
figure(9)
m=30;
q=rand(m,m); n=m^2;
p=randsample(n,200);
q2=zeros(m,m);
q2(p)=q(p);
p2=randsample(n,40);
q3=zeros(m,m);
q3(p2)=q(p2);
pcolor(-q), colormap(hot), caxis([-1 0]), axis off
figure(10)
pcolor(-q2), colormap(hot), caxis([-1 0]), axis off
figure(11)
pcolor(-q3), colormap(hot), caxis([-1 0]), axis off


function x = daub2_transform_inverse ( n, y )

%*****************************************************************************80
%
%% DAUB2_TRANSFORM_INVERSE inverts the DAUB2 transform of a vector.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 July 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the dimension of the vector.
%    N must be a power of 2.
%
%    Input, real Y(N), the transformed vector.
%
%    Output, real X(N), the original vector.
%
  c = [ 7.071067811865475E-01, ...
        7.071067811865475E-01 ];

  x(1:n,1) = y(1:n);
  z(1:n,1) = 0.0;

  m = 1;

  while ( m * 2 <= n )

    for i = 1 : m
      z(2*i-1,1) = c(1) * ( x(i) + x(i+m) );
      z(2*i,1)   = c(2) * ( x(i) - x(i+m) );
    end

    x(1:2*m,1) = z(1:2*m);

    m = m * 2;

  end

  return
end


function [x_new,P_new,w_new] = gauss_merge(x,P,w,threshold)
% gauss_merge Merge mixture components that fall in close proximity
%
% TODO:
% ----
% * Generate documentation
%
% Credit:
% -------
% * This code has been taken from http://ba-tuong.vo-au.com/codes.html.
% * Minor modifications may have been applied to the original version.

L= length(w); x_dim= size(x,1);
I= 1:L;
el= 1;

if all(w==0)
    x_new = [];
    P_new = [];
    w_new = [];
    return;
end

while ~isempty(I)
    [notused,j]= max(w); j= j(1);
    Ij= []; iPt= inv(P(:,:,j));
    x_new(:,el)= zeros(x_dim,1);
    P_new(:,:,el)= zeros(x_dim,x_dim);
    w_new(1,el)= 0;
    for i= I
        val= (x(:,i)-x(:,j))'*iPt*(x(:,i)-x(:,j));
        if val <= threshold
            Ij= [ Ij i ];
        end
    end

    x_new(:,el)= wsumvec(w(Ij),x(:,Ij),x_dim);
    P_new(:,:,el)= wsummat(w(Ij)',P(:,:,Ij),x_dim);
    w_new(1,el)= sum(w(Ij));

    x_new(:,el)= x_new(:,el)/w_new(el);
    P_new(:,:,el)= P_new(:,:,el)/w_new(el);
    I= setdiff(I,Ij);
    w(Ij)= -1;
    el= el+1;
end

function out = wsumvec(w,vecstack,xdim)
    wmat = repmat(w,[xdim,1]);
    out  = sum(wmat.*vecstack,2);

function out = wsummat(w,matstack,xdim)
    w = reshape(w,[1,1,size(w)]);
    wmat = repmat(w,[xdim,xdim,1]);
    out = sum(wmat.*matstack,3);


function out = arcLength(f, a, b)
%ARCLENGTH   Compute the length of the arc defined by a CHEBFUN.
%   ARCLENGTH(F) returns the arc length of the curve defined by CHEBFUN F in
%   the x-y plane over the interval where it is defined. If F is complex,
%   the output is the arc length of the curve in the complex plane.
%
%   ARCLENGTH(F, A, B) returns the arc length of F over the interval [A, B],
%   where [A, B] is a subinterval of the domain in which F is defined. In the
%   case of complex-valued F, ARCLENGTH(F, A, B) computes the length of the
%   arc whose ends correspond to A and B.
%
%   If F is a quasimatrix, the arc length of each CHEBFUN in F is computed
%   and a vector is returned.
%
% Examples:
%
%   f = chebfun(@(x) sin(x), [0 1]); L = arcLength(f)
%
%   arcLength(chebfun('exp(1i*pi*s)'))/(2*pi)

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

% Empty CHEBFUN:
if ( isempty(f) )
    out = [];
    return
end

% Check the first input for its type:
if ( ~isa(f, 'chebfun') )
    error('CHEBFUN:CHEBFUN:arcLength:Input', ...
        'The first argument must be a chebfun object.')
end

% Get the interval corresponding to the arc:
if ( nargin == 3 )
    % Full arguments:
    dom(1) = a;
    dom(2) = b;
elseif ( nargin == 2 )
    % Two arguments: the second argument is a vector:
    if ( max( size(a) ) ~= 2 )
        error('CHEBFUN:CHEBFUN:arcLength:Input', ...
            'The second argument must be a 1x2 vector.')
    end
    dom = a;
else
    % Single argument:
    dom = [f(1).domain(1) f(1).domain(end)];
end

% Loop over each column or row:
fPrime = f;
for i = 1:numel(f.funs)
    % This makes sure that no delta functions are
    % generated due to jump discontinuities. pointValues of fPrime are not
    % updated since they are not needed.
    fPrime.funs{i} = diff(f.funs{i});
end

if ( isreal(f) )
    g = sqrt(1 + fPrime.^2);
    out = sum(g, dom(1), dom(2));

else
    out = sum(abs(fPrime), dom(1), dom(2));
end

% Reform the output to accommodate the transposedness with the input:
if ( f(1).isTransposed )
    out = out.';
end

end


function b = c8mat_fss ( n, a, nb, b )

%*****************************************************************************80
%
%% C8MAT_FSS factors and solves a system with multiple right hand sides.
%
%  Discussion:
%
%    This routine uses partial pivoting, but no pivot vector is required.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 March 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    N must be positive.
%
%    Input, complex A(N,N), the coefficient matrix of the linear system.
%
%    Input, integer NB, the number of right hand sides.
%
%    Input, complex B(N,NB), the right hand side of the linear system.
%
%    Output, complex B(N,NB), the solution of the linear system.
%
  info = 0;

  for jcol = 1 : n
%
%  Find the maximum element in column I.
%
    piv = abs ( a(jcol,jcol) );
    ipiv = jcol;
    for i = jcol+1 : n
      if ( piv < abs ( a(i,jcol) ) )
        piv = abs ( a(i,jcol) );
        ipiv = i;
      end
    end

    if ( piv == 0.0 )
      info = jcol;
      fprintf ( 1, '\n' );
      fprintf ( 1, 'C8MAT_FSS - Fatal error!\n' );
      fprintf ( 1, '  Zero pivot on step %d\n', info );
      return
    end
%
%  Switch rows JCOL and IPIV, and B.
%
    if ( jcol ~= ipiv )

      for j = 1 : n
        t         = a(jcol,j);
        a(jcol,j) = a(ipiv,j);
        a(ipiv,j) = t;
      end

      t(1,1:nb)    = b(jcol,1:nb);
      b(jcol,1:nb) = b(ipiv,1:nb);
      b(ipiv,1:nb) = t(1,1:nb);

    end
%
%  Scale the pivot row.
%
    temp = a(jcol,jcol);
    a(jcol,jcol) = 1.0;
    a(jcol,jcol+1:n) = a(jcol,jcol+1:n) / temp;
    b(jcol,1:nb) = b(jcol,1:nb) / temp;
%
%  Use the pivot row to eliminate lower entries in that column.
%
    for i = jcol+1 : n
      if ( a(i,jcol) ~= 0.0 )
        temp = - a(i,jcol);
        a(i,jcol) = 0.0;
        a(i,jcol+1:n) = a(i,jcol+1:n) + temp * a(jcol,jcol+1:n);
        b(i,1:nb) = b(i,1:nb) + temp * b(jcol,1:nb);
      end
    end

  end
%
%  Back solve.
%
  for j = 1 : nb
    for jcol = n : -1 : 2
      b(1:jcol-1,j) = b(1:jcol-1,j) - a(1:jcol-1,jcol) * b(jcol,j);
    end
  end

  return
end


function a = latin_cover ( n, p )

%*****************************************************************************80
%
%% LATIN_COVER returns a 2D Latin Square Covering.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 August 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of points.
%
%    Input, integer P(N), a permutation which describes the
%    first Latin square.
%
%    Output, integer A(N,N), the Latin cover.  A(I,J) = K
%    means that (I,J) is one element of the K-th Latin square.
%
  perm_check ( n, p );

  for i = 1 : n
    for k = 1 : n
      ik = i4_wrap ( i + k - 1, 1, n );
      a(i,p(ik)) = k;
    end
  end

  return
end


function [ BaseN,Gray ] = GrayNumberConvertion(Value,Base,Digits )

BaseN = zeros( 1,Digits+1 );
Gray  = zeros( 1,Digits+1 );

for i=1:Digits
    BaseN(i) =  mod(( Value / Base^(i-1)),Base );
    Value = Value - BaseN(i);
end

Shift = 0;
for i = (Digits+1):(-1):1
    Gray( i ) = mod( ( BaseN( i ) - Shift ),Base);
    Shift = Shift + Gray(i);
end

end


function varargout = solver_sDantzig( A, b, delta, mu, x0, z0, opts, varargin )
% SOLVER_SDANTZIG Dantzig selector problem. Uses smoothing.
%[ x, out, opts ] = solver_sDantzig( A, b, delta, mu, x0, z0, opts )
%    Solves the smoothed Dantzig
%        minimize norm(x,1) + (1/2)*mu*norm(x-x0).^2
%        s.t.     norm(D.*(A'*(A*x-b)),Inf) <= delta
%    by constructing and solving the composite dual
%        maximize - g_sm(z) - delta*norm(z,1)
%    where
%        gsm(z) = sup_x <z,D.*A'*(Ax-b)>-norm(x,1)-(1/2)*mu*norm(x-x0)
%    A must be a linear operator, b must be a vector, and delta and mu
%    must be positive scalars. Initial points x0 and z0 are optional.
%    The standard calling sequence assumes that D=I. To supply a scaling,
%    pass the cell array { A, D } instead of A. D must either be a scalar,
%    a vector of weights, or a linear operator.

% Supply default values
error(nargchk(4,8,nargin));
if nargin < 5, x0   = []; end
if nargin < 6, z0   = []; end
if nargin < 7, opts = []; end

% -- legacy options from original software --
if isfield(opts,'lambda0')
    z0 = opts.lambda0;
    opts = rmfield(opts,'lambda0');
end
if isfield(opts,'xPlug')
    x0 = opts.xPlug;
    opts = rmfield(opts,'xPlug');
end
if isfield(opts,'solver')
    svr     = opts.solver;
    opts    = rmfield(opts,'solver');
    if isfield(opts,'alg') && ~isempty(opts.alg)
        disp('Warning: conflictiong options for the algorithm');
    else
        % if specified as "solver_AT", truncate:
        s = strfind( svr, '_' );
        if ~isempty(s), svr = svr(s+1:end); end
        opts.alg = svr;
    end
end

% Extract the linear operators
D = [];
if isa( A, 'cell' ),
    if length(A) > 1, D = A{2}; end
    A = A{1};
end
if isempty(D),
    D = @(x)x;
elseif isa( D, 'double' ),
    D = @(x)D.*x;
end
if isa( A, 'double' ),
    A = linop_matrix(A);
end

% Call TFOCS
objectiveF = prox_l1;
affineF    = { @(y,mode)linear_DS( D, A, y, mode ), -D(A(b,2)) };
dualproxF  = prox_l1( delta );
[varargout{1:max(nargout,1)}] = ...
    tfocs_SCD( objectiveF, affineF, dualproxF, mu, x0, z0, opts, varargin{:} );

% Implements x -> D*A'*A*x and its adjoint if A is a linop
function y = linear_DS( D, A, y, mode )
switch mode,
case 0,
    y = A([],0);
    if iscell( y ),
        y = { y{1}, y{1} };
    else
        y = { [y(2),1], [y(2),1] };
    end
case 1, y = D(A(A(y,1),2));
case 2, y = A(A(D(y),1),2);
end

% TFOCS v1.3 by Stephen Becker, Emmanuel Candes, and Michael Grant.
% Copyright 2013 California Institute of Technology and CVX Research.
% See the file LICENSE for full license information.


function pass = test_nufft( pref )

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

% Choose a tolerance:
tol = eps;
rng(0)

% Test NUFFT-I
count = 1;
for N = 10.^(0:4)
    omega = (0:N-1)';
    omega = omega + 1.2*rand(N,1)/N;
    c = rand(N,1) + 1i*rand(N,1);
    exact = nudft1( c, omega );
    fast = chebfun.nufft( c, omega, 1 );
    pass(count) = norm( exact - fast, inf ) < 300*N*tol*norm(c,1);
    count = count + 1;
end

% Test on random inputs:
N = 50;
omega = N*randn(1,N);
c = rand(N,1) + 1i*rand(N,1);
F = exp(-2*pi*1i*((0:N-1)/N).'*omega);
f = chebfun.nufft(c, omega.', 1);
pass(count) = ( norm( f - F*c ) < 300*N*tol*norm(c,1) );
count = count + 1;

% Test NUFFT-II:
for N = 10.^(0:4)
    x = linspace(0,1,N+1)'; x(end) = [];
    x = x + 1.2/N;
    c = rand(N,1) + 1i*rand(N,1);
    exact = nudft2( c, x );
    fast = chebfun.nufft( c, x );
    pass(count) = norm( exact - fast, inf ) < 300*N*tol*norm(c,1);
    count = count + 1;
end

% Test on random inputs:
N = 50;
x = 100*rand(N,1);
c = rand(N,1) + 1i*rand(N,1);
F = exp(-2*pi*1i*x*(0:N-1));
f = chebfun.nufft(c, x);
pass(count) = ( norm( f - F*c ) < 300*N*tol*norm(c,1) );
count = count + 1;

% Test NUFFT-III
for N = 10.^(0:4)
    omega = (0:N-1)';
    omega = omega + 1.2*rand(N,1)/N;
    x = linspace(0,1,N+1)'; x(end) = [];
    x = x + 1.2/N;
    c = rand(N,1) + 1i*rand(N,1);
    exact = nudft3( c, x, omega );
    fast = chebfun.nufft( c, x, omega, 3 );
    pass(count) = norm( exact - fast, inf ) < 10000*N*tol*norm(c,1);
    count = count + 1;
end

% Test NUFFT-II on nonsquare inputs:
n = 101;
m = 3;
x = linspace(0,1,m+1)'; x(end) = [];
x = x + 1.01/m;
c = rand(n,1);
exact = nudft2( c, x );
fast = chebfun.nufft( c, x );
pass(count) = norm( exact - fast, inf ) < 100*tol*norm(c,1);
count = count + 1;

end

function f = nudft1( c, omega)

f = zeros(size(omega,1),1);
for j = 1:numel(f)
    f(j) = exp(-2*pi*1i*(j-1)/size(omega,1)*omega.')*c;
end
end

function f = nudft2( c, x)

f = zeros(size(x,1),1);
omega = 0:size(c,1)-1;
for j = 1:numel(f)
    f(j) = exp(-2*pi*1i*x(j)*omega)*c;
end
end

function f = nudft3( c, x, omega)

f = zeros(size(omega,1),1);
for j = 1:numel(f)
    f(j) = exp(-2*pi*1i*x(j)*omega.')*c;
end
end


function f = changeMap(f, newdom)
%CHANGEMAP   Map the domain of a DELTAFUN via a linear change of variable.
%   G = MAP(F,NEWDOM), where the DELTAFUN F has a domain [a, b], returns a
%   DELTAFUN G defined on [c, d], where c = NEWDOM(1), d = NEWDOM(2), such that
%       G(x) = F(a*(d - x)/(d - c) + b*(x - c)/(d - c)) for all x in [c, d].

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

% Store the old mapping:
oldMapping = f.funPart.mapping;

% Map the funPart:
f.funPart = changeMap(f.funPart, newdom);

% Grab the new mapping:
newMapping = f.funPart.mapping;

% Map the deltaLocs:
f.deltaLoc = newMapping.For(oldMapping.Inv(f.deltaLoc));

end


function c = ultra2ultra(c, lam_in, lam_out)
%ULTRA2ULTRA   Convert between Ultraspherical (US) expansions.
%   C_OUT = ULTRA2ULTRA(C_IN, LAM_IN, LAM_OUT) converts the vector C_IN of
%   US-LAM_IN coefficients to a vector of US-LAM_OUT coefficients.
%
%   This code is essentially a wrapper for the JAC2JAC code based on [1].
%
%   References:
%     [1] A. Townsend, M. Webb, and S. Olver, "Fast polynomial transforms
%     based on Toeplitz and Hankel matrices", submitted, 2016.
%
% See also JAC2JAC.

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

n = length(c) - 1;

% Scale input from US to Jacobi basis:
c = c./scl(lam_in, n);
% Call JAC2JAC:
c = jac2jac(c, lam_in-.5, lam_in-.5, lam_out-.5, lam_out-.5);
% Scale output from Jacobi to US:
c = c.*scl(lam_out, n);

end

function s = scl(lam, n)
% Scaling from Jacobi to US polynomials. See DLMF Table 18.3.1.
    if ( lam == 0 )
        nn = (0:n-1).';
        s = [1 ; cumprod((nn+.5)./(nn+1))];
    else
        nn = (0:n).';
        s = ( gamma(2*lam) ./ gamma(lam+.5) ) * ...
            exp( gammaln(lam+.5+nn) - gammaln(2*lam+nn) );
    end
end


% Diagram of van Albada limiter

% Theory in Section 10.8 of:

% 	P. Wesseling: Principles of Computational Fluid Dynamics
% 	Springer, Heidelberg, 2000 ISBN 3-5453-0. XII, 642 pp.
% See http://ta.twi.tudelft.nl/nw/users/wesseling/cfdbook.html

% This program makes Fig. 10.27 in the book

figure(1), clf, hold on

x = 0.0:0.01:4.0; y = (x.*x + x)./(1 + x.*x);	% Graph of van Albada limiter
plot(x,y)

x = 0.0:0.01:1.1; y = 0.5*(1+sqrt(2))*x; plot(x,y)
x = 0.0:0.01:4.0; y = 0.5*(1+sqrt(2))*ones(size(x)); plot(x,y)
y = ones(size(x)); plot(x,y)
x = 0.0:0.01:1.3; plot(x,x)
y = 0.0:0.01:1; x = ones(size(y)); plot(x,y,'--')


function regression_table(y,X,names,varargin)
%regression_table(y,X,names,[do figures],[do robust])
%
% performs regression using the regress command, and prints an output table
% X should not have an intercept
%
% Tor Wager

dofigs = 1; dorobust = 0;
if length(varargin) > 0, dofigs = varargin{1}; end
if length(varargin) > 1, dorobust = varargin{2}; end

[n, k] = size(X);

if dorobust
    [b,stat]=robustfit(X,y);
    BINT = NaN * zeros(n + 1,2);
    STATS = [NaN NaN NaN];
    partialr = NaN * zeros(1, k + 1);

else
    [b,dev,stat]=glmfit(X,y);
    [B,BINT,R,RINT,STATS] = regress(y,[ones(n ,1) X]);

    % partial correlations
    partialr(1) = NaN;  % intercept
    for i = 1:k, [x,y,partialr(i+1),partialp] = partialcor(X,y,i); end
end

names = [{'Intercept'}, names];

fprintf(1,'Parameter\tb-hat\tt\tp\tConf. interval\t\tPartial Corr\tRob. Partial Corr.\n')

if dofigs
    create_figure('Regression Table output', k, 2);

end

for i = 1:length(b)

    if i == 1, %intercept
            fprintf(1,'%s\t%3.2f\t%3.2f\t%3.4f\t%3.2f\t%3.2f\t\n', ...
        names{i},b(i),stat.t(i),stat.p(i),BINT(i,1),BINT(i,2));

    else
        if dofigs
            subplot(k, 2, 2 * (i - 2) + 1); [r,str,sig] = prplot(y,X,i-1,0); xlabel([names{i}])
            subplot(k, 2, 2 * (i - 2) + 2); [r2,str,sig] = prplot(y,X,i-1,1); xlabel([names{i} ': Robust IRLS'])
        else
            r = partialr(i);
            r2 = NaN;
        end

        fprintf(1,'%s\t%3.2f\t%3.2f\t%3.4f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t\n', ...
        names{i},b(i),stat.t(i),stat.p(i),BINT(i,1),BINT(i,2),r,r2);
    end
end

fprintf(1,'\nOverall R2 = %3.2f, F = %3.2f, p = %3.4f\t\n',STATS(1),STATS(2),STATS(3));

return


function Umap=FM2map(im,U,H)
% Unpack fuzzy-membership funcitons to produce membership maps.
%
% INPUT ARGUMENTS:
%   - im    : N-dimensional grayscale image in integer format.
%   - U     : L-by-c array of fuzzy class memberships, where c is the
%             number of classes and L is the intensity range of the input
%             image, such that L=numel(min(im(:)):max(im(:))). See
%             FastFCMeans for more info.
%   - H     : image histogram returned by FastFCMeans function.
%
% OUTPUT:
%   - Umap  : membership maps. Umap has the same size as the input image
%             plus an additional dimension to account for c classes. For
%             example, if im is a 2D M-by-N image then U will be
%             M-by-N-by-c array where U(:,:,i) is a membership map for the
%             i-th class.
%
% AUTHOR    : Anton Semechko (a.semechko@gmail.com)
% DATE      : May.2013
%

if nargin<3 || isempty(H)

    % Intensity range
    Imin=double(min(im(:)));
    Imax=double(max(im(:)));
    I=(Imin:Imax)';

    % Intensity histogram
    H=hist(double(im(:)),I);
    H=H(:);

end

% Unpack memberships
Umap=zeros(sum(H),size(U,2));
i1=1; i2=0;
for i=1:numel(H)
    i2=i2+H(i);
    Umap(i1:i2,:)=repmat(U(i,:),[H(i) 1]);
    i1=i2+1;
end

% Find the positional mapping
[~,idx]=sort(im(:), 'ascend');
[~,idx]=sort(idx(:),'ascend');

% Reshape membership maps to match image dimensions.
Umap=reshape(Umap(idx,:),[size(im) size(U,2)]);


function [mu,dmu,k,gamma] = sparse_learning(Phi,T,lambda,iters,flag1,flag2,flag3)
% *************************************************************************
%
% *** PURPOSE ***
% Implements generalized versions of SBL and FOCUSS for learning sparse
% representations from possibly overcomplete dictionaries.
%
%
% *** USAGE ***
% [mu,dmu,k,gamma] = sparse_learning(Phi,T,lambda,iters,flag1,flag2,flag3);
%
%
% *** INPUTS ***
% Phi       = N X M dictionary
% T         = N X L data matrix
% lambda    = scalar trade-off parameter (balances sparsity and data fit)
% iters     = maximum number of iterations
%
% flag1     = 0: fast Mackay-based SBL update rules
% flag1     = 1: fast EM-based SBL update rule
% flag1     = 2: traditional (slow but sometimes better) EM-based SBL update rule
% flag1     = [3 p]: FOCUSS algorithm using the p-valued quasi-norm
%
% flag2     = 0: regular initialization (equivalent to min. norm solution)
% flag2     = gamma0: initialize with gamma = gamma0, (M X 1) vector
%
% flag3     = display flag; 1 = show output, 0 = supress output
%
% *** OUTPUTS ***
% mu        = M X L matrix of weight estimates
% dmu       = delta-mu at convergence
% k         = number of iterations used
% gamma     = M X 1 vector of hyperparameter values
%
%
% *************************************************************************
% Written by:  David Wipf, david.wipf@mrsc.ucsf.edu
% *************************************************************************

% *** Control parameters ***
MIN_GAMMA       = 1e-16;
MIN_DMU         = 1e-12;
MAX_ITERS       = iters;
DISPLAY_FLAG    = flag3;     % Set to zero for no runtime screen printouts

% *** Initializations ***
[N M] = size(Phi);
[N L] = size(T);

if (~flag2)         gamma = ones(M,1);
else                gamma = flag2;  end;

keep_list = [1:M]';
m = length(keep_list);
mu = zeros(M,L);
dmu = -1;
k = 0;

% *** Learning loop ***
while (1)

    % *** Prune things as hyperparameters go to zero ***
    if (min(gamma) < MIN_GAMMA )
		index = find(gamma > MIN_GAMMA);
		gamma = gamma(index);
		Phi = Phi(:,index);
		keep_list = keep_list(index);
        m = length(gamma);

        if (m == 0)   break;  end;
    end;

    % *** Compute new weights ***
    G = repmat(sqrt(gamma)',N,1);
    PhiG = Phi.*G;
    [U,S,V] = svd(PhiG,'econ');

    [d1,d2] = size(S);
    if (d1 > 1)     diag_S = diag(S);
    else            diag_S = S(1);      end;

    U_scaled = U(:,1:min(N,m)).*repmat((diag_S./(diag_S.^2 + lambda + 1e-16))',N,1);
    Xi = G'.*(V*U_scaled');

    mu_old = mu;
    mu = Xi*T;

    % *** Update hyperparameters ***
    gamma_old = gamma;
    mu2_bar = sum(abs(mu).^2,2);

    if (flag1(1) == 0)
        % MacKay fixed-point SBL
        R_diag = real( (sum(Xi.'.*Phi)).' );
        gamma = mu2_bar./(L*R_diag);

    elseif (flag1(1) == 1)
        % Fast EM SBL
        R_diag = real( (sum(Xi.'.*Phi)).' );
        gamma = sqrt( gamma.*real(mu2_bar./(L*R_diag)) );

    elseif (flag1(1) == 2)
        % Traditional EM SBL
        PhiGsqr = PhiG.*G;
        Sigma_w_diag = real( gamma - ( sum(Xi.'.*PhiGsqr) ).' );
        gamma = mu2_bar/L + Sigma_w_diag;

    else
        % FOCUSS
        p = flag1(2);
        gamma = (mu2_bar/L).^(1-p/2);
    end;

    % *** Check stopping conditions, etc. ***
  	k = k+1;
    if (DISPLAY_FLAG) disp(['iters: ',num2str(k),'   num coeffs: ',num2str(m), ...
            '   gamma change: ',num2str(max(abs(gamma - gamma_old)))]); end;
    if (k >= MAX_ITERS) break;  end;

	if (size(mu) == size(mu_old))
        dmu = max(max(abs(mu_old - mu)));
        if (dmu < MIN_DMU)  break; end;
    end;

end;

% *** Expand weights, hyperparameters ***
temp = zeros(M,1);
if (m > 0) temp(keep_list,1) = gamma;  end;
gamma = temp;

temp = zeros(M,L);
if (m > 0) temp(keep_list,:) = mu;  end;
mu = temp;

return;