function sar = sar(high, low)
    %constants
    uptrend = 1;
    downtrend = 2;
    sar(1)=NaN;

    %variables
    alpha=0.02;
    EP=high(1);%between the high and low

    %'previous trade' is either long or short on day 1. sar(1)=NaN

    %decide whether to start with long or short
    if(high(1)>high(2))
        position=uptrend;
    else
        position=downtrend;
    end

    if(position==uptrend)
        sar(1)=high(1);
    else
        sar(1)=low(1);
    end

    for n=1:length(high)-1
        if(position==uptrend)
            EP=min(EP,low(n));
        else
            EP=max(EP,high(n));
        end
        alpha=min(alpha+0.02,0.2);
        sar(n+1)=sar(n)+alpha*(EP-sar(n));

        %"Parabolic SAR is never moved within the range of the current
        %or previous day (highest High to lowest Low over the 2 days)."
        if(n>2)
            if(sar(n+1)<max(high(n-1),high(n)) && sar(n)>min(low(n-1),low(n)))
                if(position==downtrend)
                    sar(n+1)=min(low(n-1),low(n));
                else
                    sar(n+1)=max(high(n-1),high(n));
                end
            end
        end
        if(sar(n+1)<high(n+1) && sar(n+1)>low(n+1))
            sar(n+1)=EP;
            if(position==uptrend)
                position=downtrend;
                EP=low(n);
            else
                position=uptrend;
                EP=high(n);
            end
            alpha=0.02;
            %sar(n+1)=sar(n)+alpha*(EP-sar(n));
        end
    end
end


% Script used to generate a visualisation of how well EKF, UKF and PF
% approximate a distribution
% =========================================================================

% Assume a target positioned at x = 1, travelling with speed v = 0.1
state = [2000; 0.1; 0; 0];

% Instantiate an Observation model
% obs = LinGaussObsModelX_2D('NumStateDims',4,'ObsErrVariance',0.2,'Mapping',[1 2]);
obs = RangeBearing2CartesianX('NumStateDims',4,'MeasurementErrVariance',[(pi/90)^2,(1)^2],...
                              'Mapping',[1 3]);

% View the transition matrix and process covariance matrices
R = obs.covar();

% Predict the target's position and velocity after the interval has passed
measurement  = obs.feval(state);

%% EKF
[ekf_mean, H_jac] = ExtendedKalmanFilterX.computeJac_(@(x)obs.finv(x),measurement);
ekf_covar = H_jac*R*H_jac';

%% UKF
alpha = 0.5;
kappa = 0;
beta  = 2;
% Calculate unscented transformation parameters
[c, Wmean, Wcov, OOM] = matlabshared.tracking.internal.calcUTParameters(alpha,beta,kappa,2);
% Form the sigma points
X = formSigmaPoints(measurement, R, c);
% Perform Unscented Transform to get predicted measurement mean,
% covariance and cross-covariance
[ukf_mean,ukf_covar,Pxy] = unscentedTransform(@(x)obs.finv(x),X,Wmean,Wcov,OOM);

%% PF
% Generate 50 random noise samples from the dynamic model
noise = obs.random(50000000);

% Add noise to the measurements
Yk1 = noise + measurement;
Xk1 = obs.finv(Yk1);

Xk = [Xk1];%,Xk2];
Yk = [Yk1];%,Yk2];

