% Demonstration for EKF using a random sine signal model.
%
% A Very simple demonstration for extended Kalman filter (EKF), which is
% used to track a random single-component sinusoid signal,
% which is modelled as x_k = a_k*sin(\theta_k), dtheta/dt = omega_k.
% The signal is also filtered with unscented Kalman filter (UKF) for
% comparison.
%
% Copyright (C) 2007 Jouni Hartikainen
%
% This software is distributed under the GNU General Public
% Licence (version 2 or later); please refer to the file
% Licence.txt, included with the software, for details.

clc;
disp('Filtering the signal with EKF...');

save_plots = 1;

% Measurement model and it's derivative
f_func = @ekf_sine_f;
h_func = @ekf_sine_h;
dh_dx_func = @ekf_sine_dh_dx;
d2h_dx2_func = @ekf_sine_d2h_dx2;

% Initial values for the signal.
f = 0;
w = 10;
a = 1;

% Number of samples and stepsize.
d = 5;
n = 500;
dt = d/n;
x = 1:n;

% Check the derivative of the measurement function.
der_check(h_func, dh_dx_func, 1, [f w a]');

% Dynamic state transition matrix in continous-time domain.
F = [0 1 0;
     0 0 0;
     0 0 0];

% Noise effect matrix in continous-time domain.
L = [0 0;
     1 0;
     0 1];

% Spectral power density of the white noise.
q1 = 0.2;
q2 = 0.1;
Qc = diag([q1 q2]);

% Discretize the plant equation.
[A,Q] = lti_disc(F,L,Qc,dt);

% Generate the real signal.
X = zeros(3, n);
X(:,1) = [f w a]';
for i = 2:n
   X(:,i) = A*X(:,i-1) + gauss_rnd([0 0 0]', Q);
end

% Generate the observations with Gaussian noise.
sd = 1;
R = sd^2;

Y = zeros(1,n);
Y_real = feval(h_func,X);
Y = Y_real + gauss_rnd(0,R,n);

plot(x,Y,'.',x,Y_real)

% Initial guesses for the state mean and covariance.
M = [f w a]';
P = diag([3 3 3]);

% Reserve space for estimates.
MM = zeros(size(M,1),size(Y,2));
PP = zeros(size(M,1),size(M,1),size(Y,2));

% Estimate with EKF
for k=1:size(Y,2)
   [M,P] = ekf_predict1(M,P,A,Q);
   [M,P] = ekf_update1(M,P,Y(:,k),dh_dx_func,R*eye(1),h_func);
   MM(:,k)   = M;
   PP(:,:,k) = P;
end

% Initial guesses for the state mean and covariance.
M = [f w a]';
P = diag([3 3 3]);

% Reserve space for estimates.
MM2 = zeros(size(M,1),size(Y,2));
PP2 = zeros(size(M,1),size(M,1),size(Y,2));

% Estimate with EKF
for k=1:size(Y,2)
   [M,P] = ekf_predict1(M,P,A,Q);
   [M,P] = ekf_update2(M,P,Y(:,k),dh_dx_func,d2h_dx2_func,R*eye(1),h_func);
   MM2(:,k)   = M;
   PP2(:,:,k) = P;
end

clf; clc;
disp('The filtering results using the 1st order EKF is now displayed')

% Project the estimates to measurement space
Y_m = feval(h_func, MM);
Y_m2 = feval(h_func, MM2);
plot(x,Y,'.', x,Y_real,'--',x,Y_m);
legend('Measurements','Real signal', 'Filtered estimate');
xlim([0 ceil(max(x))]);
title('Estimating a random Sine signal with extended Kalman filter.');

if save_plots
    print -dpsc demo2_f1.ps
end

clc;
disp('The filtering result using the 1st order EKF is now displayed.');
fprintf('Smoothing the estimates using the RTS smoother...');

[SM1,SP1] = erts_smooth1(MM,PP,A,Q);

[SM2,SP2] = etf_smooth1(MM,PP,Y,A,Q,[],[],[],...
                         dh_dx_func,R*eye(1),h_func);

[SM1_2,SP1_2] = erts_smooth1(MM2,PP2,A,Q);

[SM2_2,SP2_2] = etf_smooth1(MM2,PP2,Y,A,Q,[],[],[],...
                         dh_dx_func,R*eye(1),h_func);

fprintf('ready.\n');
disp(' ');
disp('Push any button to display the smoothing results.');
pause

Y_s1 = feval(h_func, SM1);
plot(x,Y,'.', x, Y_real,'--',x,Y_s1);
legend('Measurements','Real signal','Smoothed estimate');
xlim([0 ceil(max(x))]);
title(['Smoothing a random Sine signal with extended ',...
       'Kalman (RTS) smoother.']);

if save_plots
    print -dpsc demo2_f2.ps
end

clc;
disp('The smoothing results using the ERTS smoother is now displayed.');
disp(' ');
disp('Push any button to see the smoothing results of a ETF smoother.');
pause

Y_s2 = feval(h_func, SM2);
plot(x,Y,'.', x, Y_real,'--',x,Y_s2);
legend('Measurements','Real signal','Smoothed estimate');
title(['Smoothing a random Sine signal with extended ',...
       'Kalman (Two Filter) smoother.']);
xlim([0 ceil(max(x))]);

if save_plots
    print -dpsc demo2_f3.ps
end

clc;
disp('The smoothing results using the ETF smoother is now displayed.');

Y_s1_2 = feval(h_func, SM1_2);
Y_s2_2 = feval(h_func, SM2_2);
% Errors.
EMM_Y = sum((Y_m-Y_real).^2)/n;
EMM_Y2 = sum((Y_m2-Y_real).^2)/n;
ESM1_Y = sum((Y_s1-Y_real).^2)/n;
ESM2_Y = sum((Y_s2-Y_real).^2)/n;
ESM1_2_Y = sum((Y_s1_2-Y_real).^2)/n;
ESM2_2_Y = sum((Y_s2_2-Y_real).^2)/n;

disp(' ');
fprintf('Filtering now with UKF...');

% In the rest the signal is filtered with UKF for comparison.

% Initial guesses for the state mean and covariance.
M = [f w a]';
P = diag([3 3 3]);

% Reserve space for estimates.
U_MM = zeros(size(M,1),size(Y,2));
U_PP = zeros(size(M,1),size(M,1),size(Y,2));

% Estimate with UKF
for k=1:size(Y,2)
   [M,P,X_s,w] = ukf_predict3(M,P,f_func,Q,R*eye(1),dt);
   [M,P] = ukf_update3(M,P,Y(:,k),h_func,R*eye(1),X_s,w,[]);
   U_MM(:,k)   = M;
   U_PP(:,:,k) = P;
end
[U_SM, U_SP] = urts_smooth1(U_MM,U_PP,f_func,Q,dt);

fprintf('ready.\n')
disp(' ');
disp('Push any button to see the filtering results.');
pause

Y_m_u = feval(h_func, U_MM);
plot(x,Y,'.', x, Y_real,'--',x,Y_m_u);
legend('Measurements','Real signal', 'Filtered estimate');
xlim([0 ceil(max(x))]);
title('Estimating a random Sine signal with unscented Kalman filter.');

clc;
disp('The filtering results of a UKF are now displayed.')
disp(' ');
disp('Push any button to display the smoothing results.');
pause

Y_m_su = feval(h_func, U_SM);
plot(x,Y,'.', x, Y_real,'--',x,Y_m_su);
legend('Measurements','Real signal', 'Filtered estimate');
xlim([0 ceil(max(x))]);
title('Estimating a random Sine signal with unscented Kalman smoother (RTS).');

clc;
disp('The smoothing results of a ERTS smoother are now displayed');

UKF_EMM_Y = sum((Y_m_u-Y_real).^2)/n;
URTS_EMM_Y = sum((Y_m_su-Y_real).^2)/n;

disp(' ');
disp('Mean square errors of all estimates:');
fprintf('EKF1-MSE = %.4f\n',sqrt(EMM_Y));
fprintf('ERTS-MSE = %.4f\n',sqrt(ESM1_Y));
fprintf('ETF-MSE = %.4f\n',sqrt(ESM2_Y));
fprintf('EKF2-MSE = %.4f\n',sqrt(EMM_Y2));
fprintf('ERTS2-MSE = %.4f\n',sqrt(ESM1_2_Y));
fprintf('ETF2-MSE = %.4f\n',sqrt(ESM2_2_Y));
fprintf('UKF-RMSE = %.4f\n',sqrt(UKF_EMM_Y));
fprintf('URTS-RMSE = %.4f\n',sqrt(URTS_EMM_Y));


function quadrule_test09 ( )

%*****************************************************************************80
%
%% QUADRULE_TEST09 tests HERMITE_SET.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 October 2009
%
%  Author:
%
%    John Burkardt
%
  order_max = 20;

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

  fprintf ( 1, '\n' );
  fprintf ( 1, 'QUADRULE_TEST09\n' );
  fprintf ( 1, '  HERMITE_SET sets up Gauss-Hermite quadrature;\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The integration interval is ( -oo, +oo ).\n' );
  fprintf ( 1, '  The weight function is exp ( - x * x ).\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 norder = 1 : order_max

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

      for i = ilo : ihi

        func_set ( 'SET', i );

        [ xtab, weight ] = hermite_set ( norder );

        result = 0.0;
        for j = 1 : norder
          result = result + weight(j) * func ( xtab(j) );
        end

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

      end

      fprintf ( 1, '\n' );

    end

  end

  return
end


function [res,fval,it] = muller (f,Z0,itmax,ztol,ftol,option)
% MULLER find a zero of a real or complex function Y = F(Z)
%
% Syntax:
%
% RES = MULLER (F,Z0) find the zero of a complex or real function
% 'F' (either an anonymous function or .m function) using three initial
% guesses contained in the vector Z0. Muller takes the function F and
% evaluetes it at each initial point using feval. F doesn't need to be
% vectorized.
% The initial guesses can be real or complex numbers close to the zero,
% bracketing the zero is not necessary. Parameters ITMAX, ZTOL and
% FTOL are set by default to 1000, 1e-5 and 1e-5, respectively.
%
% RES = MULLER (F,Z0,ITMAX) the maximum number of iterations is set
% equal to ITMAX. ZTOL and FTOL are set by default with the values mentioned
% above.
%
% RES = MULLER (F,Z0,ITMAX,ZTOL) ZTOL is used as a stopping
% criterion. If the absolute difference between the values of Z found in
% the two latest iterations is less than ZTOL, the program is stopped. FTOL
% is set by default with the value mentioned above.
%
% RES = MULLER (F,Z0,ITMAX,ZTOL,FTOL) FTOL is used as a stopping
% criterion. If the value of the function F at the Z found in the last
% iteration is less than FTOL, the program is stopped.
%
% RES = MULLER (F,Z0,ITMAX,ZTOL,FTOL,'both') indicate that both
% criteria ZTOL and FTOL must be satisfied simultaneously. By default,
% MULLER stops if one of the two criteria is fulfilled.
%
% [RES,FVAL] = MULLER (F,Z0,...) return the value of the function
% F at the Z found in the last iteration.
%
% [RES,FVAL,IT] = MULLER (F,Z0,...) return the number of iterations
% used to find the zero.
%
% Example 1:
% myf = @(x) (x-1)^3;
%
% muller(myf,[0 0.1 0.2],[],[],[],'both')
% ans =
%    1.0000 + 0.0000i
%
% Example 2:
%
% [res,fval,it] = muller2('cosh',[0 0.1 0.2],[],[],[],'both')
%
% res =
%   0.0000 + 1.5708i
%
% fval =
%   5.5845e-012 + 3.0132e-012i
%
% it =
%   5
%
% Method taken from:
% Numerical Recipes: The art of scientific computing
% W.H. Press; B.P. Flannery; S.A. Teukolsky; W.T. Vetterling
% 1986
%
% Thanks to John D'Errico for his helpfull review.
%
% Written by Daniel H. Cortes
% MAE Department, West Virginia University
% March, 2008.
%

%=================================================
% Checking proper values of the input parameters
%=================================================

if nargin > 6
    error ('Too many arguments.')
elseif nargin < 2
    error('Too few arguments.')
end

if nargin < 6
    opt = 1;
elseif ischar(option) == 1
    if size(option,2) == 4
        if sum(option == 'both') == 4
            opt = 2;
        else
            error ('Option parameter must be *both*.')
        end
    else
        error ('Option parameter must be *both*.')
    end
else
    error ('Option parameter must be a character array (string).')
end

if nargin < 5
    ftol = 1e-5;
elseif isnumeric(ftol) ~= 1
    error ('FTOL must be a numeric argument.')
elseif isempty(ftol) == 1
    ftol = 1e-5;
elseif  size(ftol,1) ~= 1 || size(ftol,2) ~= 1
    error ('FTOL cannot be an array')
end

if nargin < 4
    ztol = 1e-5;
elseif isnumeric(ztol) ~= 1
    error ('ZTOL must be a numeric argument.')
elseif isempty(ztol) == 1
    ztol = 1e-5;
elseif  size(ztol,1) ~= 1 || size(ztol,2) ~= 1
    error ('ZTOL cannot be an array.')
end

if nargin < 3
    itmax = 1000;
elseif isnumeric(itmax) ~= 1
    error ('ITMAX must be a numeric argument.')
elseif isempty(itmax) == 1
    itmax = 1000;
elseif  size(itmax,1) ~= 1 || size(itmax,2) ~= 1
    error ('ITMAX cannot be an array.')
end

if isnumeric(Z0) ~= 1
    error ('Z0 must be a vector of three numeric arguments.')
elseif isempty(Z0) == 1 || length(Z0) ~= 3 || min(size(Z0)) ~= 1
    error ('Z0 must be a vector of length 3 of either complex or real arguments.')
end

if Z0(1)==Z0(2) || Z0(1)==Z0(3) || Z0(2)==Z0(3)
    error('The initial guesses must be different')
end

%=============================
% Begining of Muller's method
%=============================

z0 = Z0(1);
z1 = Z0(2);
z2 = Z0(3);

y0 = feval ( f, z0);
y1 = feval ( f, z1);
y2 = feval ( f, z2);

for it = 1:itmax
    q = (z2 - z1)/(z1 - z0);
    A = q*y2 - q*(1+q)*y1 + q^2*y0;
    B = (2*q + 1)*y2 - (1 + q)^2*y1 + q^2*y0;
    C = (1 + q)*y2;

    if ( A ~= 0 )

        disc = B^2 - 4*A*C;

        den1 = ( B + sqrt ( disc ) );
        den2 = ( B - sqrt ( disc ) );

        if ( abs ( den1 ) < abs ( den2 ) )
            z3 = z2 - (z2 - z1)*(2*C/den2);
        else
            z3 = z2 - (z2 - z1)*(2*C/den1);
        end

    elseif ( B ~= 0 )
        z3 = z2 - (z2 - z1)*(2*C/B);
    else
        warning('Muller Method failed to find a root. Last iteration result used as an output. Result may not be accurate')
        res = z2;
        fval = y2;
        return
    end

    y3 = feval ( f, z3);

    if opt == 1
        if ( abs (z3 - z2) < ztol || abs ( y3 ) < ftol )
            res = z3;
            fval = y3;
            return
        end
    else
        if ( abs (z3 - z2) < ztol && abs ( y3 ) < ftol )
            res = z3;
            fval = y3;
            return
        end
    end

    z0 = z1;
    z1 = z2;
    z2 = z3;

    y0 = y1;
    y1 = y2;
    y2 = y3;

end

res = z2;
fval = y2;
warning('Maximum number of iterations reached. Result may not be accurate')


function pdf = arcsin_pdf ( x, a )

%*****************************************************************************80
%
%% ARCSIN_PDF evaluates the Arcsin PDF.
%
%  Discussion:
%
%    The LOGISTIC EQUATION has the form:
%
%      X(N+1) = 4.0D+00 * LAMBDA * ( 1.0D+00 - X(N) ).
%
%    where 0 < LAMBDA <= 1.  This nonlinear difference equation maps
%    the unit interval into itself, and is a simple example of a system
%    exhibiting chaotic behavior.  Ulam and von Neumann studied the
%    logistic equation with LAMBDA = 1, and showed that iterates of the
%    function generated a sequence of pseudorandom numbers with
%    the Arcsin probability density function.
%
%    The derived sequence
%
%      Y(N) = ( 2 / PI ) * Arcsin ( SQRT ( X(N) ) )
%
%    is a pseudorandom sequence with the uniform probability density
%    function on [0,1].  For certain starting values, such as X(0) = 0, 0.75,
%    or 1.0D+00, the sequence degenerates into a constant sequence, and for
%    values very near these, the sequence takes a while before becoming
%    chaotic.
%
%    PDF(X) = 1 / ( PI * Sqrt ( A**2 - X**2 ) )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Daniel Zwillinger and Stephen Kokoska,
%    CRC Standard Probability and Statistics Tables and Formulae,
%    Chapman and Hall/CRC, 2000, pages 114-115.
%
%  Parameters:
%
%    Input, real X, the argument of the PDF.
%    -A < X < A.
%
%    Input, real A, the parameter of the CDF.
%    A must be positive.
%
%    Output, real PDF, the value of the PDF.
%
  if ( a <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'ARCSIN_PDF - Fatal error!\n' );
    fprintf ( 1, '  Parameter A must be positive.\n' );
    error ( 'ARCSIN_PDF - Fatal error!' );
  end

  if ( x <= -a | a <= x )
    pdf = 0.0;
  else
    pdf = 1.0 / ( pi * sqrt ( a * a - x * x ) );
  end

  return
end


% Author: Ricardo Baptista and Matthias Poloczek
% Date:   June 2018
%
% See LICENSE.md for copyright information
%

function [binary_sample, p] = logistic_sample(n_vars, logistic_A)
% LOGISTIC_SAMPLE: Function computes binary sample from the implemented
% logistic regression method, as well as the probability of the sample
% (likelihood under the binary logistic regression proposal).

% Declare new binary sample
binary_sample = zeros(1,n_vars);

% Declare new constant p
p = 1;

% Generate sample
for j=1:n_vars

    % Evaluate logistic regression function
    if j==1
        logistic_regressor = logistic_A(j,j);
    else
        logistic_regressor = logistic_A(j,j) + ...
                             logistic_A(j,1:j-1)*binary_sample(1:j-1)';
    end
    logistic_function = 1/(1 + exp(-logistic_regressor));

    % Generate sample from a bernoulli distribution
    binary_sample(j) = binornd(1,logistic_function);

    % Compute next success probability
    if (binary_sample(j) == 1)
        p = p*logistic_function;
    elseif (binary_sample(j) == 0)
        p = p*(1-logistic_function);
    end

end

end


function demo_fast_sc(opt_choice)
% opt_choice = 1: use epslion-L1 penalty
% opt_choice = 2: use L1 penalty

if ~exist('opt_choice', 'var')
    opt_choice = 1;
end

% natural image data
load ../data/IMAGES.mat
X = getdata_imagearray(IMAGES, 14, 10000);

% sparse coding parameters
num_bases = 128;
beta = 0.4;
batch_size = 1000;
num_iters = 100;
if opt_choice==1
    sparsity_func= 'epsL1';
    epsilon = 0.01;
elseif opt_choice==2
    sparsity_func= 'L1';
    epsilon = [];
end

Binit = [];
fname_save = sprintf('../results/sc_%s_b%d_beta%g_%s', sparsity_func, num_bases, beta, datestr(now, 30));

% run fast sparse coding
[B S stat] = sparse_coding(X, num_bases, beta, sparsity_func, epsilon, num_iters, batch_size, fname_save, Binit);


function [kli, set_kli]= KLIndex(ar_est,ma_est,varx_est,ar,ma,varx,n_obs)

%KLIndex Kullback-Leibler index.
%   kli = KLIndex(a_est,b_est,varx_est,a,b,varx,nobs) is the exact Kullbach-Leibler
%   Index between two time series models for nobs observations.
%   The estimated proces a_est, b_est,var_est and the true process a,b,varx are assumed
%   to be normally distributed.
%
%   Possible second output argument: set of KLI's of AR models of increasing autoregressive
%   model order:
%   [kli set_kli] = KLIndex(a_est,b_est,varx_est,a,b,varx,nobs)

%S. de Waele, Februari 2003.

rc = ar_ma2rc(ar,ma,n_obs);
ar_order = length(rc)-1;
rc_est = ar_ma2rc(ar_est,ma_est,n_obs);
ar_est_order = length(rc_est)-1;

%Kullback-Leiber index of the estimated process
%with respect to the true process: kli_fef.
varp_est = varx_est*[1 cumprod(1-rc_est(2:end).^2)];

%Prediction errors (vareta)
set_ar_est = rc2arset(rc_est,0:ar_est_order);
cov = varx*arma2cor(ar,ma,ar_est_order);
covs = [cov(end:-1:2) cov];
for L = 0:ar_est_order,
    par_est = set_ar_est{1+L};
    sh = ar_est_order+L+1;
    vareta(L+1) = convol(par_est,convol(covs,par_est(end:-1:1)),sh,sh);
end

L = 0:ar_est_order;
set_kli_fef = cumsum(log(varp_est)) + cumsum(vareta./varp_est) ...
    + (n_obs-L-1).*(log(varp_est) + vareta./varp_est);

set_kli = set_kli_fef;
kli = set_kli(end);


function varargout = goertzel(x,fs,ft,N,varargin)
% GOERTZEL   Goertzel Algorithm.
%
%  [RE,IM] = GOERTZEL(X,FS,FT,N) calculates the goertzel algorithm
%  for X, considering a sampling frequency FS, the target
%  frequency FT and using a block of size N.
%
%  [MAG,PHASE] = GOERTZEL(...,'mag') returns the magnitude and
%  phase information of the transform.
%
%  [...] = GOERTZEL(...,'overlap',OL) will perform the calculation
%  on blocks that overlap. The amount of overlapping is defined by
%  OL, and can be set from 0 to 1.
%

[mag,OL] = parse_inputs(varargin{:});

if size(x,1)~=1 & size(x,2)~=1
    error('X must be a vector.')
end

flag_rot = 0;
if size(x,2) == 1
    flag_rot = 1;
    x = x';
end

n = length(x);
k = 1*(0.5+N*ft/fs);
w = 2*pi*k/N;
c = cos(w);
s = sin(w);
coef = 2*c;
ADV = max(round((1-OL)*N),1);

re = zeros(1,floor(n/ADV));
im = re;

for j = 1 : floor((n-N)/ADV)
    q1 = 0;
    q2 = 0;
    for i = 1 : N
        q0 = coef*q1 - q2 + x(i+(j-1)*ADV);
        q2 = q1;
        q1 = q0;
    end
    re(j) = q1 - q2*c;
    im(j) = q2*s;
end

if mag == 0
    varargout{1} = re;
    varargout{2} = im;
else
    ma = sqrt(re.^2 + im.^2);
    ph = angle(re+i*im);

    varargout{1} = ma;
    varargout{2} = ph;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [mag,OL] = parse_inputs(varargin)

mag = 0;
OL = 0;
flag_par = 0;

for i = 1 : nargin
    if flag_par == 1
        flag_par = 0;
        continue
    elseif strcmp(varargin{i},'mag')
        mag = 1;
    elseif strcmp(varargin{i},'overlap')
        if nargin == i
            error('OL not defined.')
        end
        flag_par = 1;
        OL = varargin{i+1};
    else
        error('Unknown parameter.')
    end
end

if OL>1 | OL<0
    error('OL should be set inside the interval [0,1].')
end


function [beta_median beta_std beta_lbound beta_ubound sigma_median]=olsestimates(betahat,sigmahat,X,k,q,cband)

% function [beta_median beta_std beta_lbound beta_ubound sigma_median]=olsestimates(betahat,sigmahat,X,k,q,cband)
% estimates the posterior mean, standard deviation and confidence interval for the VAR coefficients of an OLS VAR model
% inputs:  - vector 'betahat': OLS VAR coefficientsm in vectorised form (defined in 1.1.15)
%          - matrix 'sigmahat': OLS VAR variance-covariance matrix of residuals (defined in 1.1.10)
%          - matrix 'X': matrix of regressors for the VAR model (defined in 1.1.8)
%          - integer 'k': number of coefficients to estimate for each equation in the BVAR model (defined p 7 of technical guide)
%          - integer 'q': total number of coefficients to estimate for the BVAR model (defined p 7 of technical guide)
%          - scalar 'cband': confidence level for VAR coefficients
% outputs: - vector 'beta_median': median value of the posterior distribution of beta
%          - vector 'beta_std': standard deviation of the posterior distribution of beta
%          - vector 'beta_lbound': lower bound of the credibility interval of beta
%          - vector 'beta_ubound': upper bound of the credibility interval of beta
%          - vector 'sigma_median': median value of the posterior distribution of sigma (vectorised)

% compute the point estimate of beta
beta_median=betahat;
% compute estimates for the variance of each coefficient
% first obtain omeagahat, the (OLS) variance-covariance matrix for betahat, from (a.9.1)
omegahat=kron(sigmahat,(X'*X)\speye(k));
% consider only the diagonal (variance terms), and take the square root of each element
beta_std=diag(omegahat).^0.5;
% build the confidence intervals
for ii=1:q
beta_lbound(ii,:)=norminv((1-cband)/2,beta_median(ii,1),beta_std(ii,1));
beta_ubound(ii,:)=norminv(1-(1-cband)/2,beta_median(ii,1),beta_std(ii,1));
end

sigma_median=sigmahat;


function r1 = rOneCalculation(fracFreq)
%method for caculating r1 used in identifying noise type for a giving tau
%value. input argument is array of fractrional frequency values for a given
%tau.
%r1 = <(Zi - Zavg)*(Zi+1 - Zavg)> / <(Zi - Zavg)^2>
%Z is fractional freq values. Numerator sum is from 1 to N-1 and demonator
%is from 1 to N

avgFF = mean(fracFreq); %get average value
N = length(fracFreq); %get number of frac freq value in array
n=0; %store numerator of r1
j=0; %store denomenator of r1

%This loop builds the numerator for r1
for i = 1:(N-1)
    if i == 1
        n = (fracFreq(i) - avgFF)*(fracFreq(i+1) - avgFF);
    else
        n = n + (fracFreq(i) - avgFF)*(fracFreq(i+1) - avgFF);
    end
end
%this loop builds the denomentor for r1
for i = 1:N
    if i == 1
        j = (fracFreq(i) - avgFF)^2;
    else
        j = j + (fracFreq(i) - avgFF)^2;
    end
end

%calculate and return r1
r1 = n/j;


function cbrt_test ( )

%*****************************************************************************80
%
%% CBRT_TEST tests R4_CBRT and R8_CBRT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 September 2011
%
%  Author:
%
%    John Burkardt
%
  addpath ( '../test_values' );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CBRT_TEST:\n' );
  fprintf ( 1, '  Test CBRT_VALUES, R4_CBRT, R8_CBRT.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '             X        CBRT(X)\n' );
  fprintf ( 1, '                   R4_CBRT(X)         Diff\n' );
  fprintf ( 1, '                   R8_CBRT(X)         Diff\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, x, fx1 ] = cbrt_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fx2 = r4_cbrt ( single ( x ) );
    fx3 = r8_cbrt ( x );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  %14.4f  %14.6g\n', x, fx1 );
    fprintf ( 1, '                  %14.6g  %14.6g\n', fx2, abs ( fx1 - fx2 ) );
    fprintf ( 1, '                  %14.6g  %14.6g\n', fx3, abs ( fx1 - fx3 ) );

  end

  rmpath ( '../test_values' );

  return
end


function [labels rts] = graph_connected_components(C)
% C - connection matrix
% labels =[1 1 1 2 2 3 3 ...]  lenght(labels)=L, label for each vertex
% labels(i) is order number of connected component, i is vertex number
% rts - roots, numbers of started vertex in each component

L=size(C,1); % number of vertex

% Breadth-first search:
labels=zeros(1,L); % all vertex unexplored at the begining
rts=[];
ccc=0; % connected components counter
while true
    ind=find(labels==0);
    if ~isempty(ind)
        fue=ind(1); % first unexplored vertex
        rts=[rts fue];
        list=[fue];
        ccc=ccc+1;
        labels(fue)=ccc;
        while true
            list_new=[];
            for lc=1:length(list)
                p=list(lc); % point
                cp=find(C(p,:)); % points connected to p
                cp1=cp(labels(cp)==0); % get only unexplored vertecies
                labels(cp1)=ccc;
                list_new=[list_new cp1];
            end
            list=list_new;
            if isempty(list)
                break;
            end
        end
    else
        break;
    end
end


function [T, M, S, DISTR, df] = dtiTTestStat(g1, g2, Y, mask)

% Computes voxel-wise T-test statistics for two groups from a data array.
%
%   [T, M, S, DISTR, df] = dtiTTestStat(g1, g2, DT_ARRAY, [MASK])
%
% Input:
%   g1, g2      List of indices that correspond to each group out of 1:N
%                   E.g: g1 = 1:7, g2 = 8:14, N = 14
%   DT_ARRAY    Data array of size XxYxZxN (or nxN), where X, Y, Z are the volume
%                   dimensions and N is the number of subjects.
%                   (n is the number of voxels).
%   MASK        Optional XxYxZ binary array. Values of M and S are computed
%                   where mask = 1; in other voxels, M and S are set to 0.
%                   Default is entire volume.
%
% Output:
%   T           XxYxZx1 array of test statistics (0 where mask = 0)
%   M           XxYxZx2 array of means for both groups (0 where mask = 0)
%   S           XxYxZx1 array of pooled standard deviations (0 where mask = 0)
%   DISTR       The string 't'
%   df          The number of degrees of freedom = N-2
%
% Utilities:    ndfun.m, dtiSplitTensor.m, dti33to6.m
%
% WARNING: If using Pentium 4, eliminate NaN's from array before running
% (processor bug).
%
% Copyright by Armin Schwartzman, 2005

% HISTORY:
%   2004.06.23 ASH (armins@stanford.edu) wrote it.
%

% Check inputs
if (ndims(Y)==2 | ndims(Y)==3),
    Ind = 1;    % Data in indexed nxN format
    Y = shiftdim(Y, -2);
else
    Ind = 0;    % Data in XxYxZxN format
end
if (ndims(Y)<4 | ndims(Y)>5),
    error('Wrong input format');
end
if (~exist('mask')),
    mask = ones([size(Y,1) size(Y,2) size(Y,3)]);
end

% Computations
N1 = length(g1);
N2 = length(g2);
N  = N1 + N2;

M = cat(4, mean(Y(:,:,:,g1), 4), mean(Y(:,:,:,g2), 4));
Ystd1 = std(Y(:,:,:,g1), 1, 4); Ystd1(~mask) = 1;
Ystd2 = std(Y(:,:,:,g2), 1, 4); Ystd2(~mask) = 1;
S = sqrt((N1*Ystd1.^2 + N2*Ystd2.^2)/(N-2) .* (1/N1 + 1/N2));
T = (M(:,:,:,1) - M(:,:,:,2)) ./ S;

% Adjust output
if Ind,
    T = shiftdim(T, 2);
    M = shiftdim(M, 2);
    S = shiftdim(S, 2);
end

DISTR = 't';
df = [N-2 N-2];     % Second entry is dummy

return


function value = r8_gmic ( a, x, alx )

%*****************************************************************************80
%
%% R4_GMIC: complementary incomplete gamma, small X, A near negative integer.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 October 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real A, the parameter.
%
%    Input, real X, the argument.
%
%    Input, real ALX, the logarithm of X.
%
%    Output, real VALUE, the complementary incomplete
%    gamma function.
%
  persistent bot
  persistent eps

  euler = 0.5772156649015329;

  if ( isempty ( eps ) )
    eps = 0.5 * r4_mach ( 3 );
    bot = log ( r4_mach ( 1 ) );
  end

  if ( 0.0 < a )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R4_GMIC - Fatal error!\n' );
    fprintf ( 1, '  A must be near a negative integer.\n' );
    error ( 'R4_GMIC - Fatal error!' )
  end

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

  ma = r4_aint ( a - 0.5 );
  fm = - ma;
  m = - ma;

  te = 1.0;
  t = 1.0;
  s = t;
  converged = 0;
  for k = 1 : 200
    fkp1 = k + 1;
    te = - x * te / ( fm + fkp1 );
    t = te / fkp1;
    s = s + t;
    if ( abs ( t ) < eps * s )
      converged = 1;
      break
    end
  end

  if ( ~ converged )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R4_GMIC - Fatal error!\n' );
    fprintf ( 1, '  No convergence after 200 iterations.\n' );
    error ( 'R4_GMIC - Fatal error!' )
  end

  value = - alx - euler + x * s / ( fm + 1.0 );

  if ( m == 0 )
    return
  elseif ( m == 1 )
    value = - value - 1.0 + 1.0 / x;
    return
  end

  te = fm;
  t = 1.0;
  s = t;
  mm1 = m - 1;
  for k = 1 : mm1
    fk = k;
    te = - x * te / fk;
    t = te / ( fm - fk );
    s = s + t;
    if ( abs ( t ) < eps * abs ( s ) )
      break
    end
  end

  for k = 1 : m
    value = value + 1.0 / k;
  end

  if ( mod ( m, 2 ) == 1 )
    sgng = - 1.0;
  else
    sgng = + 1.0;
  end

  alng = log ( value ) - r4_lngam ( fm + 1.0 );

  if ( bot < alng )
    value = sgng * exp ( alng );
  else
    value = 0.0;
  end

  if ( s ~= 0.0 )
    value = value ...
      + abs ( exp ( - fm * alx + log ( abs ( s ) / fm ) ) * r8_sign ( s );
  end

  return
end


   function [mean_pos,ma_pos] = mean_posit_track(t,s,tau,train_len)
%| function [mean_pos,ma_pos] = mean_posit_track(t,s,tau,train_len)
%| INPUT
%|	t: time indices
%|	s: position sequence (should be same length as t)
%|	tau: delay parameter,
%|		set to be the equivalence index to about 0.5sec for breathing.
%|	train_Len: training length (starting up stage),
%|		approximately one period's equivalence
%|
%| OUTPUT
%|	mean_pos: a N*2 matrix,
%|		for practical purposes, take the first column mean_pos(:,1)
%|	ma_pos: naive moving average.
%|
%| Dan Ruan, University of Michigan, 2007
%|
%| Based on the paper by Dan Ruan, J A Fessler, James M Balter
%| Mean position tracking of respiratory motion
%| Med. Phys. 35(2):782-92, Feb. 2008
%| doi 10.1118/1.2825616

if nargin < 1, help(mfilename), error(mfilename), end
if streq(t, 'test'), self_test, return, end

s_complete = s;
s_d = s(1:end-tau);
s = s(tau+1:end);
for tt = 1:length(t)-train_len-tau;
        s_temp = s(tt:tt+train_len);
        sd_temp = s_d(tt:tt+train_len);
        params = fitellipse(s_temp,sd_temp);
        mean_pos(tt,:) =  [params(1),params(2)];
        ma_pos(tt) = mean(s(tt:tt+train_len));
end

plot(t,s_complete,'linewidth',2);
hold on
plot(t(1+tau+train_len:end),ma_pos,'g:','linewidth',2);
plot(t(1+tau+train_len:end),mean_pos(:,1),'r--','linewidth',2);
hold off
set(gca,'DataAspectRatio',[5 5 1]);
set(gca,'fontsize',14,'FontWeight','demi');
h = axis;
axis(h + [0 0 -1 1]);
xlabel('time (second)');
ylabel('displacement');
legend('Observation','Estimation with MA','Estimate with Ellipse Center','Location','SouthWest');

%
function a = fitellipse(X,Y)
% fit ellipse with sample locations in X,Y
% normalize data
mx = mean(X);
my = mean(Y);
sx = (max(X)-min(X))/2;
sy = (max(Y)-min(Y))/2;

x = (X-mx)/sx;
y = (Y-my)/sy;

% Force to column vectors
x = x(:);
y = y(:);

% Build design matrix
D = [ x.*x  x.*y  y.*y  x  y  ones(size(x)) ];

% Build scatter matrix
S = D'*D;

% Build 6x6 constraint matrix
C(6,6) = 0; C(1,3) = -2; C(2,2) = 1; C(3,1) = -2;

  % Break into blocks
  tmpA = S(1:3,1:3);
  tmpB = S(1:3,4:6);
  tmpC = S(4:6,4:6);
  tmpD = C(1:3,1:3);
  tmpE = inv(tmpC)*tmpB';
  [evec_x, eval_x] = eig(inv(tmpD) * (tmpA - tmpB*tmpE));

  % Find the positive (as det(tmpD) < 0) eigenvalue
  I = find(real(diag(eval_x)) < 1e-8 & ~isinf(diag(eval_x)));

  % Extract eigenvector corresponding to negative eigenvalue
  A = real(evec_x(:,I));

  % Recover the bottom half...
  evec_y = -tmpE * A;
  A = [A; evec_y];

% unnormalize
par = [
  A(1)*sy*sy,   ...
      A(2)*sx*sy,   ...
      A(3)*sx*sx,   ...
      -2*A(1)*sy*sy*mx - A(2)*sx*sy*my + A(4)*sx*sy*sy,   ...
      -A(2)*sx*sy*mx - 2*A(3)*sx*sx*my + A(5)*sx*sx*sy,   ...
      A(1)*sy*sy*mx*mx + A(2)*sx*sy*mx*my + A(3)*sx*sx*my*my   ...
      - A(4)*sx*sy*sy*mx - A(5)*sx*sx*sy*my   ...
      + A(6)*sx*sx*sy*sy   ...
      ]';

% Convert to geometric radii, and centers

thetarad = 0.5*atan2(par(2),par(1) - par(3));
cost = cos(thetarad);
sint = sin(thetarad);
sin_squared = sint.*sint;
cos_squared = cost.*cost;
cos_sin = sint .* cost;

Ao = par(6);
Au =   par(4) .* cost + par(5) .* sint;
Av = - par(4) .* sint + par(5) .* cost;
Auu = par(1) .* cos_squared + par(3) .* sin_squared + par(2) .* cos_sin;
Avv = par(1) .* sin_squared + par(3) .* cos_squared - par(2) .* cos_sin;

% ROTATED = [Ao Au Av Auu Avv]

tuCentre = - Au./(2.*Auu);
tvCentre = - Av./(2.*Avv);
wCentre = Ao - Auu.*tuCentre.*tuCentre - Avv.*tvCentre.*tvCentre;

uCentre = tuCentre .* cost - tvCentre .* sint;
vCentre = tuCentre .* sint + tvCentre .* cost;

Ru = -wCentre./Auu;
Rv = -wCentre./Avv;

Ru = sqrt(abs(Ru)).*sign(Ru);
Rv = sqrt(abs(Rv)).*sign(Rv);

a = [uCentre, vCentre, Ru, Rv, thetarad];

% self_test
function self_test
t = linspace(0, 60, 601);
dt = t(2) - t(1);
s = (8 + cos(2*pi/6*t)).^2 - 8^2;
tau = round(0.5 / dt);
train_len = round(5.1 / dt);

[mean_pos ma_pos] = mean_posit_track(t, s, tau, train_len);

prompt
tt = t(tau+train_len+1:end);
clf, plot(t, s, '-', tt, mean_pos(:,1), '--', tt, ma_pos, ':', 'linewidth', 2)
legend('signal', 'proposed', 'moving avg')
xlabel t


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni

%problem 3 - Inverse Fourier Transform of sin(w)/w

syms t w
X=sin(w)/w;
x=ifourier(X,t)
ezplot(x, [-3 3])


function v = sum2(f)
%SUM2   Double integral of a SPHEREFUN over its domain.
%   I = SUM2(F) returns the double definite integral of a SPHEREFUN.

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

% Split f into its plus/minus terms.  The minus terms have integral zero
% on the sphere since the rows in this case are anti-periodic with period
% pi. Thus, we only need to integrate the plus terms.

[cols, d, rows] = cdr(f);

% If there are no plus terms then the integral is zero
if ( isempty(f.idxPlus) )
    v = 0;
    return
end

cols = cols(:, f.idxPlus); % Just keep the plus terms
rows = rows(:, f.idxPlus);
d = diag(d(f.idxPlus, f.idxPlus)).';

% Integrate the rows over their domain.
intRows = sum(rows);

% One could use the following code to do the integrals in latitude (theta),
% but this can be slow due to the fact that the integrand (which are
% trigfuns) may first be converted to chebfuns when sum (with an interval
% other than the period of the integrand) is called.  Mathematically, this
% is, of course, unnecessary and I tried arguing that this should not be
% the case.  However, I was not successful in convincing everyone and so we
% are stuck with a potentially slow method to do definite integrals rather
% than a fast one. See ticket numbers #1004 and #1034 for more details.
%
% We are going to do the integral the fast way.

% Slow code: Left here in case someone ever makes sum(f,[a,b]) fast for
% trigfuns.
%
% % Create a trigfun of the measure for the sphere.
% measure = chebfun(@(x) sin(x), f.domain(3:4), 'trig');
%
% % Multiply the columns by the measure
% cols = cols.*(measure*ones(1, size(cols, 2)));
%
% % Integrate each column over the non-doubled up latitude coordinate.
% intCols = sum(cols, [0 pi]);

%
% Fast code: We know the columns are even functions, which means they have
% cosine series expansions:
% col(:, j) = sum_{k=0}^{m} a_k cos(k*t)
% So, in the case of the elevation angle being measured as co-latitude, we
% want to compute the integral
% int_{0}^{pi}col(:,j).*sin(t)dt = sum_{k=0}^{m} a_k int_{0}^{pi}cos(k*t).*sin(t)dt
% This simplifies down to
% int_{0}^{pi}col(:,j).*sin(t)dt = sum_{k=0}^{m} a_k (1+(-1)^k)/(1-k^2)

[a, ignore] = trigcoeffs(cols);

k = (0:size(a, 1)-1).';
intFactor = 2./(1-k(1:2:end).^2);
intCols = sum(bsxfun(@times, a(1:2:end, :), intFactor));

% Put the integrals together to get the final result.
v = sum(d.*intRows.*intCols);

% TODO: Add support for different domains.

end


function [bm,env,delay] = gammatoneFast(x,cfs,fs,align)
%GAMMATONEFAST Produce an array of responses from gammatone filters via FFT
%
%   BM = IOSR.AUDITORY.GAMMATONEFAST(X,CFS,FS) passes the vector X through
%   a bank of fourth-order gammatone filters, with centre frequencies
%   specified by CFS. The function returns a matrix, with each row/column
%   corresponding to a filter output with a centre frequency determined by
%   the corresponding element in CFS. The orientation of the output is
%   determined by the orientation of the input: if X is a row vector then
%   the output will contain one row for each filter output, and vice versa.
%
%   Centre frequencies may be any value below the Nyquist rate (determined
%   by the sampling frequency fs). Typically centre frequencies are equally
%   spaced on the ERB-rate scale and may be calculated thus:
%
%       CFS = iosr.auditory.makeErbCFs(LOW_CF,HIGH_CF,NUMCHANS)
%
%   where LOW_CF is the lowest frequency in the bank, HIGH_CF is the
%   highest, and NUMCHANS is the numbers of filters in the bank.
%
%   BM = IOSR.AUDITORY.GAMMATONEFAST(...,ALIGN) allows phase alignment to
%   be applied. With ALIGN=false, no alignment is applied (default). With
%   ALIGN=true, fine structure and envelope alignment is applied so that
%   the impulse response peaks occurs at t=0.
%
%   [BM,ENV] = IOSR.AUDITORY.GAMMATONEFAST(...) returns the instantaneous
%   envelopes ENV for each filter.
%
%   [BM,ENV,DELAY] = IOSR.AUDITORY.GAMMATONEFAST(...) returns the delay
%   DELAY (in samples) removed by the phase alignment of each gammatone
%   filter, i.e. DELAY(n)=0 if ALIGN=true. DELAY is a vector the same size
%   as CFS.
%
%   Based on code written by ZZ Jin, adapted by DLW in Jan'07 and JF
%   Woodruff in Nov'08
%
%   See also IOSR.AUDITORY.MAKEERBCFS.

%   Copyright 2016 University of Surrey.

    if nargin < 3
        fs = 16000; % default sampling frequency
    end
    if nargin < 4
        align = false; % default phase alignment
    end

    % check inputs
    assert(isvector(x) & isnumeric(x), 'iosr:gammatoneFast:invalidX', 'x must be a vector')
    assert(isvector(cfs) & isnumeric(cfs), 'iosr:gammatoneFast:invalidCfs', 'cfs must be a vector')
    assert(isscalar(fs), 'iosr:gammatoneFast:invalidFs', 'fs must be a scalar')
    assert(islogical(align) & numel(align)==1, 'iosr:gammatoneFast:invalidAlign', 'align must be logical')

    % number of frequency channels
    numchans = length(cfs);

    filterOrder = 4; % filter order
    gL = 2^nextpow2(0.128*fs); % gammatone filter length at least 128 ms
    b = 1.019.*24.7.*(4.37.*cfs./1000+1); % rate of decay or bandwidth

    gt = zeros(gL,numchans);  % Initialise IR
    tc = zeros(size(cfs));  % Initialise time lead
    phase = 0;

    tpt=(2*pi)/fs;
    gain=((1.019.*b.*tpt).^filterOrder)./6; % based on integral of impulse

    tmp_t = (0:gL-1)/fs;

    % calculate impulse response
    for i = 1:numchans
        if align
            tc(i) = (filterOrder-1)./(2*pi*b(i));
            phase = -2*pi*cfs(i)*tc(i);
        end
        gt(:,i) = gain(i)*fs^3*tmp_t.^(filterOrder-1).*exp(-2*pi*b(i)*tmp_t).*cos(2*pi*cfs(i)*tmp_t+phase);
    end

    % if input is row vector, transpose to column vector
    rot = false;
    if size(x,1)==1
        x = x';
        rot = true;
    end

    % gammatone filtering using FFTFILT
    bm = fftfilt(gt,repmat(x,1,numchans));

    % Hilbert envelope
    env = abs(hilbert(bm));

    % delay due to time lead
    delay = round(tc.*fs);

    % remove time lead
    for i = 1:numchans
        bm(:,i) = [bm(delay(i)+1:end,i); zeros(delay(i),1)];
        env(:,i) = [env(delay(i)+1:end,i); zeros(delay(i),1)];
    end

    % transpose output if necessary
    if rot
        bm = bm';
        env = env';
    end

end


function [pd,Pdm,pmd] = som_probability_gmm(D, sM, K, P)

%SOM_PROBABILITY_GMM Probabilities based on a gaussian mixture model.
%
% [pd,Pdm,pmd] = som_probability_gmm(D, sM, K, P)
%
%   [K,P] = som_estimate_gmm(sM,D);
%   [pd,Pdm,pmd] = som_probability_gmm(D,sM,K,P);
%   som_show(sM,'color',pmd(:,1),'color',Pdm(:,1))
%
%  Input and output arguments:
%   D    (matrix) size dlen x dim, the data for which the
%        (struct) data struct,     probabilities are calculated
%   sM   (struct) map struct
%        (matrix) size munits x dim, the kernel centers
%   K    (matrix) size munits x dim, kernel width parameters
%                 computed by SOM_ESTIMATE_GMM
%   P    (matrix) size 1 x munits, a priori probabilities for each
%                 kernel computed by SOM_ESTIMATE_GMM
%
%   pd   (vector) size dlen x 1, probability of each data vector in
%                 terms of the whole gaussian mixture model
%   Pdm  (matrix) size munits x dlen, probability of each vector in
%                 terms of each kernel
%   pmd  (matrix) size munits x dlen, probability of each vector to
%                 have been generated by each kernel
%
% See also SOM_ESTIMATE_GMM.

% Contributed to SOM Toolbox vs2, February 2nd, 2000 by Esa Alhoniemi
% Copyright (c) by Esa Alhoniemi
% http://www.cis.hut.fi/projects/somtoolbox/

% ecco 180298 juuso 050100

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% input arguments
if isstruct(sM), M = sM.codebook; else M = sM; end
[c dim] = size(M);

if isstruct(D), D = D.data; end
dlen = size(D,1);

% reserve space for output variables
pd = zeros(dlen,1);
if nargout>=2, Pdm = zeros(c,dlen); end
if nargout==3, pmd = zeros(c,dlen); end

% the parameters of each kernel
cCoeff = cell(c,1);
cCoinv = cell(c,1);
for m=1:c,
  co = diag(K(m,:));
  cCoinv{m} = inv(co);
  cCoeff{m} = 1 / ((2*pi)^(dim/2)*det(co)^.5);
end

% go through the vectors one by one
for i=1:dlen,

  x = D(i,:);

  % compute p(x|m)
  pxm = zeros(c,1);
  for m = 1:c,
    dx     = M(m,:) - x;
    pxm(m) = cCoeff{m} * exp(-.5 * dx * cCoinv{m} * dx');
    %pxm(m) = normal(dx, zeros(1,dim), diag(K(m,:)));
  end
  pxm(isnan(pxm(:))) = 0;

  % p(x|m)
  if nargin>=2, Pdm(:,i) = pxm; end

  % P(x) = P(x|M) = sum( P(m) * p(x|m) )
  pd(i) = P*pxm;

  % p(m|x) = p(x|m) * P(m) / P(x)
  if nargout==3, pmd(:,i) = (P' .* pxm) / pd(i); end

end

return;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% subfunction normal
%
% computes probability of x when mean and covariance matrix
% of a distribution are known

function result = normal(x, mu, co)

[l dim] = size(x);
coinv   = inv(co);
coeff   = 1 / ((2*pi)^(dim/2)*det(co)^.5);
diff   = x - mu;
result = coeff * exp(-.5 * diff * coinv * diff');


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  In this script, we perform phase transition analysis
%  of Orthogonal matching pursuit.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
close all;
clear all;
clc;
rng('default');
% Create the directory for storing results
[status_code,message,message_id] = mkdir('bin');
target_file_path = 'bin/mmv_phase_transition_snr_40db_s_2.mat';
N = 256;
S = 2;
pta = spx.pursuit.PhaseTransitionAnalysis(N);
pta.SNR = 40;
% pta.NumTrials = 100;
dict_model = @(M, N) spx.dict.simple.gaussian_dict(M, N);
data_model = @(N, K) spx.data.synthetic.SparseSignalGenerator(N, K, S).gaussian;
recovery_solver = @(Phi, K, y) spx.pursuit.joint.CoSaMP(Phi, K).solve(y).Z;
pta.run(dict_model, data_model, recovery_solver);
pta.save_results(target_file_path);


function [w, infos] = subsamp_svrg(problem, options)
% Sabsampled SVRG algorithm.
%
% Inputs:
%       problem     function (cost/grad/hess)
%       options     options
% Output:
%       w           solution of w
%       infos       information
%
% References:
%       Subsampled SVRG:
%       R. Kolte, M. Erdogdu and A. Ozgur,
%       "Accelerating SVRG via second-order information,"
%       OPT2015, 2015.
%
%
% Created by H.Kasai on Oct. 28, 2016
% Modified by H.Kasai on Mar. 25, 2018

    % set dimensions and samples
    d = problem.dim();
    n = problem.samples();

    % extract options
    if ~isfield(options, 'stepsizefun')
        options.stepsizefun = @stepsize_alg;
    else
    end

    if ~isfield(options, 'tol_optgap')
        tol_optgap = 1.0e-12;
    else
        tol_optgap = options.tol_optgap;
    end

    if ~isfield(options, 'batch_size')
        batch_size = 10;
    else
        batch_size = options.batch_size;
    end

    if batch_size > n
        batch_size = n;
    end
    num_of_bachces = floor(n / batch_size);

    if ~isfield(options, 'max_epoch')
        max_epoch = 100;
    else
        max_epoch = options.max_epoch;
    end

    if ~isfield(options, 'r')
        r = inf;
    else
        r = options.r;
    end

    if r > d + 1
        r = d - 1;
    end

    if ~isfield(options, 'w_init')
        w = randn(d,1);
    else
        w = options.w_init;
    end

    if ~isfield(options, 'f_opt')
        options.f_opt = -Inf;
    end

    if ~isfield(options, 'permute_on')
        permute_on = 1;
    else
        permute_on = options.permute_on;
    end

    if ~isfield(options, 'verbose')
        verbose = false;
    else
        verbose = options.verbose;
    end

    if ~isfield(options, 'store_w')
        options.store_w = false;
    end

    % initialize
    total_iter = 0;
    epoch = 0;
    grad_calc_count = 0;
    w = options.w_init;
    num_of_bachces = floor(n / options.batch_size);

    % store first infos
    clear infos;
    [infos, f_val, optgap] = store_infos(problem, w, options, [], epoch, grad_calc_count, 0);

    %
    sample_size = round(10*r*log(d));

    % set start time
    start_time = tic();

    % display infos
    if verbose > 0
        fprintf('Sub-sampled SVRG: Epoch = %03d, cost = %.16e, optgap = %.4e\n', epoch, f_val, optgap);
    end

    % main loop
    while (optgap > tol_optgap) && (epoch < max_epoch)

        % permute samples
        if permute_on
            perm_idx = randperm(n);
        else
            perm_idx = 1:n;
        end

       % compute full gradient
        full_grad = problem.grad(w,1:n);
        % store w
        w0 = w;
        grad_calc_count = grad_calc_count + n;

        % calculated Hessian using subsamples every outer loop
        sub_indices = datasample((1:n), sample_size);
        H = problem.hess(w, sub_indices);
        [u, sigma,~] = svd(H);
        Q = u(:,1:r);
        gamma = sigma(r+1,r+1);
        sigma = sigma(1:r,1:r);
        Sing_inv_gamma = diag(1./diag(sigma)) - diag(ones(r,1)/gamma);

        for j = 1 : num_of_bachces

            % update step-size
            step = options.stepsizefun(total_iter, options);

            % calculate variance reduced gradient
            start_index = (j-1) * batch_size + 1;
            indice_j = perm_idx(start_index:start_index+batch_size-1);
            grad = problem.grad(w, indice_j);
            grad_0 = problem.grad(w0, indice_j);
            grad_est = full_grad + grad - grad_0;

            % update w
            v = -Q*(Sing_inv_gamma)*(Q' * grad_est) - (1/gamma)*grad_est;
            w = w + step * v;

            % proximal operator
            if ismethod(problem, 'prox')
                w = problem.prox(w, step);
            end

            total_iter = total_iter + 1;
        end

        % measure elapsed time
        elapsed_time = toc(start_time);

        % count gradient evaluations
        grad_calc_count = grad_calc_count + j * batch_size + sample_size;
        epoch = epoch + 1;

        % store infos
        [infos, f_val, optgap] = store_infos(problem, w, options, infos, epoch, grad_calc_count, elapsed_time);

        % display infos
        if verbose > 0
            fprintf('Sub-sampled SVRG: Epoch = %03d, cost = %.16e, optgap = %.4e\n', epoch, f_val, optgap);
        end
    end

    if optgap < tol_optgap
        fprintf('Optimality gap tolerance reached: tol_optgap = %g\n', tol_optgap);
    elseif epoch == max_epoch
        fprintf('Max epoch reached: max_epochr = %g\n', max_epoch);
    end
end


function cvx_optval = sum_largest( x, k, dim )

%SUM_LARGEST Sum of the largest k values of a vector.
%   For a real vector X and an integer k between 1 and length(X) inclusive,
%   y = SUM_LARGEST(X,k) is the sum of the k largest elements of X; e.g.,
%       temp = sort( x )
%       y = sum( temp( 1 : k ) )
%   If k=1, then SUM_LARGEST(X,k) is equivalent to MAX(X); if k=length(X),
%   then SUM_LARGEST(X,k) is equivalent to SUM(X).
%
%   Both X and k must be real, and k must be a scalar. But k is not, in
%   fact, constrained to be an integer between 1 and length(X); the
%   function is extended continuously and logically to all real k. For
%   example, if k <= 0, then SUM_LARGEST(X,k)=0. If k > length(X), then
%   SUM_LARGEST(X,k)=SUM(X). Non-integer values of k interpolate linearly
%   between their integral neighbors.
%
%   For matrices, SUM_LARGEST(X,k) is a row vector containing the
%   application of SUM_LARGEST to each column. For N-D arrays, the
%   SUM_LARGEST operation is applied to the first non-singleton dimension
%   of X.
%
%   SUM_LARGEST(X,k,DIM) performs the operation along dimension DIM of X.
%
%   Disciplined convex programming information:
%       SUM_LARGEST(X,...) is convex and nondecreasing in X. Thus, when
%       used in CVX expressions, X must be convex (or affine). k and DIM
%       must both be constant.

%
% Check arguments
%

narginchk(2,3);
if ~isreal( x ),
    error( 'First argument must be real.' );
elseif ~isnumeric( k ) || ~isreal( k ) || length( k ) ~= 1,
    error( 'Second argument must be a real scalar.' );
elseif nargin < 3 || isempty( dim ),
    dim = cvx_default_dimension( size( x ) );
elseif ~cvx_check_dimension( dim, false ),
    error( 'Third argument, if supplied, must be a positive integer.' );
end

%
% Determine output size
%

sx = size( x );
nd = max( dim, length( sx ) );
sx = [ sx, ones( 1, dim - nd ) ];
sy = sx;
sy( dim ) = 1;

%
% Compute results
%

if k <= 0,

    cvx_optval = zeros( sy );

elseif k <= 1,

    cvx_optval = k * max( x, [], dim );

elseif k >= sx( dim ),

    cvx_optval = sum( x, dim );

else

    ck = ceil( k );
    x = sort( x, dim );
    ndxs = cell( 1, nd );
    [ ndxs{:} ] = deal( ':' );
    ndxs{ dim } = size( x, dim ) - ( 0 : ck - 1 );
    x = x( ndxs{ : } );
    if k ~= ck,
        ndxs{ dim } = ck;
        x( ndxs{ : } ) = ( k - floor( k ) ) * x( ndxs{ : } );
    end
    cvx_optval = sum( x, dim );

end

% Copyright 2005-2016 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function triangle_plots ( )

%*****************************************************************************80
%
%% TRIANGLE_PLOTS plots the surface and the R(Theta) function for a triangle.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 May 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TRIANGLE_PLOTS:\n' );
  fprintf ( 1, '  For a 2D triangle defined by a 0/1 characteristic function,\n' );
  fprintf ( 1, '  plot the surface, and R(Theta),\n' );
  fprintf ( 1, '  using a centered point, and then an offcentered point.\n' );
%
%  1): Plot the surface, using a centered base point.
%
  n = 61;
  theta = linspace ( 0.0, 2.0 * pi, n );

  f = zeros(n,1);
  x0 = [ 2.0 / 3.0, 1.0 / 3.0 ];
  for i = 1 : n
    r(i) = bisect_characteristic ( x0, theta(i), @triangle_characteristic );
  end
  x = x0(1) + r(1:n) .* cos ( theta(1:n) );
  y = x0(2) + r(1:n) .* sin ( theta(1:n) );

  xc = x0(1) + 0.5 .* cos ( theta(1:n) );
  yc = x0(2) + 0.5 .* sin ( theta(1:n) );

  clf
  hold on
  plot ( x0(1), x0(2), 'b.', 'MarkerSize', 50 );
  plot ( xc, yc, 'b-', 'LineWidth', 2 );
  plot ( x, y, 'ro', 'LineWidth', 3 );
  axis equal
  grid on
  xlabel ( '<---X--->', 'FontSize', 24 );
  ylabel ( '<---Y--->', 'FontSize', 24 );
  title ( 'Triangle transition surface', 'FontSize', 24)
  hold off
  filename =  'triangle_centered_surface.png';
  print ( '-dpng', filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Created plotfile "%s".\n', filename );
%
%  2): Plot R as a function of Theta for a centered base point.
%
  plot ( theta, r, 'LineWidth', 3 );
  xlabel ( '<---Theta--->', 'FontSize', 24 )
  ylabel ( '<---R(Theta)--->', 'FontSize', 24 )
  title ( 'R(Theta) from base point to surface', 'FontSize', 24 )
  grid on
  filename = 'triangle_centered_plot.png';
  print ( '-dpng', filename );
  fprintf ( 1, '  Created plotfile "%s".\n', filename );
%
%  3): Plot the surface, using an offcentered base point.
%
  n = 61;
  theta = linspace ( 0.0 , 2.0 * pi, n );

  f = zeros(n,1);
  x0 = [ 1.0, 1.50 ];
  for i = 1 : n
    r(i) = bisect_characteristic ( x0, theta(i), @triangle_characteristic );
  end

  x = x0(1) + r(1:n) .* cos ( theta(1:n) );
  y = x0(2) + r(1:n) .* sin ( theta(1:n) );
%
%  Coordinates of nominal triangle around (x0,y0), to
%  suggest coordinate system.
%
  xc = x0(1) + 0.5 .* cos ( theta(1:n) );
  yc = x0(2) + 0.5 .* sin ( theta(1:n) );

  clf
  hold on
  plot ( x0(1), x0(2), 'b.', 'MarkerSize', 50 );
  plot ( xc, yc, 'b-', 'LineWidth', 2 );
  plot ( x, y, 'ro', 'LineWidth', 3 );
  axis equal
  grid on
  xlabel ( '<---X--->', 'FontSize', 24 );
  ylabel ( '<---Y--->', 'FontSize', 24 );
  title ( 'Triangle transition surface', 'FontSize', 24 )
  hold off
  filename = 'triangle_offcentered_surface.png';
  print ( '-dpng', filename );
  fprintf ( 1, '  Created plotfile "%s".\n', filename );
%
%  4): Plot R as a function of Theta for an offcentered base point.
%
  plot ( theta, r, 'LineWidth', 3 );
  xlabel ( '<---Theta--->', 'FontSize', 24 )
  ylabel ( '<---R(Theta)--->', 'FontSize', 24 )
  title ( 'R(Theta) from base point to surface', 'FontSize', 24 )
  grid on
  filename = 'triangle_offcentered_plot.png';
  print ( '-dpng', filename );
  fprintf ( 1, '  Created plotfile "%s".\n', filename );

  return
end


function x = dvand ( n, alpha, b )

%*****************************************************************************80
%
%% DVAND solves a Vandermonde system A' * x = b.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 November 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Ake Bjorck, Victor Pereyra,
%    Solution of Vandermonde Systems of Equations,
%    Mathematics of Computation,
%    Volume 24, Number 112, October 1970, pages 893-903.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, real ALPHA(N), the parameters that define the matrix.
%    The values should be distinct.
%
%    Input, real B(N), the right hand side of the linear system.
%
%    Output, real X(N), the solution of the linear system.
%
  x(1:n) = b(1:n);

  for k = 1 : n - 1
    for j = n : -1 : k + 1
      x(j) = ( x(j) - x(j-1) ) / ( alpha(j) - alpha(j-k) );
    end
  end

  for k = n - 1 : -1 : 1
    for j = k : n - 1
      x(j) = x(j) - alpha(k) * x(j+1);
    end
  end

  return
end


function points = intersectLineSphere(line, sphere, varargin)
    %INTERSECTLINESPHERE Return intersection points between a line and a sphere
    %
    %   PTS = intersectLineSphere(LINE, SPHERE);
    %   Returns the two points which are the intersection of the given line and
    %   sphere.
    %   LINE   : [x0 y0 z0  dx dy dz]
    %   SPHERE : [xc yc zc  R]
    %   PTS     : [x1 y1 z1 ; x2 y2 z2]
    %   If there is no intersection between the line and the sphere, return a
    %   2-by-3 array containing only NaN.
    %
    %   Example
    %     % draw the intersection between a sphere and a collection of parallel
    %     % lines
    %     sphere = [50.12 50.23 50.34 40];
    %     [x, y] = meshgrid(10:10:90, 10:10:90);
    %     n = numel(x);
    %     lines = [x(:) y(:) zeros(n,1) zeros(n,2) ones(n,1)];
    %     figure; hold on; axis equal;
    %     axis([0 100 0 100 0 100]); view(3);
    %     drawSphere(sphere);
    %     drawLine3d(lines);
    %     pts = intersectLineSphere(lines, sphere);
    %     drawPoint3d(pts, 'ro');
    %
    %   See also
    %   spheres, circles3d, intersectPlaneSphere
    %
    %   ---------
    %   author : David Legland
    %   INRA - TPV URPOI - BIA IMASTE
    %   created the 18/02/2005.
    %
    %   HISTORY
    %   2011-06-21 bug for tangent lines, add tolerance
    % Process input arguments
    % check if user-defined tolerance is given
    tol = 1e-14;
    if ~isempty(varargin)
        tol = varargin{1};
    end
    % difference between centers
    dc = bsxfun(@minus, line(:, 1:3), sphere(:, 1:3));
    % equation coefficients
    a = sum(line(:, 4:6) .* line(:, 4:6), 2);
    b = 2 * sum(bsxfun(@times, dc, line(:, 4:6)), 2);
    c = sum(dc.*dc, 2) - sphere(:,4).*sphere(:,4);
    % solve equation
    delta = b.*b - 4*a.*c;
    % initialize empty results
    points = NaN * ones(2 * size(delta, 1), 3);
    % process couples with two intersection points
    % proces couples with two intersection points
    inds = find(delta > tol);
    if ~isempty(inds)
        % delta positive: find two roots of second order equation
        u1 = (-b(inds) -sqrt(delta(inds))) / 2 ./ a(inds);
        u2 = (-b(inds) +sqrt(delta(inds))) / 2 ./ a(inds);

        % convert into 3D coordinate
        points(inds, :) = line(inds, 1:3) + bsxfun(@times, u1, line(inds, 4:6));
        points(inds+length(delta),:) = line(inds, 1:3) + bsxfun(@times, u2, line(inds, 4:6));
    end
    % process couples with one intersection point
    % proces couples with two intersection points
    inds = find(abs(delta) < tol);
    if ~isempty(inds)
        % delta around zero: find unique root, and convert to 3D coord.
        u = -b(inds) / 2 ./ a(inds);

        % convert into 3D coordinate
        pts = line(inds, 1:3) + bsxfun(@times, u, line(inds, 4:6));
        points(inds, :) = pts;
        points(inds+length(delta),:) = pts;
    end
end


% Author: Julio Zaragoza
% The University of Adelaide, Adelaide, South Australia

% Example: using the CACC function for discretizing continuos data.
%
% Note that such data appeared in the paper:
%  'A Discretization Algorithm based on Class-Attribute Contingency Coefficient' (CACC),
%  by Sheng-Jung Tsai, Shien-I Lee and Wei-Pang Yang, Information Sciences, Elsevier, 2008
%  and my results are the same as those reported in the paper.

contdata = [3,1;    % here 1 (in the second column) stands for class 'Care'
            5,1;    % 2 stands for class 'Edu' and 3 stands for class 'Work'.
            6,1;
            15,2;
            17,2;
            21,2;
            35,3;
            45,3;
            46,3;
            51,2;
            56,2;
            57,2;
            66,1;
            70,1;
            71,1];
[ discdata,discvalues,discscheme ] = cacc(contdata)


function [cap res] = pll_synth_3rd_order(ipump, vco_sensitivity, fout, fcomp, bandwidth, pm)

% Synthesizes PLL loop components for a 3rd order system using the
% topology shown below.
%
% Input - ipump is the charge pump current in Amperes
%       - vco_sensitivity is the VCO sensitivity in Hertz/Volt
%       - fout is the output frequency in Hertz
%       - fcomp is the comparison frequency in Hertz
%       - bandwidth is the open loop bandwidth in Hertz
%       - pm is the phase margin in degrees
% Output - cap is the capacitors of the loop in Farads [C1, C2, C3, 0]
%        - res is the resistors of the  loop in Ohms [ R2, R3, 0]
%
%
% The method used here is derived from that presented in Dean Banerjee's
% Book "PLL Performance, Simulation, and Design" 4th Ed available at
% National Semiconductors site www.national.com.  Most of the work is
% derived from Chapter 22 pp.178-185.
%
% No closed form solution exists for 3rd
% order systems, therefore some simplification of the model has been made
% to derive the results.  Due to the simplifications the results will
% also not always be 100% accurate.  It is recommended that the
% results are verified using the pll_simulation.m script, which does not
% use a simplified model.
% Loop Topology
%
%        +  _____                                   ______
% fcomp -->|Phase|-----------------R3---------------| VCO |---->fout
%          |Det. |     |      |         |           |     |  |
%           -----      |      |         |            -----   |
%            ^ -      C1     R2        C3                    |
%            |         |      |         |                    |
%            |        GND    C2        GND                   |
%            |                |                              |
%            |               GND         _______             |
%            ----------------------------| 1/N |--------------
%                                        -------
%
%   Author: Ben Gilbert
%   Homepage: http://nicta.com.au/people/gilbertb
%   Email: ben.gilbert (wibble) nicta.com.au
%   (c) 2009 by National ICT Australia (NICTA)
%   %

%% Design Parameters
% The following values will give sensible results but not necessarily
% optimal
T31 = 0.6; % T3/T1 ratio of time constants
gamma = 1.0; % optimization factor p172 and pp220-227 of Banerjee

% Conversion of parameters to more convenient units
Kpd = ipump/2/pi; % phase detector gain
Kvco = vco_sensitivity*2*pi; % vco gain
omega = 2*pi* bandwidth; % open loop bandwidth in radians/sec

%% Synthesis
%% Find T1 using numerical methods
t1 = (sec(pm*pi/180)-tan(pm*pi/180))/(omega*(1+T31)); % approximate value for T1

% T1 is the root of this equation
num1 = @(t) atan(gamma./(omega.*t*(1+T31)))-atan(omega.*t)-atan(omega.*t*T31)- pm*pi/180;
T1 = fzero(num1,t1);% find root numerically

%% Find T2 and T3
T3 = T1*T31;
T2 = gamma/omega^2/(T1+T3);

%% Solving for Loop Components
A0 = Kpd * Kvco * fcomp/fout / omega^2 * sqrt((1+omega^2*T2^2)/(1+omega^2*T1^2)/(1+omega^2*T3^2));
A1 = A0*(T1+T3);
A2 = A0*T1*T3;

C1 = A2/T2^2*(1+sqrt(1+T2/A2*(T2*A0-A1)));
C3 = (-T2^2*C1^2+T2*A1*C1-A2*A0)/(T2^2*C1-A2);
C2 = A0-C1-C3;
R2 = T2/C2;
R3 = A2/C1/C3/T2;

cap = [C1 C2 C3 0];
res = [R2 R3 0];


function [U,s,V] = csvd(A,tst)
%CSVD Compact singular value decomposition.
%
% s = csvd(A)
% [U,s,V] = csvd(A)
% [U,s,V] = csvd(A,'full')
%
% Computes the compact form of the SVD of A:
%    A = U*diag(s)*V',
% where
%    U  is  m-by-min(m,n)
%    s  is  min(m,n)-by-1
%    V  is  n-by-min(m,n).
%
% If a second argument is present, the full U and V are returned.

% Per Christian Hansen, IMM, 06/22/93.

if (nargin==1)
  if (nargout > 1)
    [m,n] = size(A);
    if (m >= n)
      [U,s,V] = svd(full(A),0); s = diag(s);
    else
      [V,s,U] = svd(full(A)',0); s = diag(s);
    end
  else
    U = svd(full(A));
  end
else
  if (nargout > 1)
    [U,s,V] = svd(full(A)); s = diag(s);
  else
    U = svd(full(A));
  end
end


% Directly plots a simple figure using plotPub function.

clear all;
addpath('../lib');

%% lets plot 3 cycles of 50Hz AC voltage
f = 50;
Vm = 10;
phi = 0;

% generate the signal
t = [0:0.0001:3/f];
th = 2*pi*f*t;
v = Vm*sin(th+phi);

%% plot now
plotx{1} = t*1E3; %convert time in ms and create a cell array
ploty{1} = v; % assign v to a cell array
opt.XLabel = 'Time, t (ms)'; % xlabel
opt.YLabel = 'Voltage, V (V)'; %ylabel

% Save? comment the following line if you do not want to save
opt.FileName = 'plotDirectSimple1.eps';

% create the plot
plotPub(plotx, ploty, 1, opt);


% test for landmark heuristic computation

n = 300;

rep = 'images/landmark_distance/';
if exist(rep)~=7
    mkdir(rep);
end
rep_eps = [rep 'eps/'];
if exist(rep_eps)~=7
    mkdir(rep_eps);
end

name = 'bump';
name = 'map';
name = 'mountain';
name = 'stephanodiscusniagarae';
name = 'cavern';
name = 'gaussian';
name = 'road2';
name = 'constant';

save_images = 0;

nbr_landmarks = 10;
landmark_init = 'rand';
landmark_init = 'farthest';

[M,W] = load_potential_map(name, n);

% just a string representing the number of landmarks points
num_str = num2string_fixeddigit(nbr_landmarks, 3);
marker_size = 20;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% pick starting point
if ~exist('start_points')
    [start_points,end_points] = pick_start_end_point(M);
end

disp('Performing front propagation.');
[D,S] = perform_fast_marching(W, start_points);

% compute landmark
disp('Initializing Landmarks');
switch lower(landmark_init)
    case 'rand'
        landmark = floor( rand(2, nbr_landmarks)*(n-1) )+1;
    case 'farthest'
        landmark = perform_farthest_point_sampling( W, [], nbr_landmarks );
    case 'farthestboundary'
        landmark = perform_farthest_point_sampling_boundary( W, landmark, 1 );
end

% compute distance to landmark
DL = zeros(n,n,nbr_landmarks);
for i=1:nbr_landmarks
    disp('Performing front propagation.');
    [DL(:,:,i),S] = perform_fast_marching(W, landmark(:,i));
end

% compute the approximated distance
[D1,Z] = compute_heuristic_landmark(DL,start_points);

clf;
if ~save_images
    subplot(2,2,1);
end
hold on;
imagesc(W');
plot_scattered(landmark);
axis tight; axis square; axis off;
hold off;
title('Potential with landmarks');
colormap jet(256);
if save_images
    axis image; axis off;
    str = [name '_' num_str 'landmarks_' landmark_init '_potential'];
    saveas(gcf, [rep str '.png'], 'png');
    saveas(gcf, [rep_eps str '.eps'], 'eps');
end

if ~save_images
    subplot(2,2,3);
end
hold on;
imagesc(D');
contour(D', 'k');
axis tight; axis square; axis off;
plot(start_points(1,:), start_points(2,:), 'kx', 'MarkerSize', 10);
hold off;
title('Distance to point');
colormap jet(256);
if save_images
    axis image; axis off;
    str = [name '_' num_str 'landmarks_' landmark_init '_realdist'];
    saveas(gcf, [rep str '.png'], 'png');
    saveas(gcf, [rep_eps str '.eps'], 'eps');
end

if ~save_images
    subplot(2,2,4);
end
hold on;
imagesc(D1');
contour(D1', 'k');
axis tight; axis square; axis off;
plot(start_points(1,:), start_points(2,:), 'kx', 'MarkerSize', 10);
plot(landmark(1,:), landmark(2,:), 'k.', 'MarkerSize', marker_size);
hold off;
title('Approximate distance to point');
colormap jet(256);
if save_images
    axis image; axis off;
    str = [name '_' num_str 'landmarks_' landmark_init '_approxdist'];
    saveas(gcf, [rep str '.png'], 'png');
    saveas(gcf, [rep_eps str '.eps'], 'eps');
end

% str = [rep name '_' num2str(nbr_landmarks) 'landmarks_' landmark_init];
% saveas(gcf, [str '.png'], 'png');

return;

% plot the influence zones
clf;
hold on;
imagesc(Z');
plot_scattered(start_points);
axis off;
hold off;
title('Approximate distance to point');
colormap jet(256);

return;

%%% ANIMATION! %%%
r = 0.3;
nb = 200;
i = 0;
for theta = (0:1/nb:1-1/nb)*2*pi
    i = i+1;
    start_points = 0.5 + r * [cos(theta);sin(theta)];
    start_points = round( start_points*(n-1) )+1;
    [D1,Z] = compute_distance_landmark(DL,start_points);
    clf;
    imagesc(Z);
    axis image;
    axis off;
    colormap jet(256);

    num_str = num2str(i);
    if i<10
        num_str = ['0' num_str];
    end
    if i<100
        num_str = ['0' num_str];
    end
    str = ['anim/' name '_' num_str];
    saveas(gcf, [str '.png'], 'png');
end


function varargout = ellipsoid( a, b, c )
%ELLIPSOID  Generate an ellipsoid-like surface. (Not necessarily an ellipsoid!)
%   ELLIPSOID(A,B,C), where A, B, and C are CHEBFUN2 objects on the domain [0
%   pi]x[0 2*pi] plots the "ELLIPSOID" of semi axis lengths A(th,phi),
%   B(th,phi), and C(th,phi).
%
%   [X Y Z]=ELLIPSOID(A,B,C) returns X, Y, and Z as CHEBFUN2 objects such that
%   SURF(X,Y,Z) plots an ELLIPSOID of semi axis lengths A(th,phi), B(th,phi),
%   and C(th,phi).
%
%   F = ELLIPSOID(A,B,C) returns the CHEBFUN2V representing the ELLIPSOID
%   SURF(F) plots the ELLIPSOID.
%
%   Omitting output arguments causes the ELLIPSOID command to be displayed with
%   a SURF command and no outputs are returned.
%
% For the ellipsoid:
%   a = chebfun2(@(th,phi) 1+0*th,[0 pi 0 2*pi]);
%   F = ellipsoid(a,2*a,3*a); surf(F)
%
% For a badly shaped ship:
%   a = chebfun2(@(th,phi) th,[0 pi 0 2*pi])
%   F = ellipsoid(a,2*a,cos(2*a)+.25); surf(F)
%
% See also SPHERE, CYLINDER.

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

if ( nargin < 3 )
    error('CHEBFUN:CHEBFUN2:ellipsoid:inputs', 'Incorrect input arguments.');
end

% ELLIPSOID with axis lengths a(th,phi), b(th,phi), c(th,phi).
dom = [0 pi 0 2*pi];
th = chebfun2(@(th, phi) th, dom);
phi = chebfun2(@(th, phi) phi, dom);

x = a.*sin(th).*cos(phi);
y = b.*sin(th).*sin(phi);
z = c.*cos(th);

if ( nargout == 0 )
    surf(x, y, z)
    axis equal
elseif ( nargout == 1 )
    varargout = { [x ; y ; z] };
else
    varargout = { x, y, z };
end

end


[X,Y,Z] = meshgrid(-2:.07:2,-2:.07:2,-2:.07:2);
V = X.*exp(-X.^2-Y.^2-Z.^2);


function pred=lengauer_tarjan_dominator_tree(A,u,varargin)
% LENGAUER_TARJAN_DOMINATOR_TREE Compute a dominator tree for a graph.
%
% A dominator tree encodes dominates relations.  A vertex u dominates a
% vertex v if all paths to v must go through u.  In an undirected graph,
% this means that if u dominates v, then v will be disconnected from the
% graph if we remove u and start the search at the root.  A dominator tree
% is rooted at a particular vertex and the entire graph must be reachable
% from that vertex.  (Historically, these were called flowgraphs.)
%
% p = lengauer_tarjan_dominator_tree(A,u) returns the predecessor array for
% the dominator tree rooted at u.
%
% The runtime is O((V+E) log (V+E)) and the algorithm works on unweighted,
% directed graphs.
%
% ... = lengauer_tarjan_dominator_tree(A,...) takes a set of
% key-value pairs or an options structure.  See set_matlab_bgl_options
% for the standard options.
%   There are no additional options for this function.
%
% Note: this function does not depend upon the non-zero values of A, but
% only uses the non-zero structure of A.
%
% Example:
%   load('graphs/dominator_tree_example.mat');
%   p=lengauer_tarjan_dominator_tree(A,1);

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

%% History
%  2007-07-13: Initial version
%%

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

pred = dominator_tree_mex(A,u);


function [ m, d ] = easter_ds ( y )

%*****************************************************************************80
%
%% EASTER_DS computes the month and day of Easter for a Gregorian year.
%
%  Example:
%
%    Input:
%
%      Y = 2000
%
%    Output:
%
%      M = 4
%      D = 23
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 June 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Peter Duffett-Smith,
%    Practical Astronomy With Your Calculator,
%    Third Edition,
%    Cambridge University Press, 1996,
%    ISBN: 0-521-35699-7,
%    LC: QB62.5.D83.
%
%  Parameters:
%
%    Input, integer Y, the year, which must be 1583 or greater.
%    (The formula is only valid for years after the Gregorian calendar
%    was adopted.)
%
%    Output, integer M, D, the month and day of Easter.
%
  if ( y <= 0 )
    m = -1;
    d = -1;
    return
  end

  a = year_to_golden_number ( y );

  a = a - 1;

  b = floor ( y / 100 );
  c = mod ( y, 100 );

  dd = floor ( b / 4 );
  e = mod ( b, 4 );

  f = floor ( ( b + 8 ) / 25 );
  g = floor ( ( b - f + 1 ) / 3 );
  h = mod ( 19 * a + b - dd - g + 15, 30 );

  i = floor ( c / 4 );
  k = mod ( c, 4 );

  l = mod ( 32 + 2 * e + 2 * i - h - k, 7 );
  mm = floor ( ( a + 11 * h + 22 * l ) / 451 );

  m = floor ( ( h + l - 7 * mm + 114 ) / 31 );
  d = mod ( h + l - 7 * mm + 114, 31 ) + 1;

  return
end


function KC = slcenkernel(K0, K, w)
%SLCENKERNEL Compute the centralized kernel matrix
%
% $ Syntax $
%   - KC = slcenkernel(K0)
%   - KC = slcenkernel(K0, [], w)
%   - KC = slcenkernel(K0, K)
%   - KC = slcenkernel(K0, K, w)
%
% $ Arguments $
%   - K0:       the gram matrix of the referenced samples
%   - K:        the kernel matrix for target samples
%   - w:        the weights for the referenced samples
%   - KC:       the centralized kernel matrix.
%
% $ Description $
%   - KC = slcenkernel(K0) compute the centralized kernel matrix from
%     the original kernel gram matrix K0.
%
%   - KC = slcenkernel(K, [], w) compute the centralized kernel gram
%     matrix from the original kernel gram matrix K0. The mean feature
%     is obtained with the weights for referenced samples, given by w.
%
%   - KC = slcenkernel(K0, K) compute the centralized kernel matrix
%     for target samples, with the original gram matrix for referenced
%     samples K0 and the kernel matrix for target samples w.r.t the
%     referenced samples K given.
%
%   - KC = slcenkernel(K0, K, w) compute the centralied kernel matrix
%     for target samples, with feature mean computed in a weighted
%     manner.
%
% $ Remarks $
%   -# For original kernel matrix K, it is defined as
%      K(i, j) = <phi(i), phi(j)>, given that phi(i) and phi(j) are
%      the feature map of the samples x(i) and x(j) respectively.
%      Then the centralized kernel matrix is defined as
%      KC(i, j) = <phi(i) - mean_phi, phi(j) - mean_phi>, where
%      mean_phi is the mean of all referenced feature maps. If w
%      is specified mean_phi is given by weighted mean.
%      Kernel centralization plays an important role in many
%      kernelized algorithms such as Kernel PCA.
%
%   -# Suppose the mean feature map is defined by
%      mean_phi = sum_i w_i phi(i).
%      Then the centralized kernel gram matrix can be written as
%      KC = K - 1 * (w^T * K) - (K * w) * 1^T + 1 * (w^T * K * w) * 1^T.
%      It can be easily shown that the mean of columns (rows) of KC is
%      a zero vector. Thus, centralize the centralized kernel matrix
%      would keep the input unchanged.
%
%   -# Instead of applying the formula given above, the function
%      implements a more efficient computational routine by reducing
%      the redundant computations.
%
% $ History $
%   - Created by Dahua Lin on May 2nd, 2006
%   - Modified by Dahua Lin on Sep 10, 2006
%       - use sladdrowcols to replace sladd to increase efficiency
%

%% parse and verify input arguments

% for K0
n0 = size(K0, 1);
if ndims(K0) ~= 2 || size(K0, 2) ~= n0;
    error('sltoolbox:invaliddims', ...
        'K0 should be a 2D square matrix');
end

% for K
if nargin < 2 || isempty(K)
    K = K0;
else
    if ndims(K) ~= 2
        error('sltoolbox:invaliddims', ...
            'K should be a 2D matrix');
    end
    if size(K, 1) ~= n0
        error('sltoolbox:sizmismatch', ...
            'Size inconsistency between K0 and K');
    end
end

% for w
if nargin < 3 || isempty(w)
    isweighted = false;
else
    if ~isequal(size(w), [1, n0])
        error('sltoolbox:sizmismatch', ...
            'Size inconsistency between K0 and w');
    end
    isweighted = true;
end

%% compute

% Steps:
% 1. compute v1: mean row vector of K (1 x n)
% 2. compute v2: mean column vector of K0 (n x 1)
% 3. compute s3: mean value of of all elements of K0 (1 x 1)
% 4. KC = K - expand(v1) - expand(v2) + s3

if ~isweighted  % non-weighted case

    v1 = sum(K, 1) * (1 / n0);
    v2 = sum(K0, 2) * (1 / n0);
    s3 = sum(v2) * (1 / n0);

else            % weighted case

    w = w / sum(w);     % normalize the weights

    v1 = w * K;
    v2 = K0 * w';
    s3 = w * v2;

end

KC = sladdrowcols(K, -v1, -v2+s3);


function data = create_synthetic_dataset(data)
% create_synthetic_dataset creates test data for running nldr algorithms.
%
% inputs:
%    data      a struct describing the test data
%              .dataset the number of the example, see code for more infos
%              .n       the number of data points (default=400)
%              .state   the initial state for the random numbers (default=0)
%              .noise   the variance of Gaussian noise to add (default=0)
%              other options for some of the data sets (see code)
%              alternatively, data = 1 chooses the dataset directly,
%              the number of points defaults to 1000
%
% outputs:
%    data      a struct containing .x the generated data, each column is
%              a data point, and other stuff:
%              .z     the "correct" embedding
%              .e     some random noise of same dimensionality
%              .x_noisefree  the noisefree version of .x, i.e.
%                     .x = .xnoise_free + sqrt(.noise) * .e
%
% Adapted from create.m, originally written by
% (c) Stefan Harmeling, 2006
% using the examples of the original LLE and ISOMAP code.

if ~isfield(data, 'dataset'),
  number = data;
  clear data
  data.dataset = number;
end
if ~isfield(data, 'n'), data.n = 400; end
if ~isfield(data, 'noise'), data.noise = 0.0; end
if ~isfield(data, 'state'), data.state = 0; end

% set the randomness
rand('state', data.state);
randn('state', data.state);

data.typ = 'data';
switch data.dataset
 case 0 % "swiss roll with hole"
  data.name = 'swiss roll with hole';
  n = data.n;
  a = 1;   % swiss roll goes from a*pi to b*pi
  b = 4;
  y = rand(2,n);
  % punch a rectangular hole at the center
  l1 = 0.05; l2 = 0.15;
  y = y - 0.5;
  ok = find((abs(y(1,:))>l1) | (abs(y(2,:))>l2));
  i = length(ok);
  y(:, 1:i) = y(:, ok);
  while (i<n)
    p = rand(2,1) - 0.5;
    if (abs(p(1))>l1) || (abs(p(2))>l2)
      i = i + 1;
      y(:,i) = p;
    end
  end
  y = y + 0.5;
  tt = (b-a)*y(1,:) + a;
  tt = pi*tt;
  height = 21*y(2,:);
  data.col = tt;
  data.x = [tt.*cos(tt); height; tt.*sin(tt)];
  data.z = [tt; height]; % the ground truth
  data.az = -4;
  data.el = 13;

 case -1 % "swiss roll" dataset extracted from LLE's swissroll.m
  data.name = 'uniform swiss roll';
  n = data.n;
  a = 1;   % swiss roll goes from a*pi to b*pi
  b = 4;
  y = rand(2,n);
  data.z = y;  % the ground truth
  switch 1
   case 1
    % uniform distribution along the manifold (in data space)
    tt = sqrt((b*b-a*a)*y(1,:)+a*a);
   case 2
%    error('do not use this case')
    % nonuniform distribution along the manifold (in data space)
    tt = (b-a)*y(1,:) + a;
  end
  tt = pi*tt;
  % now tt should go from a*pi to b*pi
  height = 21*y(2,:);
  data.col = tt;
  data.x = [tt.*cos(tt); height; tt.*sin(tt)];
  data.az = -4;
  data.el = 13;

 case 1 % "swiss roll (uniform in embedding space)"
  % dataset extracted from LLE's swissroll.m
  data.name = 'classic swiss roll';
  n = data.n;
  a = 1;   % swiss roll goes from a*pi to b*pi
  b = 4;
  y = rand(2,n);
  tt = (b-a)*y(1,:) + a;
  tt = pi*tt;
  height = 21*y(2,:);
  data.col = tt;
  data.x = [tt.*cos(tt); height; tt.*sin(tt)];
  data.z = [tt; height]; % the ground truth
  data.az = -4;
  data.el = 13;

 case 11 % "undersampled swiss roll"
  % dataset extracted from LLE's swissroll.m
  data.name = 'undersampled swiss roll';
  data.n = 100;
  n = data.n;
  a = 1;   % swiss roll goes from a*pi to b*pi
  b = 4;
  y = rand(2,n);
  tt = (b-a)*y(1,:) + a;
  tt = pi*tt;
  height = 21*y(2,:);
  data.col = tt;
  data.x = [tt.*cos(tt); height; tt.*sin(tt)];
  data.z = [tt; height]; % the ground truth
  data.az = -4;
  data.el = 13;

 case 12 % "swiss roll"
  % dataset extracted from LLE's swissroll.m
  data.name = 'classic swiss roll';
  data.n = 400;
  n = data.n;
  a = 1;   % swiss roll goes from a*pi to b*pi
  b = 4;
  y = rand(2,n);
  tt = (b-a)*y(1,:) + a;
  tt = pi*tt;
  height = 21*y(2,:);
  data.col = tt;
  data.x = [tt.*cos(tt); height; tt.*sin(tt)];
  data.z = [tt; height]; % the ground truth
  data.az = -4;
  data.el = 13;

 case 2 % "scurve" dataset extracted from LLE's scurve.m
  data.name = 'scurve';
  n = data.n;
  % I added 'ceil' and 'floor' to account for the case that n is odd
  angle = pi*(1.5*rand(1,ceil(n/2))-1); height = 5*rand(1,n);
  data.x = [[cos(angle), -cos(angle(1:floor(n/2)))]; height;[ sin(angle), 2-sin(angle)]];
  data.col = [angle, 1.5*pi + angle];
  data.z = [angle, 1.5*pi+angle; height]; % the ground truth

 case 3 % "square" dataset, a uniformly sampled 2D square randomly
         % rotated into higher dimensions
  data.name = 'square';
  n = data.n;
  d = 2;     % intrinsic dimension
  % optional parameter for dataset==3
  % data.D      dimension of the data
  if ~isfield(data, 'D'), data.D = 3; end
  % generate random rotation matrix
  D = data.D;
  A = randn(D, D);
  options.disp = 0;
  [R, dummy] = eigs(A*A', d, 'LM', options);
  tt = rand(d, n);
  data.col = tt(1,:);
  data.x = R*tt;
  data.z = tt;   % the ground truth
  data.az = 7;
  data.el = 40;

 case 4 % spiral: two dimensional "swiss roll"
  data.name = 'spiral';
  n = data.n;
  tt = (3*pi/2)*(1+2*rand(1, n));
  data.col = tt;
  data.x = [tt.*cos(tt); tt.*sin(tt)];
  data.z = tt; % the ground truth

 case -4 % spiral: two dimensional "swiss roll"
  data.name = 'noisy spiral';
  n = data.n;
  tt = (3*pi/2)*(1+2*rand(1, n));
  data.col = tt;
  data.x = [tt.*cos(tt); tt.*sin(tt)];
  data.x = data.x + randn(size(data.x));
  data.z = tt; % the ground truth

 case 5 % hole: a dataset with a hole
  data.name = 'hole';
  n = data.n;
  data.x = rand(2,n) - 0.5;
  % punch a rectangular hole at the center
  l1 = 0.2; l2 = 0.2;
  ok = find((abs(data.x(1,:))>l1) | (abs(data.x(2,:))>l2));
  i = length(ok);
  data.x(:, 1:i) = data.x(:, ok);
  while (i<n)
    p = rand(2,1) - 0.5;
    if (abs(p(1))>l1) || (abs(p(2))>l2)
      i = i + 1;
      data.x(:,i) = p;
    end
  end
  data.col = data.x(2,:);
  data.z = data.x;

 case 6 % P : taken from Saul's slides
  % note that for k=20, isomap and lle work fine which is very different
  % from the plots that Saul showed in his slides.
  data.name = 'P';
  load x
  x(2,:) = 500-x(2,:);
  data.x = x;
  data.z = x;
  data.col = data.z(2,:);
  data.n = size(x, 2);

 case 7 % fishbowl: uniform in data space
  gamma = 0.8;
  data.name = 'fishbowl (uniform in data space)';
  n = data.n;
  data.x = rand(3,n)-0.5;
  %project all data onto the surface of the unit sphere
  data.x = data.x ./ repmat(sqrt(sum(data.x.*data.x, 1)), [3 1]);
  ok = find(data.x(3,:) < gamma);
  i = length(ok);
  data.x(:, 1:i) = data.x(:, ok);
  while (i < n)
    p = rand(3,1)-0.5;
    p = p / sqrt(p'*p);
    if (p(3) < gamma)
      i = i+1;
      data.x(:, i) = p;
    end
  end
  % the projection on the plane works as follows:
  % start a beam from (0,0,1) through each surface point on the sphere
  % and look where it hits the xy plane.
  data.z = data.x(1:2,:) ./ repmat(1-data.x(3,:), [2 1]);
  data.col = data.x(3,:);
  data.az = -18;
  data.el = 16;
 case 8 % fishbowl: uniform in embedding space
  data.name = 'fishbowl (uniform in embedding space)';
  n = data.n;
  data.z = rand(2, n) - 0.5;
  % keep the disc
  ok = find(sum(data.z .* data.z) <= 0.25);
  i = length(ok);
  data.z(:, 1:i) = data.z(:, ok);
  while (i < n)
    p = rand(2,1) - 0.5;
    if (p'*p <= 0.25)
      i = i + 1;
      data.z(:, i) = p;
    end
  end
  gamma = 0.8;  % same role/parameter as in case 7
  data.z = 2*sqrt((1+gamma)/(1-gamma))*data.z;
  % project the disc onto the sphere
  alpha = 2 ./ (1 + sum(data.z .* data.z, 1));
  data.x = [repmat(alpha, [2 1]).*data.z; zeros(1, n)];
  data.x(3,:) = 1-alpha;
  data.col = data.x(3,:);
  data.az = -18;
  data.el = 16;

 case 9  % a gaussian blob
  data.name = 'gaussian blob';
  n = data.n;
  data.x = randn(3,n);
  data.z = data.x(2:3,:);
  data.col = data.x(3,:);

end

data.D = size(data.x, 1);  % dimensionality of the data
% finally generate noise
data.e = randn(size(data.x));
data.x_noisefree = data.x;  % the noise free data
data.x = data.x_noisefree + sqrt(data.noise)*data.e;

% precalculate the distanzmatrix
data.distances = distanz(data.x);


function [pval x K P xx fx pperm] = fexact( varargin )
%FEXACT Fisher's exact test
%
% Fisher's exact test is a statistical test used to compare binary outcomes
% between two groups. For example, a laboratory test might be positive or negative
% and we may be interested in knowing whether there is a difference in frequency of positive
% results among people with a certain disease (cases) compared to those in healthy
% (controls). The test is applicable whenever there are independent tests conducted on two
% groups. A crosstable of results (pos/neg) and status (case/control) produces a 2x2 contingency
% table shown below.
%       cases  controls
%  pos   a        c       K
%  neg   b        d       -
% total  N        -       M
%
% a,b,c,d are the numbers of positive and negative results for the cases
% and controls. K, N, M are the row, column and grand totals.
%
% This function returns a pvalue against the hypothesis that the number of
% positive cases (and by extension other cells in the table) observed or a more extreme
% distribution might have occured by chance given K, N,and M.
%
% usage
%   p = fexact(X,y)
%       y is a vetor of status (1=case/0=control). X is a MxP matrix of
%       binary results (pos=1/neg=0). P can be very large for
%       genotyping assays, each of which can be considered a different way
%       to categorize the cases and controls.
%
%  p = fexact( a,M,K,N, options)
%      M and N are constants described above. a and K are P-vectors. No checks for valid
%      input are made.
%
%  [p a K C] = fexact(X,y);
%       also returns vector a. a(i) is the upper left square of the contingency
%       corresponding to the ith column of X crosstabulated by y. K is a
%       vector containing a+b for the ith column of X.
%       C is a lookup table for CDFs that can be used in subsequent calls
%       to fexact to further improve performance.
%       C( x(i)+1, K(i)+1) is the cdf for the tail specified in
%       options.
%       NB. this lookup table is only for the given M and N values
%
%  p = fexact( a,M,K,N,C]
%      C is a lookup table that improve performance. Intended to be
%      used with permutation testing where 1000s of calls are made.
%      the option "tail" is ignored if C is provided, as it implies
%      whatever tail was used to generate them.
%
%  p = fexact(..., 'options', values)
%      options:
%      tail  l(eft)','r(ight)', 'b(oth)'
%            left tail tests a negative association. p( x<=a, M, K, N)
%            right tail tests a positive association. p( x<=b, M, K, M-N)
%            both (default) is either a positive or negative association.
%            this is the sum from the left and right tails of the
%            distribution including all x where p(x|M,K,N) is less than
%            or equal p(a|M,K,N).
%      perm  Q, where Q is an integer. Permutation testing when X has multiple Columns.
%             to correct for multiple testing. The entire set of tests is repeated Q*repsz
%            times with permuted y variables. The reported p-values are
%            corrected for multiple tests by interpolating into the emprical
%            distribution of the minimum p-value obtained from each Q*repsz rounds .
%            To use this option the X and y calling convention must be used
%            (as opposed to (a,M,K,N,...) convention.
%            and Q must be greater than 1.
%      repsz S, where S is an integer. Used with the PERM option to specify
%            the number of replicates to compute at one time (default=100).
%            Larger S is faster but requires more memory.
%
%
% NOTES and LIMITATIONS:
%      This function is extremely fast when doing multiple tests and is
%      acceptable with a small number of tests. However, it uses large
%      tracks of memory. On my 2Gb home Intel Core2 Quad Core Vista machine
%      this function does 250,000 tests with 100 observations each in 0.10
%      seconds. I run out of memory when I do more
%
% example
%  x = unidrnd(2, 200, 1000)-1;
%  y = unidrnd(2,200,1)-1;
%  p = fexact( x, y );                  % generates p-values for 1000 tests in x
%  p1 = fexact( x, y, 'perm', 10); % reports p-value relative to empirial
%                                               % cdf, since these are randomly generated
%                                               % the values in p1 should be near  1
%

% $Id: fexact.m 642 2012-06-11 18:25:47Z mboedigh $
% Copyright 2012 Mike Boedigheimer
% Amgen Inc.
% Department of Computational Biology
% mboedigh@amgen.com

p = inputParser;
p.addRequired('A');
p.addOptional('y',[]);
p.addOptional('K',[]);
p.addOptional('N',[]);
p.addOptional('C',[]);
p.addOptional('tail', 'b', @(c)ismember( c, {'b','l','r'} ));
p.addOptional('perm',  0, @(x) isnumeric(x)&isscalar(x));
p.addOptional('repsz', 100, @(x) isnumeric(x)&isscalar(x));
p.parse(varargin{:});

P = p.Results.C;
if isempty(p.Results.N)     % using fexact(X,y,options)
    X = p.Results.A;
    y = p.Results.y;
    if (~islogical(y) && ~all( ismember( y, [0 1])) )
        error('linstats:fexact:InvalidArgument', 'y must be in (0,1)' );
    end
    if (~islogical(X) && ~all( ismember( X(:), [0 1])) )
        error('linstats:fexact:InvalidArgument', 'X must be in (0,1)' );
    end

    y = logical(y);
    N = sum(y);           % number of cases
    M = length(y);
    K = sum(X,1)';
    x = X'*sparse(y);  % in unofficial testing using timeit. This was faster than sum(X(y==1,:))
    % and faster than sum(bsxfun(@eq,X,y))
else % using fexact( a,M,K,N ...)
    x = p.Results.A;
    M = p.Results.y;
    N = p.Results.N;
    K = p.Results.K;
end

switch p.Results.tail
    case 'l'; tail = -1;
    case 'r'; tail = 1;
    case 'b'; tail = 2;
end

if N==0 || N==M
    pval = ones(1,length(x));
    return;
end;

if isscalar(M) && isscalar(N) && isscalar(K) && isscalar(x) && ~(p.Results.perm > 1)
    pval = doTest( x, M, K, N, tail );
    pval(pval>1) = 1;   % fix roundoff errors
    return;
end

if isempty(P)
    P = getLookup( M,N,tail);
end

pval = P( sub2ind(size(P), x+1, K+1));

if p.Results.perm > 1
    pperm   = doPerm(X,y,M,K,P,p.Results.perm,p.Results.repsz);
    [fx xx] = mecdf(pperm(:));
    xx(end) = max(pval);
    pval    = interp1( xx, fx, pval);
end

pval(pval>1) = 1;   % fix roundoff errors

function pperm = doPerm(X,y,M,K,P,nperms,repsz)
% returns pperm a matrix with each element representing the smallest p
% value from a complete study where the rows of y have been randomly
% permuted
pperm = ones(repsz,nperms); % each element is from a permutation test. It is the minimum pvalue among all variables (columns of X)

% sub2ind was previously 85% of the execution time.
% this method computes the index directly rather than
% computing x and then calliung sub2ind. It is much
% faster because it uses the fast multiple engine
% precompute K*nrows +1, then add x after it is calculated
X = [ K*size(P,1)+1 X' ];
B = ones(1,repsz);

for permi = 1:nperms
    % generate new random vector y, by shuffling the rows (do not change
    % the counts of M,K or M)
    [i i] = sort( randn(M,repsz) ); % note randn was faster than rand when I checked
    pperm(:,permi) = min( P( X*[B;y(i)] ),[], 1); % find the smallest p-value among all separate tests for each permutation
end

function P = doTest( x, M, N, K, tail)

minx = max(0,N+K-M);
maxx = min(N,K);
if x < minx
    error( 'x is can not be smaller than max(0,N+K-M)' );
end

if x > maxx;
    error( 'x cannot be larger than min(N,K)');
end

%log p(x|M,N,K) = Q(K,x)+Q(M-K,N-x)-Q(M,N) where Q(n,k) = lnchoosek(n,k)
if tail == -1
    a = minx:x;
    logP = lnchoosek( K, a) + lnchoosek(M-K,N-a) - lnchoosek(M,N);
elseif tail == 1
    a = x:min(K,N);
    logP = lnchoosek( K, a) + lnchoosek(M-K,N-a) - lnchoosek(M,N);
else
    a = 0:min(K,N);
    logP = lnchoosek( K, a) + lnchoosek(M-K,N-a) - lnchoosek(M,N);
    pcrit = logP(x+1);
    k = (logP - pcrit) <= M*eps(max(logP));
    logP  = logP(k);
end

P = sum(exp(logP));

function P = getLookup( M, N, tail)

F = gammaln( 1:M+1);  % used to compute factorials. For small problems
% It is overkill to generate all factorials up
% to M, but it takes on .001 seconds to compute
% all of them to 1..10,001, so we can afford
% it. Memory-wise may be more of an issue, and
% a reason to

% lookup value for lnchoosek(a,b) in matrix L for all 1<=b<=a<=M
% to lookup log nchoosek(n,k) use L(n+1,k+1);
[k n ind] = tri2sqind(M);
L = zeros(M);
L(ind) = lnchoosek( n, k, F);
L = blkdiag(0,L');      % pad first row and col with 0s for n=0, k=0

%log p(x|M,N,K) = Q(K,x)+Q(M-K,N-x)-Q(M,N) where Q(n,k) = lnchoosek(n,k)

% create a lookup table for Log P
% to lookup log p(x,|M,N,K) use P(x+1,K+1);
% P initially contains 1s for valid values of x
% x is in [min( 0, N+K-M), ..., min(N,K)]
% min(N,K) is on the diagonal. the first
% column that contains a 0 in the first row will be when the minimum legal
% value of x is 1, so N+K-M == 1. K will equal M-N+1, which is the column
% M-N+2
P = spdiags(ones(N+1,M+1),0:M-N,N+1,M+1);
[Xind Kind] = find(P);
MKind = M-Kind+2;
NXind = N-Xind+2;

q = sub2ind(size(P),Xind,Kind);
P = nan(size(P));
P(q) = exp( L(sub2ind(size(L),Kind,Xind))    + ... Q(K,x)
    L(sub2ind(size(L),MKind,NXind))  + ... Q(M-K, N-x)
    -L(M+1,N+1));                     % ... Q(M,N)
if tail == -1
    P = cumsum(tril(P, M-N));
elseif tail == 1
    P = flipud(triu(P));
    P = cumsum(P);
    R = N-(-1:size(P,1)-2)';
    P = P(R,:);
else
    % This function has been tested against two reference algorithm using all M
    % and N up to 30. The reference algorithms differ in how they find the
    % find the tail on the other side of the distribution.
    [q r] = size(P);
    [P idx] = sort(P,1);     % sort order, called rank, will be transformed to rank below
    ties = [abs(diff(P)) < M*numel(P)*eps(P(1:end-1,:));
        zeros(1,r)];
    R = repmat( (1:q)',1,r);
    R(col2ind(idx)) = R+ties;
    P = cumsum(P,1);          % reuse P to store cumulative P
    P = P(col2ind(R));
end

function q = lnchoosek( n, k, f)
% lnchoosek natural log of nchoosek
%
% usage
%       q = lnchoosek( n, k );
% usage
%       q = lnchoosek( n, k, f); where f is the gammaln(1:max(n,k)+1);
q    = n-k;       % allocate space same size as n
i    = q>0;       % only compute for valid n and k

if nargin < 3
    % q is 0 when n = k because log nchoosek(a,a) = 1 for any a >= 0
    % otherwise it is given by ...
    q(i) = -log(q(i)) - betaln(k(i)+1,q(i));     % otherwise this is it
else
    q(i) =  f(n(i)+1) - f(k(i)+1) - f( q(i)+1 );
end

q(~i) = 0;

function i = col2ind(order,siz)
%col2ind converts column specific 1-based indices to element based indices
%i = col2ind(A, siz)
%   return i, a set of integer indices into a m x n matrix
%   A is a p x n set of column specific indices as you'd get from the second
%   output of sort
%   size is optional.
%        if present, P is set to SIZ(1), otherwise
%        P is set to the size of the first dimension of ORDER,
%
% example
%   [xsort order] = sort(x);
%   wrong = x(order);    % this isn't what you want
%   xsort = x(col2ind(order));   % this is

[m p] = size(order);
if nargin < 2
    q = m;
else
    q = siz(1);
end
oset = 0:q:(q*p-1);
i = order + repmat( oset, m, 1 );

function [i,j,k] = tri2sqind( m, k )
%TRI2SQIND subscript and linear indices for upper tri portion of matrix
%
% get indices into a square matrix for a vector representing a the upper
% triangular portion of a matrix such as those returned by pdist.
%
% [i,j,k] = tri2sqind( m, k )
%  If V is a hypothetical vector representing the upper triangular portion
%  of a matrix (not including the diagonal) and
%  M is the size of a square matrix and
%  K is an optional vector of indices into V then tri2sqind returns
%  (i,j) the subscripted indices into the equivalent square matrix.
%  K is an integer index into the equivalent square matrix
%
% Example
%  X = randn(5, 20);
%  Y = pdist(X, 'euclidean');
%  [i,j,k] = tri2sqind( 5 );
%  S = squareform(Y);
%  isequal( Y(:), S(k) );
%  Z = zeros(5);
%  Z(k) = Y;

max_k = m*(m-1)/2;

if ( nargin < 2 )
    k = (1:max_k)';
end;

if any( k > max_k )
    error('linstats:tri2sqind:InvalidArgument', 'ind2subl:Out of range subscript');
end;

i = floor(m+1/2-sqrt(m^2-m+1/4-2.*(k-1)));
j = k - (i-1).*(m-i/2)+i;
k = sub2ind( [m m], i, j );

function [fx x] = mecdf( y )
% MECDF my ECDF function to avoid dependendencies on stats toolbox
% assumes 0 <= y <= 1
% x always includes 0 and fx(x>1) = 1;
n = length(y);
[x i] = unique(sort(y));
fx = linspace( 0, 1, n+1 )';
fx = fx(i+1);
if x(1)>0
    x = [0;x];
    fx = [0;fx];
end

if x(end) < 1    % set up the maximum x value as Inf, so interp1 never fails
    x  = [x;Inf];
    fx = [fx;1];
end


function g = gauss(N)

% gauss(N)
%
% returns N normally distributed random numbers
% reference : Num. Recipes, Chapter 7.2 Normal Deviates

rsq  = [];

M = ceil(N/2);
while length(rsq) < M	% make shure we really have at least M values
	v = 2*rand(ceil(M*1.33),2)-1;		% produce more random numbers
	rsq = v(:,1).*v(:,1)+v(:,2).*v(:,2);
	ind = find((rsq >=1) | (rsq == 0));
	rsq(ind) = [];						% because we want to remove some
end

v(ind,:) = [];
v = v(1:M,:);
rsq = rsq(1:M);
fac = sqrt(-2 * log(rsq) ./ rsq);
g = [v(:,1) .* fac ; v(:,2) .* fac];
g = g(1:N);