%% Analyzing Neural Time Series Data
% Matlab code for Chapter 16
% Mike X Cohen
%
% This code accompanies the book, titled "Analyzing Neural Time Series Data"
% (MIT Press). Using the code without following the book may lead to confusion,
% incorrect data analyses, and misinterpretations of results.
% Mike X Cohen assumes no responsibility for inappropriate or incorrect use of this code.

%% Figure 16.1

load sampleEEGdata

channel2plot = 'o1';
timewin      = 400; % in ms

timewinidx = round(timewin/(1000/EEG.srate));
tapers     = dpss(timewinidx,5); % this line will crash without matlab signal processing toolbox

% extract a bit of EEG data
d = detrend(squeeze(EEG.data(strcmpi(channel2plot,{EEG.chanlocs.labels}),200:200+timewinidx-1,10)));

% plot EEG data snippet
figure
subplot(5,2,1)
plot(d)
axis tight,axis off

% plot tapers
for i=1:5
    subplot(5,2,(2*(i-1))+2)
    plot(tapers(:,i))
    axis tight,axis off
end

% plot taper.*data
figure
for i=1:5
    subplot(5,2,(2*(i-1))+1)
    plot(tapers(:,i).*d')
    axis tight,axis off
end

% plot fft of taper.*data
f=zeros(5,timewinidx);
for i=1:5
    subplot(5,2,(2*(i-1))+2)
    f(i,:)=fft(tapers(:,i).*d');
    plot(abs(f(i,1:timewinidx/2)).^2)
    axis tight,axis off
end

figure
subplot(5,2,2)
plot(mean(abs(f(:,1:timewinidx/2)).^2,1))
axis tight, axis off

subplot(5,2,3)
hann = .5*(1-cos(2*pi*(1:timewinidx)/(timewinidx-1)));
plot(hann)
axis tight, axis off

subplot(525)
plot(hann.*d)
axis tight, axis off

subplot(526)
ff=fft(hann.*d);
plot(mean(abs(ff(1:timewinidx/2)).^2,1))
axis tight, axis off

%% Figure 16.2

channel2plot    = 'p7';
frequency2plot  = 15;  % in Hz
timepoint2plot  = 200; % ms

nw_product      = 3;  % determines the frequency smoothing, given a specified time window
times2save      = -300:50:1000;
baseline_range  = [-200 -00];
timewin         = 400; % in ms

% convert time points to indices
times2saveidx = dsearchn(EEG.times',times2save');
timewinidx    = round(timewin/(1000/EEG.srate));

% find baselinetimepoints
baseidx = zeros(size(baseline_range));
[~,baseidx(1)] = min(abs(times2save-baseline_range(1)));
[~,baseidx(2)] = min(abs(times2save-baseline_range(2)));
% note that the following line is equivalent to the previous three
%baseidx = dsearchn(times2save',baseline_range');

% define tapers
tapers = dpss(timewinidx,nw_product); % note that in practice, you'll want to set the temporal resolution to be a function of frequency
% define frequencies for FFT
f = linspace(0,EEG.srate/2,floor(timewinidx/2)+1);

% find logical channel index
chanidx = strcmpi(channel2plot,{EEG.chanlocs.labels});

% initialize output matrix
multitaper_tf = zeros(floor(timewinidx/2)+1,length(times2save));

% loop through time bins
for ti=1:length(times2saveidx)

    % initialize power vector (over tapers)
    taperpow = zeros(floor(timewinidx/2)+1,1);

    % loop through tapers
    for tapi = 1:size(tapers,2)-1

        % window and taper data, and get power spectrum
        data      = bsxfun(@times,squeeze(EEG.data(chanidx,times2saveidx(ti)-floor(timewinidx/2)+1:times2saveidx(ti)+ceil(timewinidx/2),:)),tapers(:,tapi));
        pow       = fft(data,timewinidx)/timewinidx;
        pow       = pow(1:floor(timewinidx/2)+1,:);
        taperpow  = taperpow + mean(pow.*conj(pow),2);
    end

    % finally, get power from closest frequency
    multitaper_tf(:,ti) = taperpow/tapi;
end

% db-correct
db_multitaper_tf = 10*log10( multitaper_tf ./ repmat(mean(multitaper_tf(:,baseidx(1):baseidx(2)),2),1,length(times2save)) );

% plot time courses at one frequency band
figure
subplot(121)
[junk,freq2plotidx]=min(abs(f-frequency2plot)); % can replace "junk" with "~"
plot(times2save,mean(log10(multitaper_tf(freq2plotidx-2:freq2plotidx+2,:)),1))
title([ 'Sensor ' channel2plot ', ' num2str(frequency2plot) ' Hz' ])
axis square
set(gca,'xlim',[times2save(1) times2save(end)])

subplot(122)
[junk,time2plotidx]=min(abs(times2save-timepoint2plot));
plot(f,log10(multitaper_tf(:,time2plotidx)))
title([ 'Sensor ' channel2plot ', ' num2str(timepoint2plot) ' ms' ])
axis square
set(gca,'xlim',[f(1) 40])

% plot full TF map
figure
contourf(times2save,f,db_multitaper_tf,40,'linecolor','none')
set(gca,'clim',[-2 2])
xlabel('Time (ms)'), ylabel('Frequency (Hz)')
title([ 'Power via multitaper from channel ' channel2plot ])

%% end.


function dn = dudiv(dn0,dn1)

% DDIV dual number division
%
%   DN = DDIV(DN0,DN1) returns the dual number DN which is the dual number
%     division DN0/DN1 of the dual numbers DN0 and DN1
%      - DN0 (resp. DN1) is a dual number (DN0 = a0 + eps*b0, eps^2 = 0).
%         It is a 2-vector or a 2*N array (column i represents dual number
%         i) where N is the number of dual numbers. DN0 and DN1 must have
%         the same size. The non-dual part of DN1 must be different from 0.
%      - DN is a dual number. It is a 2*N array (each column is the dual
%          multiplication of the corresponding columns in DN0 and DN1)

s0 = size(dn0);
s1 = size(dn1);
if s0 == [1 2]
    dn0 = dn0.';
    s0 = size(dn0);
end
if s1 == [1 2]
    dn1 = dn1.';
    s1 = size(dn1);
end

% wrong format
if s0(1) ~= 2 || s1(1) ~= 2
    error('DualQuaternion:Ddiv:wrongsize',...
        '%d rows in the DN0 array and %d rows in the DN1 array. It should be 2 for both.',...
        s0(1),s1(1));
end

% sizes do not match
n1 = s0(2);
n2 = s1(2);
if n1 ~= n2
    error('DualQuaternion:Ddiv:notMatch',...
        '%d dual numbers in DN0 array and %d dual numbers in DN1 array.They should be equal.',...
        n1,n2);
end

dn = sym(zeros(2,n1));
dn(1,:) = dn0(1,:)./dn1(1,:);
dn(2,:) = dn0(2,:)./dn1(1,:)-dn0(1,:).*dn1(2,:)./(dn1(1,:).^2);


% Author: Jai Juneja
% Date: 22/03/2013
%
% Find the shortest Euclidean distance from a point to a line.
% Inputs:
%   pt  :   point in space
%   v1  :   first point along line
%   v2  :   second point along line
%           pt, v1 and v2 must all be at least 3x1 vectors
% Outputs:
%   d   :   shortest distance from point pt to line
function d = pointToLine(pt, v1, v2)
    a = v1 - v2;
    b = pt - v2;
    d = norm(cross(a,b)) / norm(a);
end


function price = PROJ_GMDB_ComboExpos_Linear( P, Pbar, S_0, W, call, r, params_levy, params_mort, T)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% About: Pricing Function for Gauranteed Minimum Death Benefit Options using PROJ method
%        This version assumes a comination of exponentials model of mortality (see first reference below)
% Models Supported: Levy Processes, including jump diffusions and Black-Scholes model
% Returns: price of contract
%
% Author: Zhimin Zhang  (Original Code)
%         Justin Lars Kirkby (Convert into common framework)
%
% References:  (1) Valuing Equity-Linked Death Benefits in General Exponential
%               Levy Models, J. Comput. and Appl. Math. 2019 (Z. Zhang, Y. Yong, W. Yu)
%              (2) Efficient Option Pricing By Frame Duality with The Fast
%               Fourier Transform, SIAM J. Financial Math., 2015 (J.L. Kirkby)
%
% ----------------------
% Contract/Model Params
% ----------------------
% S_0 = initial stock price (e.g. 100)
% W   = strike  (e.g. 100)
% r   = interest rate (e.g. 0.05)
% T   = time remaining until maturity (in years, e.g. T=10)
%       NOTE: set T=-1 to price a perpetual contract (no expiry)
% call  = 1 for call (else put)
% params_levy = parameters of Levy Model
% params_mort = mortality params
%
% ----------------------
% Numerical (PROJ) Params
% ----------------------
% P  = resolution parameter (increase P to use more basis elements)
% Pbar = gridwidth parameter (increase Pbar to increase the truncated density support)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Set Contract / Numerical Params
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
kk=log(W/S_0);

a=2^P;
hat_a=2^Pbar;

N=hat_a*a;
Delta=1/a;Delta_s=2*pi/hat_a;

% Deterime if there is a Time to expiry in contract, else it's perpetual
if nargin < 8
    T = -1;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Parse Death Model Params
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
lambda = params_mort.lambda;  % expo rates
A = params_mort.A; % coefficients in expo combo

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Levy Parse Model Params, set model inputs
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model = params_levy.model;

if model == 1 %BSM (Black Scholes Merton)
    sigma = params_levy.sigmaBSM;

    Psi=@(x)-1/2*sigma^2*x.^2;
    mu=r-Psi(-1i);

    phi=@(s)mu*1i*s-1/2*sigma^2*s.^2;

    if T > 0
        diffFgc=@(c)sum(A.*lambda*(mu+sigma^2*c)./(r+lambda-phi(-1i*c)).^2.*(1-exp(-(r+lambda-phi(-1i*c))*T)-T*exp(-(r+lambda-phi(-1i*c))*T).*(r+lambda-phi(-1i*c))));
    else
        diffFgc=@(c)sum(A.*lambda*(mu+sigma^2*c)./(r+lambda-phi(-1i*c)).^2);
    end

elseif model == 3 %NIG
    alpha = params_levy.alpha;
    beta = params_levy.beta;
    NIG_delta = params_levy.delta;
    sigma = params_levy.sigma;

    gamma=sqrt(alpha^2-beta^2);

    Psi=@(x)-1/2*sigma^2*x.^2-NIG_delta*(sqrt(alpha^2-(beta+1i*x).^2)-gamma);
    mu=r-Psi(-1i);

    phi=@(s)mu*1i*s-1/2*sigma^2*s.^2-NIG_delta*(sqrt(alpha^2-(beta+1i*s).^2)-gamma);

    if T > 0
        diffFgc=@(c)sum(A.*lambda*(mu+NIG_delta*sqrt(alpha^2-(beta+c).^2).*(beta+c))./(r+lambda-phi(-1i*c)).^2.*(1-exp(-(r+lambda-phi(-1i*c))*T)-T*exp(-(r+lambda-phi(-1i*c))*T).*(r+lambda-phi(-1i*c))));
    else
        diffFgc=@(c)sum(A.*lambda*(mu+NIG_delta*sqrt(alpha^2-(beta+c).^2).*(beta+c))./(r+lambda-phi(-1i*c)).^2);
    end

elseif model == 4 %MJD (Merton Jump Diffusion)
    sigma = params_levy.sigma;
    lambda_J = params_levy.lam;
    mu_J = params_levy.muj;
    sigma_J = params_levy.sigmaj;

    Psi=@(x)-sigma^2/2*x.^2+lambda_J*(exp(1i*x*mu_J-sigma_J^2/2*x.^2)-1);
    mu=r-Psi(-1i);

    phi=@(s)1i*s*mu-sigma^2/2*s.^2+lambda_J*(exp(1i*s*mu_J-sigma_J^2/2*s.^2)-1);

    if T > 0
        diffFgc=@(c)sum(A.*lambda*(mu+sigma^2*c+lambda_J*exp(mu_J*c+1/2*sigma_J^2*c).*(mu_J+sigma_J^2*c))./(r+lambda-phi(-1i*c)).^2.*(1-exp(-(r+lambda-phi(-1i*c))*T)-T*exp(-(r+lambda-phi(-1i*c))*T).*(r+lambda-phi(-1i*c))));
    else
        diffFgc=@(c)sum(A.*lambda*(mu+sigma^2*c+lambda_J*exp(mu_J*c+1/2*sigma_J^2*c).*(mu_J+sigma_J^2*c))./(r+lambda-phi(-1i*c)).^2);
    end

elseif model == 5 %Kou Double Expo

    sigma = params_levy.sigma;
    lam_pois = params_levy.lam;
    p = params_levy.p_up;    omega=p*lam_pois;  nv=(1-p)*lam_pois;
    v = params_levy.eta1;
    w = params_levy.eta2;

    D=1/2*sigma^2;

    %risk-neutral condition
    Psi=@(x)-D*x.^2-nv.*1i.*x./(v+1i.*x)+omega.*1i.*x./(w-1i.*x);
    mu=r-Psi(-1i);

    phi=@(s)mu.*1i.*s-D.*s.^2-nv.*1i.*s./(v+1i.*s)+omega.*1i.*s./(w-1i.*s);

    if T > 0
        diffFgc=@(c)sum(A.*lambda*(mu+sigma^2*c-nv*v./(v+c).^2+omega*w./(w-c).^2)./(r+lambda-phi(-1i*c)).^2.*(1-exp(-(r+lambda-phi(-1i*c))*T)-T*exp(-(r+lambda-phi(-1i*c))*T).*(r+lambda-phi(-1i*c))));
    else
        diffFgc=@(c)sum(A.*lambda*(mu+sigma^2*c-nv*v./(v+c).^2+omega*w./(w-c).^2)./(r+lambda-phi(-1i*c)).^2);
    end

elseif model == 8 % Variance Gamma
    sigma = params_levy.sigmaGBM; % geometric brownian motion add-on
    VG_mu = params_levy.theta;
    VG_sigma = params_levy.sigma;
    nv = params_levy.nu;

    Psi=@(x)-1/2*sigma^2*x.^2-1/nv*log(1-1i*nv*VG_mu*x+nv*VG_sigma^2/2*x.^2);
    mu=r-Psi(-1i);

    phi=@(s)mu*1i*s-1/2*sigma^2*s.^2-1/nv*log(1-1i*nv*VG_mu*s+nv*VG_sigma^2/2*s.^2);

    if T > 0
        diffFgc=@(c)sum(A.*lambda*(mu+sigma^2*c-(VG_mu*1i*c-VG_sigma^2*c)./(1-nv*VG_mu*c-1/2*nv*VG_sigma^2*c.^2))./(r+lambda-phi(-1i*c)).^2.*(1-exp(-(r+lambda-phi(-1i*c))*T)-T*exp(-(r+lambda-phi(-1i*c))*T).*(r+lambda-phi(-1i*c))));
    else
        diffFgc=@(c)sum(A.*lambda*(mu+sigma^2*c-(VG_mu*1i*c-VG_sigma^2*c)./(1-nv*VG_mu*c-1/2*nv*VG_sigma^2*c.^2))./(r+lambda-phi(-1i*c)).^2);
    end
end

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

delta1=@(j,k)(j==k);

S=(0:(N-1))*Delta_s;
v=@(j)1-(delta1(j,1)+delta1(j,N))/2;
Fphi=@(s)12.*sin(s./2).^2./(s.^2.*(2+cos(s)));
Fphi1=Fphi(S/a); Fphi1(1)=1;

%coefs
if T > 0
    Ak=sum(repmat(A.*lambda,length(S),1)./(r+repmat(lambda,length(S),1)-repmat(phi(S).',1,2)).*(1-exp(-(r+repmat(lambda,length(S),1)-repmat(phi(S).',1,2))*T)),2);
    Ck=sum(repmat(A.*lambda,length(S),1)./(r+repmat(lambda,length(S),1)-repmat(phi(S-1i).',1,2)).*(1-exp(-(r+repmat(lambda,length(S),1)-repmat(phi(S-1i).',1,2))*T)),2);

    Fgc=@(c)sum(A.*lambda./(r+lambda-phi(-1i*c)).*(1-exp(-(r+lambda-phi(-1i*c))*T)));