figure;
[bandwidth,density,X,Y]=kde2d(Xk([1,3],:)');
xLim = [1985,2005];
yLim = [-200,200];

% EKF
subplot(1,3,1);
%contour(X,Y,density,1);
h = surf(X,Y,density);
z = get(h,'ZData');
% set(h,'ZData',z-10^6)
hold on;
shading interp
x1 = xLim(1):0.1:xLim(2); x2 = yLim(1):0.1:yLim(2);
[X1,X2] = meshgrid(x1,x2);
F = mvnpdf([X1(:) X2(:)],ekf_mean([1,3])',ekf_covar([1,3],[1,3]));
F = reshape(F,length(x2),length(x1));
contour(x1,x2,F,4,'r','LineWidth',2);
% plot_gaussian_ellipsoid(ekf_mean([1,3]),ekf_covar([1,3],[1,3]),'r',1);
% plot_gaussian_ellipsoid(ekf_mean([1,3]),ekf_covar([1,3],[1,3]),'r',2);
% shading interp
%colormap(jet(3000))
title('Taylor-Series Expansion (EKF)')
xlabel('X (m)');
ylabel('Y (m)');
xlim(xLim);
ylim(yLim);
view(0,-90);

% UKF
subplot(1,3,2);
h = surf(X,Y,density);
z = get(h,'ZData');
%set(h,'ZData',z-10^6)
hold on;
shading interp
x1 = xLim(1):0.1:xLim(2); x2 = yLim(1):0.1:yLim(2);
[X1,X2] = meshgrid(x1,x2);
F = mvnpdf([X1(:) X2(:)],ukf_mean([1,3])',ukf_covar([1,3],[1,3]));
F = reshape(F,length(x2),length(x1));
contour(x1,x2,F,4,'r','LineWidth',2);
%plot_gaussian_ellipsoid(ukf_mean([1,3]),ukf_covar([1,3],[1,3]),'r',1);
%plot_gaussian_ellipsoid(ukf_mean([1,3]),ukf_covar([1,3],[1,3]),'r',2);
%colormap(jet(3000))
title('Unscented Transform (UKF)')
xlabel('X (m)');
ylabel('Y (m)');
xlim(xLim);
ylim(yLim);
view(0,-90);

% PF
subplot(1,3,3);
%contour(X,Y,density,1);
h = surf(X,Y,density);
z = get(h,'ZData');
%set(h,'ZData',z-10^6)
hold on;
shading interp
%plot(Xk(1,1:1000),Xk(3,1:1000),'r.');
%[bandwidth,density,X,Y]=kde2d(Xk([1,3],1:10000)');
contour(X,Y,density,4,'r','LineWidth',2)
%colormap(jet(3000))
title('Sampling (PF)')
%set(h,'ZData',z-10)
xlabel('X (m)');
ylabel('Y (m)');
%set(gca,'Xdir','reverse')
set(gca,'Ydir','reverse')
xlim(xLim);
ylim(yLim);
view(0,-90);

% model = PositionalObsModelX(config);%PositionalObsModelX(config);%Polar2CartGaussianModelX(config);
%
% xkm1 = [0.5; 0.3; sqrt(2); sqrt(2)];
% pkm1 = [0 0.5; 0 0.3; sqrt(2) sqrt(2); sqrt(2) sqrt(2)];
% Pkm1 = [0 1 0 1; 0 1 0 1; 0 1 0 1; 1 1 0 1];
% yk = model.obs(1,xkm1);
% Yk = model.sample(1, yk, 500000);
% pk = model.obs_cov();
% Pk = model.eval(1,Yk,xkm1);
% [bandwidth,density,X,Y]=kde2d(Yk');
% figure;
% %contour3(X,Y,density,50);
% surf(X,Y,density);
% hold on;


function s = logsumexp(x, dim)
% Returns log(sum(exp(x),dim)) while avoiding numerical underflow.
% Default is dim = 1 (columns).
% Written by Mo Chen (mochen@ie.cuhk.edu.hk). March 2009.

% Downloaded from:
% http://www.mathworks.com/matlabcentral/fileexchange/28899-logsumexp/content/logsumexp.m
%
% Copyright (c) 2010, Michael Chen
% All rights reserved.

% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are
% met:

%     * Redistributions of source code must retain the above copyright
%       notice, this list of conditions and the following disclaimer.
%     * Redistributions in binary form must reproduce the above copyright
%       notice, this list of conditions and the following disclaimer in
%       the documentation and/or other materials provided with the distribution

% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
% POSSIBILITY OF SUCH DAMAGE.

if nargin == 1,
    % Determine which dimension sum will use
    dim = find(size(x)~=1,1);
    if isempty(dim), dim = 1; end
end

% subtract the largest in each column
y = max(x,[],dim);
x = bsxfun(@minus,x,y);
s = y + log(sum(exp(x),dim));
i = find(~isfinite(y));
if ~isempty(i)
    s(i) = y(i);
end


% Fig. 5.37  Feedback Control of Dynamic Systems, 5e
%             Franklin, Powell, Emami
%script to generate figure 5.37, Piper Dakota autopilot design
numG = 160*conv ([1 2.5],[1 0.7]);
denG = conv([1 5 40],[1  .03  .06]);
sysG = tf(numG,denG);
sysD=tf([1   3],[1   20]);
sysDG=sysD*sysG;
figure(1)
rlocus(sysG)
axis([-30 2 -12 12])
grid
hold on
pause;
rlocus(sysDG)
hold off
pause;
figure(2)
rlocus(sysG)
hold on
pause;
rlocus(sysDG)
axis([-1.5 .1    -.6 .6])
grid on
hold off
pause;
K = 0.3;
sysH=tf(1,1);
sysT = feedback (K*sysG,sysH);
sysTD=feedback(sysDG,sysH);
figure(3)
step(sysT)
hold on
step(sysTD)
grid on
hold off


function bti_test06 ( )

%*****************************************************************************80
%
%% BTI_TEST06 tests BURGERS_TIME_INVISCID.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 April 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BTI_TEST06\n' );
  fprintf ( 1, '  Test BURGERS_TIME_INVISCID\n' );

  method = 6;
  nx = 81;
  nt = 200;
  t_max = 2.0;
  bc = 4;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Method: 6, Godonov.\n' );
  fprintf ( 1, '  Initial condition: expansion\n' );
  fprintf ( 1, '  Number of space nodes = %d\n', nx );
  fprintf ( 1, '  Number of time steps = %d\n', nt );
  fprintf ( 1, '  Final time T_MAX = %g\n', t_max );
  fprintf ( 1, '  Boundary condition = %d\n', bc );

  U = burgers_time_inviscid ( method, @ic_expansion, nx, nt, t_max, bc );

  x = linspace ( -1.0, +1.0, nx );

  figure ( 6 )

  plot ( x, U(1:50:(nt+1),:), 'Linewidth', 3 )
  grid on
  xlabel ( '<-- X -->' )
  ylabel ( '<-- U(X,T) -->' )
  title ( 'Burgers, inviscid, initial expansion, Godonov' )

  filename = 'bti_test06.png';
  print ( '-dpng', filename )
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Saved plot as "%s"\n', filename );

  return
end


function [speedup dsize ksize] = testcudaconv()
%TESTCUDACONV Benchmarks GPU vs CPU convolution.
%   [SPEEDUP DSIZE KSIZE] = TESTCUDACONV() runs 2-D
%   convolutions on random data using both CONV2 and CUDACONV.
%
%   SPEEDUP contains the relative speed of CONV2 and CUDACONV.
%   Thus if SPEEDUP(i,j) = 10, CUDACONV completed the convolution
%   of random DSIZE(i) x DSIZE(i) and KSIZE(j) x KSIZE(j)
%   matrices in 1/10 the time of CONV2.
%
%   DSIZE and KSIZE contain the number of elements in the data
%   and kernel matrices, respectively, for each convolution.

data_sizes		= 100:100:800;
kernel_sizes 	= 10:20:90;
nrepeats		= 3;

gpu_results		= zeros(numel(data_sizes),numel(kernel_sizes));
cpu_results		= zeros(numel(data_sizes),numel(kernel_sizes));

for ds = 1:numel(data_sizes)
	disp(sprintf('Data size: %d elements\n',data_sizes(ds)^2));
	data = rand(data_sizes(ds));
	for ks = 1:numel(kernel_sizes)
		disp(sprintf(' Kernel size: %d elements\n',kernel_sizes(ks)^2));
		kernel = rand(kernel_sizes(ks));

		gpu_temp = zeros(1,nrepeats);
		cpu_temp = zeros(1,nrepeats);

		for t = [1 1:nrepeats]
			tic;
			temp = cudaconv(data,kernel);
			gpu_temp(t) = toc;

			tic;
			temp = conv2(data,kernel,'same');
			cpu_temp(t) = toc;
		end

		gpu_results(ds,ks) = mean(gpu_temp);
		cpu_results(ds,ks) = mean(cpu_temp);
	end
end

speedup = cpu_results./gpu_results;

dsize = data_sizes.^2;
ksize = kernel_sizes.^2;

[x y] = meshgrid(ksize,dsize);

figure;
[c h] = contourf(x,y,speedup,[1 5:5:100]); colorbar;
clabel(c,h,'color',[0 0 0],'FontSize',13,'LabelSpacing',400);
xlabel('Kernel size (# of elements)')
ylabel('Data size (# of elements)')
title('Relative speedup (GPU vs. CPU)')

%dlmwrite('performance2.dat',speedup,',');


function [y,x]=newlagmatrix(x,nlags,c)
% Construction of a matrix of lags (X) and a vector (Y) for use in an
% autoregression
%
% USAGE:
%     [Y,X]=newlagmatrix(Y,NLAGS,C)
%
% INPUTS:
%     Y     - The dependant variable(Tx1)
%     NLAGS - The number of lags(scalar)
%     C     - [OPTIONAL] 1 if you want to include a constant; 0 is default
%
% OUTPUTS:
%     Y     - (n-p) by 1 vector of contemporaneous values
%     X     - (n-p) by p (or p+1 if c=1) matrix of lags and possibly constant.  The
%               matrix is of the form [constant(if included) t-1 t-2 ... t-p]
%
% COMMENTS:
%     Original name, 'lagmatrix' conflicts with a Matlab file, so newlagmatrix
%     replaces original

% Author: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 5    Date: 12/1/2005

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
switch nargin
    case 2
        c=0;
    case 3
        %Nothing
    otherwise
        error('newlagmatrix requires either 2 or 3 arguments.')
end

[T,K]=size(x);
if min(T,K)~=1
    error('X must be a vector')
end

if T<K
    x=x'; %Transpose if a column vector is passed
end

if  length(c)~=1 || any(c~=0 && any(c~=1))
    error('C must be 1 or 0')
end

if length(nlags)~=1 || floor(nlags)~=nlags || any(nlags<0)
    error('NLAGS must be a positive integer')
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nlags>0
    nlags=nlags+1;
    newX=[x;zeros(nlags,1)];
    lagmatrix=repmat(newX,nlags,1);
    lagmatrix=reshape(lagmatrix(1:size(lagmatrix,1)-nlags),T+nlags-1,nlags);
    lagmatrix=lagmatrix(nlags:T,:);
    y=lagmatrix(:,1);
    x=lagmatrix(:,2:nlags);
    if c==1
        x=[ones(size(x,1),1) x];
    end
else
    if c==1
        y=x;
        x=ones(T,1);
    else
        y=x;
        x=[];
    end
end


function cnoise_test02 ( m, n, var, r, alpha )

%*****************************************************************************80
%
%% CNOISE_TEST02 tests F_ALPHA_TGAUSSIAN.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 April 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the number of realizations to create.
%
%    Input, integer N, the size of the noise vector.
%
%    Input, real VAR, the variance of the Gaussian distribution.
%
%    Input, real R, defines the truncated range of the distribution.
%
%    Input, real ALPHA, the value of ALPHA.
%
  scale = exp ( 0.5 * ( 1.0 - alpha ) * log ( n ) );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CNOISE_TEST02:\n' );
  fprintf ( 1, '  Test F_ALPHA_TGAUSSIAN.\n' );
  fprintf ( 1, '  Noise is generated from a truncated Gaussian distribution\n' );
  fprintf ( 1, '  with a specified variance and range.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of realizations M = %d\n', m );
  fprintf ( 1, '  Dimension of noise vector N = %d\n', n );
  fprintf ( 1, '  Noise exponent ALPHA = %f\n', alpha );
  fprintf ( 1, '  Variance of Gaussian distribution VAR = %f\n', var );
  fprintf ( 1, '  Truncated range = [ %f, %f ]\n', -r, r );
  fprintf ( 1, '  Scale factor SCALE = %f\n', scale );

  res = zeros ( m, 1 );

  tic
  for i = 1 : m
    x = f_alpha_tgaussian ( n, var, r, alpha );
    x = scale * x;
    res(i) = mean ( x );
  end
  result_time = toc;

  result_mean = mean ( res );
  result_std = std ( res );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Mean value of noise = %f\n', result_mean );
  fprintf ( 1, '  STD of noise        = %f\n', result_std );
  fprintf ( 1, '  Computation time    = %f seconds\n', result_time );

  return
end


function A = randsym(n,cnd)
%RANDSYM      Random symmetric matrix
%
%   res = randsym(n,cnd)
%
% cnd, if specified, is the approximate condition number of the generated matrix
%

% written  11/05/12     S.M. Rump
% modified 12/06/12     S.M. Rump  lower case
%

  if nargin==1
    A = randn(n);
  else
    s = exp( [ 0 rand(1,n-2) 1 ] * log(cnd) ) / cnd;
    A = randorth(n);
    A = A' * diag(s.*sign(randn(1,n))) * A;
  end

  % make sure A is symmetric
  A = tril(A)+tril(A,-1)';


function [K] = spm_sptop(sigma,q,c)
% Sparse Toeplitz convolution matrix given convolution kernel
% FORMAT [K] = spm_sptop(sigma,q,c)
%
% sigma - of Gaussian kernel K (or kernel itself)
% q     - order of matrix
% c     - kernel index at t = 0 {default c = length(sigma)/2)
% K     - q x q sparse convolution matrix
%_______________________________________________________________________
%
% Returns a q x q sparse convolution matrix.  If sigma is a scalar then
% a symmetrical Gaussian convolution matrix is returned with kernel width
% = sigma.  If sigma is a vector than sigma constitutes the kernel.  To
% obtain an assymmetrical convolution matrix (i.e. implement a phase shift
% set c = 1.
%
% Boundary handling: The row-wise sum of K is set to unity (kernel truncation)
%
%_______________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_sptop.m 1143 2008-02-07 19:33:33Z spm $

% if sigma = 0, return identity matrix; if q = 1, return 1.
%-----------------------------------------------------------------------
if ~any(sigma); K = speye(q); return; end
if q == 1;      K = 1;        return; end

% otherwise get kernel function
%-----------------------------------------------------------------------
if length(sigma) == 1
    E  = ceil(3*sigma);
    x  = [-E:E];
    k  = exp(-x.^2/(2*sigma^2));
else
    if nargin < 3
        c = length(sigma)/2;
    end
    E  = length(sigma);
    x  = [1:E] - fix(c);
    k  = sigma;
end

% and create convolution matrix
%-----------------------------------------------------------------------
K  = k(:)*ones(1,q);
j  = ones(length(k),1)*[1:q];
i  = x(:)*ones(1,q) + j;

% setting the row-wise sum to unity
%-----------------------------------------------------------------------
Q  = find((i >= 1) & (i <= q));
K  = sparse(i(Q),j(Q),K(Q));
Q  = sum(K');
K  = inv(diag(Q + (~Q)))*K;


% Demonstration of the basic operation of a kernel adaptive filtering
% algorithm with fixed budget. This script generates several images and
% saves them to disk.
%
% KRLS-T is chosen as the example algorithm. Inspired by
% http://www.tsc.uc3m.es/~miguel/MLG/adjuntos/slidesKRLST.pdf
%
% This file is part of the Kernel Adaptive Filtering Toolbox for Matlab.
% https://github.com/steven2358/kafbox/

close all
clear
rng('default'); rng(11);

%% PARAMETERS

n = 10; % number of training data
n_star = 201; % number of points to evaluate

ell = 2; % length scale
sigvar = 1; % signal variance
noisevar = .02; % noise variance

budget = 5; % KRLS-T budget

savefig = 0;
manual = 0;

%% GENERATE DATA

fprintf('Generating data...\n');
covfunc = {'kafbox_covSum', {'kafbox_covSEiso','kafbox_covNoise'}};
loghyper = [log(ell); log(sigvar); log(noisevar)];

x = (1:n)' + 0.2*randn(n,1);
y = chol(feval(covfunc{:}, loghyper, x))'*randn(n,1);      % Cholesky decomp.

x_star = linspace(0,n+2,n_star)';

covfunc_no_noise = {'kafbox_covSEiso'};
loghyper_no_noise = [log(ell); log(sigvar)];

%% RUN ALGORITHM

kaf = krlst_split(struct('kerneltype','gauss','kernelpar',ell,...
    'sn2',noisevar/sigvar,'M',budget+1));

fprintf('Running KRLS-T\n');
t1 = tic;
for i=1:n
    if ~mod(i,floor(n/10)), fprintf('.'); end
    Qold = kaf.Q;
    kaf.train(x(i),y(i));

    for stage = 1:2
        switch stage
            case 1 % stage 1: after including a sample
                stage_string = 'after inclusion';
                fname_i = sprintf('fig/kafbox_sample_run_%02da',i);
            case 2 % stage 2: after pruning
                if length(kaf.mu) > budget
                    kaf = kaf.prune(Qold,1);
                    stage_string = 'after pruning  ';
                    fname_i = sprintf('fig/kafbox_sample_run_%02db',i);
                end
        end

        y_est = kaf.evaluate(x(1:i));
        [y_star_est,y_star_var] = kaf.evaluate(x_star);

        y_star_var = y_star_var/kaf.s02*sigvar; % fix scale

        x_dict = kaf.dict;
        y_dict = kaf.evaluate(x_dict);

        papersize = [8 4];

        figure(1); clf
        set(gcf,'Position',[500 400 papersize(1)*100 papersize(2)*100])
        hold all
        f = [y_star_est+2*sqrt(y_star_var); flipud(y_star_est-2*sqrt(y_star_var))];
        p0 = fill([x_star; flipud(x_star)], f, [7 7 7]/8,'EdgeColor','none');
        p1 = plot(x_star,y_star_est,'r','LineWidth',1.5);
        p2 = plot(x_dict,y_dict,'ro','LineWidth',2,'MarkerSize',8);
        p3 = plot(x(1:i),y(1:i),'bx','LineWidth',2,'MarkerSize',8);

        set(gca,'box','on');
        set(gca, 'FontSize', 14);
        set(gca, 'FontName', 'Helvetica');

        axis([0 n+2 floor(min(y))-1 ceil(max(y))+1])
        legend([p3,p1,p2],'data','prediction','dictionary bases')

        title(sprintf('KRLS-T iteration %d: %s',i,stage_string));

        set(gca, 'Position', get(gca, 'OuterPosition') - ...
            get(gca, 'TightInset') * [-1 0 1 0; 0 -1 0 1; 0 0 1 0; 0 0 0 1]);

        set(gcf, 'PaperUnits', 'inches');
        set(gcf, 'PaperSize', papersize);
        set(gcf, 'PaperPositionMode', 'manual');
        set(gcf, 'PaperPosition', [0 0 papersize(1) papersize(2)]);
        set(gcf, 'renderer', 'painters');

        drawnow
        if manual
            keyboard; %#ok<UNRCH>
        end

        if savefig
            if ~exist('fig','file') %#ok<UNRCH>
                mkdir('fig');
            end
            % write individual png
            print('-dpng','-r100',fname_i);
            print('-dpdf','-r100',fname_i);
            % write animated gif
            fname = 'fig/kafbox_sample_run.gif';
            frame = getframe(1);
            im = frame2im(frame);
            [imind,cm] = rgb2ind(im,256);
            if i==1 && stage==1
                imwrite(imind,cm,fname,'gif','Loopcount',inf,'DelayTime',1.5);
            else
                if stage==2 && i==n && kaf.pruned
                    imwrite(imind,cm,fname,'gif','WriteMode','append','DelayTime',2.5);
                elseif ~(stage==2 && ~kaf.pruned)
                    imwrite(imind,cm,fname,'gif','WriteMode','append','DelayTime',1.5);
                end
            end
        end
    end

end
fprintf('\n')


function mbasis = basis_matrix_overhauser_nul ( alpha )

%*****************************************************************************80
%
%% BASIS_MATRIX_OVERHAUSER_NUL sets up the nonuniform left Overhauser spline basis matrix.
%
%  Discussion:
%
%    This basis matrix assumes that the data points P1, P2, and
%    P3 are not uniformly spaced in T, and that P1 corresponds to T = 0,
%    and P2 to T = 1. (???)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 February 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real ALPHA.
%    ALPHA = || P2 - P1 || / ( || P3 - P2 || + || P2 - P1 || )
%
%    Output, real MBASIS(3,3), the basis matrix.
%
  mbasis(1,1) =   1.0 / alpha;
  mbasis(1,2) = - 1.0 / ( alpha * ( 1.0 - alpha ) );
  mbasis(1,3) =   1.0 / ( 1.0 - alpha );

  mbasis(2,1) = - ( 1.0 + alpha ) / alpha;
  mbasis(2,2) =   1.0 / ( alpha * ( 1.0 - alpha ) );
  mbasis(2,3) = - alpha / ( 1.0 - alpha );

  mbasis(3,1) =   1.0;
  mbasis(3,2) =   0.0;
  mbasis(3,3) =   0.0;

  return
end


function [ Y, gR, gPhi, gTheta ] = AFMT3( X, sigma, mAFMT, nAFMT, pAFMT, extrapVal, interpMethod, algorithm )
%Computes discrete approximation to analytic fourier-mellin transformation.
%Parameters:
%           x                   ...matrix with input data
%           sigma               ...strictly positive number, ensuring
%                                  existence of transformation
%           mAFMT               ...size of mellin-transformation (check)
%
%           nAFMT               ...size of fourier-transformation (check)

%algorithm selects: F-AFMT      ...fast AFMT
%                   D-AFMT      ...discrete AFMT
%
% (c) Lukas Mauch, Thomas Kuestner
% ---------------------------------------------------------------------

switch algorithm
    %% forward-transformation (3d):
    case 'F-AFMT'
        [yLogPol, gR, gPhi, gTheta] = convToLogPol(X, mAFMT, nAFMT, pAFMT, interpMethod);
        scaleFactor = exp(2*pi*(gR./max(max(max(gR)))) * sigma); %maximal radius normalized to 2*pi
        Y = fftnshift(scaleFactor .* yLogPol, 1:3);

    case 'D-AFMT'

    %% backward-transformation:
    case 'iF-AFMT'
        [sizeY, sizeX, sizeZ]  = size(X);

        %Compute center positions oM and oN:
        oY = fix(mAFMT/2)+1;
        oX = fix(nAFMT/2)+1;
        oZ = fix(pAFMT/2)+1;

        %Cartesian sample points:
        [gridCartX, gridCartY, gridCartZ] = meshgrid( -oX+1:nAFMT-oX, -oY+1:mAFMT-oY, -oZ+1:pAFMT-oZ );

        %Log-polar plane sample points:
        R  = sqrt(max(abs(oY-1), abs(mAFMT-oY))^2 + max(abs(oX-1), abs(nAFMT-oX))^2 + ...
             max(abs(oZ-1), abs(pAFMT-oZ))^2);
        [gridR, gridPhi, gridTheta]   = meshgrid( log(R)/sizeX:log(R)/sizeX:log(R), ...
                                       linspace(0,2*pi, sizeY), ...
                                       linspace(-pi/2, pi/2, sizeZ));

        scaleFactor = exp(-2*pi*sigma*(gridR./max(max(max(gridR)))));
        yLogPol = scaleFactor .* ifftnshift(X, 1:3);

        Y = convToCart(yLogPol, gridR, gridPhi, gridTheta, gridCartX, gridCartY, gridCartZ, interpMethod);

    case 'iD-AFMT'

    otherwise
    error('Unknown type of algorithm!')

end

    %% helper functions:
    function [xLogPol, gridR, gridPhi, gridTheta] = convToLogPol( xCart, sizeR, sizePhi, sizeTheta, interpMethod )
        [m, n, p]  = size(xCart);

        %Compute center positions oM and oN:
        oM = fix(m/2)+1;
        oN = fix(n/2)+1;
        oP = fix(p/2)+1;

        %Cartesian sample points:
        [gridX, gridY, gridZ] = meshgrid( -oN+1:n-oN, fliplr(-oM+1:m-oM), -oP+1:p-oP );

        %Log-polar plane sample points:
        maxR = sqrt(max(abs(oN-1), abs(n-oN))^2 + max(abs(oM-1), abs(m-oM))^2 + max(abs(oP-1), abs(p-oP))^2);
        [gridR, gridPhi, gridTheta]   = meshgrid( log(maxR)/sizeR:log(maxR)/sizeR:log(maxR), ...
                                      linspace(0,2*pi,sizePhi), ...
                                      linspace(-pi/2, pi/2, sizeTheta));

        %Perform conversion to log-polar coordinates, using linear interpolation:
        convGridX   = exp(gridR) .* cos(gridPhi) .* cos(gridTheta);
        convGridY   = exp(gridR) .* sin(gridPhi) .* cos(gridTheta);
        convGridZ   = exp(gridR) .* sin(gridTheta);
        xLogPol     = interp3(gridX, gridY, gridZ, xCart, ...
                        convGridX, convGridY, convGridZ, interpMethod, extrapVal);
    end

    function xCart = convToCart( xLogPol, gridR, gridPhi, gridTheta, gridX, gridY, gridZ, interpMethod )
        %Perform conversion to cartesian coordinates:
%         convGridX   = reshape(exp(gridR) .* cos(gridPhi) .* cos(gridTheta),[], 1);
%         convGridY   = reshape(exp(gridR) .* sin(gridPhi) .* cos(gridTheta),[], 1);
%         convGridZ   = reshape(exp(gridR) .* sin(gridTheta),[], 1);
%         F = TriScatteredInterp(convGridX, convGridY, convGridZ, reshape(xLogPol, [], 1), interpMethod);
%         xCart       = F(gridX, gridY, gridZ);

        %interpolation in r-phi plane (worse but fast):
        convGridR       = sqrt(abs(gridX).^2 + abs(gridY).^2 + abs(gridZ).^2);
        convGridPhi     = atan(gridY./gridX);
        convGridTheta   = asin(gridZ./convGridR);
        %unwrap:
        qA = logical(logical((gridX >= 0)) .* logical((gridY<=0)));
        qB = logical(logical((gridX < 0)) .* logical((gridY<=0)));
        qC = logical(logical((gridX < 0)) .* logical((gridY>0)));
        qD = logical(logical((gridX >= 0)) .* logical((gridY>0)));
        convGridPhi(qA) = abs(convGridPhi(qA));
        convGridPhi(qB) = pi-convGridPhi(qB);
        convGridPhi(qC) = pi+abs(convGridPhi(qC));
        convGridPhi(qD) = 2*pi-convGridPhi(qD);
        convGridPhi(isnan(convGridPhi)) = 0;
        convGridTheta(isnan(convGridTheta)) = 0;

        xCart       = interp3(exp(gridR), gridPhi, gridTheta, xLogPol, ...
                        convGridR, convGridPhi, convGridTheta, interpMethod, extrapVal);

    end

end


function [beta_gibbs sigma_gibbs]=mgibbs(It,Bu,betabar,omegabar,sigma,q)

% function [beta_gibbs sigma_gibbs]=mgibbs(It,Bu,betabar,omegabar,sigma,q)
% performs the Gibbs algorithm 2.1.2 for the Minnesota prior, and returns draws from posterior distribution
% inputs:  - integer 'It': total number of iterations of the Gibbs sampler (defined p 28 of technical guide)
%          - integer 'Bu': number of burn-in iterations of the Gibbs sampler (defined p 28 of technical guide)
%          - vector 'betabar': posterior mean vector (defined in 1.3.18)
%          - matrix 'omegabar': posterior covariance matrix for the VAR coefficients (defined in 1.3.17)
%          - matrix 'sigma': 'true' variance-covariance matrix of VAR residuals, for the original Minnesota prior
%          - integer 'q': total number of coefficients to estimate for the BVAR model (defined p 7 of technical guide)
% outputs: - matrix 'beta_gibbs': record of the gibbs sampler draws for the beta vector
%          - matrix'sigma_gibbs': record of the gibbs sampler draws for the sigma matrix (vectorised)

% start iterations
for ii=1:(It-Bu)
% draw a random vector beta from N(betabar,sigmabar) (step 3 of the algorithm)
beta=betabar+chol(bear.nspd(omegabar),'lower')*randn(q,1);

% record values before starting next iteration
beta_gibbs(:,ii)=beta;
% go for next iteration
end

% record the values for the variance-covariance matrix sigma
% vectorize sigma and record it (it-Bu) times
sigma_gibbs=repmat(sigma(:),1,It-Bu);


function [c, ceq] = lotkaConstraintFCN(u,x,Ts,N,p)
%% Constraint function of nonlinear MPC for Lotka-Volterra system
%
% Inputs:
%   u:      optimization variable, from time k to time k+N-1
%   x:      current state at time k
%   Ts:     controller sample time
%   N:      prediction horizon
%
% Output:
%   c:      inequality constraints applied across prediction horizon
%   ceq:    equality constraints (empty)
%

%% Nonlinear MPC design parameters
% Predator population size always positive: >0, min pop size
zMin = [10];

%% Inequality constraints calculation
c = zeros(N,1);
% Apply N population size constraints across prediction horizon, from time
% k+1 to k+N
xk = x;
uk = u(1);
for ct=1:N
    % obtain new cart position at next prediction step
    xk1 = rk4u(@sparseGalerkinControl_Discrete,xk,uk,Ts,1,[],p);

    % -z + zMin < 0 % lower bound
    c(ct) = -xk1(2)+zMin; %c(2*ct-1)
    % z - zMax < 0 % upper bound
    %c(2*ct) = xk1(1)-zMax;
    % update plant state and input for next step
    xk = xk1;
    if ct<N
        uk = u(ct+1);
    end
end
%% No equality constraints
ceq = [];


function [L, U, P, Q] = gecp(A)
%GECP calculate Gauss elimination with complete pivoting
%
% (G)aussian (E)limination (C)omplete (P)ivoting
% Input : A nxn matrix
% Output
% L = Lower triangular matrix with ones as diagonals
% U = Upper triangular matrix
% P and Q permutations matrices so that P*A*Q = L*U
%
% See also LU
%
% written by : Cheilakos Nick
[n, n] = size(A);
p = 1:n;
q = 1:n;
for k = 1:n-1
    [maxc, rowindices] = max( abs(A(k:n, k:n)) );
    [maxm, colindex] = max(maxc);
    row = rowindices(colindex)+k-1; col = colindex+k-1;
    A( [k, row], : ) = A( [row, k], : );
    A( :, [k, col] ) = A( :, [col, k] );
    p( [k, row] ) = p( [row, k] ); q( [k, col] ) = q( [col, k] );
    if A(k,k) == 0
      break
    end
    A(k+1:n,k) = A(k+1:n,k)/A(k,k);
    i = k+1:n;
    A(i,i) = A(i,i) - A(i,k) * A(k,i);
end
L = tril(A,-1) + eye(n);
U = triu(A);
   P = eye(n);
   P = P(p,:);
   Q = eye(n);
   Q = Q(:,q);


function pass = test_ballfun( pref )

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

% Example 1 :
f = ballfun(@(x,y,z)x);
exact = ballfun(@(r,lam,th)r.*sin(th).*cos(lam),'spherical');
pass(1) = norm( f - exact ) < tol;

% Example 2 :
f = ballfun(@(x,y,z)y);
exact = ballfun(@(r,lam,th)r.*sin(th).*sin(lam),'spherical');
pass(2) = norm( f - exact ) < tol;

% Example 3 :
f = ballfun(@(x,y,z)z);
exact = ballfun(@(r,lam,th)r.*cos(th),'spherical');
pass(3) = norm( f - exact ) < tol;

% Example 4 :
f = ballfun(@(x,y,z)x.*z);
exact = ballfun(@(r,lam,th)r.*sin(th).*cos(lam).*r.*cos(th),'spherical');
pass(4) = norm( f - exact ) < tol;

% Example 5 :
f = ballfun(@(x,y,z)sin(x.*y.*z));
exact = ballfun(@(r,lam,th)sin(r.*sin(th).*cos(lam).*r.*sin(th).*sin(lam).*r.*cos(th)),'spherical');
pass(5) = norm( f - exact ) < tol;

% Example 6 : (Test 'coeffs' flag.) with rcos(th)
m = 10; n = 11; p = 12;
F = zeros(m,n,p);
F(2,floor(n/2)+1,floor(p/2))=1/2;F(2,floor(n/2)+1,floor(p/2)+2)=1/2;
f = ballfun(F, 'coeffs');
g = ballfun(@(r,lam,th)r.*cos(th),'spherical');
pass(6) = norm( f - g ) < tol;

% Example 7 :
f = ballfun(@(x,y,z)cos(x.*y.*z));
g = ballfun(@(r,lam,th)cos(r.*sin(th).*cos(lam).*r.*sin(th).*sin(lam).*r.*cos(th)),'spherical');
pass(7) = norm( f - g ) < tol;

% Example 8
f = ballfun(@(x,y,z)cos(x.*y.*z));
g = ballfun(@(x,y,z)cos(x.*y.*z),'vectorize');
pass(8) = norm( f - g ) < tol;

% Example 9 :
f = ballfun(@(x,y,z)x.*z);
exact = ballfun(@(r,lam,th)r.*sin(th).*cos(lam).*r.*cos(th),'spherical');
pass(9) = norm( f - exact ) < tol;

% Example 10 :
f = ballfun(@(x,y,z)sin(x.*y.*z));
exact = ballfun(@(r,lam,th)sin(r.*sin(th).*cos(lam).*r.*sin(th).*sin(lam).*r.*cos(th)),'spherical');
pass(10) = norm( f - exact ) < tol;

% Example 11:
f = ballfun(@(x,y,z)x*y,'vectorize');
exact = ballfun(@(x,y,z)x.*y);
pass(11) = norm( f - exact ) < tol;

% Example 12:
f = ballfun(@(x,y,z)x*z*y,'vectorize');
exact = ballfun(@(x,y,z)x.*z.*y);
pass(12) = norm( f - exact ) < tol;

% Example 13
f = ballfun(@(x,y,z)cos(x.*y),[50,51,52]);
exact = ballfun(@(x,y,z)cos(x.*y));
pass(13) = norm( f - exact ) < tol;

% Example 14
exact = ballfun(@(x,y,z)cos(x.*y));
f = ballfun(exact, [51,50,49], 'vectorize');
pass(14) = norm( f - exact ) < tol;

% Example 15
exact = ballfun(@(x,y,z)sin(x.*y));
E = coeffs3(exact,49,50,51);
f = ballfun(E, 'coeffs', [51,53,51]);
pass(15) = norm( f - exact ) < tol;

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


function line = fitLine3d(points)
%FITLINE3D Fit a 3D line to a set of points.
%
%   LINE = fitLine3d(PTS)
%
%   Example
%     pts = randn(300, 3);
%     pts = transformPoint3d(pts, createScaling3d([6 4 2]));
%     pts = transformPoint3d(pts, createRotationOx(pi/6));
%     pts = transformPoint3d(pts, createRotationOy(pi/4));
%     pts = transformPoint3d(pts, createRotationOz(pi/3));
%     pts = transformPoint3d(pts, createTranslation3d([5 4 3]));
%     elli = equivalentEllipsoid(pts);
%     figure; drawPoint3d(pts); axis equal;
%     hold on; drawEllipsoid(elli, ...
%         'drawEllipses', true, 'EllipseColor', 'b', 'EllipseWidth', 3);
%     line = fitLine3d(pts);
%     drawLine3d(line, 'color', 'm', 'LineWidth', 4);
%
%   See also
%     lines3d, equivalentEllipsoid, fitPlane
%

% ------
% Author: David Legland
% E-mail: david.legland@grignon.inra.fr
% Created: 2012-11-11, using Matlab 7.9.0.529 (R2009b)
% Copyright 2012-2022 INRA - Cepia Software Platform

% number of points
n = size(points, 1);

% compute centroid
center = mean(points);

% compute the covariance matrix
covPts = cov(points)/n;

% perform a principal component analysis with 2 variables,
% to extract inertia axes
[U, S] = svd(covPts);

% sort axes from greater to lower
[dummy, ind] = sort(diag(S), 'descend'); %#ok<ASGLU>

% format U to ensure first axis points to positive x direction
U = U(ind, :);
if U(1,1) < 0
    U = -U;
    % keep matrix determinant positive
    U(:,3) = -U(:,3);
end

line = [center U(:,1)'];


function [gx, gy, gz] = gaussPtetra(X1, X2, X3, X4, ng)

%% USAGE: generate Gaussian nodes on tetra mesh
%
% INPUTS:
% p --- nt-by-3 vector
% t --- nt-by-4 vector
% ng --- number of Gaussian nodes in an triangle
%            possible values = 1, 4, 5, 11, 15.
%
% OUTPUTS:
% gx --- nt-by-ng vector: stores x coordinates of Gaussian nodes
% gy --- nt-by-ng vector: stores y coordinates of Gaussian nodes
% gz --- nt-by-ng vector: stores z coordinates of Gaussian nodes
%
% Last Modified: 07/02/2020 by Xu Zhang
%%

G = zeros(size(X1,1),3*ng); i = 1;

if ng == 1 % accurate up to polynomial degree p = 1

    w1 = 1/4; % point at centroid
    G(:,[i,i+ng,i+2*ng]) = w1*(X1+X2+X3+X4);

elseif ng == 4 % accurate up to polynomial degree p = 2

    w1 = 0.585410196624969; w2 = 0.138196601125011; % median line
    G(:,[i,i+ng,i+2*ng]) = w1*X1 + w2*(X2+X3+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*X2 + w2*(X1+X3+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*X3 + w2*(X1+X2+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*X4 + w2*(X1+X2+X3);

elseif ng == 5 % accurate up to polynomial degree p = 3

    w1 = 1/4; % point at centroid
    G(:,[i,i+ng,i+2*ng]) = w1*(X1+X2+X3+X4); i = i+1;

    w1 = 1/2; w2 = 1/6;% median line
    G(:,[i,i+ng,i+2*ng]) = w1*X1 + w2*(X2+X3+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*X2 + w2*(X1+X3+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*X3 + w2*(X1+X2+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*X4 + w2*(X1+X2+X3);

elseif ng == 11 % accurate up to polynomial degree p = 4

    w1 = 1/4; % point at centroid
    G(:,[i,i+ng,i+2*ng]) = w1*(X1+X2+X3+X4); i = i+1;

    w1 = 0.785714285714286; w2 = 0.071428571428571; % median line
    G(:,[i,i+ng,i+2*ng]) = w1*X1 + w2*(X2+X3+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*X2 + w2*(X1+X3+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*X3 + w2*(X1+X2+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*X4 + w2*(X1+X2+X3); i = i+1;

    w1 = 0.399403576166799; w2 = 0.100596423833201; % line opposite sides
    G(:,[i,i+ng,i+2*ng]) = w1*(X1+X2) + w2*(X3+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*(X1+X3) + w2*(X2+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*(X1+X4) + w2*(X2+X3); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*(X2+X3) + w2*(X1+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*(X2+X4) + w2*(X1+X3); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*(X3+X4) + w2*(X1+X2);

elseif ng == 15 % accurate up to polynomial degree p = 5

    w1 = 1/4; % point at centroid
    G(:,[i,i+ng,i+2*ng]) = w1*(X1+X2+X3+X4); i = i+1;

    w1 = 0; w2 = 1/3; % median line
    G(:,[i,i+ng,i+2*ng]) = w1*X1 + w2*(X2+X3+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*X2 + w2*(X1+X3+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*X3 + w2*(X1+X2+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*X4 + w2*(X1+X2+X3); i = i+1;

    w1 = 0.727272727272727; w2 = 0.090909090909091; % median line
    G(:,[i,i+ng,i+2*ng]) = w1*X1 + w2*(X2+X3+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*X2 + w2*(X1+X3+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*X3 + w2*(X1+X2+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*X4 + w2*(X1+X2+X3); i = i+1;

    w1 = 0.066550153573664; w2 = 0.433449846426336; % line opposite sides
    G(:,[i,i+ng,i+2*ng]) = w1*(X1+X2) + w2*(X3+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*(X1+X3) + w2*(X2+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*(X1+X4) + w2*(X2+X3); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*(X2+X3) + w2*(X1+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*(X2+X4) + w2*(X1+X3); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*(X3+X4) + w2*(X1+X2);

end
gx = G(:,1:ng); gy = G(:,ng+1:2*ng); gz = G(:,2*ng+1:3*ng);


function [pospeakind,negpeakind]=peakdetect(signal)

%	PEAKDETECT peak detection
%
%	[pospeakind,negpeakind]=peakdetect(signal)
%
%	The positive and negative polarity (concave down and up) peak index vectors are
%	generated from the signal vector and graphically displayed.  Positive and negative
%	polarity peaks occur at points of positive to negative and negative to positive
%	slope adjacency, respectively.  The typically rare contingencies of peaks
%	occurring at the lagging edges of constant intervals are supported.  Complex
%	signals are modified to the modulus of the elements.  If unspecified, the signal
%	vector is entered after the prompt from the keyboard.

%	Implemented using MATLAB 6.0.0
%
%	Examples:
%
%	ť [p,n]=peakdetect([-1 -1 0 1 0 1 0 -1 -1])
%
%	p =
%
%	     4     6
%
%	n =
%
%	     1     5     8
%
%	ť [p,n]=peakdetect(cos(2*pi*(0:999999)/500000))
%
%	p =
%
%	           1      500001     1000000
%
%	n =
%
%	      250001      750001
%
%	Copyright (c) 2001
%	Tom McMurray
%	mcmurray@teamcmi.com

%	if signal is not input, enter signal or return for empty outputs

if ~nargin
   signal=input('enter signal vector or return for empty outputs\n');
   if isempty(signal)
      pospeakind=[];
      negpeakind=[];
      return
   end
end
sizsig=size(signal);

%	while signal is unsupported, enter supported signal or return for empty outputs

while isempty(signal)|~isnumeric(signal)|~all(all(isfinite(signal)))...
      |length(sizsig)>2|min(sizsig)~=1
   signal=input(['signal is empty, nonnumeric, nonfinite, or nonvector:\nenter '...
         'finite vector or return for empty outputs\n']);
   if isempty(signal)
      pospeakind=[];
      negpeakind=[];
      return
   end
   sizsig=size(signal);
end

%	if signal is complex, modify to modulus of the elements

if ~isreal(signal)
   signal=abs(signal);
end

%	if signal is constant, return empty outputs

if ~any(signal-signal(1))
   pospeakind=[];
   negpeakind=[];
   disp('constant signal graph suppressed')
   return
end
sizsig1=sizsig(1);
lensig=sizsig1;

%	if signal is a row vector, modify to a column vector

if lensig==1
   signal=signal(:);
   lensig=sizsig(2);
end
lensig1=lensig-1;
lensig2=lensig1-1;

%	if signal length is 2, return max/min as positive/negative polarity peaks

if ~lensig2
   [sig,pospeakind]=max(signal);
   [sig,negpeakind]=min(signal);
   disp('2 element signal graph suppressed')
   return
end

%	generate difference signal

difsig=diff(signal);

%	generate vectors corresponding to positive slope indices

dsgt0=difsig>0;
dsgt00=dsgt0(1:lensig2);
dsgt01=dsgt0(2:lensig1);

%	generate vectors corresponding to negative slope indices

dslt0=difsig<0;
dslt00=dslt0(1:lensig2);
dslt01=dslt0(2:lensig1);

%	generate vectors corresponding to constant intervals

dseq0=difsig==0;
dseq01=dseq0(2:lensig1);
clear difsig

%	positive to negative slope adjacencies define positive polarity peaks

pospeakind=find(dsgt00&dslt01)+1;

%	negative to positive slope adjacencies define negative polarity peaks

negpeakind=find(dsgt01&dslt00)+1;

%	positive slope to constant interval adjacencies initiate positive polarity peaks

peakind=find(dsgt00&dseq01)+1;
lenpeakind=length(peakind);

%	determine positive polarity peak terminations

for k=1:lenpeakind
   peakindk=peakind(k);
   l=peakindk+1;

%	if end constant interval occurs, positive polarity peak exists

   if l==lensig
      pospeakind=[pospeakind;peakindk];

%	else l<lensig, determine next nonzero slope index

   else
      dseq0l=dseq0(l);
      while dseq0l&l<lensig1
         l=l+1;
         dseq0l=dseq0(l);
      end

%	if negative slope or end constant interval occurs, positive polarity peaks exist

      if dslt0(l)|dseq0l;
         pospeakind=[pospeakind;peakindk];
      end
   end
end

%	negative slope to constant interval adjacencies initiate negative polarity peaks

peakind=find(dslt00&dseq01)+1;
lenpeakind=length(peakind);
clear dseq01

%	determine negative polarity peak terminations

for k=1:lenpeakind
   peakindk=peakind(k);
   l=peakindk+1;

%	if end constant interval occurs, negative polarity peak exists

   if l==lensig
      negpeakind=[negpeakind;peakindk];

%	else l<lensig, determine next nonzero slope index

   else
      dseq0l=dseq0(l);
      while dseq0l&l<lensig1
         l=l+1;
         dseq0l=dseq0(l);
      end

%	if positive slope or end constant interval occurs, negative polarity peaks exist

      if dsgt0(l)|dseq0l;
         negpeakind=[negpeakind;peakindk];
      end
   end
end
clear dsgt0 peakind

%	if initial negative slope occurs, initial positive polarity peak exists

if dslt00(1)
   pospeakind=[1;pospeakind];

%	elseif initial positive slope occurs, initial negative polarity peak exists

elseif dsgt00(1)
   negpeakind=[1;negpeakind];

%	else initial constant interval occurs, determine next nonzero slope index

else
   k=2;
   dseq0k=dseq0(2);
   while dseq0k
      k=k+1;
      dseq0k=dseq0(k);
   end

%	if negative slope occurs, initial positive polarity peak exists

   if dslt0(k)
      pospeakind=[1;pospeakind];

%	else positive slope occurs, initial negative polarity peak exists

   else
      negpeakind=[1;negpeakind];
   end
end
clear dsgt00 dslt0 dslt00 dseq0

%	if final positive slope occurs, final positive polarity peak exists

if dsgt01(lensig2)
   pospeakind=[pospeakind;lensig];

%	elseif final negative slope occurs, final negative polarity peak exists

elseif dslt01(lensig2)
   negpeakind=[negpeakind;lensig];
end
clear dsgt01 dslt01

%	if peak indices are not ascending, order peak indices

if any(diff(pospeakind)<0)
   pospeakind=sort(pospeakind);
end
if any(diff(negpeakind)<0)
   negpeakind=sort(negpeakind);
end

%	if signal is a row vector, modify peak indices to row vectors

if sizsig1==1
   pospeakind=pospeakind.';
   negpeakind=negpeakind.';
end

%	plot signal peaks

plot(0:lensig1,signal,pospeakind-1,signal(pospeakind),'b^',negpeakind-1,...
   signal(negpeakind),'bv')
xlabel('Sample')
ylabel('Signal')
grid


function shepard_interp_2d_test01 ( prob, g, p )

%*****************************************************************************80
%
%% SHEPARD_INTERP_2D_TEST01 tests SHEPARD_INTERP_2D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 August 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer PROB, the problem number.
%
%    Input, integer G, the grid number.
%
%    Input, real P, the power used in the distance weighting.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SHEPARD_INTERP_2D_TEST01:\n' );
  fprintf ( 1, '  Interpolate data from TEST_INTERP_2D problem #%d\n', prob );
  fprintf ( 1, '  using grid #%d\n', g );
  fprintf ( 1, '  using Shepard interpolation with P = %g\n', p );

  nd = g00_size ( g );
  fprintf ( 1, '  Number of data points = %d\n', nd );

  [ xd, yd ] = g00_xy ( g, nd );

  zd = f00_f0 ( prob, nd, xd, yd );

  if ( p == 1.0 )
    r8vec3_print ( nd, xd, yd, zd, '  X, Y, Z data:' );
  end
%
%  #1:  Does interpolant match function at interpolation points?
%
  ni = nd;
  xi = xd;
  yi = yd;
  zi = shepard_interp_2d ( nd, xd, yd, zd, p, ni, xi, yi );

  int_error = norm ( zi - zd ) / ni;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  L2 interpolation error averaged per interpolant node = %g\n', int_error );
%
%  #2: Plot the linear interpolant.
%
  if ( p == 1.0 )
    figure ( 1 );
    clf
    hold on
    tri = delaunay ( xd, yd );
    h = trisurf ( tri, xd, yd, zd );
    ft = f00_title ( prob );
    title ( ft );
    grid on
    xlabel ( '<--- X --->' );
    ylabel ( '<--- Y --->' );
    zlabel ( '<---Z(X,Y)--->' );
    l = light ( 'Position', [ 0, 0, 25 ] );
    set ( gca, 'CameraPosition', [ -10, -10, 20 ] );
    lighting phong
    shading interp
    hold off
    filename = sprintf ( 'p%02d_data.png', prob );
    print ( '-dpng', filename );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Created plot file "%s".\n', filename );
  end
%
%  #3: Plot the Shepard interpolant.
%
  figure ( 2 );
  clf
  nix = 21;
  niy = 21;
  xmin = 0.0;
  xmax = 1.0;
  ymin = 0.0;
  ymax = 1.0;
  xi = linspace ( xmin, xmax, nix );
  yi = linspace ( ymin, ymax, niy );
  [XI,YI] = meshgrid ( xi, yi );
  NI = nix * niy;
  XI = reshape ( XI, NI, 1 );
  YI = reshape ( YI, NI, 1 );
  ZI = shepard_interp_2d ( nd, xd, yd, zd, p, NI, XI, YI );
  XI = reshape ( XI, nix, niy );
  YI = reshape ( YI, nix, niy );
  ZI = reshape ( ZI, nix, niy );
  surf ( XI, YI, ZI );
  xlabel ( '<--- X --->' );
  ylabel ( '<--- Y --->' );
  zlabel ( '<---Z(X,Y)--->' );
  title ( sprintf ( 'Shepard interpolant for problem %d with P = %g', prob, p ) )
  grid on
  hold off
  filename = sprintf ( 'p%02d_power%g.png', prob, p );
  print ( '-dpng', filename );
  fprintf ( 1, '  Created plot file "%s".\n', filename );

  return
end


function mesh = mshSphere(N,rad)
%+========================================================================+
%|                                                                        |
%|                 OPENMSH - LIBRARY FOR MESH MANAGEMENT                  |
%|           openMsh is part of the GYPSILAB toolbox for Matlab           |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2018.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : mshSphere.m                                   |
%|    #    |   VERSION    : 0.40                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 14.03.2018                                    |
%| ( === ) |   SYNOPSIS   : Build uniform mesh for a sphere               |
%|  `---'  |                                                              |
%+========================================================================+

% Fibonnacci rules
or     = (1+sqrt(5))/2;
theta  = (mod((2*pi/or) .* (0:N-1),2*pi))';
phi    = asin( -1 + 2/(N-1) * (0:N-1))';

% Carthesian coordinates
[x,y,z] = sph2cart(theta,phi,rad);
X       = [0 0 0 ; x y z];

% Delaunay triangulation
DT        = delaunayTriangulation(X);
[elt,vtx] = freeBoundary(DT);

% Mesh
mesh = msh(vtx,elt);
end


%  Figure 7.47      Feedback Control of Dynamic Systems, 6e
%                        Franklin, Powell, Emami
%
% script to generate Fig. 7.47
%
%
clf;
dp=[1 10 16 0];
np=[10];
nc=94.5*conv([1 7.98],[1 2.52]);
dc=conv([1 8.56 59.5348],[1 10.6]);
num=conv(np,nc);
den=conv(dp,dc);
dcl=[0 0 0 0 num]+den; % closed-loop denominator
t=0:.1:5;
sys=tf(num,dcl);
y=step(sys,t);
plot(t,y)
grid;
xlabel('Time (sec)');
ylabel('y');
title('Fig.7.47 Step response for SRL compensation design: continuous');

[a,b,c,d]=tf2ss(nc,dc);
% Samplig period
ts=0.1;
% Convert controller to discrete
[ad,bd,cd,dd]=c2dm(a,b,c,d,ts,'zoh');
[ncd,dcd]=ss2tf(ad,bd,cd,dd)

% response comparison of continuous and digital control.
sim('fig7_46')
figure(1)
plot(ycd(:,1),ycd(:,2))
hold on
plot(ycd(:,1),ycd(:,3),'g--')
title('Figure 7.47(a) Output Responses of Digital and Continuous Controllers')
xlabel('Time (sec)');
ylabel('y');
text(1.1,0.95,'continuous controller');
text(1,1.25,'digital controller');
nicegrid;
hold off

figure(2)
plot(ucd(:,1),ucd(:,2))
hold on
plot(ucd(:,1),ucd(:,3),'g--')
title('Figure 7.47(b)  Control Responses of Digital and Continuous Controllers')
xlabel('Time (sec)');
ylabel('u');
grid on;
text(0.1,-1.4,'continuous controller')
text(0.7,0.5,'digital controller')
nicegrid
hold off


function frac=dtiClusteringReability(E, reps, nClusters)
%O'Donnell PhD Thesis (2006): measure clustering solution reliability
%We quantified this clustering variability by testing whether pairs
%of data points (paths from tractography) clustered always in the same cluster, never
%in the same cluster, or sometimes in the same and sometimes in different clusters.
%The fraction of pairs that sometimes were in the same cluster, and sometimes in
%different clusters, was measured for various numbers of clusters -- thus
%producing evaluation graph

%"In any case, the fraction of pairs that are inconsistent is less than 5 percent for any number of clusters greater
%than about 75."
nFibers=size(E, 1);
nPairs=nFibers*(nFibers-1)/2;
reps=500; nClusters=23;

for  rep=1:reps
rep
[IDX(:, rep), C(:, : , rep)]=kmeans(E, nClusters);
end
same=zeros(1, reps);
for fiber1=1:size(E, 1)
fiber1
for fiber2=fiber1+1:size(E, 1)
%For those that are sometimes in same, sometimes in differnt
%clusters == "clusters that were not always in the same or always in
%different clusters" . The lowest values show best performance

if sum(IDX(fiber1, :)==IDX(fiber2, :))>0 & sum(IDX(fiber1, :)==IDX(fiber2, :))<reps
same(sum(IDX(fiber1, :)==IDX(fiber2, :)))=            same(sum(IDX(fiber1, :)==IDX(fiber2, :)))+1;
end
end
end

%Fraction of pairs of fibers always in different or always (out of reps) in same
%clusters. "consistency" index.
frac=1-sum(same(:))/nPairs;


function vis_logreg(pred,acc,idxPos,idxNeg)

p1_pos =  pred(idxPos,1);
p1_neg =  pred(idxNeg,1);
cost_pos = log((1-p1_pos)./p1_pos);
cost_neg = log((1-p1_neg)./p1_neg);

% vis_violate_pairwise(cost_positive, cost_negative);

% visualize training examples VS pairwise cost
thr_cost = 5;
x_range_cost = -thr_cost:0.1:thr_cost;
x_range_prob =  0:0.01:1;

% positive
subplot(2,3,1);
[hist_cost_pos,x] = hist(cost_pos, x_range_cost);
bar(x,hist_cost_pos);
xlim([x_range_cost(1), x_range_cost(end)]);
title(['Accuracy: ' num2str(acc(1))]);

hist_cost_pos = hist_cost_pos./sum(hist_cost_pos);
subplot(2,3,2);
plot(x_range_cost,hist_cost_pos,'Linewidth',2);
title('norm cost');

subplot(2,3,3);
plot(x_range_cost,cumsum(hist_cost_pos) ,'Linewidth',2);
title('cum norm cost');ylim([0, 1]); % plot cumulative normalized histogram

% negative
subplot(2,3,1);hold on;
[hist_cost_neg,x] =  hist(cost_neg, x_range_cost);
bar(x,hist_cost_neg, 'r');
xlim([x_range_cost(1), x_range_cost(end)]);
title('cost');
legend('pos ex costs', 'neg ex costs');
hist_cost_neg = hist_cost_neg./sum(hist_cost_neg);

subplot(2,3,2); hold on;
plot(x_range_cost,hist_cost_neg, 'Color','r');
title('norm cost');
legend('pos ex costs', 'neg ex costs');
subplot(2,3,3); hold on;
plot(x_range_cost,cumsum(hist_cost_neg), 'Color','r', 'Linewidth',2);
title('cum norm cost');ylim([0, 1]); % plot cumulative normalized histogram
legend('pos ex costs', 'neg ex costs');

% visualize training example VS pairwise probability
% positive
subplot(2,3,4);[g1_pos,x] =  hist(p1_pos,x_range_prob);
bar(x,g1_pos);
xlim([x_range_prob(1), x_range_prob(end)]);
title('hist of probability');  % plot histogram of distance

g1_pos = g1_pos./sum(g1_pos);
subplot(2,3,5); plot(x_range_prob, g1_pos); title('norm prob'); % plot normalized histogram

subplot(2,3,6);
plot(x_range_prob,cumsum(g1_pos), 'Linewidth',2);
title('cum norm prob'); ylim([0, 1]); % plot cumulative normalized histogram

% negative
subplot(2,3,4); hold on;
[g1_neg,x] = hist(p1_neg,x_range_prob);
bar(x,g1_neg,  'r'); xlim([x_range_prob(1), x_range_prob(end)]);
title('prob');% plot histogram of distance
legend('p(i) = 1 pos ', 'p(i) = 1 neg');

g1_neg = g1_neg./sum(g1_neg);
subplot(2,3,5);hold on;
plot(x_range_prob,g1_neg,'Color','r');
title('norm prob'); % plot normalized histogram
legend('p(i) = 1 pos ', 'p(i) = 1 neg');

subplot(2,3,6); hold on;
plot(x_range_prob, cumsum(g1_neg), 'Color','r','Linewidth',2);
title('cum norm prob');ylim([0, 1]);% plot cumulative normalized histogram
legend('p(i) = 1 pos ', 'p(i) = 1 neg');
end


function result = AngleAxisRotatePoint(angle_axis, pt)

angle_axis = angle_axis(1:3);

theta2 = dot(angle_axis,angle_axis);

if (theta2 > 0.0)
    % Away from zero, use the rodriguez formula
    %
    %   result = pt costheta + (w x pt) * sintheta + w (w . pt) (1 - costheta)
    %
    % We want to be careful to only evaluate the square root if the
    % norm of the angle_axis vector is greater than zero. Otherwise
    % we get a division by zero.

    theta = sqrt(theta2);
    w = angle_axis / theta;

    costheta = cos(theta);
    sintheta = sin(theta);

    % w_cross_pt = cross(w, pt);
    w_cross_pt = xprodmat(w) * pt;

    %w_dot_pt = dot(w, pt);
    w_dot_pt = w * pt;

    result = pt * costheta + w_cross_pt * sintheta + (w' * (1 - costheta)) * w_dot_pt;

else
    % Near zero, the first order Taylor approximation of the rotation
    % matrix R corresponding to a vector w and angle w is
    %
    %   R = I + hat(w) * sin(theta)
    %
    % But sintheta ~ theta and theta * w = angle_axis, which gives us
    %
    %  R = I + hat(w)
    %
    % and actually performing multiplication with the point pt, gives us
    % R * pt = pt + w x pt.
    %
    % Switching to the Taylor expansion at zero helps avoid all sorts
    % of numerical nastiness.

    %w_cross_pt = cross(angle_axis, pt);
    w_cross_pt = xprodmat(angle_axis) * pt; % vectorize version

    result = pt + w_cross_pt;

end


function [IF_phase_unwrap, GDD_phase, MGD_phase]=phase_feature_extraction(sp_complex, sp_delay)

% This function is using for different kinds of phase feature extraction,
% include instantaneous frequency deviation (IF), group delay deviation
% (GDD), modified group delay (MGD), phase distortion (PD), phase
% distortion standard deviation (PDD) and relative phase shift (RPS).
%
% input:
%     sp_complex:   [n*d] n is frame number (time), d is the feature dimension (frequency)
%                   STFT of target waveform x(n) which contain both magnitude and phase information
%     sp_delay:     [n*d] n is frame number, d is the feature dimension
%                   STFT of modified target waveform n*x(n)
%
% output:
%                   [n*d] n is frame number (time), d is the feature dimension (frequency)
%     IF_phase:     instantaneous frequency deviation
%     GDD_phase:    group delay deviation
%     MGD_phase:    modified group delay
%     PD_phase:     phase distortion
%     PDD_phase:    phase distortion standard deviation
%     RPS_phase:    relative phase shift
%

is_plot = 1;

phase = angle(sp_complex);
phase_unwrap = unwrap(phase);

% [~, IF_phase] = gradient(phase); % deviation along time aixs
[~, IF_phase_unwrap] = gradient(phase_unwrap); % deviation along time aixs

GDD_phase = group_delay_feature(sp_complex); % deviation along frequency aixs

MGD_phase = modified_group_delay_raw(sp_complex, sp_delay);

if is_plot
    figure
    subplot(5,1,1)
    imagesc(phase');colormap jet
    title('original phase')

    subplot(5,1,2)
    imagesc(phase_unwrap');colormap jet
    title('original unwraped phase')

    subplot(5,1,3)
    imagesc(IF_phase_unwrap');colormap jet
    title('instantaneous frequency deviation')

    subplot(5,1,4)
    gdd = GDD_phase.^ 0.4;
    imagesc(real(gdd'));colormap jet
    title('group delay deviation')

    subplot(5,1,5)
    imagesc(MGD_phase');colormap jet
    title('modified group delay')
end

% RPS_phase = relative_phase_shift(phase);
%
% PD_phase = phase_distortion(phase);

% PDD_phase = phase_distortion_std_deviation(phase)


% [mu,S] = mean_cov(x)
% x is n x d
function [mu,S] = mean_cov(x)

%[n,d] = size(x);

z = size(x,1);
mu = sum(x,1)/z;

diffs = bsxfun(@minus,x,mu);
S = (diffs'*diffs)/z;


function [f] = spm_mc_fx_1(x,v,P)
% equations of motion for the mountain car problem using basis functions
% problem
% FORMAT [f] = spm_mc_fx_1(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_1.m 3140 2009-05-21 18:38:17Z karl $

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

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

% flow
%--------------------------------------------------------------------------
dt = 1/8;
f  = [x(2);
      G - x(2)*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 quadrule_test41 ( )

%*****************************************************************************80
%
%% QUADRULE_TEST41 tests RADAU_SET and SUM_SUB.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 October 2005
%
  order_max = 15;

  nfunc = func_set ( 'COUNT', 'DUMMY' );

  a = 0.0;
  b = 1.0;

  nsub = 1;
  xlo = -1.0;
  xhi = +1.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'QUADRULE_TEST41\n' );
  fprintf ( 1, '  RADAU_SET sets up Radau quadrature;\n' );
  fprintf ( 1, '  SUM_SUB carries it out.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The integration interval is [%f, %f]\n', a, b );
  fprintf ( 1, '  Number of subintervals is %d\n', nsub );
  fprintf ( 1, '  Quadrature order will vary.\n' );
  fprintf ( 1, '  Integrand will vary.\n' );
  fprintf ( 1, '\n' );

  for ilo = 1 : 5 : nfunc

    ihi = min ( ilo + 4, nfunc );

    fprintf ( 1, '\n' );
    fprintf ( 1, '    ' );
    for i = ilo : ihi
      fprintf ( '%14s', fname(i) );
    end
    fprintf ( 1, '\n' );
    fprintf ( 1, '\n' );

    for n = 1 : order_max

      fprintf ( 1, '  %2d', n );

      for i = ilo : ihi

        func_set ( 'SET', i );

        [ xtab, weight ] = radau_set ( n );

        result(i) = sum_sub ( @func, a, b, nsub, n, xlo, xhi, xtab, weight );

        fprintf ( 1, '  %12f', result(i) );

      end

      fprintf ( 1, '\n' );

    end

  end

  return
end


function idx = findClosestCentroids(X, centroids)
%FINDCLOSESTCENTROIDS computes the centroid memberships for every example
%   idx = FINDCLOSESTCENTROIDS (X, centroids) returns the closest centroids
%   in idx for a dataset X where each row is a single example. idx = m x 1
%   vector of centroid assignments (i.e. each entry in range [1..K])
%

% Set K
K = size(centroids, 1);

% You need to return the following variables correctly.
idx = zeros(size(X,1), 1);

% ====================== YOUR CODE HERE ======================
% Instructions: Go over every example, find its closest centroid, and store
%               the index inside idx at the appropriate location.
%               Concretely, idx(i) should contain the index of the centroid
%               closest to example i. Hence, it should be a value in the
%               range 1..K
%
% Note: You can use a for-loop over the examples to compute this.
%

m = size(X, 1);

for i = 1:m
    deltas = zeros(K, 1);
    rep_examples = repmat(X(i, :), K, 1);
    for j = 1:K
        rep_examples(j, :) = rep_examples(j, :) - centroids(j, :);
        deltas(j, 1) = rep_examples(j, :) * rep_examples(j, :)';
    end
    [min_delta, min_delta_index] = min(deltas);
    idx(i, 1) = min_delta_index;
end

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

end


function stats = signtest_matrix(dat)
% :Usage:
% ::
%
%     stats = signtest_matrix(dat)
%
% This is a matric-ized version of Matlab 2010's signtest.m
% it returns identical P-values to Matlab's function
%
% ..
%    copyright 2011, tor wager
% ..

n = size(dat, 1); % n: how many obs in sign test
n_omit = sum(dat == 0 | isnan(dat));
n = (n - n_omit)';

isvalid = n > 0;

dat(:, ~isvalid) = [];
n(~isvalid) = [];

npos = sum(dat > 0);
nneg = sum(dat < 0);
sgn = min(npos, nneg)';
mydirection = double(npos > nneg)';
mydirection(mydirection == 0) = -1;

if max(n) < 100
    method = 'exact';
else
    method = 'approximate';
end

if isequal(method,'exact')
    p = min(1, 2 .* binocdf(sgn, n, 0.5));  % p > 1 means center value double-counted
    zval = NaN .* ones(size(p));
else
    % Do a continuity correction, keeping in mind the right direction
    z = (npos-nneg - sign(npos-nneg)) ./ (n .^ .5);
    p = 2 .* normcdf(-abs(z), 0, 1);
    stats.zval = z;
    stats.p = p;
end

% FDR correction
% p = p(stats.isvalid); % already done
pthr = FDR(p, .05);
if isempty(pthr), pthr = Inf; end

stats.pthr_fdr05 = pthr;
sig = p < .05;
fdr_sig = p < pthr;

% Re-insert removed CASES (rows) and fill with zeros
mydirection = zeroinsert(~isvalid, mydirection);
n = zeroinsert(~isvalid, n);
p = zeroinsert(~isvalid, p);
zval = zeroinsert(~isvalid, zval);
sig = zeroinsert(~isvalid, sig);
fdr_sig = zeroinsert(~isvalid, fdr_sig);

stats = struct('method', method, 'isvalid', isvalid, 'n', n, ...
'direction', mydirection, 'p', p, 'zval', zval, 'pthr_fdr05', pthr, ...
'sig', sig, 'fdr_sig', fdr_sig);

end % function


function [yd2] = mm22yd2(mm2)
% Convert area from square millimeters to square yards.
% Chad A. Greene 2012
yd2 = mm2*0.000001195990046301;


clc;
close all;
clearvars;
rng('default');
n = 10;
% Let's get a symmetric positive definite matrix
A = gallery('gcdmat', n);
% Compute its Cholesky decomposition
U = chol(A);
b = randn(n, 1);
% solve the problem
x = U \b
x = spx.la.tris.back_col(U, b)


function f = kappa_inf(tau, P)
%  This function computes approximation to kappa in the limit of n->inf
%
x0 = norminv( (tau+1)/2, 0, 1);
VP = 2 * (1 + 2*P);
VQ = 2 * (1 + P)^2;
f = 2*normcdf(sqrt(VP/VQ) * x0, 0, 1) - 1;


function smean = strongMean(data, thr_perc, thr_perc_global, n_sigma)
% Returns the mean per column with a certaint percentile of values in data
% The code uses only the data filtered by the requested percentile to estimate
% an std column by column
% With this estimation only the data within n_sigma range are used for the
% robust mean estimation
%
% SYNTAX:
%   smean = strongMean(data, thr_perc, thr_perc_global, n_sigma)
%
% INPUT:
%   data                matrix of values
%   thr_perc            percentile requested [0 1] per column
%   thr_perc_global     percentile requested [0 1] global
%   n_sigma             number of sigmas
%
% OUTPUT:
%   smean   strong mean per column

%--- * --. --- --. .--. ... * ---------------------------------------------
%               ___ ___ ___
%     __ _ ___ / __| _ | __|
%    / _` / _ \ (_ |  _|__ \
%    \__, \___/\___|_| |___/
%    |___/                    v 1.0RC1
%
%--------------------------------------------------------------------------
%  Copyright (C) 2021 Geomatics Research & Development srl (GReD)
%  Written by:       Andrea Gatti
%  Contributors:     Andrea Gatti ...
%  A list of all the historical goGPS contributors is in CREDITS.nfo
%--------------------------------------------------------------------------
%
%   This program is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation, either version 3 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You should have received a copy of the GNU General Public License
%   along with this program.  If not, see <http://www.gnu.org/licenses/>.
%
%--------------------------------------------------------------------------
% 01100111 01101111 01000111 01010000 01010011
%--------------------------------------------------------------------------
    if nargin < 2
        thr_perc = 1;
    end

    if nargin < 4
        n_sigma = 2;
    end

    % remove the median
    sensor = data;
    smean = zeros(1, size(data, 2));
    for c = 1 : size(data, 2)
        if any(nan2zero(sensor(:, c)))
            sensor(:, c) = sensor(:, c) - median(sensor(:, c), 'omitnan');
        end
    end

    if nargin >= 3 && ~isnan(thr_perc_global) && (thr_perc_global < 1)
        thr = perc(serialize(noNaN(abs(sensor))), thr_perc_global);
        id_ko = abs(sensor) > thr;
    else
        id_ko = false(size(sensor));
    end

    for c = 1 : size(data, 2)
        if any(nan2zero(sensor(:, c)))
            if ~isempty(thr_perc) && thr_perc < 1
                id_ok = abs(sensor(:, c)) < perc(noNaN(abs(sensor(:, c))), thr_perc) & ~id_ko(:, c);
            else
                id_ok = ~id_ko(:, c);
            end

            id_ok = abs(sensor(:, c)) < n_sigma * std(sensor(id_ok, c), 'omitnan');
            if any(id_ok)
                smean(c) = mean(data(id_ok, c), 'omitnan');
            else
                smean(c) = median(data(:, c), 'omitnan');
            end
        end
    end
end


function subGraphs=getAllSubGraphs(model)
% getAllSubGraphs
%   Get all metabolic subgraphs in a model. Two metabolites
%   are connected if they share a reaction.
%
%   Input:
%   model         a model structure
%
%   Output:
%   subGraphs     a boolean matrix where the rows correspond to the metabolites
%                 and the columns to which subgraph they are assigned to. The
%                 columns are ordered so that larger subgraphs come first
%
%   Usage: subGraphs=getAllSubGraphs(model)

%Generate the connectivity graph. Metabolites are connected through
%reactions. This is not a bipartite graph with the reactions.
G=model.S*model.S';
G(G~=0)=1;

%Keeps track of which mets have been assigned to a subgraph
isAssigned=false(numel(model.mets),1);

%Allocate space for subgraphs, initially one graph for each met
subGraphs=false(numel(model.mets),numel(model.mets));

%Main loop continues until all mets have been assigned to a subgraph
counter=1;
while ~all(isAssigned)
    currentSG=false(numel(model.mets),1);
    %Find the first non-assigned metabolite and assign it to the current SG
    I=find(isAssigned==false,1);
    currentSG(I)=true;
    isAssigned(I)=true;
    %Then iteratively add all mets that are connected to each other, until
    %no more such mets can be found
    while true
        J=sum(G(:,currentSG),2);

        %Get the new mets for this SG. Also assign them to the current SG
        newOnes=J~=0 & currentSG==false;
        isAssigned(newOnes)=true;
        currentSG(newOnes)=true;

        %If there are no new mets, then abort
        if ~any(newOnes)
            subGraphs(currentSG,counter)=true;
            counter=counter+1;
            break;
        end
    end
end
subGraphs=subGraphs(:,1:counter);

[~, I]=sort(sum(subGraphs),'descend');

subGraphs=subGraphs(:,I);

%Also remove empty subgraphs (can happen when metabolites are never used)
subGraphs(:,sum(subGraphs)==0)=[];
end


function spheremap(status)
%SPHEREMAP - Map planar points to unit sphere
%    This function takes points drawn on x-y axis and maps them to a sphere
%    in real time.  Click and drag on the planar area to draw.
%
% Michael Hanchak, Dayton, OH, USA  2000-2011

if nargin <1
    status = 'build';
end

switch status
    case 'build'
        clf;set(gcf,'position',[100 100 500 500]);
        axes('units','normalized','position',[.05 .05 .9 .45],'xlim',[-180 180],...
            'ylim',[-90 90],'tag','plane','xtick',[-180 -135 -90 -45 0 45 90 135 180],...
            'ytick',[-90 -45 0 45 90]);hold on;grid
        line(190,0,'marker','.','linestyle','none','markersize',5,...
            'tag','lineplane','color','black');
        axes('units','normalized','position',[.05 .5 .9 .45],'xlim',[-1 1],...
            'ylim',[-1 1],'zlim',[-1 1],'tag','sphere','visible','off');hold on;

        axis vis3d
        [X,Y,Z] = sphere(24);
        h0 = mesh(X,Y,Z);
        set(h0,'facecolor',[1 1 1],'edgecolor',[0 0 0]);
        line(0,0,0,'marker','.','linestyle','none','markersize',5,...
            'tag','linesphere','color','black');
        view(100,30);
        set(gcf,'windowbuttondownfcn',...
            'spheremap(''mouse'');set(gcf,''windowbuttonmotionfcn'',''spheremap(''''mouse'''')'')',...
            'windowbuttonupfcn','set(gcf,''windowbuttonmotionfcn'','' '')');
        clbk = ['h = findobj(''tag'',''lineplane'');set(h,''xdata'',[],''ydata'',[]);',...
            'h = findobj(''tag'',''linesphere'');set(h,''xdata'',[],''ydata'',[],''zdata'',[]);'];
        uicontrol('style','pushbutton','units','normalized','position',[.02 .85 .1 .05],...
            'callback',clbk,'string','Clear');
        set(gcf,'toolbar','figure')

    case 'mouse'
        whichclick = get(gcf,'selectiontype');
        switch whichclick
            case 'normal'
                temp = get(findobj('tag','plane'),'currentpoint');
                point = temp(1,1:2)*pi/180;
                if point(1) <= pi && point(1) >= -pi && point(2) <= pi/2 &&...
                        point(2) >= -pi/2

                    h = findobj('tag','lineplane');
                    x = get(h,'xdata');
                    y = get(h,'ydata');
                    set(h,'xdata',[x, point(1)*180/pi],'ydata',[y, point(2)*180/pi]);

                    h = findobj('tag','linesphere');
                    x = get(h(1),'xdata');
                    y = get(h(1),'ydata');
                    z = get(h(1),'zdata');
                    set(h,'xdata',[x, cos(point(1))*cos(point(2))],...
                        'ydata',[y, sin(point(1))*cos(point(2))],...
                        'zdata',[z, sin(point(2))]);

                    drawnow
                end

            %case 'alt'
            %    set(gcf,'windowbuttondownfcn','','windowbuttonmotionfcn','');
            %    axes(findobj('tag','sphere'));
            %    rot3d;view(100,30);
        end

end


function [H] = rigidbody(f);

% RIGIDBODY creates the homogenous spatial transformation matrix
% for a 6 parameter rigid-body transformation
%
% Use as
%   [H] = rigidbody(f)
%
% The transformation vector f should contain the
%   x-shift
%   y-shift
%   z-shift
% followed by the
%   pitch (rotation around x-axis)
%   roll  (rotation around y-axis)
%   yaw   (rotation around z-axis)

% Copyright (C) 2000-2005, Robert Oostenveld
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

% $Log: rigidbody.m,v $
% Revision 1.1  2009/01/30 04:02:11  arno
% *** empty log message ***
%
% Revision 1.4  2006/04/13 10:37:38  roboos
% added a ; to the end of a line
%
% Revision 1.3  2005/08/15 08:15:32  roboos
% reimplemented the rotate function, which contained an error (the error is in the AIR technical reference)
% changed all functions to be dependent on the rotate, translate and scale function
% all functions now behave consistenly, which also means that they are not compleetly backward compatible w.r.t. the order of the rotations
%
% Revision 1.2  2004/05/19 09:57:07  roberto
% added GPL copyright statement, added CVS log item
%

% compute the homogenous transformation matrix for the translation
T = translate(f([1 2 3]));

% compute the homogenous transformation matrix for the rotation
R = rotate(f([4 5 6]));

% compute the homogenous transformation matrix for the combination
H = T*R;


function star = getStarVerticies(n,r)
% star = getStarVerticies(n,r)
%
% This function draws a star with n points centered on the origin.
%
% INPUTS:
%   n = number of points
%   r = inner radius (outer radius == 1)
%
% OUTPUTS:
%   star = [2, 2*n+1] = [x;y] coordinates of verticies
%

th = linspace(0,2*pi,2*n+1);
star = [...
    sin(th);
    cos(th)];
idx = 2:2:(2*n);
star(:,idx) = r*star(:,idx);

end


%                        experimentImage2D.m
%
% Create a Fourier measurement operator and measurements for reconstructing
% an image.
%
% Inputs:
% numMasks: number of random octanary Fourier masks.
% imagePath: The path of the image to be recovered
%
% Outputs:
%  A     : A function handle: n1*n2 x 1 -> n1*n2*numMasks x 1. It returns
%          A*x.
%  At    : The transpose of A
%  b0    : A n1*n2*numMasks x 1 real, non-negative vector consists of the
%          measurements abs(A*x).
%  Xt    : The true signal - a vectorization of the image
%
%
%
% PhasePack by Rohan Chandra, Ziyuan Zhong, Justin Hontz, Val McCulloch,
% Christoph Studer, & Tom Goldstein
% Copyright (c) University of Maryland, 2017

%% -----------------------------START----------------------------------

function [A, At, b0, Xt, plotter] = experimentImage2D(numMasks, imagePath)
    %%  Read Image
    % Below Xt is n1 x n2 x 3; i.e. we have three n1 x n2 images, one for each of the 3 color channels
    image = rgb2gray(imread([imagePath]));
    image = double(image);
    dims = size(image);
    L = numMasks;

    %% Make octanary masks and linear sampling operators
    % Each mask has iid entries following the octanary pattern.
    % Entries have the form b = b1*b2 , where b1 is sampled
    % from {-1, 1, -i, i} with equal probability 1/4, and b2 from
    % { sqrt(1/2), sqrt(3)} with probability 4/5 and 1/5 respectively.
    b1 = [-1;1;-1i;1i];
    b2 = [repmat(sqrt(0.5),4,1); sqrt(3)];
    % Masks has size [n1,n2,L]
    masks = b1(randi(4,[dims,L])) .* b2(randi(5,[dims,L])); % Storage for L masks, each of dim n1 x n2

    %% Make linear operators that act on a vectorized image
    A = @(x) fourierMeasurementOperator(x, masks, dims);
    At = @(y) transposeOperator(y, masks, dims);

    Xt = image(:);
    b0 = abs(A(Xt(:)));

    % Set up plotting
    subplot(1,2,1);
    imagesc(image);
    title('original');
    subplot(1,2,2);

    plotter = @(x) imagesc(reshape(abs(x),dims));

end

function y = fourierMeasurementOperator(x, masks, dims)
    x = reshape(x, dims);   % image comes in as a vector.  Reshape to rectangle
    [n1,n2] = size(x);
    L = size(masks,3);              % get number of masks

    % Compute measurements
    copies = repmat(x,[1,1,L]);
    y = fft2(masks.*copies);
    y = y(:);
end

function x = transposeOperator(y, masks, dims)
    n1 = dims(1);
    n2 = dims(2);
    L = size(masks,3);              % get number of masks
    y = reshape(y, [n1,n2,L]);   % image comes in as a vector.  Reshape to rectangle

    x = n1*n2*ifft2(y).*conj(masks);
    x = sum(x,3);
    x = x(:);
end


function w = WeightTransform_Merched(W,E,Dint,N,M)

% WeightTransform_Merched    Mapping of Subband Adaptive Filters Coefficients
%                            to Full-band Filter
%
% Weight trnasformation by Merched, Diniz, and Petraglia (see Section 4.4.3.2)
%
% by Lee, Gan, and Kuo, 2008
% Subband Adaptive Filtering: Theory and Implementation
% Publisher: John Wiley and Sons, Ltd

W = [W, conj(fliplr(W(:,2:end-1)))];
W = W.';
Wp = real(ifft(W,N));

G = zeros(N,M/N);

G(1,:) = Wp(1,1:M/N);                % Discard the last sample
for m = 2:N
    aux = conv(E(m-1,:),Wp(m,:));    % Filtering by subfilters with polyphase
                                     %   components of the analysis filter
    G(m,:) = aux(Dint+2:Dint+1+M/N);
end

w = reshape(G,M,1);	             % Equivalent full-band filter


function y=bvpme(t,x,flag,U)
A=[1-2*cos(2*t) 0 1+2*sin(2*t);
    0 2 0;
    -1+2*sin(2*t) 0 1+2*cos(2*t);];
q=exp(t)*[-1+2*cos(2*t)-2*sin(2*t);
    -1;
    1-2*cos(2*t)-2*sin(2*t);];
y = A*x+q;
end


% FHN evolution function:
%
%                   [fx] = f_CaBBI_FHN(Xt,Theta,Isyn,inF)
%
% this function contains the equations and parameters of the FHN neuron
% model, and the over-time integration scheme

function [fx] = f_CaBBI_FHN(Xt,Theta,Isyn,inF)

dt = inF.dt;
Tau_Ca0 = inF.Tau_Ca0;

% Hidden variables
V = Xt(1);                                                                 % voltage
W = Xt(2);                                                                 % recovery variable
Ca = Xt(3);                                                                % [Ca2+] kinetics

% evolution parameters
Tau_Ca  = Tau_Ca0* exp(Theta(1));                                          % decay time-constant of [Ca2+] kinetics
Ca_base = Theta(2);                                                        % basal value of [Ca2+]
scale   = inF.k_Ca0* exp(Theta(3));                                        % scale parameter of [Ca2+] kinetis

% parameters of the evolution mode
g_Ca  = 5;                                                                 % maximal conductance of the calcium channel
E_Ca  = 120;                                                               % reversal potential of the calcium channel

% compute the current of HVA calcium channel
V_scaled = (30*V) - 40;
s_inf = 1./( 1 + exp(- (V_scaled+45)/5 ) );
I_Ca  = g_Ca .* s_inf.* (V_scaled-E_Ca);                                   % HVA calcium current

% build the right hand side (RHS) of the evolution's differential equations
xdot = [ ( V - (power(V,3)/3) - W  + Isyn )
         (0.08*(V+0.7-(0.8*W)))
         ( -scale*I_Ca - ( (Ca-Ca_base)/Tau_Ca) )
         ];

fx = Xt + dt.*xdot;


  function [fw, hr, hl] = fwhm1(psfs, varargin)
%|function [fw, hr, hl] = fwhm1(psfs, options)
%|
%| compute fwhm of point-spread function centered at pixel imid
%| and half-right width and half-left width (fw = hr + hl)
%|
%| in
%|	psfs	[np nc]	column psf(s)
%|
%| option
%|	imid	[1]	which pixel is the middle (default: peak)
%|	dx	[1]	pixel size (default: 1)
%|	min0	0|1	if 1, then negative psf value set to 0 (default: 1)
%|	chat	0|1	if 1, then plot
%| out
%|	fw	[nc]	fwhm of each psf column
%|
%| 2012-09-13, added 'min0' option to avoid fwhm < 1

if ~nargin, ir_usage, end
if streq(psfs, 'test'), fwhm1_test, return, end

arg.chat = 0;
arg.dx = 1;
arg.imid = [];
arg.min0 = true;
arg = vararg_pair(arg, varargin);

psfs = squeeze(psfs); % in case [1 1 np]
[np nc] = size(psfs);
if (np == 1) % single row
	psfs = psfs';
	[np nc] = size(psfs);
end

warned = false;
for ic = 1:nc
	psf = psfs(:,ic);
	if isempty(arg.imid)
		imid = imax(psf);
	else
		imid = arg.imid;
	end

	% normalize
	psf = psf / psf(imid);
	if ~warned && (1 ~= max(psf))
		warn 'peak not at center'
		warned = true;
	end

	% right
	ir = sum(cumprod(double(psf((imid+1):np) >= 0.5)));
	if (imid + ir == np)
		hr(ic,1) = ir;
	else
		high = psf(imid + ir);
		ilow = imid + ir + 1;
		low = psf(ilow);
		if arg.min0 && low < 0
			warn('psf(%d) = %g < 0, so using 0', ilow, low)
			low = 0;
		end
		hr(ic,1) = ir + (high - 1/2) / (high-low);
	end

	% left
	il = sum(cumprod(double(psf((imid-1):-1:1) >= 0.5)));
	if (il == imid-1)
		hl(ic,1) = il;
	else
		high = psf(imid - il);
		ilow = imid - il - 1;
		low = psf(ilow);
		if arg.min0 && low < 0
			warn('psf(%d) = %g < 0, so using 0', ilow, low)
			low = 0;
		end
		hl(ic,1) = il + (high - 1/2) / (high-low);
	end
end

hr = hr * arg.dx;
hl = hl * arg.dx;
fw = hr + hl;

if arg.chat && im
	plot(([1:np]-imid)*arg.dx, psf, '-o', ...
	[-hl hr], [0.5 0.5], '-')
	xlabel 'x', ylabel 'psf(x)'
	title(sprintf('fwhm=%g', fw))
end

% fwhm1_test
function fwhm1_test
dx = 3;
nx = 100;
xx = [-nx/2:nx/2-1]' * dx;
fx = 30;
sx = fx / sqrt(log(256));
psf = exp(-((xx/sx).^2)/2);
fw = fwhm1(psf, 'dx', dx, 'chat', 1);

psf = zeros(7,1); psf(5) = 2; psf([4 6]) = -0.2;
fw = fwhm1(psf, 'dx', dx, 'chat', 1);


function x = r8sto_sl ( n, a, b )

%*****************************************************************************80
%
%% R8STO_SL solves a R8STO system.
%
%  Discussion:
%
%    The R8STO storage format is used for a symmetric Toeplitz matrix.
%    It stores the N elements of the first row.
%
%    For this routine, the matrix is also required to be positive definite.
%
%    This implementation of the algorithm assumes that the diagonal element
%    is 1.
%
%    The real symmetric Toeplitz matrix can be described by N numbers, which,
%    for convenience, we will label A(0:N-1).
%
%    Note that there is a typographical error in the presentation
%    of this algorithm in the reference, and another in the presentation
%    of a sample problem.  Both involve sign errors.  A minor error
%    makes the algorithm incorrect for the case N = 1.
%
%  Example:
%
%    To solve
%
%     1.0 0.5 0.2    x1    4.0
%     0.5 1.0 0.5 *  x2 = -1.0
%     0.2 0.5 1.0    x3    3.0
%
%    we input:
%
%      N = 3
%      A(0:N-1) = (/ 1.0, 0.5, 0.2 /)
%      B(1:3) = (/ 4.0, -1.0, 3.0 /)
%
%    with output:
%
%      X(1:3) = (/ 355, -376, 285 /) / 56
%             = (/ 6.339, -6.714, 5.089 /)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 March 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Gene Golub and Charles Van Loan,
%    Section 4.7.3, "The General Right Hand Side Problem",
%    Matrix Computations,
%    Third Edition,
%    Johns Hopkins, 1996.
%
%  Parameters:
%
%    Input, integer N, the order of the system.
%
%    Input, real A(N), the R8STO matrix.
%
%    Input, real B(N), the right hand side of the linear system.
%
%    Output, real X(N), the solution of the linear system.
%
  k = 0;

  beta = 1.0E+00;
  x(k+1) = b(k+1) / beta;

  if ( k < n-1 )
    y(k+1) = -a(k+2) / beta;
  end

  for k = 1 : n-1

    beta = ( 1.0E+00 - y(k) * y(k) ) * beta;

    x(k+1) = ( b(k+1) - a(2:k+1) * x(k:-1:1)' ) / beta;

    x(1:k) = x(1:k) + x(k+1) * y(k:-1:1);

    if ( k < n - 1 )
      y(k+1) = ( -a(k+2) - a(2:k+1) * y(k:-1:1)' ) / beta;
      y(1:k) = y(1:k) + y(k+1) * y(k:-1:1);
    end

  end

  return
end


function [ n_data, mu, sigma, a, x, fx ] = ...
  truncated_normal_a_pdf_values ( n_data )

%*****************************************************************************80
%
%% TRUNCATED_NORMAL_A_PDF_VALUES: values of the lower Truncated Normal AB PDF.
%
%  Discussion:
%
%    The Normal distribution, with mean Mu and standard deviation Sigma,
%    is truncated to the interval [A,+oo).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 September 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real MU, the mean of the distribution.
%
%    Output, real SIGMA, the standard deviation of the distribution.
%
%    Output, real A, the lower truncation limit.
%
%    Output, real X, the argument of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 11;

  a_vec = [ ...
     50.0, ...
     50.0, ...
     50.0, ...
     50.0, ...
     50.0, ...
     50.0, ...
     50.0, ...
     50.0, ...
     50.0, ...
     50.0, ...
     50.0 ];

  fx_vec = [ ...
     0.01507373507401876, ...
     0.01551417047139894, ...
     0.01586560931024694, ...
     0.01612150073158793, ...
     0.01627701240029317, ...
     0.01632918226724295, ...
     0.01627701240029317, ...
     0.01612150073158793, ...
     0.01586560931024694, ...
     0.01551417047139894, ...
     0.01507373507401876 ];

  mu_vec = [ ...
     100.0, ...
     100.0, ...
     100.0, ...
     100.0, ...
     100.0, ...
     100.0, ...
     100.0, ...
     100.0, ...
     100.0, ...
     100.0, ...
     100.0 ];

  sigma_vec = [ ...
    25.0, ...
    25.0, ...
    25.0, ...
    25.0, ...
    25.0, ...
    25.0, ...
    25.0, ...
    25.0, ...
    25.0, ...
    25.0, ...
    25.0  ];

  x_vec = [ ...
     90.0, ...
     92.0, ...
     94.0, ...
     96.0, ...
     98.0, ...
    100.0, ...
    102.0, ...
    104.0, ...
    106.0, ...
    108.0, ...
    110.0 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

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

  return
end


function pass = test_stringParser(pref)
% This test ensures that the STRINGPARSER class is doing the correct thing in a
% few different situations.

%% BVPS

% Strings we want to test.
str = { 'u(0) = 1'
        'u(-1) = 1'
        'u(0)=1, u(2) = 3'
        'u = 1,v = 0,w = 3'
        'u(end),u(1,left)'
        'diff(sin(u))'
        'diff(sin(u),2)'
        'diff(u^2) + sin(x)*u = 1'
        'feval(u,sqrt(2))'
        'feval(u,0,left)'
        'sum(u) = 0'
        'sum(u,0,.5) = 0'
        'volt(sin(x-y),u) = 0'
        'fred(sin(x-y),u) = fred(cos(x-z),u)'
        'u + fred(sin(2*pi*(y-x)),u) = feval(u,0,left)'};

% Store the number of strings
n = numel(str);
outBVP = cell(n, 1);

for k = 1:n
    outBVP{k} = stringParser.str2anon(str{k}, 'bvp');
end

correctBVP = {  '@(u) feval(u,0)-1'
                '@(u) feval(u,-1)-1'
                '@(u) feval(u,0)-1;feval(u,2)-3'
                '@(u,v,w) [u-1;v;w-3]'
                '@(u) feval(u,''end'');feval(u,1,''left'')'
                '@(u) diff(sin(u))'
                '@(u) diff(sin(u),2)'
                '@(x,u) diff(u.^2)+sin(x).*u-1'
                '@(u) feval(u,sqrt(2))'
                '@(u) feval(u,0,''left'')'
                '@(u) sum(u)'
                '@(u) sum(u,0,.5)'
                '@(x,u) volt(@(x,y)sin(x-y),u)'
                '@(x,u) fred(@(x,y)sin(x-y),u)-fred(@(x,z)cos(x-z),u)'
                '@(x,u) u+fred(@(x,y)sin(2.*pi.*(y-x)),u)-feval(u,0,''left'')'};

passBVP = zeros(n, 1);
for k = 1:n
    passBVP(k) = strcmp(outBVP{k}, correctBVP{k});
end

%% EIGS

str = {'u''''-lambda*u = 0'
       'u'' + sin(x)*u = lambda*u'
       'u''''+u'' = lambda*(u + u'') + u'};
n2 = numel(str);
outEIG = cell(n2, 1);

for k = 1:n2
    outEIG{k} = stringParser.str2anon(str{k}, 'eig');
end

correct = {{'@(u) diff(u,2)'
            '@(u) u'}
            {'@(x,u) diff(u)+sin(x).*u'
            '@(x,u) u'}
            {'@(u) diff(u,2)+diff(u)-u'
            '@(u) u+diff(u)'}};

passEIG = zeros(n2, 1);
for k = 1:n2
    for j = 1:numel(outEIG{k})
        passEIG(k) = strcmp(outEIG{k}{j}, correct{k}{j});
    end
end

%% PDES

str = {'u_t+x*u'''' = u'''};
n3 = numel(str);

outPDE = cell(n3, 1);
for k = 1:n3
    outPDE{k} = stringParser.str2anon(str{k}, 'pde');
end

correct = {'@(x,t,u) -x.*diff(u,2)+diff(u)'};

passPDE = zeros(n3, 1);
for k = 1:n3
    passPDE(k) = strcmp(outPDE{k}, correct{k});
end

%% Concatenate all types
pass = [passBVP; passEIG; passPDE];

end


function H = hopkins (dataIn,xlim1, xlim2, ylim1, ylim2, m)
% HOPKINS calculates H for Hopkins statistics estimnation
%H = hopkins (dataIn,xlim1, xlim2, ylim1, ylim2, m)

sizeData = size(dataIn);
nPoints = sizeData(1); %number of datapoints
sizeVec = [xlim2-xlim1, ylim2 - ylim1];

%generate m random sampling points (not data points)
sp(:,1) = xlim1 + sizeVec(1)*rand(m,1);
sp(:,2) = ylim1 + sizeVec(2)*rand(m,1);

%select m random data points:
pointIndex = [];
sdp = [];
ii = 1;
while ii <= m
    rn = round(nPoints * rand);
    if ~any(pointIndex == rn)
        if ~rn==0
            pointIndex(ii) = rn;
            sdp(ii,:) = dataIn (rn,:);
            ii = ii + 1;
        end
    end
end

% minimal distance between random sampling points and data points:
U = sum(min(dist2 (dataIn, sp), [], 1)); %minimum distances

leaveIndex = removerows((1:nPoints)',pointIndex); %excludes data points in sdp
dataR = dataIn(leaveIndex,:);
W = sum(min(dist2 (dataR, sdp), [], 1));

H = U/(U+W);


clear all; close all; clc;

t=linspace(-pi,pi,10000);
delay=-pi/2;
delay2=0;
amp1=0.8;
amp2=1;
amp3=1;
slowness=150;

for ii=1:1:length(t)

    A=amp1*sin(amp1*2*pi*1*(ii/slowness));
    B=amp2*sin(amp2*2*pi*1*(ii/slowness)+delay);
    C=amp3*sin(amp3*2*pi*1*(ii/slowness)+delay2);
    sin_graph=amp1*sin(amp1*2*pi*1*(t+ii/slowness));
    cos_graph=amp2*sin(amp2*2*pi*1*(t+ii/slowness)+delay);
    third_graph=amp3*sin(amp3*2*pi*1*(t+ii/slowness)+delay2);

    subplot(411);
    plot(t,sin_graph);hold on;
    plot(0,A,'r*','Markersize',10);hold off;title('Signal A : X-Axis');
    xlim([-pi pi]);ylim([-1 1]);
    set(gcf,'Position',[250,100, 800, 800]);
    grid on;
    subplot(412);
    T=num2str(delay);
    Title_for_B=strcat('Signal B with Delay = ',T,' : Y-Axis');
    plot(t,cos_graph);hold on;title(Title_for_B);
    xlim([-pi pi]);ylim([-1 1]);
    plot(0,B,'r*','MarkerSize',10);hold off;
    grid on;
    subplot(413);
    T3=num2str(delay2);
    Title_for_C=strcat('Signal C with Delay = ',T3,' : Z-Axis');
    plot(t,third_graph);hold on;title(Title_for_C);
    xlim([-pi pi]);ylim([-1 1]);
    plot(0,C,'r*','MarkerSize',10);hold off;
    grid on;
    subplot(338);
%     T2=num2str(amp1/amp2);
%     Title_for_C=strcat('Ratio A/B = ',T2);
    plot3(sin_graph,cos_graph,third_graph);%title(Title_for_C);
    hold on;
    plot3(A,B,C,'r*','MarkerSize',10);hold off;%xlim([-1 1]); ylim([-1 1]);
    grid on;
%     drawnow;
    pause(0.00000001)

end


function ihs_test03 ( )

%*****************************************************************************80
%
%% IHS_TEST03 tests the improved distributed hypercube sampling algorithm.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 February 2014
%
%  Author:
%
%    John Burkardt
%
  dim_num = 2;
  duplication = 5;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'IHS_TEST03' );
  fprintf ( 1, '  IHS implements the IHS Algorithm\n' );
  fprintf ( 1, '  (Improved Distributed Hypercube Sampling)\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Demonstrate the code for a fixed dimension\n' );
  fprintf ( 1, '  and duplication value, and increasing number of points.\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Spatial dimension =        %d\n', dim_num );
  fprintf ( 1, '  Duplication factor =       %d\n', duplication );

  for i = 1 : 5

    point_num = 10 * 2^(i-1);

    opt = point_num / point_num^( 1.0 / dim_num );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Number of points =         %d\n', point_num );
    fprintf ( 1, '  Desired minimum distance = %f\n', opt );
%
%  Get the points.
%
    x = ihs ( dim_num, point_num, duplication );
%
%  Compute the covariance.
%
    [ average, sd, covc ] = covariance ( dim_num, point_num, x );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Average minimum distance %f\n', average );
    fprintf ( 1, '  Standard deviation:      %f\n', sd );
    fprintf ( 1, '  Covariance:              %f\n', covc );

    fprintf ( 1, '\n' );

    for j = 1 : point_num

      if ( j <= 10 | point_num - 10 <= j )
        fprintf ( 1, '%4d    ', j );
        for i = 1 : dim_num
          fprintf ( 1, '%4d  ', x(i,j) );
        end
        fprintf ( 1, '\n');
      elseif ( j == 11 )
        fprintf ( 1, '....  ........\n' );
      end
    end

  end

  return
end