function x=ifmt(mellin,beta,M);
%IFMT	Inverse fast Mellin transform.
%	X=IFMT(MELLIN,BETA,M) computes the inverse fast Mellin
%	transform of MELLIN.
%	WARNING : the inverse of the Mellin transform is correct only
%	if the Mellin transform has been computed from FMIN to 0.5 Hz,
%	and if the original signal is analytic.
%
%	MELLIN : Mellin transform to be inverted. Mellin must have been
%	 obtained from FMT with frequency running from FMIN to 0.5 Hz.
%	BETA : Mellin variable issued from FMT.
%	M : number of points of the inverse Mellin transform.
%					(default : length(MELLIN)).
%	X : inverse Mellin transform with M points in time.
%
%	Example :
%	 sig=atoms(128,[64,0.25,32,1]);
%	 [MELLIN,BETA]=fmt(sig,0.05,0.5,256); clf;
%	 X=ifmt(MELLIN,BETA,128); plot(real(X)); hold;
%	 plot(real(sig),'g'); hold;
%
%	See also : fmt, fft, ifft.
%

%	P. Goncalves 9-95 - O. Lemoine, June 1996.
%	Copyright (c) 1995 Rice University - CNRS (France).
%
%  This program is free software; you can redistribute it and/or modify
%  it under the terms of the GNU General Public License as published by
%  the Free Software Foundation; either version 2 of the License, or
%  (at your option) any later version.
%
%  This program is distributed in the hope that it will be useful,
%  but WITHOUT ANY WARRANTY; without even the implied warranty of
%  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%  GNU General Public License for more details.
%
%  You should have received a copy of the GNU General Public License
%  along with this program; if not, write to the Free Software
%  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

N=length(mellin);

if nargin<2,
 error('At least 2 input parameters required');
elseif nargin==2,
 M=N;
end

No2 = (N+rem(N,2))/2;
q   = exp(1/(N*(beta(2)-beta(1))));
fmin = 0.5/(q^(No2-1));

% Inverse Mellin transform computation
p = 0:(N-1);
L = log(fmin)/log(q);
S = fft(fftshift(mellin.*exp(-j*2*pi*L*(p/N-1/2))/(N*log(q))));
S = S(1:No2);