else
    Ak=sum(repmat(A.*lambda,length(S),1)./(r+repmat(lambda,length(S),1)-repmat(phi(S).',1,2)),2);
    Ck=sum(repmat(A.*lambda,length(S),1)./(r+repmat(lambda,length(S),1)-repmat(phi(S-1i).',1,2)),2);

    Fgc=@(c)sum((A.*lambda)./(r+lambda-phi(-1i*c)));
end

% Compute Projections
x10=real(double(diffFgc(0))/Fgc(0))-Delta*(N/2-1);
X0=x10+(0:(N-1))*Delta;
beta0=a^(-1/2)/pi*real(fft((Ak.').*Fphi1.*v(1:N).*Delta_s.*exp(-1i*x10.*S)));

x11=real(double(diffFgc(1))/Fgc(1))-Delta*(N/2-1);
X1=x11+(0:(N-1))*Delta;
beta1=a^(-1/2)/pi*real(fft((Ck.').*Fphi1.*v(1:N).*Delta_s.*exp(-1i*x11.*S)));

% Compute Final Payoff and Value
nn=find((kk<X0)==1);
n=nn(1);
PHi0=zeros(1,length(S));

nn1=find((kk<X1)==1);
n1=nn1(1);
PHi1=zeros(1,length(S));

if call == 1
    PHi0(n)=a^(1/2)*(X0(n)-kk)-a^(3/2)/2*(kk-X0(n))^2+1/2*a^(-1/2);
    PHi0(n-1)=a^(1/2)*(X0(n-1)+1/a-kk)-a^(3/2)/2*(1/(a^2)-(kk-X0(n-1))^2);
    PHi0(n+1:length(S))=1/sqrt(a);

    PHi1(n1)=a^(1/2)*(X1(n1)-kk)-a^(3/2)/2*(kk-X1(n1))^2+1/2*a^(-1/2);
    PHi1(n1-1)=a^(1/2)*(X1(n1-1)+1/a-kk)-a^(3/2)/2*(1/(a^2)-(kk-X1(n1-1))^2);
    PHi1(n1+1:length(S))=1/sqrt(a);

    price=S_0*beta1*PHi1.'-W*beta0*PHi0.';
else
    PHi0(n)=a^(1/2)*(kk-X0(n)+1/a)-a^(3/2)/2*(1/(a^2)-(kk-X0(n))^2);
    PHi0(n-1)=a^(1/2)*(kk-X0(n-1))-a^(3/2)/2*(kk-X0(n-1))^2+1/2*a^(-1/2);
    PHi0(1:n-2)=1/sqrt(a);

    PHi1(n1)=a^(1/2)*(kk-X1(n1)+1/a)-a^(3/2)/2*(1/(a^2)-(kk-X1(n1))^2);
    PHi1(n1-1)=a^(1/2)*(kk-X1(n1-1))-a^(3/2)/2*(kk-X1(n1-1))^2+1/2*a^(-1/2);
    PHi1(1:n1-2)=1/sqrt(a);

    price=W*beta0*PHi0.'-S_0*beta1*PHi1.';
end

end


function [yn,en,S] = PRAadapt(un,dn,S)

% PRAadapt          Partial Rank Algorithm (See Section 6.3)
%
% Arguments:
% un                Input signal
% dn                Desired signal
% S                 Adptive filter parameters as defined in PRAinit.m
% yn                History of output signal
% en                History of error signal
%
% by Lee, Gan, and Kuo, 2008
% Subband Adaptive Filtering: Theory and Implementation
% Publisher: John Wiley and Sons, Ltd

M = length(S.coeffs);        % Length of FIR filter
mu = S.step;                 % Step size
P = S.order;                 % Projection order
alpha = S.alpha;             % Small constant
AdaptStart = S.AdaptStart;
w = S.coeffs;                % Weight vector of FIR filter
u = zeros(M,1);              % Input signal vector
A = zeros(M,P);              % Projection matrix

ITER = length(un);           % Length of input sequence
yn = zeros(1,ITER);          % Initialize output sequence to zero
en = zeros(1,ITER);          % Initialize error sequence to zero

if isfield(S,'unknownsys')
    b = S.unknownsys;
    norm_b = norm(b);
    eml = zeros(1,ITER);
    ComputeEML = 1;
else
    ComputeEML = 0;
end

for n = 1:ITER
    u = [un(n); u(1:end-1)]; % Input signal vector [u(n),u(n-1),...,u(n-M+1)]'
    A = [u A(:,1:end-1)];    % Projection matrix
    yn(n) = u'*w;            % Output of adaptive filter
    en(n) = dn(n) - yn(n);
    if ComputeEML == 1;
        eml(n) = norm(b-w)/norm_b;        % System error norm (normalized)
    end
    if (mod(n-1,P)==0)&(n >= AdaptStart)
        e = en(n:-1:n-P+1)';
        w = w + mu*A*inv(A'*A + alpha)*e; % Tap-weight adaptation for the next cycle
    end
end

S.coeffs = w;                             % Coefficient values at the final iteration
if ComputeEML == 1;
    S.eml = eml;
end


function rollPitchYaw = rollPitchYawFromRotation(R)

    % ROLLPITCHYAWFROMROTATION converts a rotation matrix into Euler angles.
    %                          The Euler angles convention follows the one
    %                          of iDyntree and is such that the rotation
    %                          matrix is:  R = Rz(yaw)*Ry(pitch)*Rx(roll).
    %
    % FORMAT: rollPitchYaw = rollPitchYawFromRotation(R)
    %
    % INPUT:  - R = [3 * 3] rotation matrix
    %
    % OUTPUT: - rollPitchYaw = [3 * 1] vector of Euler angles [rad]
    %
    % Authors: Daniele Pucci, Marie Charbonneau, Gabriele Nava
    %
    %          all authors are with the Italian Istitute of Technology (IIT)
    %          email: name.surname@iit.it
    %
    % Genoa, Dec 2017
    %

    %% --- Initialization ---

    % For documentation, see also:
    %
    % http://wiki.icub.org/codyco/dox/html/idyntree/html/classiDynTree_1_1Rotation.html#a600352007d9250f7f227f21db85611f2
    %
    % http://www.geometrictools.com/Documentation/EulerAngles.pdf
    %
    rollPitchYaw = zeros(3,1);

    if (R(3,1) < +1)

        if (R(3,1) > -1)
            rollPitchYaw(2) = asin(-R(3,1));
            rollPitchYaw(3) = atan2(R(2,1),R(1,1));
            rollPitchYaw(1) = atan2(R(3,2), R(3,3));
        else
            % Not a unique solution : roll − yaw = atan2(−R23,R22)
            rollPitchYaw(2) = pi/2;
            rollPitchYaw(3) =-atan2(-R(2,3),R(2,2));
            rollPitchYaw(1) = 0;
        end
    else
        % Not a unique solution : roll − yaw = atan2(−R23,R22)
        rollPitchYaw(2) = -pi/2;
        rollPitchYaw(3) = atan2(-R(2,3),R(2,2));
        rollPitchYaw(1) = 0;
    end
end


function toms097_test02 ( )

%*****************************************************************************80
%
%% TOMS097_TEST02 tests R8MAT_SHORTEST_PATH.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 March 2014
%
%  Author:
%
%    John Burkardt
%
  n = 6;

  a = [ ...
     0.0, -1.0, -1.0, -1.0, -1.0, -1.0; ...
     2.0,  0.0, -1.0, -1.0, -1.0,  5.0; ...
     5.0,  7.0,  0.0, -1.0,  2.0, -1.0; ...
    -1.0,  1.0,  4.0,  0.0, -1.0,  2.0; ...
    -1.0, -1.0, -1.0,  3.0,  0.0,  4.0; ...
    -1.0,  8.0, -1.0, -1.0,  3.0,  0.0 ]';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TOMS097_TEST02\n' );
  fprintf ( 1, '  R8MAT_SHORTEST_PATH uses Floyd''s algorithm to find the\n' );
  fprintf ( 1, '  shortest distance between all pairs of nodes\n' );
  fprintf ( 1, '  in a directed graph, starting from the initial array\n' );
  fprintf ( 1, '  of direct node-to-node distances.\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  In the initial direct distance array, if\n' );
  fprintf ( 1, '    A(I,J) = -1,\n' );
  fprintf ( 1, '  this indicates there is NO directed link from\n' );
  fprintf ( 1, '  node I to node J.  In that case, the value of\n' );
  fprintf ( 1, '  of A(I,J) is essentially "infinity".\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Initial direct-link distance matrix:\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    for j = 1 : n
      fprintf ( 1, '%10.4f', a(i,j) );
    end
    fprintf ( 1, '\n' );
  end

  for j = 1 : n
    for i = 1 : n
      if ( a(i,j) == -1.0 )
        a(i,j) = Inf;
      end
    end
  end

  a = r8mat_shortest_path ( n, a );

  for j = 1 : n
    for i = 1 : n
      if ( a(i,j) == Inf )
        a(i,j) = -1.0;
      end
    end
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  In the final shortest distance array, if\n' );
  fprintf ( 1, '    A(I,J) = -1,\n' );
  fprintf ( 1, '  this indicates there is NO directed path from\n' );
  fprintf ( 1, '  node I to node J.\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Final distance matrix:\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    for j = 1 : n
      fprintf ( 1, '%10.4f', a(i,j) );
    end
    fprintf ( 1, '\n' );
  end

  return
end


function [w,mu,P]=RunnalsGaussMixRed(w,mu,P,K,gammaBound,KMax)
%%RUNNALSGAUSSMIXRED Perform Gaussian mixture reduction using the greedy
%                    merging algorithm by Runnals in [1].
%
%INPUTS: w An NX1 or 1XN vector of weights of the components of the
%          original Gaussian mixture.
%       mu An xDimXN matrix of the means of the vector components of the
%          original Gaussian mixture.
%        P An xDim XxDim XN hypermatrix of the covariance matrices for the
%          components of the original Gaussian mixture.
%        K The minimum desired number of components desired in the mixture
%          after reduction. This will be always achieved unless gammaBound
%          is set. Often, if gammaBound is set, one will just make K=1.
% gammaBound If the number of components is less than or equal to KMax,
%          then reduction will continue until the lowest cost between two
%          components is greater than gammaBound or there are K components
%          left. The default if this parameter is omitted or an empty
%          matrix is passed is Inf, which means that reduction will
%          continue until K components is reached.
%     KMax Regardless of the gammaBound setting, it won't be allowed that
%          more than KMax components are present. The default if omitted or
%          an empty matrix is passed is Inf.
%
%OUTPUTS: w The length K vector of weights of the mixture after reduction.
%           (the length can be less than K if the input mixture had fewer
%           than K components).
%        mu The xDimXK means of the mixture after reduction.
%         P The xDimXxDimXK covariance matrices of the mixture after
%           reduction.
%
%This implements the suboptimal Gaussian mixture reduction algorithm of
%[1]. Care is taken in the implementation to avoid copies and reallocations
%associated with actually resizing the cost array every time something is
%eliminated.
%
%EXAMPLE 1:
%This scalar example plots a full 9-component PDF and then the PDF reduced
%to 5 components using sequential brute-force reduction and Runnals'
%algorithm. Increasing the number of components maintained to 6 makes
%the results of Runnals' algorithm much closer to that of the brute-force
%solution.
% w=[0.03, 0.18, 0.12, 0.19, 0.02, 0.16, 0.06, 0.1, 0.08, 0.06];
% mu=[1.45, 2.20, 0.67, 0.48, 1.49, 0.91, 1.01, 1.42, 2.77, 0.89];
% P=reshape([0.0487, 0.0305, 0.1171, 0.0174, 0.0295,0.0102, 0.0323, 0.0380, 0.0115, 0.0679],[1,1,10]);
%
% K=6;
% %Runnals' cost function.
% [wRed,muRed,PRed]=RunnalsGaussMixRed(w,mu,P,K,[],[],0);
% %Sequential brute-force reduction.
% [wRedBF,muRedBF,PRedBF]=bruteForceGaussMixRed(w,mu,P,K,true);
% numPoints=500;
% xVals=linspace(0,3,numPoints);
% PDFVals1=GaussianMixtureD.PDF(xVals,w,mu,P);
% PDFVals2=GaussianMixtureD.PDF(xVals,wRedBF,muRedBF,PRedBF);
% PDFVals3=GaussianMixtureD.PDF(xVals,wRed,muRed,PRed);
% figure(1)
% clf
% hold on
% plot(xVals,PDFVals1,'-k','linewidth',4)
% plot(xVals,PDFVals2,'--r','linewidth',2)
% plot(xVals,PDFVals3,'-m','linewidth',1)
% legend('Full Mixture','Brute-Force','Cost Function of Runnalls')
% set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
%
%EXAMPLE 2:
%This is similar to example 1, but we only consider Runnal's cost function
%and we set K=1 and use gammaVal to control how many elements it ultimately
%has in the end. In this example, it reduced to from 10 to KNew=6
%components.
% w=[0.03, 0.18, 0.12, 0.19, 0.02, 0.16, 0.06, 0.1, 0.08, 0.06];
% mu=[1.45, 2.20, 0.67, 0.48, 1.49, 0.91, 1.01, 1.42, 2.77, 0.89];
% P=reshape([0.0487, 0.0305, 0.1171, 0.0174, 0.0295,0.0102, 0.0323, 0.0380, 0.0115, 0.0679],[1,1,10]);
%
% K=1;
% gammaVal=0.05;
% %Runnals' cost function.
% [wRed,muRed,PRed]=RunnalsGaussMixRed(w,mu,P,K,gammaVal);
% KNew=length(wRed)%The number of reduced components.
% numPoints=500;
% xVals=linspace(0,3,numPoints);
% PDFVals1=GaussianMixtureD.PDF(xVals,w,mu,P);
% PDFVals2=GaussianMixtureD.PDF(xVals,wRed,muRed,PRed);
% figure(1)
% clf
% hold on
% plot(xVals,PDFVals1,'-k','linewidth',4)
% plot(xVals,PDFVals2,'-m','linewidth',1)
% legend('Full Mixture','Reduced Mixture')
% set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
%
%REFERENCES:
%[1] A. R. Runnalls, "Kullback-Leibler approach to Gaussian mixture
%    reduction," IEEE Transactions on Aerospace and Electronic Systems,
%    vol. 43, no. 3, pp. 989-999, Jul. 2007.
%
%October 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    if(nargin<5||isempty(gammaBound))
        gammaBound=Inf;
    end

    if(nargin<6||isempty(KMax))
        KMax=Inf;
    end

    N=length(w);

    %If no reduction is necessary.
    if(N<=K)
        return;
    end

    %We will only be using one triangular of this matrix.
    M=Inf*ones(N,N);%This is the cost matrix.
    wLogDetPVals=zeros(N,1);
    for k=1:N
        wLogDetPVals(k)=w(k)*log(det(P(:,:,k)));
    end

    %We shall fill the cost matrix with the cost of all pairs.
    for cur1=1:(N-1)
        for cur2=(cur1+1):N
            M(cur1,cur2)=BDist(w(cur1),w(cur2),mu(:,cur1),mu(:,cur2),P(:,:,cur1),P(:,:,cur2),wLogDetPVals(cur1),wLogDetPVals(cur2));
        end
    end

    KCur=N;
    selIdxPresent=true(N,1);
    for mergeRound=1:(N-K)
        [Mm,minRows]=min(M);%Minimize over the rows
        [minVal,minCol]=min(Mm);%Minimize over the columns.
        minRow=minRows(minCol);

        if(minVal>gammaBound&&KCur<=KMax)
            %If the distribution has been sufficiently reduced in terms of
            %cost.
            break;
        end

        %Now we know which two hypotheses to merge: The ones with indices
        %minRow and minCol. We will merge those hypotheses and put the
        %results in minRow.
        wSum=w(minRow)+w(minCol);
        w1=w(minRow)/wSum;
        w2=w(minCol)/wSum;
        muMerged=w1*mu(:,minRow)+w2*mu(:,minCol);
        diff1=mu(:,minRow)-muMerged;
        diff2=mu(:,minCol)-muMerged;
        PMerged=w1*(P(:,:,minRow)+diff1*diff1')+w2*(P(:,:,minCol)+diff2*diff2');
        w(minRow)=wSum;
        mu(:,minRow)=muMerged;
        P(:,:,minRow)=PMerged;
        wLogDetPVals(minRow)=wSum*log(det(PMerged));

        %The column is removed.
        selIdxPresent(minCol)=false;

        %Make all the costs Inf so that nothing will be assigned to the
        %removed column.
        M(minCol,:)=Inf;
        M(:,minCol)=Inf;

        %We must now fill in the costs for the merged estimate, which is in
        %minRow. We shall fill the cost matrix with the cost of all pairs.
        for cur1=1:(minRow-1)
            if(selIdxPresent(cur1))
                M(cur1,minRow)=BDist(w(cur1),w(minRow),mu(:,cur1),mu(:,minRow),P(:,:,cur1),P(:,:,minRow),wLogDetPVals(cur1),wLogDetPVals(minRow));
            end
        end

        for cur2=(minRow+1):N
            if(selIdxPresent(cur2))
                M(minRow,cur2)=BDist(w(minRow),w(cur2),mu(:,minRow),mu(:,cur2),P(:,:,minRow),P(:,:,cur2),wLogDetPVals(minRow),wLogDetPVals(cur2));
            end
        end

        KCur=KCur-1;
    end

    %The merged items are marked with selIdxPresent and can now be all
    %grouped together to be returned.
    w=w(selIdxPresent);
    w=w/sum(w);%Guarantee continued normalization.
    mu=mu(:,selIdxPresent);
    P=P(:,:,selIdxPresent);
end

function val=BDist(w1,w2,mu1,mu2,P1,P2,wLogDetP1,wLogDetP2)
%%BDIST This is the distance measure given in Equation 21 in Section VI-B
%       in [1]. This is a bound related the Kullback-Leibler divergence.
%
%REFERENCES:
%[1] A. R. Runnalls, "Kullback-Leibler approach to Gaussian mixture
%    reduction," IEEE Transactions on Aerospce and Electronic Systems,
%    vol. 43, no. 3, pp. 989-999, Jul. 2007.
%
%October 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.

    wSum=w1+w2;
    w1m=w1/wSum;
    w2m=w2/wSum;

    %In [1], there is a constant factor of (1/2) out front, but since all
    %components have it, we omit it here. Also, the dterminant is a slow
    %step, so we put in explicit determinants for things less than 4D.
    switch(size(mu1,1))
        case 1
            diff=mu1-mu2;
            P=w1m*P1+w2m*P2+w1m*w2m*(diff*diff);
            val=wSum*log(P)-wLogDetP1-wLogDetP2;
        case 2
            diff1=mu1(1)-mu2(1);
            diff2=mu1(2)-mu2(2);
            wmProd=w1m*w2m;
            val=wSum*log((w1m*P1(1,1)+w2m*P2(1,1)+wmProd*(diff1*diff1))*(w1m*P1(2,2)+w2m*P2(2,2)+wmProd*(diff2*diff2))-(w1m*P1(1,2)+w2m*P2(1,2)+wmProd*(diff1*diff2)).^2)-wLogDetP1-wLogDetP2;
        case 3
            diff=mu1-mu2;
            P=w1m*P1+w2m*P2+w1m*w2m*(diff*diff');
            detVal=-P(1,3)*P(2,2)*P(3,1)+P(1,2)*P(2,3)*P(3,1)+P(1,3)*P(2,1)*P(3,2)-P(1,1)*P(2,3)*P(3,2)-P(1,2)*P(2,1)*P(3,3)+P(1,1)*P(2,2)*P(3,3);
            val=wSum*log(detVal)-wLogDetP1-wLogDetP2;
        otherwise
            diff=mu1-mu2;
            P12=w1m*P1+w2m*P2+w1m*w2m*(diff*diff');
            val=wSum*log(det(P12))-wLogDetP1-wLogDetP2;
    end

    %Deal with the case where w1 and w2 are both essentially zero.
    if(~isfinite(val))
        val=0;
    end
end

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


function [ a, done, change ] = vec_gray_next ( n, base, a, done )

%*****************************************************************************80
%
%% VEC_GRAY_NEXT computes the elements of a product space.
%
%  Discussion:
%
%    The elements are produced one at a time.
%
%    This routine handles the case where the number of degrees of freedom may
%    differ from one component to the next.
%
%    A method similar to the Gray code is used, so that successive
%    elements returned by this routine differ by only a single element.
%
%    The routine uses internal static memory.
%
%  Example:
%
%    N = 2, BASE = ( 2, 3 ), DONE = TRUE
%
%     A    DONE  CHANGE
%    ---  -----  ------
%    0 0  FALSE    1
%    0 1  FALSE    2
%    0 2  FALSE    2
%    1 2  FALSE    1
%    1 1  FALSE    2
%    1 0  FALSE    2
%    1 0   TRUE   -1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 May 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Dennis Stanton, Dennis White,
%    Constructive Combinatorics,
%    Springer, 1986,
%    ISBN: 0387963472,
%    LC: QA164.S79.
%
%  Parameters:
%
%    Input, integer N, the number of components.
%
%    Input, integer BASE(N), contains the number of degrees of
%    freedom of each component.  The output values of A will
%    satisfy 0 <= A(I) < BASE(I).
%
%    Input, integer A(N).  On the first call, the input value
%    of A doesn't matter.  Thereafter, it should be the same as
%    its output value from the previous call.
%
%    Input, logical DONE.  On the first call, the user must
%    set DONE to TRUE.  Thereafter, DONE should be set to the output
%    value of DONE on the previous call.
%
%    Output, integer A(N), the next element of the space.
%
%    Output, logical DONE.  If DONE is FALSE, the program has computed
%    another entry, which is contained in A.  If DONE is TRUE,
%    then there are no more entries.
%
%    Output, integer CHANGE, is set to the index of the element whose
%    value was changed.  On return from the first call, CHANGE
%    is 1, even though all the elements have been "changed".  On
%    return with DONE equal to TRUE, CHANGE is -1.
%
  persistent active;
  persistent dir;
%
%  The user is calling for the first time.
%
  if ( done )

    done = 0;
    a(1:n) = 0;

    dir(1:n) = 1;
    active(1:n) = 1;

    for i = 1 : n

      if ( base(i) < 1 )
        fprintf ( 1, '\n' );
        fprintf ( 1, 'VEC_GRAY_NEXT - Warning!\n' );
        fprintf ( 1, '  For index I = %d\n', i );
        fprintf ( 1, '  the nonpositive value of BASE(I) = %d\n',  base(i) );
        fprintf ( 1, '  which was reset to 1!\n' );
        base(i) = 1;
        active(i) = 0;
      elseif ( base(i) == 1 )
        active(i) = 0;
      end

    end

    change = 1;

    return

  end
%
%  Find the maximum active index.
%
  change = -1;

  for i = 1 : n
    if ( active(i) ~= 0 )
      change = i;
    end
  end
%
%  If there are NO active indices, we have generated all vectors.
%
  if ( change == -1 )
    done = 1;
    return
  end
%
%  Increment the element with maximum active index.
%
  a(change) = a(change) + dir(change);
%
%  If we attained a minimum or maximum value, reverse the direction
%  vector, and deactivate the index.
%
  if ( a(change) == 0 | a(change) == base(change) - 1 )
    dir(change) = -dir(change);
    active(change) = 0;
  end
%
%  Activate all subsequent indices.
%
  for i = change + 1 : n
    if ( 1 < base(i) )
      active(i) = 1;
    end
  end

  return
end


function inside = p08_inside ( m, n, point )

%*****************************************************************************80
%
%% P08_INSIDE reports if a point is inside the region in problem 08.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 April 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer N, the number of points.
%
%    Input, real POINT(M,N), the coordinates
%    of the points.
%
%    Output, logical INSIDE(N), is TRUE if the point is in the region.
%
  nv = 6;

  c1 = [ 0.0, 0.0 ];
  c2 = [ 0.6, 0.0 ];

  r1 = 1.0;
  r2 = 0.1;
  theta1 = - pi / 12.0;
  theta2 =   pi / 12.0;
%
%  These coordinates are for a square with one corner at (0.9, 0.0),
%  and sides of length 0.2.  It's somewhat bigger than the "bite"
%  it takes out of the piece of pie.
%
  x_poly = [ ...
    0.9000000,  0.0000000,
    1.0414213, -0.1414213,
    1.1828427,  0.0000000,
    1.0414213,  0.1414213 ]';

  inside(1:n) = 0;

  for j = 1 : n
%
%  Is the point inside the circular sector?
%
    if ( circle_sector_contains_point_2d ( r1, c1, theta1, theta2, ...
       point(1:2,j) ) )
%
%  Is the point NOT inside the box?
%
      if ( ~polygon_contains_point_2d ( 4, x_poly, point(1:2,j) ) )
%
%  Is the point NOT inside the circle?
%
        if ( ~circle_imp_contains_point_2d ( r2, c2, point(1:2,j) ) )
          inside(j) = 1;
        end
      end
    end

  end

  return
end


function [K,dK] = gpcov_ratquad(D, p1, p2, p3)
% f = 1 + D.^2/(2*p3^2*p2^2);
% K = p1^2*f.^(-p3^2);

f = 1 + D.^2/(2*p3^2*p2^2);
K = p1^2*f.^(-p3^2);

if nargout >= 2
  dK = zeros([size(D),3]);
  dK(:,:,1) = K .* 2 / p1;
  dK(:,:,2) = K .* (-p3^2).*f.^(-1) .* (-2).*D.^2/(2*p3^2)*p2^(-3);
  dK(:,:,3) = K .* (-2*p3*log(f) + (-p3^2)./f.*D.^2/(2*p2^2) * (-2) * p3^(-3));
end


% Gabor filters with ERBs between points from a scale
%
% Description
% Gabor filters are complex, mostly analytic filters that have a
% Gaussian envelope in both the time and frequency domains. They are
% defined as
%
% .. math::
%
%      f(t) = C \sigma^{-1/2} \pi^{-1/4}
%             e^{\frac{-t^2}{2\sigma^2} + i\xi t}
%
% in the time domain and
%
% .. math::
%
%      \widehat{f}(\omega) = C \sqrt{2\sigma} \pi^{1/4}
%                            e^{\frac{-\sigma^2(\xi - \omega)^2}{2}}
%
% in the frequency domain. Though Gaussians never truly reach 0, in
% either domain, they are effectively compactly supported. Gabor
% filters are optimal with respect to their time-bandwidth product.
%
% `scaling_function` is used to split up the frequencies between
% `high_hz` and `low_hz` into a series of filters. Every subsequent
% filter's width is scaled such that, if the filters are all of the
% same height, the intersection with the precedent filter's response
% matches the filter's Equivalent Rectangular Bandwidth (erb == True)
% or its 3dB bandwidths (erb == False). The ERB is the width of a
% rectangular filter with the same height as the filter's maximum
% frequency response that has the same L_2 norm.
%
% Properties
% centers_hz : [2-D Array]
% is_real : [bool]
% is_analytic : [bool]
% num_filts : [int]
% sampling_rate : [double]
% supports_hz : [double] 2-D Array
% supports : [double] 2-D Array
% supports_ms : [double] 2-D Array
% scaled_l2_norm : [bool]
% erb : [bool]
%
% See Also
% Config.EFFECTIVE_SUPPORT_THRESHOLD : the absolute
%     value below which counts as zero
%
% Copyright (c) 2018 Department of Computer Science,
%                    University of Toronto, Canada,
%                    Vector Institute, Canada
%
% License
% This file is under the LGPL license,  you can
%  redistribute it and/or modify it under the terms of the GNU Lesser General
%  Public License as published by the Free Software Foundation, either version 3
%  of the License, or (at your option) any later version. This file 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 Lesser General Public License for more
%  details.
%
% This function is part of the Covarep project: http://covarep.github.io/covarep
%
% Author
%  Yingxue Wang <yingxue@cs.toronto.edu>
%  Sean Robertson <sdrobert@cs.toronto.ca>
%

classdef GaborFilterBank < LinearFilterBank

    properties
        centers_hz
        is_real
        is_analytic = false
        num_filts = 40
        sampling_rate = 16000
        supports_hz
        supports
        supports_ms
    end

    properties (Access=private)
        scaled_l2_norm = false
        erb = false
        wrap_below
        centers_ang
        stds
        supports_ang
        wrap_supports_ang
        scale_l2_norm
    end

    methods

        function obj = GaborFilterBank(scaling_function,...
                num_filts,...
                low_hz,...
                sampling_rate,...
                scale_l2_norm,...
                erb,...
                high_hz)

            % Constructor for GaborFilterBank
            %
            % Input
            % scaling_function : [filterbanks/scales/ScalingFunction]
            %     Dictates the layout of filters in the Fourier domain. Can be
            %     a ScalingFunction
            % num_filts : [int]
            %     The number of filters in the bank
            % high_hz, low_hz : [double], optional
            %     The topmost and bottommost edge of the filters, respectively.
            %     The default for high_hz is the Nyquist
            % sampling_rate : [double], optional
            %     The sampling rate (cycles/sec) of the target recordings
            % scale_l2_norm : [bool]
            %     Whether to scale the l2 norm of each filter to 1. Otherwise the
            %     frequency response of each filter will max out at an absolute
            %     value of 1.
            % erb : [bool]
            %
            % Output
            % obj : [GaborFilterBank object]
            %
            % Example
            % >>scaling_function = MelScaling();
            % >>num_filts = 11;
            % >>low_hz = 0;
            % >>sampling_rate = 8000;
            % >>scale_l2_norm = false;
            % >>erb = false;
            % >>bank = GaborFilterBank(scaling_function,num_filts,...
            %                low_hz, sampling_rate, scale_l2_norm, erb);

            if nargin == 1
                num_filts = 40;
                low_hz = double(20);
                sampling_rate = double(16000);
                scale_l2_norm = false;
                erb = false;
                high_hz = double.empty;
            elseif nargin == 2
                low_hz = double(20);
                sampling_rate = double(16000);
                scale_l2_norm = false;
                erb = false;
                high_hz = double.empty;
            elseif nargin == 3
                sampling_rate = double(16000);
                scale_l2_norm = false;
                erb = false;
                high_hz = double.empty;
            elseif nargin == 4
                scale_l2_norm = false;
                erb = false;
                high_hz = double.empty;
            elseif nargin == 5
                erb = false;
                high_hz = double.empty;
            elseif nargin == 6
                high_hz = double.empty;
            end

            obj = obj@LinearFilterBank();
            obj.scaled_l2_norm = scale_l2_norm;
            obj.erb = erb;
            obj.sampling_rate = sampling_rate;
            if isempty(high_hz)
               high_hz = floor(sampling_rate/2);
            end

            % Input validation
            if low_hz < 0 || high_hz <= low_hz || high_hz > floor(sampling_rate/2)
                error('Invalid frequency range: (%d, %d)', low_hz, high_hz);
            end
            scale_low = scaling_function.hertz_to_scale(low_hz);
            scale_high = scaling_function.hertz_to_scale(high_hz);
            scale_delta = (scale_high - scale_low) / (num_filts + 1);
            % edges dictate the points where filters should intersect. We
            % make a pretend intersection halfway between low_hz and
            % the first filter center in the scaled domain. Likewise with
            % high_hz and the last filter center. Intersections are spaced
            % uniformly in the scaled domain
            edges = [];
            for idx= 1:num_filts+1
                edges = [edges ; scaling_function.scale_to_hertz(scale_low + scale_delta * ((idx-1) + .5))];
            end
            centers_hz = [];
            centers_ang = [];
            stds = [];
            supports_ang = [];
            supports = [];
            wrap_supports_ang = [];
            obj.wrap_below = false;
            log_2 = log(2);
            log_pi = log(pi);
            t_support_const = -2 * log(Config.EFFECTIVE_SUPPORT_THRESHOLD);
            f_support_const = t_support_const;

            if scale_l2_norm
               f_support_const = f_support_const + log_2 + .5 * log_pi;
               t_support_const = t_support_const - .5 * log_pi;
            else
               t_support_const = t_support_const - log_2 - log_pi;
            end
            if erb
               bandwidth_const = sqrt(pi) / 2;
            else
               bandwidth_const = sqrt(3 / 10 * log(10));
            end

            zip_edges = [edges(1:end-1) edges(2:end)];

            for ind = 1:size(zip_edges,1)  % 1st dimension of zip_edges
                left_intersect = zip_edges(ind);
                right_intersect = zip_edges(ind + size(zip_edges,1));
                center_hz = (left_intersect + right_intersect)/2;
                center_ang = Util.hertz_to_angular(center_hz, obj.sampling_rate);
                std = bandwidth_const / Util.hertz_to_angular(center_hz - left_intersect, obj.sampling_rate);
                log_std = log(std);

                if scale_l2_norm
                    diff_ang = sqrt(log_std + f_support_const) / std;
                    wrap_diff_ang = sqrt(log_std + f_support_const + log_2) / std;
                    diff_samps = ceil(std * sqrt(t_support_const - log_std));
                else
                    diff_ang = sqrt(f_support_const) / std;
                    wrap_diff_ang = sqrt(f_support_const + log_2) / std;
                    diff_samps = ceil(std * sqrt(t_support_const - 2 * log_std));
                end
                supp_ang_low = center_ang - diff_ang;
                if supp_ang_low < 0
                    obj.wrap_below = true;
                end
                centers_hz = [centers_hz; center_hz];
                centers_ang = [centers_ang; center_ang];
                supports_ang = [supports_ang; [center_ang - diff_ang center_ang + diff_ang]];
                wrap_supports_ang = [wrap_supports_ang; 2 * wrap_diff_ang];
                supports = [supports; [-diff_samps diff_samps]];
                stds = [stds; std];
            end

            obj.centers_ang = centers_ang;
            obj.centers_hz = centers_hz;
            obj.stds = stds;
            obj.supports_ang = supports_ang;
            obj.wrap_supports_ang = wrap_supports_ang;
            obj.num_filts = num_filts;

            supports_hz =[];
            for idx = 1:size(supports_ang,1) % 1st dimension of supports_ang
                supports_hz = [supports_hz; ...
                    [Util.angular_to_hertz(supports_ang(idx), obj.sampling_rate) ...
                    Util.angular_to_hertz(supports_ang(idx+size(supports_ang,1)), obj.sampling_rate)]];
            end
            obj.supports_hz = supports_hz;
            obj.supports = supports;
        end

    end

    methods
        function res = get_impulse_response (obj, filt_idx, width)
            % Helper function to get impulse response of a GaborFilterBank
            %
            % Inputs
            % filt_idx  : [int] filter index
            % width     : [int] width of the signal
            %
            % Outputs
            % res : [double]
            %
            % Example
            % >>scaling_function = MelScaling();
            % >>num_filts = 11;
            % >>low_hz = 0;
            % >>sampling_rate = 8000;
            % >>scale_l2_norm = false;
            % >>erb = false;
            % >>bank = GaborFilterBank(scaling_function,num_filts,...
            %                low_hz, sampling_rate, scale_l2_norm, erb);
            % >>x = bank.get_impulse_response(filt_idx, dft_size);
            %
            center_ang = obj.centers_ang(filt_idx);
            std = obj.stds(filt_idx);
            res = zeros(width,1); % we want it to store complex values
            if obj.scale_l2_norm
                const_term = -0.5 * log(std) - 0.25 * log(pi);
            else
                const_term = -0.5 * log(2 * pi) - log(std);
            end
            denom_term = 2 * std ^ 2;

            for t = 1: width+1
                val = complex(-(t-1) ^ 2 / denom_term + const_term, center_ang * (t-1));
                val = exp(val);
                if t ~= width+1
                    res(t) = res(t) + val;
                end
                if width-t+2 > 0 && t>1
                    res(end-t+2) = res(end-t+2) + conj(val);
                end
            end
        end

        function res = get_frequency_response(obj, filt_idx, width, half)
            % Helper function to get frequency response of a GaborFilterBank
            %
            % Inputs
            % filt_idx  : [int] filter index
            % width     : [int] width of the signal
            % half      : [bool] (def. false)
            %
            % Outputs
            % res : [hz] double
            %
            % Example
            % >>scaling_function = MelScaling();
            % >>num_filts = 11;
            % >>low_hz = 0;
            % >>sampling_rate = 8000;
            % >>scale_l2_norm = false;
            % >>erb = false;
            % >>bank = GaborFilterBank(scaling_function,num_filts,...
            %                low_hz, sampling_rate, scale_l2_norm, erb);
            % >>X = bank.get_frequency_response(filt_idx, dft_size, false);
            %
            if nargin == 3
                half = false;
            end
            center_ang = obj.centers_ang(filt_idx);
            lowest_ang = obj.supports_ang(filt_idx,1);
            highest_ang = obj.supports_ang(filt_idx,2);
            lowest_ang = filt_idx;
            std = obj.stds(filt_idx);
            dft_size = width;

            if half == true
                if mod(width,2) ~= 0
                    dft_size = floor((width + 1) / 2);
                else
                    dft_size = floor(width / 2) + 1;
                end
            end
            res = zeros(dft_size, 1);
            if obj.scale_l2_norm
                const_term = .5 * log(2 * std) + .25 * log(pi);
            else
                const_term = 0;
            end
            num_term = -(std ^ 2) / 2;

            for idx = 1: dft_size
                for period =  -1 - floor(max(-lowest_ang, 0) / (2 * pi)): 2 + floor(highest_ang / (2 * pi)) - 1
                    omega = ((double(idx)-1) / double(width) + double(period)) * 2 * pi;
                    val = num_term * (center_ang - omega) ^ 2 + const_term;
                    val = exp(val);
                    res(idx) = res(idx) + val;
                end
            end
        end

        function  [start_idx, truncated_response] = get_truncated_response(obj, filt_idx, width)
            % Helper function to get truncated response of a GaborFilterBank
            %
            % Desription
            % wrap_supports_ang contains the angular supports of each filter
            % if the effective support threshold were halved. If this
            % support exceeds the 2pi period, overlap from aliasing in the
            % periphery will exceed the effective support, meaning the
            % entire period lies in the support
            %
            % Inputs
            % filt_idx  : [int] filter index
            % width     : [int] width of the signal
            %
            % Outputs
            % start_idx : [int]
            % truncated_response : [1-D column vector]
            %
            % Example
            % >>scaling_function = MelScaling();
            % >>num_filts = 11;
            % >>low_hz = 0;
            % >>sampling_rate = 8000;
            % >>scale_l2_norm = false;
            % >>erb = false;
            % >>bank = GaborFilterBank(scaling_function,num_filts,...
            %                low_hz, sampling_rate, scale_l2_norm, erb);
            % [start_idx, truncated_response] = bank.get_truncated_response(filt_idx, dft_size);
            %
            % Please refer to test_fbank.test_truncated_matches_full
            % for more information
            %

            if obj.wrap_supports_ang(filt_idx) >= 2 * pi
                start_idx = 1;
                truncated_response = obj.get_frequency_response(filt_idx, width);
            else
                center_ang = obj.centers_ang(filt_idx);
                std = obj.stds(filt_idx);
                lowest_ang = obj.supports_ang(filt_idx, 1);
                highest_ang = obj.supports_ang(filt_idx, 2);
                left_idx = ceil(width * lowest_ang / (2 * pi));
                right_idx = round(width * highest_ang / (2 * pi));
                res = zeros(1 + right_idx - left_idx, 1);

                if obj.scale_l2_norm
                    const_term = .5 * log(2 * std) + .25 * log(pi);
                else
                    const_term = 0;
                end
                num_term = -(std ^2) / 2;

                for idx = left_idx: right_idx
                    for period = - round(max(-lowest_ang, 0) / (2 * pi)): 1 + round(highest_ang / (2 * pi)) - 1 % -1 because the syntax is different in matlab and python
                        omega = ((idx-1) / width + period) * 2 * pi;
                        val = num_term * (center_ang - omega) ^ 2 + const_term;
                        val = exp(val);
                        res(idx - left_idx + 1) = res(idx - left_idx + 1) + val; % Need to check if this is right
                    end
                end
                start_idx = mod(left_idx, width);
                truncated_response = res;
            end
        end
    end

end


function HTotal=uvHessian(xG,lRx,M,includeW)
%%UVHESSIAN Determine the Hessian matrix (second derivative matrix) of a
%           direction cosine measurement with respect to 3D position.
%           Relativity and atmospheric effects are not taken into account.
%
%INPUTS: xG The 3XN target position vectors in the global coordinate system
%           with [x;y;z] components for which gradients are desired.
%       lRx The 3X1  position vector of the receiver. If omitted, the
%           receiver is placed at the origin.
%         M A 3X3 rotation matrix from the global coordinate system to the
%           orientation of the coordinate system at the receiver. If
%           omitted, it is assumed to be the identity matrix.
%  includeW An optional boolean value indicating whether a third direction
%           cosine component should be included. The u and v direction
%           cosines are two parts of a 3D unit vector. Generally, one might
%           assume that the target is in front of the sensor, so the third
%           component would be positive and is not needed. However, the
%           third component can be included if ambiguity exists. The
%           default if this parameter is omitted or an empty matrix is
%           passed is false.
%
%OUTPUTS: HTotal A 3X3X(2+includeW)XN matrix such that for the ith measurement,
%          HTotal(:,:,1,i) is the Hessian matrix with respect to the u
%          component and HTotal(:,:,2,i) is the Hessian matrix with respect
%          to the v component and, if included, HTotal(:,:,3,i) is the
%          Hessian matrix with respect to the w component. The elements in
%          the matrices for each component/ point are
%          ordered [d^2/(dxdx), d^2/(dxdy), d^2/(dxdz);
%                   d^2/(dydx), d^2/(dydy), d^2/(dydz);
%                   d^2/(dzdx), d^2/(dzdy), d^2/(dzdz)];
%          note that each matrix is symmetric (i.e.
%           d^2/(dydx)=d^2/(dxdy) ).
%
%A derivation of the components of the Jacobian is given in [1]. The
%Hessian is just one derivative higher.
%
%EXAMPLE:
%Here, we verify that a numerically differentiated Hessian is consistent
%with the analytic one produced by this function.
% xG=[100;-1000;500];
% lRx=[500;20;-400];
% epsVal=1e-6;
% M=randRotMat(3);
% includeW=true;
%
% H=uvHessian(xG,lRx,M,includeW);
% J=uvGradient(xG,lRx,M,includeW);
% JdX=uvGradient(xG+[epsVal;0;0],lRx,M,includeW);
% JdY=uvGradient(xG+[0;epsVal;0],lRx,M,includeW);
% JdZ=uvGradient(xG+[0;0;epsVal],lRx,M,includeW);
% HNumDiff=zeros(3,3,2);
% for k=1:(2+includeW)
%     HNumDiff(1,1,k)=(JdX(k,1)-J(k,1))/epsVal;
%     HNumDiff(1,2,k)=(JdX(k,2)-J(k,2))/epsVal;
%     HNumDiff(2,1,k)=HNumDiff(1,2,k);
%     HNumDiff(1,3,k)=(JdX(k,3)-J(k,3))/epsVal;
%     HNumDiff(3,1,k)=HNumDiff(1,3,k);
%     HNumDiff(2,2,k)=(JdY(k,2)-J(k,2))/epsVal;
%     HNumDiff(2,3,k)=(JdY(k,3)-J(k,3))/epsVal;
%     HNumDiff(3,2,k)=HNumDiff(2,3,k);
%     HNumDiff(3,3,k)=(JdZ(k,3)-J(k,3))/epsVal;
% end
% max(abs((H(:)-HNumDiff(:))./H(:)))
%The relative error will be on the order of 1e-6 or better, indicating good
%agreement between the numerical Hessian matrix and the actual Hessian
%matrix.
%
%REFERENCES:
%[1] D. F. Crouse, "Basic tracking using nonlinear 3D monostatic and
%    bistatic measurements," IEEE Aerospace and Electronic Systems
%    Magazine, vol. 29, no. 8, Part II, pp. 4-53, Aug. 2014.
%
%June 2017 David F.Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<4||isempty(includeW))
    includeW=false;
end

if(nargin<3||isempty(M))
    M=eye(3,3);
end

if(nargin<2||isempty(lRx))
    lRx=zeros(3,1);
end

N=size(xG,2);
HTotal=zeros(3,3,2+includeW,N);
for curPoint=1:N
    %Convert the state into the local coordinate system.
    xLocal=M*(xG(1:3)-lRx(1:3));

    x=xLocal(1);
    y=xLocal(2);
    z=xLocal(3);

    x2=x*x;
    y2=y*y;
    z2=z*z;

    r5=norm(xLocal)^5;

    H=zeros(3,3,2);

    %u Hessian values
    %du/(dxdx)
    H(1,1,1)=-((3*x*(y2+z2))/r5);
    %du/(dydy)
    H(2,2,1)=-((x*(x2-2*y2+z2))/r5);
    %du/(dzdz)
    H(3,3,1)=-((x*(x2+y2-2*z2))/r5);
    %du/(dxdy)
    H(1,2,1)=-((y*(-2*x2+y2+z2))/r5);
    %du/(dydx)
    H(2,1,1)=H(1,2,1);
    %du/(dxdz)
    H(1,3,1)=-((z*(-2*x2+y2+z2))/r5);
    %du/(dzdx)
    H(3,1,1)=H(1,3,1);
    %du/(dydz)
    H(2,3,1)=(3*x*y*z)/r5;
    %du/(dzdy)
    H(3,2,1)=H(2,3,1);

    %v Hessian values
    %dv/(dxdx)
    H(1,1,2)=-((y*(-2*x2+y2+z2))/r5);
    %dv/(dydy)
    H(2,2,2)=-((3*y*(x2+z2))/r5);
    %dv/(dzdz)
    H(3,3,2)=-((y*(x2+y2-2*z2))/r5);
    %dv/(dxdy)
    H(1,2,2)=-((x*(x2-2*y2+z2))/r5);
    %dv/(dydx)
    H(2,1,2)=H(1,2,2);
    %dv/(dxdz)
    H(1,3,2)=(3*x*y*z)/r5;
    %dv/(dzdx)
    H(3,1,2)=H(1,3,2);
    %dv/(dydz)
    H(2,3,2)=-((z*(x2-2*y2+z2))/r5);
    %dv/(dzdy)
    H(3,2,2)=H(2,3,2);

    %Rotate the values back into global coordinates
    HTotal(:,:,1,curPoint)=M'*H(:,:,1)*M;
    HTotal(:,:,2,curPoint)=M'*H(:,:,2)*M;

    %w Hessian values
    if(includeW)
        %dw/(dxdx)
        H(1,1,3)=-z*(-2*x2+y2+z2)/r5;
        %dw/(dydy)
        H(2,2,3)=-z*(x2-2*y2+z2)/r5;
        %dw/(dzdz)
        H(3,3,3)=-3*(x2+y2)*z/r5;
        %dw/(dxdy)
        H(1,2,3)=3*x*y*z/r5;
        %dw/(dydx)
        H(2,1,3)=H(1,2,3);
        %dw/(dxdz)
        H(1,3,3)=-x*(x2+y2-2*z2)/r5;
        %dw/(dzdx)
        H(3,1,3)=H(1,3,3);
        %dw/(dydz)
        H(2,3,3)=-y*(x2+y2-2*z2)/r5;
        %dw/(dzdy)
        H(3,2,3)=H(2,3,3);

        HTotal(:,:,3,curPoint)=M'*H(:,:,3)*M;
    end
end
end

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


function [ahm, AHM_u, AHM_s, AHM_k, AHM_c] = invPinHoleAhm(u,s,k,c)

% INVPINHOLEAHM Retro-project anchored homogeneous point AHP.
%   AHM = INVPINHOLEAHM(U,S) gives the retroprojected anchored homogeneous
%   point (AHM) of a pixel U at depth S (S is actually the inverse
%   depth), from a canonical pin-hole camera, that is, with calibration
%   parameters
%     u0 = 0 v0 = 0 au = 1 av = 1
%   It uses reference frames {RDF,RD} (right-down-front for the 3D world
%   points and right-down for the pixel), according to this scheme:
%
%         / z (forward)
%        /
%       +------- x                 +------- u
%       |                          |
%       |      3D : P=[x;y;z]      |     image : U=[u;v]
%       | y                        | v
%
%   AHM = INVPINHOLEAHM(U,S,K) allows the introduction of the camera's
%   calibration parameters:
%     K = [u0 v0 au av]'
%
%   AHM = INVPINHOLEAHM(U,S,K,C) allows the introduction of the camera's radial
%   distortion correction parameters:
%     C = [c2 c4 c6 ...]'
%   so that the new pixel is corrected following the distortion equation:
%     U = U_D * (1 + K2*R^2 + K4*R^4 + ...)
%   with R^2 = sum(U_D.^2), being U_D the distorted pixel in the image
%   plane for a camera with unit focal length.
%
%   If U is a pixels matrix, INVPINHOLEAHM(U,...) returns a AHMS matrix AHM,
%   with these matrices defined as
%     U = [U1 ... Un];   Ui = [ui;vi]
%     AHM = [AHM1 ... AHMn];   AHMi = [Xi;Yi;Zi,pi,yi,ri]
%   where pi, yi are pitch and yaw angles of the AHM ray, and ri is the
%   inverse of the distance (wrongly named "inverse depth")
%
%   [AHM,AHM_u,AHM_s,AHM_k,AHM_c] returns the Jacobians of AHM wrt U, S, K and C. It
%   only works for single pixels U=[u;v], and for distortion correction
%   vectors C of up to 3 parameters C=[c2;c4;c6]. See UNDISTORT for
%   information on longer distortion vectors.
%
%   See also RETRO, UNDISTORT, DEPIXELLISE, PINHOLE.

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

if nargout == 1 % only point

    switch nargin
        case 2
            v = retro(u,1);
        case 3
            v = retro(depixellise(u,k),1);
        case 4
            v = retro(undistort(depixellise(u,k),c),1);
    end
    n = normvec(v);
    ahm  = [0;0;0;n;s];

else % Jacobians

    if size(u,2) > 1
        error('Jacobians not available for multiple pixels')
    else

        switch nargin
            case 2
                [v, V_u] = invPinHole(u,1);
                [n,N_v]  = normvec(v,1);
                ahm      = [0;0;0;n;s];
                AHM_v    = [zeros(3,3);N_v;zeros(1,3)];
                AHM_s    = [0;0;0;0;0;0;1];
                AHM_u    = AHM_v*V_u;

            case 3
                [v, V_u, V_1, V_k] = invPinHole(u,1,k);
                [n,N_v]  = normvec(v,1);
                ahm      = [0;0;0;n;s];
                AHM_v    = [zeros(3,3);N_v;zeros(1,3)];
                AHM_s    = [0;0;0;0;0;0;1];
                AHM_u    = AHM_v*V_u;
                AHM_k    = AHM_v*V_k;

            case 4
                [v, V_u, V_1, V_k, V_c] = invPinHole(u,1,k,c);
                [n,N_v]  = normvec(v,1);
                ahm      = [0;0;0;n;s];
                AHM_v    = [zeros(3,3);N_v;zeros(1,3)];
                AHM_s    = [0;0;0;0;0;0;1];
                AHM_u    = AHM_v*V_u;
                AHM_k    = AHM_v*V_k;
                AHM_c    = AHM_v*V_c;

        end
    end

end

return

%% jacobians
syms u v s u0 v0 au av c2 c4 c6 real
U=[u;v];
k=[u0;v0;au;av];
c=[c2;c4;c6];

% [ahm,AHM_u,AHM_s] = invPinHoleAhm(U,s);
[ahm,AHM_u,AHM_s,AHM_k] = invPinHoleAhm(U,s,k);
% [ahm,AHM_u,AHM_s,AHM_k,AHM_c] = invPinHoleAhm(U,s,k,c);

simplify(AHM_u - jacobian(ahm,U))
simplify(AHM_s - jacobian(ahm,s))
simplify(AHM_k - jacobian(ahm,k))
% simplify(AHM_c - jacobian(ahm,c))

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

%   # START GPL LICENSE

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

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

%   # END GPL LICENSE


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

%*****************************************************************************80
%
%% CG_sparse uses the conjugate gradient method for a sparse storage matrix.
%
%  Discussion:
%
%    The linear system has the form A*x=b, where A is a positive-definite
%    symmetric matrix, stored as a full storage matrix.
%
%    The method is designed to reach the solution to the linear system
%      A * x = b
%    after N computational steps.  However, roundoff may introduce
%    unacceptably large errors for some problems.  In such a case,
%    calling the routine a second time, using the current solution estimate
%    as the new starting guess, should result in improved results.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Frank Beckman,
%    The Solution of Linear Equations by the Conjugate Gradient Method,
%    in Mathematical Methods for Digital Computers,
%    edited by John Ralston, Herbert Wilf,
%    Wiley, 1967,
%    ISBN: 0471706892,
%    LC: QA76.5.R3.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, sparse real A(N,N), the matrix.
%
%    Input, real B(N), the right hand side vector.
%
%    Input/output, real X(N).
%    On input, an estimate for the solution, which may be 0.
%    On output, the approximate solution vector.
%

%
%  Initialize
%    AP = A * x,
%    R  = b - A * x,
%    P  = b - A * x.
%
  ap(1:n,1) = a * x;

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

    if ( pap == 0.0 )
      return;
    end

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

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

  end

  return
end


function [Keff_p_medio, Keff_s_medio, Keff_n_medio] = interpolateElectrolyteConductivities(Keff_p,Keff_s,Keff_n,param)
%	interpolateElectrolyteConductivities interpolates electrolyte conductivities at the edges of control volumes using harmonic mean.

%   This file is part of the LIONSIMBA Toolbox
%
%	Official web-site: 	http://sisdin.unipv.it/labsisdin/lionsimba.php
% 	Official GitHUB: 	https://github.com/lionsimbatoolbox/LIONSIMBA
%
%   LIONSIMBA: A Matlab framework based on a finite volume model suitable for Li-ion battery design, simulation, and control
%   Copyright (C) 2016-2018 :Marcello Torchio, Lalo Magni, Davide Raimondo,
%                            University of Pavia, 27100, Pavia, Italy
%                            Bhushan Gopaluni, Univ. of British Columbia,
%                            Vancouver, BC V6T 1Z3, Canada
%                            Richard D. Braatz,
%                            Massachusetts Institute of Technology,
%                            Cambridge, Massachusetts 02142, USA
%
%   Main code contributors to LIONSIMBA 2.0:
%                           Ian Campbell, Krishnakumar Gopalakrishnan,
%                           Imperial college London, London, UK
%
%   LIONSIMBA is a free Matlab-based software distributed with an MIT
%   license.

%% Positive electrode mean conductivity
beta_p = 0.5;
Keff_p_medio = Keff_p(1:end-1).*Keff_p(2:end)./(beta_p*Keff_p(2:end)+(1-beta_p)*Keff_p(1:end-1));

% The last element of Keff_p_medio will be the harmonic mean of the
% elements at the interface positive-separator

beta_p_s = param.deltax_p*param.len_p/2 /(param.deltax_s*param.len_s/2+param.deltax_p*param.len_p/2);

Keff_p_s_interface = Keff_p(end)*Keff_s(1) / (beta_p_s*Keff_s(1) + (1-beta_p_s)*Keff_p(end));

Keff_p_medio = [Keff_p_medio;Keff_p_s_interface];

%% Separator mean conductivity
% Compute the harmonic mean values for the separator
beta_s = 0.5;
Keff_s_medio = Keff_s(1:end-1).*Keff_s(2:end)./(beta_s*Keff_s(2:end)+(1-beta_s)*Keff_s(1:end-1));

% The last element of Keff_s_medio will be the harmonic mean of the
% elements at the interface separator-negative

beta_s_n = param.deltax_s*param.len_s/2 /(param.deltax_s*param.len_s/2+param.deltax_n*param.len_n/2);

Keff_s_n_interface = Keff_s(end)*Keff_n(1) / (beta_s_n*Keff_n(1) + (1-beta_s_n)*Keff_s(end));

Keff_s_medio = [Keff_s_medio;Keff_s_n_interface];

%% Negative electrode mean conductivity
% Compute the harmonic mean values for the negative electrode

beta_n = 0.5;
Keff_n_medio = Keff_n(1:end-1).*Keff_n(2:end)./(beta_n*Keff_n(2:end)+(1-beta_n)*Keff_n(1:end-1));

Keff_n_medio = [Keff_n_medio;0]; % the cc interface is not used. The zero is only to match the dimensions
end