% Inverse Fourier transform
k = (1:No2);
x = zeros(M,1);
t = (1:M)-(M+rem(M,2))/2-1;
geo_f = fmin*(exp((k-1).*log(q))) ;
itfmatx = zeros(M,No2);
itfmatx = exp(2*i*t'*geo_f*pi);
for k=1:M,
 x(k) = real(integ(itfmatx(k,:).*S,geo_f));
end;

x = hilbert(x);

% Normalization
SP = fft(x); fmax=0.5;
indmin = 1+round(fmin*(M-2));
indmax = 1+round(fmax*(M-2));
SPana = SP(indmin:indmax);
nu = (indmin:indmax)'/M;
SPp = SPana./nu;
Esm = SPp'*SPana;
x = x*norm(mellin)/sqrt(Esm);


function x=v_rsfft(y,n)
%V_RSFFT    fft of a real symmetric spectrum X=(Y,N)
% Y is the "first half" of a symmetric real input signal and X is the
% "first half" of the symmetric real fourier transform.
% If the length, N, of the full signal is even, then the "first half"
% contains 1+N/2 elements (the first and last are excluded from the reflection).
% If N is odd, the "first half" conatins 0.5+N/2 elements and only the first
% is excluded from the reflection.
% If N is specified explicitly, then Y will be truncated of zero-padded accordingly.
% If N is omitted it will be taken to be 2*(length(Y)-1) and is always even.
%
% If Y is a matrix, the transform is performed along each column
%
% The inverse function is y=v_rsfft(x,n)/n

% Could be made faster for even n by using symmetry

%      Copyright (C) Mike Brookes 1998
%      Version: $Id: v_rsfft.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if ~isreal(y) error('RSFFT: Input must be real'); end
fl=size(y,1)==1;
if fl y=y(:); end
[m,k]=size(y);
if nargin<2 n=2*m-2;
else
    mm=1+fix(n/2);
    if mm>m y=[y; zeros(mm-m,k)];
    elseif mm<m y(mm+1:m,:)=[];
    end
    m=mm;
end
x=real(fft([y;y(n-m+1:-1:2,:)]));
x(m+1:end,:)=[];
if fl x=x.'; end


function prob_test144 ( )

%*****************************************************************************80
%
%% TEST144 tests TRIANGLE_MEAN, TRIANGLE_SAMPLE, TRIANGLE_VARIANCE;
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  nsample = 1000;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST144\n' );
  fprintf ( 1, '  For the Triangle PDF:\n' );
  fprintf ( 1, '  TRIANGLE_MEAN returns the mean;\n' );
  fprintf ( 1, '  TRIANGLE_SAMPLE samples;\n' );
  fprintf ( 1, '  TRIANGLE_VARIANCE returns the variance;\n' );

  a = 1.0;
  b = 3.0;
  c = 10.0;

  check = triangle_check ( a, b, c );

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

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

  mean = triangle_mean ( a, b, c );
  variance = triangle_variance ( a, b, c );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  PDF parameter MEAN =          %14f\n', mean );
  fprintf ( 1, '  PDF parameter VARIANCE =      %14f\n', variance );

  for i = 1 : nsample
    [ x(i), seed ] = triangle_sample ( a, b, c, seed );
  end

  mean = r8vec_mean ( nsample, x );
  variance = r8vec_variance ( nsample, x );
  xmax = max ( x(1:nsample) );
  xmin = min ( x(1:nsample) );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Sample size =     %6d\n', nsample );
  fprintf ( 1, '  Sample mean =     %14f\n', mean );
  fprintf ( 1, '  Sample variance = %14f\n', variance );
  fprintf ( 1, '  Sample maximum =  %14f\n', xmax );
  fprintf ( 1, '  Sample minimum =  %14f\n', xmin );

  return
end


function prob_test064 ( )

%*****************************************************************************80
%
%% TEST064 tests EXTREME_VALUES_CDF, EXTREME_VALUES_CDF_INV, EXTREME_VALUES_PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 April 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST064\n' );
  fprintf ( 1, '  For the Extreme Values CDF:\n' );
  fprintf ( 1, '  EXTREME_VALUES_CDF evaluates the CDF;\n' );
  fprintf ( 1, '  EXTREME_VALUES_CDF_INV inverts the CDF.\n' );
  fprintf ( 1, '  EXTREME_VALUES_PDF evaluates the PDF;\n' );

  a = 2.0;
  b = 3.0;

  check = extreme_values_check ( a, b );

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

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

  seed = 123456789;

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

  for i = 1 : 10

    [ x, seed ] = extreme_values_sample ( a, b, seed );

    pdf = extreme_values_pdf ( x, a, b );

    cdf = extreme_values_cdf ( x, a, b );

    x2 = extreme_values_cdf_inv ( cdf, a, b );

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

  end

  return
end


function y = sinc_fn(x)

i=find(x==0);
x(i)= 1;
y = sin(pi*x)./(pi*x);
y(i) = 1;

end


function [H] = quaternion(q)

% QUATERNION returns the homogenous coordinate transformation matrix corresponding to
% a coordinate transformation described by 7 quaternion parameters.
%
% Use as
%   [H] = quaternion(Q)
% where
%   Q       [q0, q1, q2, q3, q4, q5, q6] vector with parameters
%   H       corresponding homogenous transformation matrix
%
% If the input vector has length 6, it is assumed to represent a unit quaternion without scaling.
%
% See Neuromag/Elekta MaxFilter manual version 2.2, section "D2 Coordinate Matching", page 77 for more details and
% https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Conversion_to_and_from_the_matrix_representation

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

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

if numel(q)==6
  % this is used a lot by the Neuromag/Elekta software, where the first element is left out and a rigid body transformation wothout scaling is used.
  % see also https://github.com/mne-tools/mne-python/blob/maint/0.15/mne/transforms.py#L1137
  q0 = sqrt(1 - q(1)^2 - q(2)^2 - q(3)^2);
  q = [q0 q];
end

if numel(q)~=7
  ft_error('incorrect input vector');
end

% all of these quaternions are zero-offset in the original equation, but one-offset in the MATLAB vector
q0 = q(0+1);
q1 = q(1+1);
q2 = q(2+1);
q3 = q(3+1);
q4 = q(4+1);
q5 = q(5+1);
q6 = q(6+1);

R = [
  q0^2+q1^2-q2^2-q3^2  2*(q1*q2-q0*q3)      2*(q1*q3+q0*q2)
  2*(q1*q2+q0*q3)      q0^2+q2^2-q1^2-q3^2  2*(q2*q3-q0*q1)
  2*(q1*q3-q0*q2)      2*(q2*q3+q0*q1)      q0^2+q3^2-q1^2-q2^2
  ];

T = [q4 q5 q6]';

H = eye(4,4);
H(1:3,1:3) = R;
H(1:3,4)   = T;


%this examples fits a simple neural network to the function
%z= 0.5*sin(pi*y(1,:).^2).*sin(2*pi*y(2,:));

N_neurons=15;%number of neurons
N_in=2;%number of inputs

N_params=N_neurons*(N_in+2)+1;%number of parameters

N_pop_min=N_params+1;%minimum population

N_pop=round(N_pop_min*1.5);%population used

fcn_name='fitness_function';%name of function to maximize

bounds=[-20*ones(1,N_params); 20*ones(1,N_params)];

gen_max=100000;%number of times to evaluate fcn_name;

x_start=[];%let the complex function initialize the population randomly

fit_start=[];%let the complex funciton initialize the fitness;

fcn_opts.N_neurons=N_neurons;%this parameter is passed to the fitness function

%%%%%%%%%%%%crunch numbers!%%%%%%%%%%
tic;
[x_best, fit_best, x_pop, fit_pop stats]=complexmethod(fcn_name,bounds,gen_max,x_start,fit_start,fcn_opts);
timtoc=toc;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

fprintf('Total time=%f s. Time per generation=%f s\n',timtoc,timtoc/gen_max);
fprintf('Final RMSE=%f\n',-fit_best)

%%%%the rest is plotting results%%%%%%
figure(1)
loglog(-stats.trace_fitness,'.')
xlabel('Generation')
ylabel('RMSE')

N_p=10;%number of points in each dimension
y=[reshape(linspace(0,1,N_p)'*ones(1,N_p),1,[]);reshape((linspace(0,1,N_p)'*ones(1,N_p))',1,[])];%make mesh

N_in=size(y,1);
z= 0.5*sin(pi*y(1,:).^2).*sin(2*pi*y(2,:));

W1=reshape(x_best(1:N_neurons*N_in),N_neurons,N_in);%extract weights for first layer
B1=reshape(x_best((N_neurons*N_in+1):N_neurons*(N_in+1)),N_neurons,1);
W2=reshape(x_best((N_neurons*(N_in+1)+1):N_neurons*(N_in+2)),1,N_neurons);
B2=x_best(N_neurons*(N_in+2)+1);
z_hat=W2*(tanh(W1*y+B1*ones(1,size(y,2))))+B2; %neural calculation

figure(2);
clf
surf(linspace(0,1,N_p),linspace(0,1,N_p),reshape(z_hat,N_p,N_p));
shading interp;
hold on
plot3(y(2,:),y(1,:),z,'kx')
hold off


% Given a reference multidimensional signal y and a series of penalty terms P(x,lambda,d,p), solves the generalized Total Variation
% proximity operator
%
%        min_x 0.5 ||x-y||^2 + sum_i P(x,lambda_i,d_i,p_i) .
%
% where P(x,lambda_i,d_i,p_i) = lambda_i * sum_j TV(x(d_i)_j,p_i) with x(d)_j every possible 1-dimensional slice of x
% following the dimension d_i, TV(x,p) the TV-Lp prox operator for x.
%
% Inputs:
%   - y: input signal.
%   - lambdas: vector of lambda penalties of each penalty term.
%   - ds: vector of dimensions of application of each penalty term.
%   - norms: vector of norms of each penalty term (1, 2 or inf are accepted).
%   - [threads]: number of threads to use (default: 1)
%
% Outputs:
%   - x: solution of the proximity problem.
%   - info: statistical info of the run algorithm:
%       info.iters: number of major iterations run.
%       info.stop: value of the stopping criterion.
%
% Examples:
%
%   - Filter 2D signal using TV-L1 norm:
%        prox_TVgen(X,[lambda lambda],[1 2],[1 1])
%
%   - Filter 2D signal using TV-L2 norm:
%        prox_TVgen(X,[lambda lambda],[1 2],[2 2])
%
%   - Filter 2D signal using TV-L1 norm for the rows, TV-L2 for the columns, and different penalties:
%        prox_TVgen(X,[lambdaRows lambdaCols],[1 2],[1 2])
%
%   - Filter 1D signal using both TV-L1 and TV-L2 norms:
%        prox_TVgen(X,[lambda1 lambda2],[1 1],[1 2])
%
%   - Filter 3D signal using TV-L1 norm:
%        prox_TVgen(X,[lambda lambda lambda],[1 2 3],[1 1 1])
%
%   - Filter 3D signal using TV-L2 norm, not penalizing over the second dimension:
%        prox_TVgen(X,[lambda lambda],[1 3],[2 2])
%
%   - Filter 2D signal using both TV-L1 and TV-L2 norms:
%        prox_TVgen(X,[lambda1 lambda1 lambda2 lambda2],[1 2 1 2],[1 1 2 2])
%
%   - ... and so on, any combination of norms and dimensions is possible.
function [x,info] = prox_TVgen(y,lambdas,ds,norms,threads)
    % Check inputs
    if nargin < 5, threads = 1; end;
    if length(lambdas) ~= length(ds) || length(ds) ~= length(norms)
        fprintf(1,'ERROR (prox_TVgen): arguments defining penalties differ in length.\n');
        x = y;
        return;
    end;
    if sum(norms == 1 | norms == 2) ~= length(norms)
        fprintf(1,'ERROR (prox_TVgen): unacceptable norms requested. Available norms: 1, 2.\n');
        x = y;
        return;
    end;

    % Invoke C solver
    [x,in] = solveTVgen_PDykstrac(y,lambdas,ds,norms,threads);
    info.iters = in(1);
    info.stop = in(2);
end


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

% Simulates 2nd, 3rd and 4th order PLL loops for the topologies shown
% below using basic control systems theory.  This is useful for design
% verification.
%
% Input
%       - cap is the capacitors of the loop in Farads [C1, C2, C3, C4].  If
%           these components are not present in the loop, set their value
%           to zero.
%       - res is the resistors of the  loop in Ohms [ R2, R3, R4].  If
%           these components are not present in the loop, set their value to zero.
%       - order of the loop, either 2,3, or 4.
%       - 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
%
% Output
%       - bandwidth is the open loop bandwidth in Hertz
%       - pm is the phase margin in degrees
%
% The methods used here are derived from those 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 8 pp.43-47 and Chapter 9 pp. 48-53.
%
%
% Loop Topologies
% 2nd Order
%        +  _____                                   ______
% fcomp -->|Phase|----------------------------------| VCO |---->fout
%          |Det. |     |      |                     |     |  |
%           -----      |      |                      -----   |
%            ^ -      C1     R2                              |
%            |         |      |                              |
%            |        GND    C2                              |
%            |                |                              |
%            |               GND         _______             |
%            ----------------------------| 1/N |--------------
%                                        -------
%
% 3rd Order
%        +  _____                                   ______
% fcomp -->|Phase|-----------------R3---------------| VCO |---->fout
%          |Det. |     |      |         |           |     |  |
%           -----      |      |         |            -----   |
%            ^ -      C1     R2        C3                    |
%            |         |      |         |                    |
%            |        GND    C2        GND                   |
%            |                |                              |
%            |               GND         _______             |
%            ----------------------------| 1/N |--------------
%                                        -------
%
%
% 4th Order
%        +  _____                                   ______
% fcomp -->|Phase|-----------------R3------R4-------| VCO |---->fout
%          |Det. |     |      |         |     |     |     |  |
%           -----      |      |         |     |      -----   |
%            ^ -      C1     R2        C3     C4             |
%            |         |      |         |     |              |
%            |        GND    C2        GND   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)
%

%% Setup Parameters
C1 = cap(1);
C2 = cap(2);
C3 = cap(3);
C4 = cap(4);

R2 = res(1);
R3 = res(2);
R4 = res(3);

% Conversion of parameters to more convenient units
Kpd = ipump/2/pi; % phase detector gain
Kvco = vco_sensitivity*2*pi; % vco gain

%% Plot Setup
% Generates logarithmic spaced points for the calculations
fplotstart = 10; % Hz
fplotstop = 10E6; % Hz
plotpoints = 100;
pltfreqs = [];
for ppts=0:plotpoints
    pltfreqs = [ pltfreqs fplotstart * 10 ^ (ppts/plotpoints* log10(fplotstop/fplotstart)) ];
end

%% Loop Poll's Polynomial Coefficients

A0 = C1 + C2 + C3 + C4;
A1 = C2*R2*(C1+C3+C4) + R3*(C1+C2)*(C3+C4) + C4*R4*(C1+C2+C3);
A2 = C1*C2*R2*R3*(C3+C4) + C4*R4*(C2*C3*R3+C1*C3*R3+C1*C2*R2+C2*C3*R2);
A3 = C1*C2*C3*C4*R2*R3*R4;

T2 = R2*C2;

%% Filter Transfer Function
Zfilt = @(s)  (1 + s.*T2)./s./(A3.*s.^3 + A2.*s.^2 + A1.*s + A0); % filter transfer function
zfilt = @(f) Zfilt(2*pi*1i*f); % filter transfer function as a function of frequency

%% VCO Transfer Function
Gvco = @(s) Kvco./s; % vco transfer function
gvco = @(f) Gvco(2*pi*1i*f); % vco transfer function expressed as a function of frequency

%% Forward Path Transfer Function
G = @(s)  Kpd * Gvco(s) .* Zfilt(s); % forward path transfer function
%% Reverse (Feedback) Transfer Function
H = fcomp/fout; % feedback path transfer function
%% Open Loop Transfer Function
GH = @(s) G(s)*H; % open loop transfer function
gh = @(f) GH(2*pi*1i*f); % open loop transfer function expressed as a function of frequency

%% Gain Plots
% Plots of transfer function magnitudes
% figure;
% loglog( ...
%     pltfreqs, (abs(gh(pltfreqs))), ...
%     pltfreqs, (abs(zfilt(pltfreqs))), ...
%     pltfreqs, (abs(gvco(pltfreqs))) ...
% );
% grid on; title('Open Loop Gain and Contributing Factors');
% xlabel('Frequency [Hz]');
% legend('Open Loop Gain','Loop Filter','VCO', 'Location', 'SouthWest');

%% Bode Plot
figure;
    subplot(2,1,1);
        semilogx(pltfreqs, 20*log10(abs(gh(pltfreqs))));
        grid on;
        title('Open Loop Magnitude');
    subplot(2,1,2);
        semilogx(pltfreqs, angle(gh(pltfreqs)).*180/pi);
        grid on;
        title('Open Loop Phase');
        ylim([-180 180]);

%% Find open loop bandwidth and phase margin
ghdB = @(f) 20*log10(abs(gh(f)));
bandwidth = fzero(ghdB, [fplotstart fplotstop]); % find bandwidth numerically
pm = 180 + angle(gh(bandwidth)).*180/pi;

%% Closed Loop Transfer Function
Gcl = @(s) GH(s)./(1 + GH(s)); % closed loop transfer function
gcl = @(f) Gcl(2*pi*1i*f);
%% Closed Loop Plot (Magnitude)
% figure;
%     subplot(2,1,1);
%         semilogx(pltfreqs, 20*log10(abs(gcl(pltfreqs))));
%         grid on;
%         title('Closed Loop Magnitude');
%     subplot(2,1,2);
%         semilogx(pltfreqs, angle(gcl(pltfreqs)).*180/pi);
%         grid on;
%         title('Closed Loop Phase');
%         ylim([-180 180]);


%% test modulate for all oasis functions.
col = {[0 114 178],[0 158 115], [213 94 0],[230 159 0],...
    [86 180 233], [204 121 167], [64 224 208], [240 228 66]}; % colors
plot_cvx = false;

%% threshold foopsi, convolution kernel
g = [1.7, -0.712];         % AR coefficient
noise = 1;
T = 3000;
framerate = 30;
firerate = 0.5;
b = 0;              % baseline
N = 20;              % number of trials
seed = 3;          % seed for genrating random variables
[Y, trueC, trueS] = gen_data(g, noise, T, framerate, firerate, b, N, seed);
y = Y(1,:);
true_c = trueC(1,:);  %#ok<*NASGU>
true_s = trueS(1,:);
temp = roots([1, -g(1), -g(2)]);
d = max(temp);
r = min(temp);
w = 200;
ht = (exp(log(d)*(1:w)) - exp(log(r)*(1:w))) / (d-r); % convolution kernel

% case 1: all parameters are known, the kernel is the sum of two
% exponential functions
smin = 0.5;
pars = [d, r];
[c_oasis, s_oasis] = deconvolveCa(y, 'exp2', pars, 'thresholded', 'smin', smin);  %#ok<*ASGLU>
figure('name', 'threshold, exp2, known: taur, taud, smin', 'papersize', [15, 4]);
show_results;

% case 1: all parameters are known
smin = 0.5;
[c_oasis, s_oasis] = deconvolveCa(y, 'kernel', ht, 'thresholded', 'smin', smin);  %#ok<*ASGLU>
figure('name', 'threshold, kernel, known: kernel, smin', 'papersize', [15, 4]);
show_results;
%%%%%%%%%%%%%%  END %%%%%%%%%%%%%%%%%%


function jed = ymdf_to_jed_saka ( y, m, d, f )

%*****************************************************************************80
%
%% YMDF_TO_JED_SAKA converts a Saka YMDF date to a JED.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 March 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Edward Richards,
%    Algorithm E,
%    Mapping Time, The Calendar and Its History,
%    Oxford, 1999, pages 323-324.
%
%  Parameters:
%
%    Input, integer Y, M, D, real F, the YMDF date.
%
%    Output, real JED, the corresponding JED.
%

%
%  Convert the calendar date to a computational date.
%
  y_prime = y + 4794 - floor ( ( 13 - m ) / 12 );
  m_prime = mod ( m + 10, 12 );
  d_prime = d - 1;
%
%  Convert the computational date to a JED.
%
  j1 = floor ( ( 1461 * y_prime ) / 4 );

  z = floor ( m_prime / 6 );

  j2 = ( 31 - z ) * m_prime + 5 * z;

  g = floor ( ( y_prime + 184 ) / 100 );
  g = floor ( ( 3 * g ) / 4 ) - 36;

  jed = j1 + j2 + d_prime - 1348 - g - 0.5 + f;

  return
end


meas_model = LinearGaussianX('NumMeasDims',2,'NumStateDims',2,'Mapping', [1,2], 'MeasurementErrVariance', 1);
H = meas_model.matrix();
R = meas_model.covar();
measurements = [10 15;10 15];

dist = GaussianDistributionX([1;1], 10*eye(2));

numParticles = 1000;
particles = dist.random(numParticles);
weights = repmat(1/numParticles,1,numParticles);

xPred = dist.Mean;
PPred=dist.Covar;
[yPred, S, K] = KalmanFilterX.predictMeasurement_(xPred,PPred,H,R);

lik = meas_model.pdf(measurements,particles);
l = meas_model.pdf(measurements,xPred,PPred);

% Data assoc
lambda = 1/10000;
Pd = 0.9;
hypothesiser = EfficientHypothesisManagementX();

W = hypothesiser.hypothesise([lambda*(1-Pd); sum(Pd*lik,2)]');
% W = lambda*(1-Pd);
% W = [W Pd*sum(l)];
% W = W./sum(W);
%
% Wp = lambda*(1-Pd);
% Wp = [Wp Pd*sum(lik)];
% Wp = Wp./sum(Wp);
%W = [0.009 1-0.009];

%[xPost, PPost] = KalmanFilterX.update_(xPred,dist.Covar,measurements,yPred, S, K);
[xPost, PPost] = KalmanFilterX.updatePDA_(xPred,PPred,measurements,W,yPred, S, K);

[newWeights] = ParticleFilterX_UpdatePDA(@(y,x)meas_model.pdf(y,x),measurements,particles,weights,W,lik);
XPost = particles*newWeights';
resampler = SystematicResamplerX();
new_weights = weights.*lik;
%new_weights =  W(1)*dist.Weights + sum(W(2:end).*lik.*dist.Weights,1);
new_weights = new_weights./sum(new_weights);
[new_particles, new_weights] = resampler.resample(particles,new_weights);

new_dist = ParticleDistributionX(new_particles, new_weights);

% [X,Y] = meshgrid(-3:0.1:5,0:0.1:8);
% Z = dist.pdf([X;Y]);
% for i=1:size(X,1)
%     Z(i,:) = dist.pdf([X;Y]);
% end

figure;
hold on;
[bandwidth,Z,X,Y]=kde2d([particles, measurements+meas_model.random(numParticles)]');
%contour3(X,Y,density,50);
h = surf(X,Y,Z);
shading interp
colormap(jet(3000))
[bandwidth,Z,X,Y]=kde2d([new_dist.Particles, measurements+meas_model.random(numParticles)]');
%contour3(X,Y,density,50);
h = surf(X,Y,Z);
shading interp
colormap(jet(3000))
plot(measurements(1,:),measurements(2,:),'r*');
plot_gaussian_ellipsoid(dist.Particles*dist.Weights',weightedcov(dist.Particles,dist.Weights));
plot_gaussian_ellipsoid(new_dist.Particles*new_dist.Weights',weightedcov(new_dist.Particles,new_dist.Weights));


function asa047_test01 ( )

%*****************************************************************************80
%
%% ASA047_TEST01 demonstrates the use of NELMIN on ROSENBROCK.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 February 2008
%
%  Author:
%
%    John Burkardt
%
  n = 2;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'ASA047_TEST01\n' );
  fprintf ( 1, '  Apply NELMIN to ROSENBROCK function.\n' );

  start(1:n) = [ -1.2; 1.0 ];
  reqmin = 1.0E-08;
  step(1:n) = 1.0;
  konvge = 10;
  kcount = 500;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Starting point X:\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %f\n', start(i) );
  end

  ynewlo = rosenbrock ( start );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X) = %f\n', ynewlo );

  [ xmin, ynewlo, icount, numres, ifault ] = nelmin ( @rosenbrock, n, start, ...
  reqmin, step, konvge, kcount );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Return code IFAULT = %d\n', ifault );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Estimate of minimizing value X*:\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %f\n', xmin(i) );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X*) = %f\n', ynewlo );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of iterations = %d\n', icount );
  fprintf ( 1, '  Number of restarts =   %d\n', numres );

  return
end


function linplus_test0195 ( )

%*****************************************************************************80
%
%% TEST0195 tests R8VEC_TO_R8GB, R8GB_TO_R8VEC.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 March 2009
%
%  Author:
%
%    John Burkardt
%
  m = 5;
  n = 8;
  ml = 2;
  mu = 1;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST0195\n' );
  fprintf ( 1, '  For a general banded matrix,\n' );
  fprintf ( 1, '  R8VEC_TO_R8GB converts a real vector to a R8GB matrix.\n' );
  fprintf ( 1, '  R8GB_TO_R8VEC converts a R8GB matrix to a real vector.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix rows M =    %d\n', m );
  fprintf ( 1, '  Matrix columns N = %d\n', n );
  fprintf ( 1, '  Lower bandwidth ML  = %d\n', ml );
  fprintf ( 1, '  Upper bandwidth MU  = %d\n', mu );

  a = r8gb_indicator ( m, n, ml, mu );

  r8gb_print ( m, n, ml, mu, a, '  The R8GB indicator matrix:' );

  x = r8gb_to_r8vec ( m, n, ml, mu, a );

  k = 0;
  for j = 1 : n
    for i = 1 : 2*ml+mu+1
      k = k + 1;
      fprintf ( 1, '%4d  %4d  %4d  %14f\n', i, j, k, x(k) );
    end
  end

  a = r8vec_to_r8gb ( m, n, ml, mu, x );

  r8gb_print ( m, n, ml, mu, a, '  The recovered R8GB indicator matrix:' );

  return
end


function varargout=sde_decorrelate(d,r2)
%SDE_DECORRELATE  Decorrelate correlated values.
%   R1 = SDE_DECORRELATE(D, R2) returns the matrix R1 of M decorrelated values
%   given the N-by-N diffusion matrix D and the M-by-N matrix of (correlated)
%   values R2. The first column of R1 is always SQRT(D(1,1)) divided by the
%   first column of R2.
%
%   [R1, C] = SDE_DECORRELATE(D, R2) also returns the N-by-N correlation matrix,
%   C, decorrelated from the diffusion matrix, D.
%
%   C = SDE_DECORRELATE(D) without a second input argument returns the N-by-N
%   correlation matrix, C, decorrelated from the diffusion matrix, D.
%
%   Note:
%       R1 = SDE_DECORRELATE(D, SDE_CORRELATE(C, R1)) is only accurate to within
%       the floating point machine precision, EPS.
%
%   Example:
%       % Correlate and then decorrelate normally-distributed samples
%       r1 = randn(5,3)
%       c = [1 0.8 0.2;0.8 1 0.5;0.2 0.5 1];
%       [r2, d] = sde_correlate(c,r1)
%       r3 = sde_decorrelate(d,r2)
%
%   See also: SDE_CORRELATE, RAND, RANDN, RANDSTREAM, RANDSTREAM/RANDN, EPS

%   Andrew D. Horchler, horchler @ gmail . com, Created 5-20-13
%   Revision: 1.2, 7-12-13

if nargout > min(nargin,2)
    error('SDETools:sde_decorrelate:TooManyOutputs',...
          'Too many output arguments for number of supplied inputs.');
end

if isempty(d) || isempty(r2)
    if nargin == 1
        varargout{1} = [];
    else
        varargout{2} = [];
    end
else
    [m,n] = size(d);
    if ndims(d) ~= 2 || m ~= n              %#ok<ISMAT>
        error('SDETools:sde_decorrelate:NonSquareMatrix',...
              'The diffusion matrix must be square.');
    end

    if isscalar(d)
        c = d^2;
        isDiag = false;
	elseif sde_isdiag(d)
        d = diag(d);
        c = diag(d.^2);
        isDiag = true;
    else
        c = d^2;
        isDiag = false;
    end

    if nargin == 1
        varargout{1} = c;
    else
        if ndims(r2) ~= 2 || size(r2,2) ~= m	%#ok<ISMAT>
            error('SDETools:sde_decorrelate:DimensionMismatch',...
                 ['The number of columns in the matrix of normally '...
                  'distributed values must equal the dimension of the '...
                  'correlation matrix.']);
        end

        if isDiag
            varargout{1} = bsxfun(@mrdivide,r2,d);
        else
            varargout{1} = r2/d;
        end
        if nargout == 2
            varargout{2} = c;
        end
    end
end


function value = p23_exact ( dim_num )

%*****************************************************************************80
%
%% P23_EXACT returns the exact integral for problem 23.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 January 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Output, real VALUE, the exact value of the integral.
%
  c = 0.0;
  c = p23_r8 ( 'G', 'C', c );

  e = [];
  e = p23_i4vec ( 'G', 'E', dim_num, e );

  value = c;
  for i = 1: dim_num
    value = value * gamma ( e(i) + 1 );
  end

  value = value / gamma ( sum ( e(1:dim_num) ) + 1 );

  return
end


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

png_export = true;
pdf_export = false;

mf = spx.graphics.Figures();

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

mf.new_figure('Recovery probability with S for different MMV recovery algorithms');

load ('bin/success_with_s_comparison.mat');

hold all;
legends = cell(1, 4);

plot(Ss, success_with_s.ra_ormp, '-+');
legends{1} = 'RA-ORMP';
% plot(Ss, success_with_s.ra_omp, '-o');
% legends{2} = 'RA-OMP';
plot(Ss, success_with_s.somp, '-s');
legends{2} = 'SOMP';
plot(Ss, success_with_s.gomp_2_mmv, '-d');
legends{3} = 'GOMP-MMV (L=2)';
plot(Ss, success_with_s.gomp_4_mmv, '-x');
legends{4} = 'GOMP-MMV (L=4)';

grid on;
xlabel('Number of signals');
ylabel('Recovery Probability');
legend(legends, 'Location', 'southeast');
title('Comparison of recovery performance for MMV algorithms');


function [E]=gramSchmidtOrtho(E)

k=size(E,1);
for i=1:1:k
    E(i,:)=E(i,:)./norm(E(i,:));
    for j=i+1:1:k
        Ei=E(i,:);
        Ej=E(j,:);
        proj_ei_ej=dot(Ei,Ej).*Ei./norm(Ei);
        E(j,:)=E(j,:)-proj_ei_ej;
    end
end

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


function f = oka1( x )

	x1p = cos(pi./12.0).*x(:,1) - sin(pi./12.0).*x(:,2);
	x2p = sin(pi./12.0).*x(:,1) + cos(pi./12.0).*x(:,2);

	f(:,1) = x1p;
	f(:,2) = sqrt(2*pi) - sqrt(abs(x1p)) + 2 .* (abs(x2p-3.*cos(x1p)-3).^0.33333333);
end


function [centered_data,centroid] = CenterRowData(input)
% each row is a data point

centroid = mean(input);

centered_data = input-(ones(size(input, 1), 1)*centroid);


function [spec,freqs,A]=parafrep(Rx,N,method,q);
% [spec,freqs]=parafrep(Rx,N,method,q) parametric frequency representation
% of a signal.
%
% Rx     : correlation matrix of size (p+1)x(p+1)
% N      : number of frequency bins between 0 and 0.5
% method : can be either 'ar', 'periodogram', 'capon', 'capnorm', 'lagunas',
%          or 'genlag'.
% q      : parameter for the generalized Lagunas method.
%
% noise=rand(1000,1); signal=filter([1 0 0],[1 1 1],noise);
% figure(1);parafrep(correlmx(signal,2,'hermitian'),128,'AR');title('AR (2)');
% figure(2);parafrep(correlmx(signal,4,'hermitian'),128,'Capon');title('Capon (4)');
% figure(3);parafrep(correlmx(signal,2,'hermitian'),128,'lagunas');title('Lagunas (2)');
% figure(4);parafrep(correlmx(signal,40,'hermitian'),128,'periodogram');title('periodogram (40)');

% F. Auger, july 1998, april 99.

if nargin<1,
 error('At least one parameter required');
elseif nargin==1,
 N=128; method='capon';
elseif nargin==2,
 method='capon';
end;

[Rxrow,Rxcol]=size(Rx);
if (Rxrow ~= Rxcol),
 error('Rx must be a square matrix');
end;

p=Rxrow-1;
freqs=linspace(0,0.5,N);
spec=zeros(N,1);

method=upper(method);
if strcmp(method,'AR'),
 Un=ones(Rxrow,1); Rxm1Un= (Rx\Un);
 P1=real(Un'*Rxm1Un); A=Rxm1Un/P1;
 for ifreq=1:N,
  Z=exp(2j*pi*freqs(ifreq)*(0:Rxrow-1)');
  spec(ifreq)=P1 ./ abs(Z' * A)^2 ;
 end;
elseif strcmp(method,'PERIODOGRAM'),
 for ifreq=1:N,
  Z=exp(2j*pi*freqs(ifreq)*(0:Rxrow-1)');
  spec(ifreq)=real(Z' * Rx *Z)/(p+1)^2;
 end;
elseif strcmp(method,'CAPON'),
 for ifreq=1:N,
  Z=exp(2j*pi*freqs(ifreq)*(0:Rxrow-1)');
  spec(ifreq)=1.0 / real(Z' * (Rx\Z));
 end;
elseif strcmp(method,'CAPNORM'),
 for ifreq=1:N,
  Z=exp(2j*pi*freqs(ifreq)*(0:Rxrow-1)');
  spec(ifreq)=(p+1) / real(Z' * (Rx\Z));
 end;
elseif strcmp(method,'LAGUNAS'),
 for ifreq=1:N,
  Z=exp(2j*pi*freqs(ifreq)*(0:Rxrow-1)');
  Rxm1Z=Rx\Z; spec(ifreq)=real(Z' * Rxm1Z)/real(Z' * (Rx\Rxm1Z));
 end;
elseif strcmp(method,'GENLAG'),
 for ifreq=1:N,
  Z=exp(2j*pi*freqs(ifreq)*(0:Rxrow-1)');
  Rxqm1Z=(Rx)^q \Z; spec(ifreq)=real(Z' * Rx * Rxqm1Z)/real(Z' * (Rx\Rxqm1Z));
 end;
else
 error('unknown frequency representation');
end;

if (nargout==0),
 figure(gcf); plot(freqs,10.0*log10(spec)); grid;
 xlabel('normalized frequency');
 ylabel('DSP  (dB)');
end;

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


%function [pval, t_orig, crit_t, est_alpha, seed_state]=mult_comp_perm_t1(data,n_perm,tail,alpha_level,mu,reports,seed_state)
%
% mult_comp_perm_t1-One sample/paired sample permutation test based on a
% t-statistic.  This function can perform the test on one variable or
% simultaneously on multiple variables.  When applying the test to multiple
% variables, the "tmax" method is used for adjusting the p-values of each
% variable for multiple comparisons (Blair & Karnisky, 1993).  Like
% Bonferroni correction, this method adjusts p-values in a way that controls
% the family-wise error rate.  However, the permutation method will be more
% powerful than Bonferroni correction when different variables in the test
% are correlated.
%
% Required Input:
%  data   - 2D matrix of data (Observation x Variable)
%
% Optional Inputs:
%  n_perm      - Number of permutations used to estimate the distribution of
%                the null hypothesis. If the number of observations is less
%                than or equal to 12, all possible permutations are used and
%                this optional input has no effect.  If the number of
%                observations is greater than 12, n_perm specifies the
%                number of random permutations computed. Manly (1997)
%                suggests using at least 1000 permutations for an alpha level
%                of 0.05 and at least 5000 permutations for an alpha level of
%                0.01. {default=5000}
%  alpha_level - Desired family-wise alpha level. Note, because of the finite
%                number of possible permutations, the exact desired family-wise
%                alpha may not be possible. Thus, the closest approximation
%                is used and output as est_alpha. {default=.05}
%  tail        - [1, 0, or -1] If tail=1, the alternative hypothesis is that the
%                mean of the data is greater than 0 (upper tailed test).  If tail=0,
%                the alternative hypothesis is that the mean of the data is different
%                than 0 (two tailed test).  If tail=-1, the alternative hypothesis
%                is that the mean of the data is less than 0 (lower tailed test).
%                {default: 0}
%  mu          - The mean of the null hypothesis.  Must be a scalar or
%                1 x n vector (where n=the number of variables). {default: 0}
%  reports     - [0 or 1] If 0, function proceeds with no command line
%                reports. Otherwise, function reports what it is doing to
%                the command line. {default: 1}
%  seed_state  - The initial state of the random number generating stream
%                (see MATLAB documentation for "randstream"). If you pass
%                a value from a previous run of this function, it should
%                reproduce the exact same values. Note, this input only has
%                an effect if you have more than 12 observations.
%
% Outputs:
%  pval       - p-value (adjusted for multiple comparisons) of each
%               variable
%  t_orig     - t-score for each variable
%  crit_t     - Lower and upper critical t-scores for given alpha level.
%               t-scores that exceed these values significantly deviate from
%               the null hypothesis.  For upper tailed tests, the lower
%               critical t-score is NaN. The opposite is true of lower
%               tailed tests.
%  est_alpha  - The estimated family-wise alpha level of the test.  With
%               permutation tests, a finite number of p-values are possible.
%               This function tries to use an alpha level that is as close
%               as possible to the desired alpha level.  However, if the
%               sample size is small, a very limited number of p-values are
%               possible and the desired family-wise alpha level may be
%               impossible to approximately achieve.
%  seed_state - The initial state of the random number generating stream
%               (see MATLAB documentation for "randstream") used to
%               generate the permutations. You can use this to reproduce
%               the output of this function.  If the number of observations
%               is less than or equal to 12, seed_state='exact' since
%               all possible permutations are used in lieu of random
%               permutations.
%
% Note:
% -Unlike a parametric test (e.g., an ANOVA), a discrete set of p-values
% are possible (at most the number of possible permutations).  Since the
% number of possible permutations grows exponentially with the number of
% participants, this is only an issue for small sample sizes (e.g., 6
% participants).  When you have such a small sample size, the
% limited number of p-values may make the test overly conservative (e.g.,
% you might be forced to use an alpha level of .0286 since it is the biggest
% possible alpha level less than .05).
%
% -The null hypothesis of the permutation test is that the data come from a
% distribution that is symmetric around the mean of the null hypothesis.
% This is probably a generally reasonable assumption for
% paired-sample/repeated measures tests, but it might not be appropriate for
% one-sample tests.
%
%
% One Sample Example:
% >> data=randn(16,5);  %5 variables, 16 observations
% >> data(:,1:2)=data(:,1:2)+1; %mean of first two variables is 1
% >> [pval, t_orig, crit_t, est_alpha, seed_state]=mult_comp_perm_t1(data,50000);
% >> disp(pval); %adjusted p-values
%
% Paired-Sample/Repated Measures Example:
% >> dataA=randn(16,5);  %data from Condition A (5 variables, 16 observations)
% >> dataA(:,1:2)=dataA(:,1:2)+1; %mean of first two variables is 1
% >> dataB=randn(16,5); %data from Condition B (all variables have mean of 0)
% >> dif=dataA-dataB; %difference between conditions
% >> [pval, t_orig, crit_t, est_alpha, seed_state]=mult_comp_perm_t1(dif,50000);
% >> disp(pval); %adjusted p-values
%
% References:
% Blair, R.C. & Karniski, W. (1993) An alternative method for significance
% testing of waveform difference potentials. Psychophysiology.
%
% Manly, B.F.J. (1997) Randomization, Bootstrap, and Monte Carlo Methods in
% Biology. 2nd ed. Chapman and Hall, London.
%
%
%
% For a review on permutation tests and other contemporary techniques for
% correcting for multiple comparisons see:
%
%   Groppe, D.M., Urbach, T.P., & Kutas, M. (2011) Mass univariate analysis
% of event-related brain potentials/fields I: A critical tutorial review.
% Psychophysiology, 48(12) pp. 1711-1725, DOI: 10.1111/j.1469-8986.2011.01273.x
% http://www.cogsci.ucsd.edu/~dgroppe/PUBLICATIONS/mass_uni_preprint1.pdf
%
%
% Author:
% David Groppe
% Dec, 2010
% Kutaslab, San Diego
%

function [pval, t_orig, crit_t, est_alpha, seed_state]=mult_comp_perm_t1(data,n_perm,tail,alpha_level,mu,reports,seed_state)

if nargin<1,
    error('You need to provide data.');
end

if nargin<2,
    n_perm=5000;
end

if nargin<3,
    tail=0;
elseif (tail~=0) && (tail~=1) && (tail~=-1),
    error('Argument ''tail'' needs to be 0,1, or -1.');
end

if nargin<4,
    alpha_level=0.05;
elseif (alpha_level>=1) || (alpha_level<=0),
   error('Argument ''alpha_level'' needs to be a number between 0 and 1.');
end

if nargin<5,
    mu=0;
end

if nargin<6,
    reports=1;
end

defaultStream=RandStream.getDefaultStream; %random # generator state
if (nargin<7) || isempty(seed_state),
    %Store state of random number generator
    seed_state=defaultStream.State;
else
    defaultStream.State=seed_state; %reset random number generator to saved state
end

[n_obs n_var]=size(data);
if n_obs<2,
    error('You need data from at least two observations to perform a hypothesis test.')
end

if n_obs<7,
    n_psbl_prms=2^n_obs;
    if reports,
        watchit(sprintf(['Due to the very limited number of observations,' ...
            ' the total number of possible permutations is small.\nThus only a limited number of p-values (at most %d) are possible and the test might be overly conservative.'], ...
            n_psbl_prms));
    end
end

%% Remove null hypothesis mean from data
if isscalar(mu),
    data=data-mu;
elseif isvector(mu)
    s_mu=size(mu);
    if s_mu(1)>1,
        mu=mu';
        s_mu=size(mu);
    end
    if s_mu(2)~=n_var,
        error('mu needs to be a scalar or a 1 x %d vector (%d is the number of variables).',n_var,n_var);
    end
    data=data-repmat(mu,n_obs,1);
else
    error('mu needs to be a scalar or a 1 x %d vector (%d is the number of variables).',n_var,n_var);
end

if reports,
    fprintf('mult_comp_perm_t1: Number of variables: %d\n',n_var);
    fprintf('mult_comp_perm_t1: Number of observations: %d\n',n_obs);
    fprintf('t-score degrees of freedom: %d\n',n_obs-1);
end

%% Set up permutation test
if n_obs<=12,
    n_perm=2^n_obs; %total number of possible permutations
    exact=1;
    seed_state='exact';
    if reports,
        fprintf('Due to the limited number of observations, all possible permutations of the data will be computed instead of random permutations.\n');
    end
else
    exact=0;
end

if reports,
    fprintf('Executing permutation test with %d permutations...\n',n_perm);
    fprintf('Permutations completed: ');
end

%% Compute permutations

%Constant factor for computing t, speeds up computing t to precalculate
%now
sqrt_nXnM1=sqrt(n_obs*(n_obs-1));
if exact,
    %Use all possible permutations
    mxt=zeros(1,n_perm);
    for perm=1:n_perm
        if ~rem(perm,100)
            if reports,
                if ~rem(perm-100,1000)
                    fprintf('%d',perm);
                else
                    fprintf(', %d',perm);
                end
                if ~rem(perm,1000)
                    fprintf('\n');
                end
            end
        end
        %set sign of each participant's data
        if exist('de2bi.m','file') %% ?? check
            temp=de2bi(perm-1);
        else %% check ??
            temp=perm-1; %% check ??
        end %% check ??
        n_temp=length(temp);
        sn=-ones(n_obs,1);
        sn(1:n_temp,1)=2*temp-1;
        sn_mtrx=repmat(sn,1,n_var);
        d_perm=data.*sn_mtrx;

        %computes t-score of permuted data across all channels and time points
        sm=sum(d_perm,1);
        mn=sm/n_obs;
        sm_sqrs=sum(d_perm.^2,1)-(sm.^2)/n_obs;
        stder=sqrt(sm_sqrs)/sqrt_nXnM1;
        t=mn./stder;

        %get most extreme t-score
        [dummy mxt_id]=max(abs(t));
        mxt(perm)=t(mxt_id); %get the most extreme t-value with its sign (+ or -)
    end
else
    %Use random permutations
    mxt=zeros(1,n_perm*2);
    for perm=1:n_perm
        if ~rem(perm,100)
            if reports,
                if ~rem(perm-100,1000)
                    fprintf('%d',perm);
                else
                    fprintf(', %d',perm);
                end
                if ~rem(perm,1000)
                    fprintf('\n');
                end
            end
        end
        %randomly set sign of each participant's data
        sn=(rand(n_obs,1)>.5)*2-1;
        sn_mtrx=repmat(sn,1,n_var);

        d_perm=data.*sn_mtrx;

        %computes t-score of permuted data across all channels and time points
        sm=sum(d_perm,1);
        mn=sm/n_obs;
        sm_sqrs=sum(d_perm.^2,1)-(sm.^2)/n_obs;
        stder=sqrt(sm_sqrs)/sqrt_nXnM1;
        t=mn./stder;

        %get most extreme t-score (sign isn't immportant since we asumme
        %symmetric distribution of null hypothesis for one sample test)
        mxt(perm)=max(abs(t));
    end
    mxt(n_perm+1:2*n_perm)=-mxt(1:n_perm); %add the negative of all values since we assumme
    %null hypothesis distribution is symmetric
end

%End permutations completed line
if reports && rem(perm,1000)
    fprintf('\n');
end

%% Computes t-scores of observations at all variables and time points
sm=sum(data,1);
mn=sm/n_obs;
sm_sqrs=sum(data.^2,1)-(sm.^2)/n_obs;
stder=sqrt(sm_sqrs)/sqrt_nXnM1;
t_orig=mn./stder;

%% Compute p-values
pval=zeros(1,n_var);
for t=1:n_var,
    if tail==0,
        pval(t)=mean(mxt>=abs(t_orig(t)))*2;
    elseif tail==1,
        pval(t)=mean(mxt>=t_orig(t));
    elseif tail==-1,
        pval(t)=mean(mxt<=t_orig(t));
    end
end

%% Compute critical t-scores for specified alpha level,
if tail==0,
    %two-tailed
    crit_t(1)=prctile(mxt,100*alpha_level/2);
    crit_t(2)=-crit_t(1);
    est_alpha=mean(mxt>=crit_t(2))*2;
elseif tail==1,
    %upper tailed
    crit_t(1)=NaN;
    crit_t(2)=prctile(mxt,100-100*alpha_level);
    est_alpha=mean(mxt>=crit_t(2));
else
    %tail=-1, lower tailed
    crit_t(1)=prctile(mxt,alpha_level*100);
    est_alpha=mean(mxt<=crit_t(1));
    crit_t(2)=NaN;
end
if reports,
    fprintf('Desired family-wise alpha level: %f\n',alpha_level);
    fprintf('Estimated actual family-wise alpha level for returned values of crit_t: %f\n',est_alpha);
end

function watchit(msg)
%function watchit(msg)
%
% Displays a warning message on the Matlab command line.  Used by
% several Mass Univariate ERP Toolbox functions.
%

disp(' ');
disp('****************** Warning ******************');
disp(msg);
disp(' ');


function x = r8ci_sl ( n, a, b, job )

%*****************************************************************************80
%
%% R8CI_SL solves a R8CI system.
%
%  Discussion:
%
%    The R8CI storage format is used for an N by N circulant matrix.
%    An N by N circulant matrix A has the property that the entries on
%    row I appear again on row I+1, shifted one position to the right,
%    with the final entry of row I appearing as the first of row I+1.
%    The R8CI format simply records the first row of the matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 February 2004
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, real A(N), the R8CI matrix.
%
%    Input, real B(N), the right hand side.
%
%    Input, integer JOB, specifies the system to solve.
%    0, solve A * x = b.
%    nonzero, solve A' * x = b.
%
%    Output, real X(N), the solution of the linear system.
%
  if ( job == 0 )
%
%  Solve the system with the principal minor of order 1.
%
    r1 = a(1);
    x(1) = b(1) / r1;

    r2 = 0.0;
%
%  Recurrent process for solving the system.
%
    for nsub = 2 : n
%
%  Compute multiples of the first and last columns of
%  the inverse of the principal minor of order N.
%
      r5 = a(n+2-nsub);
      r6 = a(nsub);

      if ( 2 < nsub )

        work(nsub-1) = r2;

        for i = 1 : nsub-2
          r5 = r5 + a(n+1-i) * work(nsub-i);
          r6 = r6 + a(i+1) * work(n-1+i);
        end

      end

      r2 = -r5 / r1;
      r3 = -r6 / r1;
      r1 = r1 + r5 * r3;

      if ( 2 < nsub )

        r6 = work(n);
        work(n-1+nsub-1) = 0.0;
        for i = 2 : nsub-1
          r5 = work(n-1+i);
          work(n-1+i) = work(i) * r3 + r6;
          work(i) = work(i) + r6 * r2;
          r6 = r5;
        end

      end

      work(n) = r3;
%
%  Compute the solution of the system with the principal minor of order NSUB.
%
      r5 = 0.0;
      for i = 1 : nsub-1
        r5 = r5 + a(n+1-i) * x(nsub-i);
      end

      r6 = ( b(nsub) - r5 ) / r1;
      x(1:nsub-1) = x(1:nsub-1) + work(n:n+nsub-2) * r6;
      x(nsub) = r6;

    end

  else
%
%  Solve the system with the principal minor of order 1.
%
    r1 = a(1);
    x(1) = b(1) / r1;

    r2 = 0.0;
%
%  Recurrent process for solving the system.
%
    for nsub = 2 : n
%
%  Compute multiples of the first and last columns of
%  the inverse of the principal minor of order N.
%
      r5 = a(nsub);
      r6 = a(n+2-nsub);

      if ( 2 < nsub )

        work(nsub-1) = r2;

        for i = 1 : nsub-2
          r5 = r5 + a(i+1) * work(nsub-i);
          r6 = r6 + a(n+1-i) * work(n-1+i);
        end

      end

      r2 = -r5 / r1;
      r3 = -r6 / r1;
      r1 = r1 + r5 * r3;

      if ( 2 < nsub )

        r6 = work(n);
        work(n-1+nsub-1) = 0.0;
        for i = 2 : nsub-1
          r5 = work(n-1+i);
          work(n-1+i) = work(i) * r3 + r6;
          work(i) = work(i) + r6 * r2;
          r6 = r5;
        end

      end

      work(n) = r3;
%
%  Compute the solution of the system with the principal minor of order NSUB.
%
      r5 = 0.0E+00;
      for i = 1 : nsub-1
        r5 = r5 + a(i+1) * x(nsub-i);
      end

      r6 = ( b(nsub) - r5 ) / r1;
      for i = 1 : nsub-1
        x(i) = x(i) + work(n-1+i) * r6;
      end

      x(nsub) = r6;

    end

  end

  return
end


function NsVals=reduceStdRefrac2Spher(NMeas,height,expConst,multConst,varargin)
%%REDUCESTDREFRAC2SPHER Given a measurement of the atmospheric refractivity
%                       at a particular height above sea level, determine
%                       the equivalent refractivity at sea level using  a
%                       standard exponential atmospheric model. The
%                       algorithm can handle sea surface refractivities
%                       from 200 to 450.
%
%INPUTS: NMeas The measured atmospheric refractivity. Note that the
%              refractivity is (n-1)*1e6, where n is the index of
%              refraction.
%       height The height in meters at which the measurement of NMeas was
%              taken. One would expect this to be an orthometic height
%              (with respect to mean sea level). However, the model is low
%              fidelity, so using a height with respect to the Earth's
%              reference ellipsoid would presumably not introduce a
%              significant amount of error.
%  expConst, multConst The two optional, positive  parameters
%              parameterizing the decay constant in the model. Assume that
%              at a point, the refractivity is N. Increasing the height by
%              1km, the model is that the refractivity changes by
%              deltaN=-multConst*exp(expConst*N). deltaN cannot be
%              negative. If these parameters are omitted or empty matrices
%              are passed, then the values fitting the data in [1] are
%              used: expConst=0.005577 and multConst=7.32. Note that the
%              value ce=log(Ns/(Ns+DeltaN))/1000 is the decay constant in
%              inverse meters.
%     varargin Parameters to pass to the fminbnd function. These are
%              standard comma-separated keys and values, such as
%              'TolX',1e-8.
%
%OUTPUTS: Ns The indices of refraction reduced to sea level under a
%            standard exponential atmospheric model. There can be 1-2
%            solutions.
%
%The standard exponential atmospheric model is derived in [1]. This
%function determines the refractivity at the given altitudes for sea
%surface refractivities from 200 to 450 and subtracts the observed
%refractivity. Any zero crossings indicate potential solutions. The
%function fminbnd is then used to find each of the zeros given bounded
%regions for the zero crossings.
%
%The use of this type of very basic refraction model is discussed in [2].
%
%REFERENCES:
%[1] B. R. Bean and G. D. Thayer, CRPL Exponential Reference Atmosphere.
%     Washington, D.C.: U. S. Department of Commerce, National Bureau of
%     Standards, Oct. 1959. [Online]. Available:
%     http://digicoll.manoa.hawaii.edu/techreports/PDF/NBS4.pdf
%[2] D. F. Crouse, "Basic tracking using 3D monostatic and bistatic
%    measurements in refractive environments," IEEE Aerospace and
%    Electronic Systems Magazine, vol. 29, no. 8, Part II, pp. 54-75, Aug.
%    2014.
%
%June 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<3||isempty(expConst))
    expConst=0.005577;
end

if(nargin<4||isempty(multConst))
	multConst=7.32;
end

%Evaluate the refractivity on a grid.
numPoints=100;
Ns=linspace(200,450,numPoints);
theVals=Ns.*(Ns./(Ns-multConst*exp(expConst*Ns))).^(-height/1000)-NMeas;

%Find where the zero crossings are
diffIdx=find(diff(theVals>0));

%Each crossing gives a region to search to find the zero. Because it is
%bounded, we will use fminbnd to find the minimum of the squared value in
%the bounded region.
numNs=length(diffIdx);
NsVals=zeros(numNs,1);

costFun=@(Ns)(Ns.*(Ns./(Ns-multConst*exp(expConst*Ns))).^(-height/1000)-NMeas)^2;
for curNs=1:numNs
    NsCur=fminbnd(costFun,Ns(diffIdx(curNs)),Ns(diffIdx(curNs)+1),varargin{:});
    NsVals(curNs)=NsCur;
end

end

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


%DEMEV3	Demonstrate Bayesian regression for the RBF.
%
%	Description
%	The problem consists an input variable X which sampled from a
%	Gaussian distribution, and a target variable T generated by computing
%	SIN(2*PI*X) and adding Gaussian noise. An RBF network with linear
%	outputs is trained by minimizing a sum-of-squares error function with
%	isotropic Gaussian regularizer, using the scaled conjugate gradient
%	optimizer. The hyperparameters ALPHA and BETA are re-estimated using
%	the function EVIDENCE. A graph  is plotted of the original function,
%	the training data, the trained network function, and the error bars.
%
%	See also
%	DEMEV1, EVIDENCE, RBF, SCG, NETEVFWD
%

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

clc;
disp('This demonstration illustrates the application of Bayesian')
disp('re-estimation to determine the hyperparameters in a simple regression')
disp('problem using an RBF netowk. It is based on a the fact that the')
disp('posterior distribution for the output weights of an RBF is Gaussian')
disp('and uses the evidence maximization framework of MacKay.')
disp(' ')
disp('First, we generate a synthetic data set consisting of a single input')
disp('variable x sampled from a Gaussian distribution, and a target variable')
disp('t obtained by evaluating sin(2*pi*x) and adding Gaussian noise.')
disp(' ')
disp('Press any key to see a plot of the data together with the sine function.')
pause;

% Generate the matrix of inputs x and targets t.

ndata = 16;			% Number of data points.
noise = 0.1;			% Standard deviation of noise distribution.
randn('state', 0);
rand('state', 0);
x = 0.25 + 0.07*randn(ndata, 1);
t = sin(2*pi*x) + noise*randn(size(x));

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

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

disp('Next we create a two-layer MLP network having 3 hidden units and one')
disp('linear output. The model assumes Gaussian target noise governed by an')
disp('inverse variance hyperparmeter beta, and uses a simple Gaussian prior')
disp('distribution governed by an inverse variance hyperparameter alpha.')
disp(' ');
disp('The network weights and the hyperparameters are initialised and then')
disp('the output layer weights are optimized with the scaled conjugate gradient')
disp('algorithm using the SCG function, with the hyperparameters kept')
disp('fixed. After a maximum of 50 iterations, the hyperparameters are')
disp('re-estimated using the EVIDENCE function. The process of optimizing')
disp('the weights with fixed hyperparameters and then re-estimating the')
disp('hyperparameters is repeated for a total of 3 cycles.')
disp(' ')
disp('Press any key to train the network and determine the hyperparameters.')
pause;

% Set up network parameters.
nin = 1;		% Number of inputs.
nhidden = 3;		% Number of hidden units.
nout = 1;		% Number of outputs.
alpha = 0.01;		% Initial prior hyperparameter.
beta_init = 50.0;	% Initial noise hyperparameter.

% Create and initialize network weight vector.
net = rbf(nin, nhidden, nout, 'tps', 'linear', alpha, beta_init);
[net.mask, prior] = rbfprior('tps', nin, nhidden, nout, alpha, alpha);
net = netinit(net, prior);

options = foptions;
options(14) = 5;  % At most 5 EM iterations for basis functions
options(1) = -1;  % Turn off all messages
net = rbfsetbf(net, options, x);  % Initialise the basis functions

% Now train the network
nouter = 5;
ninner = 2;
options = foptions;
options(1) = 1;
options(2) = 1.0e-5;		% Absolute precision for weights.
options(3) = 1.0e-5;		% Precision for objective function.
options(14) = 50;		% Number of training cycles in inner loop.

% Train using scaled conjugate gradients, re-estimating alpha and beta.
for k = 1:nouter
  net = netopt(net, options, x, t, 'scg');
  [net, gamma] = evidence(net, x, t, ninner);
  fprintf(1, '\nRe-estimation cycle %d:\n', k);
  fprintf(1, '  alpha =  %8.5f\n', net.alpha);
  fprintf(1, '  beta  =  %8.5f\n', net.beta);
  fprintf(1, '  gamma =  %8.5f\n\n', gamma);
  disp(' ')
  disp('Press any key to continue.')
  pause;
end

fprintf(1, 'true beta: %f\n', 1/(noise*noise));

disp(' ')
disp('Network training and hyperparameter re-estimation are now complete.')
disp('Compare the final value for the hyperparameter beta with the true')
disp('value.')
disp(' ')
disp('Notice that the final error value is close to the number of data')
disp(['points (', num2str(ndata),') divided by two.'])
disp(' ')
disp('Press any key to continue.')
pause; clc;
disp('We can now plot the function represented by the trained network. This')
disp('corresponds to the mean of the predictive distribution. We can also')
disp('plot ''error bars'' representing one standard deviation of the')
disp('predictive distribution around the mean.')
disp(' ')
disp('Press any key to add the network function and error bars to the plot.')
pause;

% Evaluate error bars.
[y, sig2] = netevfwd(netpak(net), net, x, t, plotvals);
sig = sqrt(sig2);

% Plot the data, the original function, and the trained network function.
[y, z] = rbffwd(net, plotvals);
figure(h); hold on;
plot(plotvals, y, '-r')
xlabel('Input')
ylabel('Target')
plot(plotvals, y + sig, '-b');
plot(plotvals, y - sig, '-b');
legend('data', 'function', 'network', 'error bars');

disp(' ')
disp('Notice how the confidence interval spanned by the ''error bars'' is')
disp('smaller in the region of input space where the data density is high,')
disp('and becomes larger in regions away from the data.')
disp(' ')
disp('Press any key to end.')
pause; clc; close(h);


function [ R ] = V2R( V )
%V2R converts a 1x3 angle-axis vector into a 3x3 rotation matrix
%   Inputs -
%   V - 1x3 vector of form [rx,ry,rz] where rx,ry,rz is an angle-axis
%   representation of the angle where the unit vector representing the axis
%   has been multipled by the angle of rotation about it
%
%   Outputs -
%   R - a standard 3x3 transformation matrix

validateattributes(V, {'numeric'},{'size',[1,3]});

V = double(V(:));

s = norm(V);
if(s == 0)
    R = eye(3);
    return;
end

V = [V/s; s];
R = vrrotvec2mat(V);

end


function [] = draw_funciton(f, x_range_min, x_range_max, y_range_min, y_range_max, unit_len)

    xCoarse = x_range_min:unit_len:x_range_max;
    yCoarse = y_range_min:unit_len:y_range_max;
    [XX,YY] = meshgrid(xCoarse,yCoarse);
    row_size = size(XX,1);
    col_size = size(XX,2);
    for j=1:col_size
        for i=1:row_size
           w = [XX(i,j); YY(i,j)];
           ZZ(i,j) = f(w);
        end
    end

    figure

    % draw contour
    contour(XX,YY,ZZ,50)

    % draw x-axis
    x = [x_range_min x_range_max];
    y = [0 0];
    line(x,y,'Color','black','LineStyle','-')

    % draw y-axis
    x = [0 0];
    y = [y_range_min y_range_max];
    line(x,y,'Color','black','LineStyle','-')

end


function [h, array] = display_samples(A, opt_normalize, opt_graycolor, cols, opt_colmajor)
% This function visualizes filters in matrix A. Each column of A is a
% filter. We will reshape each column into a square image and visualizes
% on each cell of the visualization panel.
% All other parameters are optional, usually you do not need to worry
% about it.
% opt_normalize: whether we need to normalize the filter so that all of
% them can have similar contrast. Default value is true.
% opt_graycolor: whether we use gray as the heat map. Default is true.
% cols: how many columns are there in the display. Default value is the
% squareroot of the number of columns in A.
% opt_colmajor: you can switch convention to row major for A. In that
% case, each row of A is a filter. Default value is false.
warning off all

if ~exist('opt_normalize', 'var') || isempty(opt_normalize)
    opt_normalize= true;
end

if ~exist('opt_graycolor', 'var') || isempty(opt_graycolor)
    opt_graycolor= true;
end

if ~exist('opt_colmajor', 'var') || isempty(opt_colmajor)
    opt_colmajor = false;
end

% rescale
A = A - mean(A(:));

if opt_graycolor, colormap(gray); end

% compute rows, cols
[L M]=size(A);
sz=sqrt(L);
buf=1;
if ~exist('cols', 'var')
    if floor(sqrt(M))^2 ~= M
        n=ceil(sqrt(M));
        while mod(M, n)~=0 && n<1.2*sqrt(M), n=n+1; end
        m=ceil(M/n);
    else
        n=sqrt(M);
        m=n;
    end
else
    n = cols;
    m = ceil(M/n);
end

array=-ones(buf+m*(sz+buf),buf+n*(sz+buf));

if ~opt_graycolor
    array = 0.1.* array;
end

if ~opt_colmajor
    k=1;
    for i=1:m
        for j=1:n
            if k>M,
                continue;
            end
            clim=max(abs(A(:,k)));
            if opt_normalize
                array(buf+(i-1)*(sz+buf)+(1:sz),buf+(j-1)*(sz+buf)+(1:sz))=reshape(A(:,k),sz,sz)/clim;
            else
                array(buf+(i-1)*(sz+buf)+(1:sz),buf+(j-1)*(sz+buf)+(1:sz))=reshape(A(:,k),sz,sz)/max(abs(A(:)));
            end
            k=k+1;
        end
    end
else
    k=1;
    for j=1:n
        for i=1:m
            if k>M,
                continue;
            end
            clim=max(abs(A(:,k)));
            if opt_normalize
                array(buf+(i-1)*(sz+buf)+(1:sz),buf+(j-1)*(sz+buf)+(1:sz))=reshape(A(:,k),sz,sz)/clim;
            else
                array(buf+(i-1)*(sz+buf)+(1:sz),buf+(j-1)*(sz+buf)+(1:sz))=reshape(A(:,k),sz,sz);
            end
            k=k+1;
        end
    end
end

if opt_graycolor
    h=imagesc(array,'EraseMode','none',[-1 1]);
else
    h=imagesc(array,'EraseMode','none',[-1 1]);
end
axis image off

drawnow;

warning on all


function value = r4_csevl ( x, cs, n )

%*****************************************************************************80
%
%% R4_CSEVL evaluates a Chebyshev series.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 September 2011
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Roger Broucke,
%    Algorithm 446:
%    Ten Subroutines for the Manipulation of Chebyshev Series,
%    Communications of the ACM,
%    Volume 16, Number 4, April 1973, pages 254-256.
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Input, real CS(N), the Chebyshev coefficients.
%
%    Input, integer N, the number of Chebyshev coefficients.
%
%    Output, real VALUE, the Chebyshev series evaluated at X.
%
  if ( n < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R4_CSEVL - Fatal error!\n' );
    fprintf ( 1, '  Number of terms <= 0.\n' );
    error ( 'R4_CSEVL - Fatal error!' )
  end

  if ( 1000 < n )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R4_CSEVL - Fatal error!\n' );
    fprintf ( 1, '  Number of terms > 1000.\n' );
    error ( 'R4_CSEVL - Fatal error!' )
  end

  if ( x < -1.1 || 1.0 < x )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R4_CSEVL - Fatal error!\n' );
    fprintf ( 1, '  X outside (-1,+1).\n' );
    fprintf ( 1, '  X = %f\n', x );
    error ( 'R4_CSEVL - Fatal error!' )
  end

  b1 = 0.0;
  b0 = 0.0;
  twox = 2.0 * x;

  for i = n : -1 : 1
    b2 = b1;
    b1 = b0;
    b0 = twox * b1 - b2 + cs(i);
  end

  value = 0.5 * ( b0 - b2 );

  return
end


function differ_test01 ( )

%*****************************************************************************80
%
%% DIFFER_TEST01 tests DIFFER_MATRIX.
%
%  Discussion:
%
%    DIFFER_MATRIX computes a modified Vandermonde matrix A1.
%
%    The solution of a system A1 * X1 = B is related to the solution
%    of the system A2 * X2 = B, where A2 is the standard Vandermonde
%    matrix, simply by X2(I) = X1(I) * A(I,1).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 November 2013
%
%  Author:
%
%    John Burkardt
%
  n = 4;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'DIFFER_TEST01\n' );
  fprintf ( 1, '  Demonstrate that the DIFFER matrix is "really"\n' );
  fprintf ( 1, '  a Vandermonde matrix.\n' );

  stencil(1:n,1) = [ 2.5; 3.3; -1.3; 0.5 ];
  x1(1:n,1) = [ 1.0; 2.0; 3.0; 4.0 ];
  a = differ_matrix ( n, stencil );
  r8mat_print ( n, n, a, '  Stencil matrix:' );
  b(1:n,1) = a(1:n,1:n) * x1(1:n,1);
%
%  Set up and solve the DIFFER system.
%
  x1 = a \ b;

  r8vec_print ( n, x1, '  Solution of DIFFER system:' );
%
%  R8VM_SL solves the related Vandermonde system.
%  A simple transformation gives us the solution to the DIFFER system.
%
  job = 0;
  [ x2, info ] = r8vm_sl ( n, stencil, b, job );

  if ( info ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TEST01 - Warning!\n' );
    fprintf ( 1, '  VANDERMONDE system is singular.\n' );
    error ( 'TEST01 - Vandermonde system is singular.' );
    return
  end

  r8vec_print ( n, x2, '  Solution of VANDERMONDE system:' );

  x2(1:n,1) = x2(1:n,1) ./ stencil(1:n,1);
  r8vec_print ( n, x2, '  Transformed solution of VANDERMONDE system:' );

  return
end


function parent = tree_rb_to_parent ( n, a )

%*****************************************************************************80
%
%% TREE_RB_TO_PARENT converts rooted binary tree to parent node representation.
%
%  Discussion:
%
%    Parent node representation of a tree assigns to each node a "parent" node,
%    which represents the first link of the path between the node and the
%    root node.  The root node itself is assigned a parent of 0.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 June 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of nodes in the tree.
%
%    Input, integer A(N), the preorder traversal form for the
%    rooted binary tree.
%
%    Output, integer PARENT(N), the parent node representation
%    of the tree.
%
  node = 0;
  node_num = 0;

  for k = 1 : n

    dad = node;
    node_num = node_num + 1;
    node = node_num;
    parent(node) = dad;

    if ( a(k) == 1 )

      use(node) = 0;

    else

      use(node) = 2;

      while ( use(node) == 2 )
        node = dad;
        if ( node == 0 )
          break
        end
        use(node) = use(node) + 1;
        dad = parent(node);
      end

    end

  end

  return
end


function mckinnon_test ( )

%*****************************************************************************80
%
%% MCKINNON_TEST works with the McKinnon function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 January 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Michael McKinnon,
%    An Iterative Method for Finding Stationary Values of a Function
%    of Several Variables,
%    Computer Journal,
%    Volume 5, 1962, pages 147-151.
%
  global phi
  global tau
  global theta

  fprintf ( 1, '\n' );
  fprintf ( 1, 'MCKINNON_TEST:\n' );
  fprintf ( 1, '  Test COORDINATE_SEARCH with the McKinnon function.\n' );
  n = 2;
%
%  Test 1
%
  a = ( 1.0 + sqrt ( 33.0 ) ) / 8.0;
  b = ( 1.0 - sqrt ( 33.0 ) ) / 8.0;

  phi = 10.0;
  tau = 1.0;
  theta = 15.0;

  x = [ a, b ];
  r8vec_print ( n, x, '  Initial point X0:' );
  fprintf ( 1, '  PHI = %f, TAU = %f, THETA = %f\n', phi, tau, theta );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X0) = %g\n', mckinnon ( x ) );

  flag = 0;
  x = coordinate_search ( x, @mckinnon, flag );
  r8vec_print ( n, x, '  Estimated minimizer X1:' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X1) = %g\n', mckinnon ( x ) );

  x = [ 0.0, -0.5 ];
  r8vec_print ( n, x, '  Correct minimizer X*:' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X*) = %g\n', mckinnon ( x ) );
%
%  Test 2
%
  a = ( 1.0 + sqrt ( 33.0 ) ) / 8.0;
  b = ( 1.0 - sqrt ( 33.0 ) ) / 8.0;

  phi = 60.0;
  tau = 2.0;
  theta = 6.0;

  x = [ a, b ];
  r8vec_print ( n, x, '  Initial point X0:' );
  fprintf ( 1, '  PHI = %f, TAU = %f, THETA = %f\n', phi, tau, theta );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X0) = %g\n', mckinnon ( x ) );

  flag = 0;
  x = coordinate_search ( x, @mckinnon, flag );
  r8vec_print ( n, x, '  Estimated minimizer X1:' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X1) = %g\n', mckinnon ( x ) );

  x = [ 0.0, -0.5 ];
  r8vec_print ( n, x, '  Correct minimizer X*:' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X*) = %g\n', mckinnon ( x ) );
%
%  Test 3
%
  a = ( 1.0 + sqrt ( 33.0 ) ) / 8.0;
  b = ( 1.0 - sqrt ( 33.0 ) ) / 8.0;

  phi = 4000.0;
  tau = 3.0;
  theta = 6.0;

  x = [ a, b ];
  r8vec_print ( n, x, '  Initial point X0:' );
  fprintf ( 1, '  PHI = %f, TAU = %f, THETA = %f\n', phi, tau, theta );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X0) = %g\n', mckinnon ( x ) );

  flag = 0;
  x = coordinate_search ( x, @mckinnon, flag );
  r8vec_print ( n, x, '  Estimated minimizer X1:' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X1) = %g\n', mckinnon ( x ) );

  x = [ 0.0, -0.5 ];
  r8vec_print ( n, x, '  Correct minimizer X*:' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X*) = %g\n', mckinnon ( x ) );
  return
end


function bessel_jx_values_test ( )

%*****************************************************************************80
%
%% BESSEL_JX_VALUES_TEST demonstrates the use of BESSEL_JX_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 April 2007
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BESSEL_JX_VALUES_TEST:\n' );
  fprintf ( 1, '  BESSEL_JX_VALUES stores values of \n' );
  fprintf ( 1, '  the Bessel Jn function for NONINTEGER order.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '     N      X            JN(X)\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, n, x, fx ] = bessel_jx_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %12f  %12f  %24.16e\n', n, x, fx );

  end

  return
end


function yval = r8poly2_val2 ( dim_num, ndata, tdata, ydata, left, tval )

%*****************************************************************************80
%
%% R8POLY2_VAL2 evaluates a parabolic interpolant through tabular data.
%
%  Discussion:
%
%    This routine is a utility routine used by OVERHAUSER_SPLINE_VAL.
%    It constructs the parabolic interpolant through the data in
%    3 consecutive entries of a table and evaluates this interpolant
%    at a given abscissa value.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the dimension of a single data point.
%    DIM_NUM must be at least 1.
%
%    Input, integer NDATA, the number of data points.
%    NDATA must be at least 3.
%
%    Input, real TDATA(NDATA), the abscissas of the data points.
%    The values in TDATA must be in strictly ascending order.
%
%    Input, real YDATA(DIM_NUM,NDATA), the data points
%    corresponding to the abscissas.
%
%    Input, integer LEFT, the location of the first of the three
%    consecutive data points through which the parabolic interpolant
%    must pass.  1 <= LEFT <= NDATA - 2.
%
%    Input, real TVAL, the value of T at which the parabolic
%    interpolant is to be evaluated.  Normally, TDATA(1) <= TVAL <= T(NDATA),
%    and the data will be interpolated.  For TVAL outside this range,
%    extrapolation will be used.
%
%    Output, real YVAL(DIM_NUM), the value of the parabolic
%    interpolant at TVAL.
%

%
%  Check.
%
  if ( left < 1 || ndata-2 < left )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8POLY2_VAL2 - Fatal error!\n' );
    fprintf ( 1, '  LEFT < 1 or NDATA-2 < LEFT.\n' );
    fprintf ( 1, '  LEFT = %d\n', left );
    error ( 'R8POLY2_VAL2 - Fatal error!' );
  end

  if ( dim_num < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8POLY2_VAL2 - Fatal error!\n' );
    fprintf ( 1, '  DIM_NUM < 1.\n' );
    fprintf ( 1, '  DIM_NUM = %d\n', dim_num );
    error ( 'R8POLY2_VAL2 - Fatal error!' );
  end
%
%  Copy out the three abscissas.
%
  t1 = tdata(left);
  t2 = tdata(left+1);
  t3 = tdata(left+2);

  if ( t2 <= t1 || t3 <= t2 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8POLY2_VAL2 - Fatal error!\n' );
    fprintf ( 1, '  T2 <= T1 or T3 <= T2.\n' );
    fprintf ( 1, '  T1 = %f\n', t1 );
    fprintf ( 1, '  T2 = %f\n', t2 );
    fprintf ( 1, '  T3 = %f\n', t3 );
    error ( 'R8POLY2_VAL2 - Fatal error!' );
  end
%
%  Construct and evaluate a parabolic interpolant for the data
%  in each dimension.
%
  for i = 1 : dim_num

    y1 = ydata(i,left);
    y2 = ydata(i,left+1);
    y3 = ydata(i,left+2);

    dif1 = ( y2 - y1 ) / ( t2 - t1 );
    dif2 = ( ( y3 - y1 ) / ( t3 - t1 ) ...
           - ( y2 - y1 ) / ( t2 - t1 ) ) / ( t3 - t2 );

    yval(i) = y1 + ( tval - t1 ) * ( dif1 + ( tval - t2 ) * dif2 );

  end

  return
end


function [ xy, elem ] = triangulate_rectangle ( xl, xr, xn, yb, yt, yn, base )

%*****************************************************************************80
%
%% TRIANGULATE_RECTANGLE makes a triangular grid of a rectangle.
%
%  Discussion:
%
%    The rectangle is presumed to lie between (XL,YB) and (XR,YT).
%
%    We subdivide the rectangle into XN regions in the X direction,
%    and YN regions in the Y direction, and then split each quadrilateral,
%    creating 2 * XN * YN triangular elements.
%
%    The locations of the NN=(XN+1)*(YN+1) nodes are stored in the 2 by NN
%    real array XY.
%
%    The triples of node indices that make up each triangle are stored int
%    the 3 by NE integer array ELEM.  Here, NE = 2 * XN * YN, and the
%    nodes are stored in counterclockwise order.  The node indices will
%    begin at 0 if the input quantity BASE is set to 0.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 March 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real XL, XR, the left and right X limits.
%
%    Input, integer XN, the number of elements along the X direction.
%
%    Input, real YB, YT, the bottom and top Y limits.
%
%    Input, integer YN, the number of elements along the Y direction.
%
%    Input, integer BASE, the indexing base:
%    0, the first node has index 0.
%    1, the first node has index 1.
%
  if ( nargin < 7 )
    base = 1;
  end
%
%  Generate 1D data.
%
  x1d = linspace ( xl, xr, xn + 1 );
  y1d = linspace ( yb, yt, yn + 1 );
%
%  Create (X,Y) table.
%  This can be done faster, using meshgrid, but then you lose control
%  of ordering and arranging.
%
  xyn = ( xn + 1 ) * ( yn + 1 );
  xy = zeros ( 2, xyn );

  k = 0;
  for j = 1 : yn + 1
    for i = 1 : xn + 1
      k = k + 1;
      xy(1,k) = x1d(i);
      xy(2,k) = y1d(j);
    end
  end
%
%  Create connectivity.
%
  en = 2 * xn * yn;
  elem = zeros ( 3, en );
  e = 0;

  for j = 1 : yn
    for i = 1 : xn
      sw = ( j - 1 ) * ( xn + 1 ) + i;
      se = ( j - 1 ) * ( xn + 1 ) + i + 1;
      ne = ( j     ) * ( xn + 1 ) + i + 1;
      nw = ( j     ) * ( xn + 1 ) + i;
      e = e + 1;
      elem(1,e) = sw;
      elem(2,e) = se;
      elem(3,e) = nw;
      e = e + 1;
      elem(1,e) = ne;
      elem(2,e) = nw;
      elem(3,e) = se;
    end
  end
%
%  Shift base if requested.
%
  if ( base == 0 )
    elem = elem - 1;
  end

  return
end


function [ grid_order, grid_point ] = cc_grids_minmax ( dim_num, q_min, ...
  q_max, grid_num, point_num )

%*****************************************************************************80
%
%% CC_GRIDS_MINMAX computes CC orders and grids with Q_MIN <= Q <= Q_MAX.
%
%  Discussion:
%
%    The necessary dimensions of GRID_ORDER and GRID_POINT can be
%    determined by calling CC_GRIDS_MINMAX first.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 October 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer Q_MIN, Q_MAX, the minimum and maximum values of
%    Q, the sum of the orders in each spatial coordinate.
%
%    Input, integer GRID_NUM, the number of Clenshaw Curtis
%    grids whose Q value is between Q_MIN and Q_MAX.
%
%    Input, integer POINT_NUM, the total number of points in the grids.
%
%    Output, integer GRID_ORDER(DIM_NUM,GRID_NUM), contains, for each
%    grid, the order of the Clenshaw-Curtis rule in each dimension.
%
%    Output, real GRID_POINT(DIM_NUM,POINT_NUM), contains
%    a list of all the abscissas of all the rules, listed one grid at
%    a time.  If a point occurs in several grids, it will be listed
%    several times.
%

%
%  Outer loop generates Q's from Q_MIN to Q_MAX.
%
  point_num = 0;
  grid_num = 0;

  for q = q_min : q_max
%
%  Middle loop generates next partition that adds up to Q.
%
    more = 0;
    order_1d = [];

    while ( 1 )

      [ order_1d, more ] = compnz_next ( q, dim_num, order_1d, more );
%
%  Inner (hidden) loop generates all CC points corresponding to given grid.
%
      order_nd = prod ( order_1d(1:dim_num) );

      grid_point(1:dim_num,point_num+1:point_num+order_nd) = cc_grid ( ...
        dim_num, order_1d, order_nd );

      point_num = point_num + order_nd;

      grid_num = grid_num + 1;
      grid_order(1:dim_num,grid_num) = order_1d(1:dim_num);

      if ( ~more )
        break
      end

    end

  end

  return
end


function xy = padua_points ( l )

%*****************************************************************************80
%
%% PADUA_POINTS returns the Padua points of level L.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Marco Caliari, Stefano de Marchi, Marco Vianello,
%    Bivariate interpolation on the square at new nodal sets,
%    Applied Mathematics and Computation,
%    Volume 165, Number 2, 2005, pages 261-274.
%
%  Parameters:
%
%    Input, integer L, the level of the set.
%    0 <= L
%
%    Output, real XY(2,N), the Padua points of level L.
%
  n = ( ( l + 1 ) * ( l + 2 ) ) / 2;

  xy = zeros ( 2, n );

  if ( l == 0 )
    xy(1,1) = 0.0;
    xy(2,1) = 0.0;
    return
  end

  k = 0;

  for i = 0 : l

    j_hi = floor ( l / 2 ) + 1;
    if ( mod ( l, 2 ) == 1 && mod ( i, 2 ) == 1 )
      j_hi = j_hi + 1;
    end;

   for j = 1 : j_hi

     k = k + 1;

      if ( i * 2 == l )
        xy(1,k) = 0.0;
      else
        angle1 = i * pi / l;
        xy(1,k) = cos ( angle1 );
      end

      if ( mod ( i, 2 ) == 0 )
        if ( 2 * ( 2 * j - 1 ) == l + 1 )
          xy(2,k) = 0.0;
        else
          angle2 = ( 2 * j - 1 ) * pi / ( l + 1 );
          xy(2,k) = cos ( angle2 );
        end
      else
        if ( 2 * ( 2 * j - 2 ) == l + 1 )
          xy(2,k) = 0.0;
        else
          angle2 = ( 2 * j - 2 ) * pi / ( l + 1 );
          xy(2,k) = cos ( angle2 );
        end
      end

    end

  end

  return
end


function linplus_test44 ( )

%*****************************************************************************80
%
%% TEST44 tests R8LT_DET, R8LT_INVERSE, R8LT_MXM, R8LT_RANDOM.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  n = 5;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST44\n' );
  fprintf ( 1, '  For a matrix in lower triangular storage,\n' );
  fprintf ( 1, '  R8LT_DET computes the determinant.\n' );
  fprintf ( 1, '  R8LT_INVERSE computes the inverse.\n' );
  fprintf ( 1, '  R8LT_MXM computes matrix products.\n' );
  fprintf ( 1, '  R8LT_RANDOM sets a random value.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix order N = %d\n', n );

  [ a, seed ] = r8lt_random ( n, n, seed );

  r8lt_print ( n, n, a, '  Matrix A:' );
%
%  Compute the determinant.
%
  det = r8lt_det ( n, a );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Determinant is %f\n', det );
%
%  Compute the inverse matrix.
%
  a_inverse = r8lt_inverse ( n, a );

  r8lt_print ( n, n, a_inverse, '  Inverse matrix B:' );
%
%  Check
%
  c = r8lt_mxm ( n, a, a_inverse );

  r8lt_print ( n, n, c, '  Product A * B:' );

  return
end


function truncated_normal_ab_pdf_test ( )

%*****************************************************************************80
%
%% TRUNCATED_NORMAL_AB_PDF_TEST tests TRUNCATED_NORMAL_AB_PDF;
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 September 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TRUNCATED_NORMAL_AB_PDF_TEST\n' );
  fprintf ( 1, '  TRUNCATED_NORMAL_AB_PDF evaluates the Truncated Normal PDF.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The "parent" normal distribution has\n' );
  fprintf ( 1, '    mean = mu\n' );
  fprintf ( 1, '    standard deviation = sigma\n' );
  fprintf ( 1, '  The parent distribution is truncated to\n' );
  fprintf ( 1, '  the interval [a,b]\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '                                                           Stored         Computed\n' );
  fprintf ( 1, '       X        Mu         S         A         B             PDF             PDF\n' );
  fprintf ( 1, '\n');

  n_data = 0;

  while ( 1 )

    [ n_data, mu, sigma, a, b, x, pdf1 ] ...
      = truncated_normal_ab_pdf_values ( n_data );

    if ( n_data == 0 )
      break
    end

    pdf2 = truncated_normal_ab_pdf ( x, mu, sigma, a, b );

    fprintf( 1, '  %8.1f  %8.1f  %8.1f  %8.1f  %8.1f  %14g  %14g\n', x, mu, sigma, a, b, pdf1, pdf2 );

  end

  return
end


function [xlat, alt] = geodet2(decl, rmag)

% geocentric to geodetic coordinates
% exact solution (Borkowski, 1989)

% input

%  decl = geocentric declination (radians)
%  rmag = geocentric distance (kilometers)

% output

%  xlat = geodetic latitude (radians)
%  alt  = geodetic altitude (kilometers)

% Orbital Mechanics with Matlab

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

global req flat

fr = 1 / flat;

% determine x and z components of the geocentric distance

rx = rmag * cos(decl);

rz = rmag * sin(decl);

% compute geodetic latitude and altitude

if (rz == 0)
   % special case - equatorial

   xlat = 0;
   alt = rmag - req;

elseif (abs(decl) == 0.5 * pi)
   % special case - polar

   xlat = decl;
   alt = rmag - (req - req/fr);

else
   % general case

   b = sign(rz) * (req - req/fr);

   e = ((rz + b) * b/req - req)/rx;

   f = ((rz - b) * b/req + req)/rx;

   p = (e * f + 1) * 4/3;

   q = (e * e - f * f) * 2;

   d = p * p * p + q * q;

   if (d >= 0.0)
      s = sqrt(d) + q;
      s = sign(s) * (exp(log(abs(s))/3));
      v = p/s - s;
      v = -(q + q + v * v * v)/(3 * p);
   else
      v = 2 * sqrt(-p) * cos(acos(q/p/sqrt(-p))/3);
   end

   g = 0.5 * (e + sqrt(e * e + v));

   t = sqrt(g * g + (f - v * g)/(g + g - e)) - g;

   xlat = atan((1 - t * t) * req/(2 * b * t));

   alt = (rx - req * t) * cos(xlat) + (rz - b) * sin(xlat);
end


function H = histcImWin( I, edges, wtMask, shape )
% Calculates local histograms at every point in an image I.
%
% H(i,j,...,k,:) will contain the histogram at location (i,j,...,k), as
% calculated by weighing values in I by placing wtMask at that
% location.  For example, if wtMask is ones(windowSize) then the
% histogram at every location will simply be a histogram of the pixels
% within that window.  See histc2 for more information about histgorams.
% See convnFast for information on shape flags.
%
% USAGE
%  H = histcImWin( I, edges, wtMask, [shape] )
%
% INPUTS
%  I           - image (possibly multidimensional) [see above]
%  edges       - quantization bounds, see histc2
%  wtMask      - numeric array of weights, or cell array of sep kernels
%  shape       - ['full'], 'valid', 'same', or 'smooth'
%
% OUTPUTS
%  H           - [size(I)xnBins] array of size(I) histograms
%
% EXAMPLE
%  load trees; L=conv2(X,filterDog2d(10,4,1,0),'valid'); figure(1); im(L);
%  f1=filterGauss(25,[],25);  f2=ones(1,15);
%  H1 = histcImWin(L, 15, {f1,f1'}, 'same');  figure(2); montage2(H1);
%  H2 = histcImWin(L, 15, {f2,f2'}, 'same');  figure(3); montage2(H2);
%
% See also ASSIGNTOBINS, HISTC2, CONVNFAST, HISTCIMLOC
%
% Piotr's Image&Video Toolbox      Version 2.0
% Copyright 2012 Piotr Dollar.  [pdollar-at-caltech.edu]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the Simplified BSD License [see external/bsd.txt]

if( nargin<4 || isempty(shape) ); shape = 'full';  end;
if( ~iscell(wtMask) ); wtMask={wtMask}; end;

% split I into channels
I = assignToBins( I, edges );
nBins=length(edges)-1; if(nBins==0); nBins=edges; end;
nd = ndims(I); siz=size(I);  maxI = max(I(:));
if( nd==2 && siz(2)==1); nd=1; siz=siz(1); end;
QI = false( [siz maxI] );
inds = {':'}; inds = inds(:,ones(1,nd));
for i=1:nBins;  QI(inds{:},i)=(I==i); end;
H = double( QI );

% convolve with wtMask to get histograms, scale appropriately
for i=1:length(wtMask)
  wtMaski = wtMask{i};
  for d=1:ndims(wtMaski); wtMaski = flipdim(wtMaski,d); end;
  wtMaski = wtMaski / sum(wtMaski(:));
  H = convnFast( H, wtMaski, shape );
end;


function value = index_to_level_open ( dim_num, t, order, level_max )

%*****************************************************************************80
%
%% INDEX_TO_LEVEL_OPEN determines the level of a point given its index.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 April 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Fabio Nobile, Raul Tempone, Clayton Webster,
%    A Sparse Grid Stochastic Collocation Method for Partial Differential
%    Equations with Random Input Data,
%    SIAM Journal on Numerical Analysis,
%    Volume 46, Number 5, 2008, pages 2309-2345.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer T(DIM_NUM), the grid index of a point.
%
%    Input, integer ORDER, the order of the rule.
%
%    Input, integer LEVEL_MAX, the level with respect to which the
%    index applies.
%
%    Output, integer VALUE, the first level on which
%    the point associated with the given index will appear.
%
  value = 0;

  for dim = 1 : dim_num

    s = round ( t(dim) );

    s = i4_modp ( t, order );

    if ( s == 0 )

      level = 0;

    else

      level = level_max;

      while ( mod ( s, 2 ) == 0 )
        s = floor ( s / 2 );
        level = level - 1;
      end

    end

    if ( level == 0 )
      level = 1;
    elseif ( level == 1 )
      level = 0;
    end

    value = value + level;

  end

  return
end


function [X,D,e] = sparse_solver_irls(p, A, Y, beta, terminationValue, maxIterations)
%SPARSE_SOLVER_IRLS Steered-response power map using a MVDR beamformer
%
%   This routine performs sparse recovery using the iterative reweighted
%   least squares (IRLS) algorithm, with an L_{p,2} norm, assuming
%   spatiotemporal signals Y, of M channels/sensors/microphones and T
%   temporal frames (snapshots), and a linear mixing model of the form
%   Y = A*X, where A is a MxK overcomplete basis dictionary, with K>>M.
%   The original algorithm is described in:
%
%   Chartrand, R., & Yin, W. (2008). Iteratively reweighted algorithms for
%   compressive sensing. In 2008 IEEE International Conference on Acoustics,
%   Speech and Signal Processing (ICASSP), pp. 3869-3872.
%
%   and further inspired by the microphone array processing work of
%
%   Wabnitz, A., Epain, N., McEwan, A., & Jin, C. (2011). Upscaling ambisonic
%   sound scenes using compressed sensing techniques. In 2011 IEEE Workshop
%   on Applications of Signal Processing to Audio and Acoustics (WASPAA).
%
%   Inputs:
%       p:  sparsity exponent p<=1, of the L_{p,2} norm
%       A:  overcomplete dictionary (in CS jargon) of steering vectors for
%           a dense grid of K directions, with A of size MxK.
%       Y:  measurement vectors (in CS jargon), multichannel microphone signals
%           in audio/acoustics, of size MxT
%       beta:   regularization parameter
%       terminationValue:   error threshold to stop iterations in the
%                           solver
%       maxIterations:  maximum number of iterations before stopping the
%                       solver, if terminationValue is not reached
%
%   Outputs:
%       X:  KxT sparse signal solution
%       D:  demixing matrix of size KxM
%       e:  Kx1 sparse signal powers
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% SPARSE_SOLVER_IRLS.M - 13/5/2019
% Archontis Politis, archontis.politis@tuni.fi
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargin<6
    maxIterations = 0;
end

nChan = size(A,1); % number of channels/sensors
nDict = size(A,2); % size of steering vector dictionary
nSnap = size(Y,2); % size of temporal snapshots (e.g. STFT frames, or time-domain samples)
% initial values for amplitudes and weights
W = eye(nDict);
minEpsilon = terminationValue;
epsilon = 1;
n = 0;

if nSnap>nChan
    % dimensionality reduction using SVD
    [U, L, ~] = svd(Y);
    L_trunc = L(:,1:nChan);
    Y2 = U*L_trunc;
else
    Y2 = Y;
end
X_prev = A\Y2; % initial LS solution
while epsilon>minEpsilon
    % regularization
    gamma = beta/(1-beta) * trace(A*W*A')/nChan;
    % gamma = beta^0.15/(1-beta^0.15) * trace(A*W*A')/nChan; % Epain & Jin
    % reconstruction matrix
    D = (W*A')/(A*W*A' + gamma*eye(nChan));
    % amplitude estimates
    X_current = D * Y2;
    % norm of difference between previous and current solution
    er_k = sqrt(sum((X_current - X_prev).^2,2));
    er = sqrt(sum(er_k.^2));
    % sparse solution signal energies
    e = sum(X_current.^2,2);
    e_max = max(e);
    epsilon = e_max/nDict; % Epain & Jin
%     if er < epsilon^(1/2)/100 % Chartrand & Yin
%         epsilon = epsilon /10;
%     end
    w = (e + epsilon).^(1-p/2);
    W = diag(w);

    n = n+1;
    disp(['Iteration ' num2str(n) ': ' num2str(epsilon)])
    if maxIterations
        if n>maxIterations, break; end
    end
end
if nSnap>nChan
    D = X_current*inv(U*L_trunc);
    X = D*Y;
else
    X = X_current;
end


function [ point_num, edge_num, face_num, face_order_max ] = ...
  soccer_size_3d ( )

%*****************************************************************************80
%
%% SOCCER_SIZE_3D gives "sizes" for a truncated icosahedron in 3D.
%
%  Discussion:
%
%    The shape is a truncated icosahedron, which is the design used
%    on a soccer ball.  There are 12 pentagons and 20 hexagons.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 July 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    http://polyhedra.wolfram.com/uniform/u25.html
%
%  Parameters:
%
%    Output, integer POINT_NUM, the number of points.
%
%    Output, integer EDGE_NUM, the number of edges.
%
%    Output, integer FACE_NUM, the number of faces.
%
%    Output, integer FACE_ORDER_MAX, the maximum order of any face.
%
  point_num = 60;
  edge_num = 90;
  face_num = 32;
  face_order_max = 6;

  return
end