function [history, stopReason] = lars(yin, xin, XTX, type, stopCriterion, regularization, trace, quiet)
% %{
% This program implements "LARS" algorithm and its lasso modification
% introduced by Efron et. al. 2003.  Read the paper to understand codes
% of this function.  Each line of this file has corresponding equation
% number in Efron et. al. 2003 for reader's convenience.
%
%
% *** CAUTION
% history(1).mu_OLS contains original 'yin' to provide convinience in
% writing user defined stop criterion function. Actual mu_OLS of the first
% step should be just mean of yin which is a simple array of copy of
% history(1).mu. So, if history(1).mu_OLS contains that information, it is
% redundant. In fact, this is also contained in history(1).b, which is a
% bias of the output. Therefore, to provide more information to user who
% want to write his/her own stop criterion function, history(1).mu_OLS
% contains yin.
%
%
% Example 1: moderate size x.
%     stopCrioterion = {};
%     stopCrioterion{1,1} = 'maxKernels';
%     stopCrioterion{1,2} = 100;
%
%     XTX = lars_getXTX(x_original);      % this takes long time.
%     sol = lars(y, x, 'lasso', XTX, stopCriterion);
%
% Example 2: very small size x, or a really really big size x
%     stopCrioterion = {};
%     stopCrioterion{1,1} = 'maxKernels';
%     stopCrioterion{1,2} = 100;
%
%     sol = lars(y, x, 'lasso', XTX, stopCriterion);
%
% Note:
%     Users can add any kind of stop criterion by editing
%     the corresponding portion of this file.  See the code
%     for existing examples.
%
% Note 2:
%     This m-file does not implement routine for missing data.
% %}
%

global USING_CLUSTER;
global RESOLUTION_OF_LARS;
global REGULARIZATION_FACTOR;
lars_init();

regularization_factor = REGULARIZATION_FACTOR; % Tikhonov regularization factor (or the ridge regression factor)
                              % -> This should be small enough in this case to get
                              %    a reasonable pseudoinverse.

stopReason = {};

%%% Check parameters
if length(yin)==0 | length(xin)==0
    warning('\nInput or Output has zero length.\n');
    history.active_set = [];
    stopReason{1} = 'Parameter error';
    stopReason{2} = 0;
    return;
end
if size(yin,1) ~= size(xin,1)
    warning('\nSize of y does not match to that of x.\n');
    history.active_set = [];
    stopReason{1} = 'Parameter error';
    stopReason{2} = 0;
    return;
end
if ~strcmp(type, 'lasso') & ~strcmp(type, 'lars') & ~strcmp(type, 'forward_stepwise')
    warning('\nUnknown type of regression.\n');
    history.active_set = [];
    stopReason{1} = 'Parameter error';
    stopReason{2} = 0;
    return;
end
if strcmp(type, 'forward_stepwise')
    warning('\nForward_stepwise is not implemented.\n');
    history.active_set = [];
    stopReason{1} = 'Parameter error';
    stopReason{2} = 0;
    return;
end

if exist('regularization','var') & ~isempty(regularization)
    regularization = 10;
else
    regularization  = 0;
end

if ~exist('trace','var') | isempty(trace)
    trace=0;
end

if ~exist('quiet','var') | isempty(quiet)
    quiet=0;
elseif quiet==1
    trace=0;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Data preparation

% Program automatically centers and standardizes predictors.
if ~exist('XTX','var')
    XTX=[];
end
no_xtx = 0;
if ~isempty(XTX)
    if ~quiet & trace >=0
        fprintf('\nLars is using the provided xtx.\n');
    end
elseif size(xin,2)^2 > 10^6
    if ~quiet & trace >=0
        fprintf('Too large matrix (size(x,2)^2 > 10^6). lars will not pre-calculate xtx.\n');
    end
    no_xtx      = 1;
    XTX         = lars_getXTX(xin,no_xtx);
else
%     fprintf('\nCalculating xtx.\n');
    XTX         = lars_getXTX(xin);
end

x               = XTX.x;            % normalized xin
mx              = XTX.mx;           % mean xin
sx              = XTX.sx;           % length of each column of xin
ignores         = XTX.ignores;      % indices for constant terms
all_candidate   = XTX.all_candidate;% indices for all possible columns
if ~no_xtx
    xtx         = XTX.xtx;          % xtx matrix
    dup_columns = XTX.dup_columns;  % duplicated columns which will be automatically ignored.
end

my              = mean(yin);
y               = yin-my;

n               = size(x,1);        % # of samples
m               = size(x,2);        % # of predictors

% Now, we can determine the maximum number of kernels.
%maxKernels = min(maxKernels, min(size(xin,1)-1, length(all_candidate)));
%maxKernels = min(maxKernels, min(rank(xin), length(all_candidate)));
existMaxKernels = 0;
existMSE        = 0;
for is = 1:size(stopCriterion,1)
    if strcmp(stopCriterion{is,1},'maxKernels')
        existMaxKernels = 1;
%        stopCriterion{is,2} = min(stopCriterion{is,2}, min(size(xin,1)-1, length(all_candidate)));
        stopCriterion{is,2} = min(stopCriterion{is,2}, min(rank(xin), length(all_candidate)));
        if stopCriterion{is,2}<1
            warning('Max Kernel is less than 1. It must be larger than 0.\n');
            stopCriterion{is,2} = 1;
        end
    end
    if strcmp(stopCriterion{is,1},'MSE')
        existMSE        = 1;
        if stopCriterion{is,2}<1.0e-10
            warning('Maximum MSE is too small. Automatically set to 1.0e-10\n');
            stopCriterion{is,2} = 1.0e-10;
        end
    end
end
if ~existMaxKernels
    is = size(stopCriterion,1);
    stopCriterion{is+1,1} = 'maxKernels';
%    stopCriterion{is+1,2} = min(size(xin,1)-1, length(all_candidate));  % Stop when size of active set is data.maxKernels.
    stopCriterion{is+1,2} = min(rank(xin), length(all_candidate));  % Stop when size of active set is data.maxKernels.
end
if ~existMSE
    is = size(stopCriterion,1);
    stopCriterion{is+1,1} = 'MSE';
    stopCriterion{is+1,2} = 1.0e-10;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Initialization

active      = [];               % active set
inactive    = all_candidate;    % inactive set

mu_a        = zeros(n,1);       % current estimate (eq. 2.8)
mu_a_plus   = 0;                % next estimate (eq. 2.12)
mu_a_OLS    = 0;                % OLS estimate  (eq. 2.19)

beta        = zeros(1,size(x,2));
beta_new    = beta;
beta_OLS    = beta;

history.active_set          = [];
history.add                 = [];
history.drop                = [];
history.beta_norm           = [];
history.beta                = [];
history.b                   = my;
history.mu                  = my;
history.beta_OLS_norm       = [];
history.beta_OLS            = [];
history.b_OLS               = my;
history.mu_OLS              = my*ones(size(yin));
history.MSE                 = sum(y.^2)/length(y);
history.R_square            = 0;
history.resolution_warning  = [];

if var(yin)==0
    stopReason{1} = 'zeroVarY';
    stopReason{2} = var(yin);
    return;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Main loop
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

c               = 0;                % correlation vector
C_max           = max(abs(c));
C_max_ind       = [];
C_max_ind_pl    = [];
drop            = [];               % used for 'lasso'
k               = 1;                % iteration index
if ~quiet & trace >= 0
    fprintf('Active predictors / total  :::::  Current iteration\n                            ');
end
while 1

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%           Exit Criterions
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    if exist('stopCriterion','var')     % If there is any stop criterion...
        % Any of these is satisfied, algorithm stops.
        %
        % Other criterions: Make your own cost function.

        for is = 1:size(stopCriterion,1)  % there can be a number of stop criterions.

            % Default Criterions.
            % Maximum number of consecutive drops or maximum number of drops within a window.
            if strcmp(stopCriterion{is,1},'maxDrops')
                drop_window = stopCriterion{is,2}(1);   % number of backward history to be considerd. 0 means all history.
                if drop_window==0
                    drop_window=k;
                end
                drop_n      = min(drop_window,stopCriterion{is,2}(2));   % number of maximum drops within the window.
                drop_vector = [];
                for z = max(k-drop_window+1,1):k
                    drop_vector=[drop_vector, history(z).drop];
                end
                if length(drop_vector)>=drop_n
                    stopReason{1} = 'maxDrops';
                    stopReason{2} = drop_n;
                    break;
                end
            end
            % Maximum number of kernels.
            if strcmp(stopCriterion{is,1},'maxKernels')
                if length(active) >= min(stopCriterion{is,2}, min(size(xin,1)-1, length(all_candidate)))
                    stopReason{1} = 'maxKernels';
                    stopReason{2} = length(active);
                    break;
                end
            end
            % Maximum number of iterations.
            if strcmp(stopCriterion{is,1},'maxIterations')
                if k >= stopCriterion{is,2}
                    stopReason{1} = 'maxIterations';
                    stopReason{2} = k;
                    break;
                end
            end
            % MSE.
            if strcmp(stopCriterion{is,1},'MSE')
                if history(k).MSE <= stopCriterion{is,2}
                    stopReason{1} = 'MSE';
                    stopReason{2} = history(k).MSE;
                    break;
                end
            end

            % User defined stop criterion.
            if strcmp(stopCriterion{is,1},'userDefinedCriterion')
                fhandle = stopCriterion{is,2}.fhandle;
                r_fhandle = fhandle(history, stopCriterion{is,2}.data);
                if r_fhandle.stop
                    stopReason{1} = 'userDefinedCriterion';
                    stopReason{2} = r_fhandle;
                    break;
                end
            end

        end

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

    end % end of stop criterion checking :  if exist('stopCriterion','var')
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    if length(stopReason)>0 % if there is any reason of stopping the algorithm, exit loop.
        break;
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%           LARS Algorithm
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    %if ~USING_CLUSTER
    %fprintf('\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b%5d/%5d    :::::   %5d',length(active),length(all_candidate),k);
    %end
    % start of algorithm
    c                   = x'*(y-mu_a);                      % eq 2.8
    [C_max,C_max_ind]   = max(abs(c(inactive)));            % eq 2.9
    C_max_ind           = inactive(C_max_ind);
    % active          = sort(union(active,C_max_ind));      % If there is no machine limit, this can be used.
    % But because of machine limit, there can be multiple new predictors.
    % This dramatically improves the overall precision of the result,
    % and speeds up the whole process.
    C_max_ind_pl        = abs(c(inactive))>C_max-RESOLUTION_OF_LARS;
    C_max_ind_pl        = inactive(C_max_ind_pl);
    active          = sort(union(active,C_max_ind_pl));
    inactive        = setdiff(all_candidate, active);   % eq 2.9
    if strcmp(type,'lasso')
        if ~isempty(drop) & length(find(drop==C_max_ind))==0           % If there is a drop, that must have the maximum correlation in inactive set.
            if ~quiet & trace >=0
                fprintf('\n');
                warning('Dropped item and index of maximum correlation is not the same. But it is being ignored here...');
                fprintf('\n                            ');
            end
            active(find(active==C_max_ind))=[];
        end
        if ~isempty(drop)
            C_max_ind   = [];
            C_max_ind_pl= [];
        end
        active          = setdiff(active,drop);             % eq 3.6
        inactive        = sort(union(inactive,drop));       % eq 3.6
    end

    s       = sign(c(active));              % eq 2.10
    xa      = x(:,active).*repmat(s',n,1);  % eq 2.4
    if ~no_xtx
        ga  = xtx(active,active).*(s*s');   % eq 2.5
    else
        ga  = xa'*xa;                       % eq 2.5
    end
    if regularization > 2
        ga  = ga+eye(length(ga))*regularization_factor; % This routine will make the test below
    end
    invga   = ga\eye(size(ga,1));           % eq 2.5
    aa      = sum(sum(invga))^(-1/2);       % eq 2.5
    wa      = aa*sum(invga,2);              % eq 2.6
    ua      = xa*wa;                        % eq 2.6

    % test using eq 2.7
    test_1      = xa'*ua;
    test_2      = aa*ones(size(test_1));
    test_1_2    = sum(sum(abs(test_1-test_2)));
    test_3      = norm(ua) - 1;

    history(k+1).resolution_warning=0;
    if test_1_2 > RESOLUTION_OF_LARS*100 | abs(test_3 ) > RESOLUTION_OF_LARS*100
        if regularization <=2
            if ~quiet & trace>0
                fprintf('\n');
                warning('Eq 2.7 test failure.');
                fprintf('\n                            ');
            end
            regularization = regularization + 1;
            if regularization > 2
                if ~quiet & trace>0
                    fprintf('\n');
                    warning('Lots of Eq 2.7 test failure. Regularization will be applied from now on.');
                    fprintf('\n                            ');
                end
            end
        end
        history(k+1).resolution_warning=1;
    end

    a               = x'*ua;                % eq 2.11
    tmp_1           = (C_max - c(inactive))./(aa - a(inactive));
    tmp_2           = (C_max + c(inactive))./(aa + a(inactive));
    tmp_3           = [tmp_1, tmp_2];
    tmp             = tmp_3(find(tmp_3>0));
    gamma = min(tmp);                       % eq 2.13
    if length(gamma)==0 % if this is the last step (i.e. length(active)==maxKernels)
        gamma = C_max/aa;                   % eq 2.19, eq 2.21 and 5 lines below eq 2.22
    end

    d               = zeros(1,m);
    d(active)       = s.*wa;

    if length(find(d(active)==0))
        fprintf('\n');
        warning('Something wrong with vector d: eq 3.4.');
        fprintf('\n                            ');
    end

    tmp             = zeros(1,m);
    tmp(active)     = -1*beta(active)./d(active);   % eq 3.4
    tmp2            = tmp(find(tmp>0));

    drop            = [];
    gamma_tilde = inf;                              % eq 3.5
    if ~isempty(tmp2) & gamma >= min(tmp2)
        gamma_tilde = min(tmp2);                    % eq 3.5
        drop        = find(tmp==gamma_tilde);       % eq 3.6
    end

    if strcmp(type, 'lars')
        mu_a_plus   = mu_a + gamma*ua;              % eq 2.12
        beta_new    = beta + gamma*d;               % eq 3.3
        drop        = [];
    elseif strcmp(type, 'lasso')
        mu_a_plus   = mu_a + min(gamma, gamma_tilde)*ua;    % eq 3.6
        beta_new    = beta + min(gamma, gamma_tilde)*d;     % eq 3.3
        active      = setdiff(active,drop);                 % eq 3.6
        inactive    = setdiff(all_candidate,active);
        beta_new(drop) = 0;
    elseif strcmp(type, 'forward_stepwise')
        drop        = [];
        error('forward.stepwise has not been implemented yet.');
        return;
    end

    mu_a_OLS    = mu_a + C_max/aa*ua;          % eq 2.19, 2.21
    beta_OLS    = beta + C_max/aa*d;           % eq 2.19, 2.21
    MSE         = sum((y - mu_a_OLS).^2)/length(y);

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % update and save
    mu_a = mu_a_plus;
    beta = beta_new;

    % history with scale correction
    k = k+1;
    history(k).active_set   = active;
    history(k).drop         = drop;
    history(k).add          = C_max_ind_pl;
    history(k).beta_norm    = beta(active);
    history(k).beta         = beta(active)./sx(active);
    history(k).b            = my - sum(mx./sx.*beta);
    history(k).mu           = xin * (beta./sx)' + history(k).b;
    history(k).beta_OLS_norm= beta_OLS(active);
    history(k).beta_OLS     = beta_OLS(active)./sx(active);
    history(k).b_OLS        = my - sum(mx./sx.*beta_OLS);
    history(k).mu_OLS       = xin * (beta_OLS./sx)' + history(k).b_OLS;
    history(k).MSE          = MSE;
    history(k).R_square     = 1 - var(yin  - history(k).mu_OLS)/var(yin);

    % exit if exact mathing is achieved.
    if abs(C_max/aa - min(gamma,gamma_tilde)) < RESOLUTION_OF_LARS
        stopReason{1}   = 'ExactMatching';
        stopReason{2}   = 0;
        break;
    end

end % end of while loop
if ~quiet & trace >=0
    fprintf('\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b%5d/%5d    :::::   %5d\n',length(active),length(all_candidate),k);
end

return;


function K = sdlfmaXsdlfmvKernComputeBlock(lfmKern1, lfmKern2, t1, t2, ...
    kyy, kyv, kvy, kvv, i, j, generalConst)

% SDLFMAXSDLFMVKERNCOMPUTEBLOCK Computes SDLFM kernel matrix for block i,j
% FORMAT
% DESC computes the kernel matrix for the SDLFM kernel function in the
% block specified at indeces i,j. It assumes the computation for functions
% that describe acceleration (system 1) and velocity (system 2)
% ARG lfmKern1 : structure containing parameters for the system 1
% ARG lfmKern2 : structure containing parameters for the system 2
% ARG t1 : times at which the system 1 is evaluated
% ARG t2 : times at which the system 2 is evaluated
% ARG kyy : covariance for the initial conditions between position 1 and
% position 2 at block i,j
% ARG kyv : covariance for the initial conditions between position 1 and
% velocity 2 at block i,j
% ARG kvy : covariance for the initial conditions between velocity 1 and
% position 2 at block i,j
% ARG kvv : covariance for the initial conditions between velocity 1 and
% velocity 2 at block i,j
% ARG i : interval to be evaluated for system 1
% ARG j : interval to be evaluated for system 2
% ARG generalConstant : constants evaluated with sdlfmKernComputeConstant.m
% RETURN K : the kernel matrix portion of block i,j
%
% COPYRIGHT : Mauricio A. Alvarez, 2010.

% KERN

if nargin<11
    j = i;
    generalConst = [];
end

a1 = sdlfmaMeanCompute(lfmKern1(1), t1, 'Pos');
b1 = sdlfmaMeanCompute(lfmKern1(1), t1, 'Vel');
g2 = sdlfmvMeanCompute(lfmKern2(1), t2, 'Pos');
h2 = sdlfmvMeanCompute(lfmKern2(1), t2, 'Vel');

K = kyy*a1*g2.' + kyv*a1*h2.' + kvy*b1*g2.' + kvv*b1*h2.';

if i==j
    for k=1:length(lfmKern1)
        K  = K + lfmaXlfmvKernCompute(lfmKern1(k), lfmKern2(k), t1, t2);
    end
else
    if i>j
        PosVel = zeros(1, length(t2));
        VelVel = zeros(1, length(t2));
        for k=1:length(lfmKern1)
            PosVel = PosVel + lfmvXlfmKernCompute(lfmKern2(k), lfmKern1(k), t2, lfmKern2(k).limit).';
            VelVel = VelVel + lfmvXlfmvKernCompute(lfmKern1(k), lfmKern2(k), lfmKern2(k).limit, t2);
        end
        if isempty(generalConst{i,j})
            K = K + a1*PosVel + b1*VelVel;
        else
            K = K + (generalConst{i,j}(1,1)*a1 + generalConst{i,j}(2,1)*b1)*PosVel + ...
                (generalConst{i,j}(1,2)*a1 + generalConst{i,j}(2,2)*b1)*VelVel;
        end
    else
        AccelPos = zeros(length(t1),1);
        AccelVel = zeros(length(t1),1);
        for k =1:length(lfmKern1)
            AccelPos = AccelPos + lfmaXlfmKernCompute(lfmKern1(k), lfmKern2(k), t1, lfmKern1(k).limit);
            AccelVel = AccelVel + lfmaXlfmvKernCompute(lfmKern1(k), lfmKern2(k), t1, lfmKern1(k).limit);
        end
        if isempty(generalConst{i,j})
            K = K + AccelPos*g2.' + AccelVel*h2.';
        else
            K = K + AccelPos*(generalConst{i,j}(1,1)*g2.' + generalConst{i,j}(2,1)*h2.') + ...
                AccelVel*(generalConst{i,j}(1,2)*g2.' + generalConst{i,j}(2,2)*h2.');
        end
    end
end


% Function to calculate H2-H1 and creak-F0 using resonator based method
%
% Description
%  Function to calculate H2-H1 and creak-F0 using resonator based method
%
%  This version is a slightly later one than that described in the
%  above published 2013 CSL paper [2]. The algorithm here rather than using binary
%  decision trees using artificial neural networks and combines the
%  features used in the CSL paper with those proposed in Ishi et al.
%  (2008). This updated version has been submitted to CSL for a special
%  issue on glottal source processing on April 14th 2013. It will have
%  reference [1].
%
% Inputs
%  res      : [samples] [Nx1] Linear prediction residual
%  fs       : [Hz]      [1x1] sampling frequency
%  F0mean   : [Hz]      [1x1] fundamental frequency
%
% Outputs
%  H2H1     : [dB]      [Mx2] H2-H1 parameter
%  f0       : [Hz]      [Mx1] Creak-F0 parameter
%
% Example
%  Please see the HOWTO_glottalsource.m example file.
%
% Octave Compatibility
%  As this algorithm relies on the Matlab neural networks toolbox it is not
%  compatible with Octave. Furthermore, this algorithm requires Matlab
%  R2011 or later
%
% References
%  [1] Drugman, T., Kane, J., Gobl, C., `Automatic Analysis of Creaky
%       Excitation Patterns', Submitted to Computer Speech and
%       Language.
%  [2] Kane, J., Drugman, T., Gobl, C., (2013) `Improved automatic
%       detection of creak', Computer Speech and Language 27(4), pp.
%       1028-1047.
%  [3] Drugman, T., Kane, J., Gobl, C., (2012) `Resonator-based creaky
%       voice detection', Interspeech 2012, Portland, Oregon, USA.
%
%
% Copyright (c) 2013 Trinity College Dublin
%
% License
%  This code is a part of the GLOAT toolbox with the following
%  licence:
%  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.
%
% This function is part of the Covarep project: http://covarep.github.io/covarep
%
% Authors
%  Thomas Drugman <thomas.drugman@umons.ac.be> & John Kane <kanejo@tcd.ie>
%

function [H2H1,f0] = get_creak_h2h1(res,fs,F0mean)

%% Initial settings
buffer=0;
Start=1;
Stop=50/1000*fs;

Hop=10/1000*fs;
Win=hanning(Stop);
Ind=1;
f0=[];
Delta=[];

moving_average_size=100/1000*fs;
moving_average_frame=moving_average_size/Hop;

% Resonator 2 settings
Phi=2*pi*1*F0mean/fs;
Rho=0.97;
rep=filter([1 0 0],[1 -2*Rho*cos(Phi) Rho^2],res);

rep=rep/max(abs(rep));

% Resonator 1 settings
Phi=2*pi*1*F0mean/fs;
Rho=0.8;
rep2=filter([1 0 0],[1 -2*Rho*cos(Phi) Rho^2],res);

%% Do processing
while Stop<length(rep)
    Sig=rep2(Start:Stop)';
    Sig=Sig.*Win;

    Corrs=xcorr(Sig,Sig);
    Corrs(1:length(Sig))=[];

    for k=1:length(Corrs)
       Corrs(k)=Corrs(k)*(length(Corrs)/(length(Corrs)-(k-1)));
    end

    Corrs2=Corrs;
    k=1;
    while (Corrs2(k)>0)
        Corrs2(k)=0;
        k=k+1;
        if k==length(Corrs2)
            break;
        end
    end
    [maxi,posi]=max(Corrs2);

    if posi<5
        posi=5;
    end

    F0=round(fs/posi);

    f0(Ind)=F0;
    F0=round(f0(Ind));

    Sig=rep(Start:Stop)';
    Sig=Sig.*Win;

    Corrs=xcorr(Sig,Sig);
    Corrs(1:length(Sig))=[]; Sig=Sig.*Win;

    Corrs=xcorr(Sig,Sig);
    Corrs(1:length(Sig))=[];

    Spec=fft(Corrs,fs);
%     Spec=fft(Sig,fs);
    Spec=abs(Spec(1:fs/2));

    Spec=Spec/sqrt(sum(Spec.^2));
    Spec=20*log10(Spec);

    Delta(Ind)=(Spec(2*F0)-Spec(F0))+buffer;

    Start=Start+Hop;
    Stop=Stop+Hop;
    Ind=Ind+1;
end

%% Interpolate to update every sample
if exist('Delta','var')
    if isempty(Delta)==0
        Delta=smooth(Delta,moving_average_frame);
        f0=medfilt1(f0,7);
        f0=smooth(f0,moving_average_frame);
        Delta=interp1(linspace(1,length(res),length(Delta)),Delta,1:length(res));
        %Delta=resample(Delta,length(res),length(Delta));
        Delta=Delta-buffer;

        f0=interp1(linspace(1,length(res),length(f0)),f0,1:length(res));

        %Delta2=smooth(Delta,moving_average_size); % 100ms moving average filter
        %f0=smooth(f0,moving_average_size); % 100ms moving average filter
        Delta2=Delta;

        H2H1=Delta2;
    else H2H1=zeros(1,length(res))-buffer;
        f0=zeros(1,length(res));
    end
else H2H1=zeros(1,length(res))-buffer;
    f0=zeros(1,length(res));
end


function fire_serial ( forest_size, prob_spread )

%*****************************************************************************80
%
%% MAIN is the main program for FIRE_SERIAL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 July 2013.
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Input, integer FOREST_SIZE, specifies the forest as a square of
%    FOREST_SIZE by FOREST_SIZE trees.  Default is 20.
%
%    Input, real PROB_SPREAD, the probability that fire will spread from
%    a burning tree to its neighbor.  Default is 0.5.
%
  if ( nargin < 1 )
    forest_size = 20;
  elseif ( ischar ( forest_size ) )
    forest_size = str2num ( forest_size );
  end

  if ( nargin < 2 )
    prob_spread = 0.5;
  elseif ( ischar ( prob_spread ) )
    prob_spread = str2num ( prob_spread );
  end

  SMOLDERING = 1;

  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'FIRE_SERIAL\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  A probabilistic simulation of a forest fire.\n' );
  fprintf ( 1, '  The forest size is %d\n', forest_size );
  fprintf ( 1, '  The probability of tree-to-tree spread is %g\n', prob_spread );
%
%  Initialize the random number generator.
%
  seed = get_seed ( );
  fprintf ( 1, '  The random number generator is seeded by %d\n', seed );
%
%  Initialize the values in the forest.
%
  forest = forest_initialize ( forest_size );
%
%  Choose a tree at random where the fire will start.
%
  [ i_ignite, seed ] = i4_uniform_ab ( 1, forest_size, seed );
  [ j_ignite, seed ] = i4_uniform_ab ( 1, forest_size, seed );
  forest(i_ignite,j_ignite) = SMOLDERING;
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Fire starts at tree(%d,%d)\n', i_ignite, j_ignite );
%
%  Let time run until nothing is burning any more.
%
  while ( forest_is_burning ( forest_size, forest ) )
    [ forest, seed ] = forest_burns ( forest_size, forest, seed, prob_spread );
  end
%
%  Display the final forest state.
%
  forest_print ( forest_size, forest, i_ignite, j_ignite );
%
%  Report the percentage of forest burned.
%
  percent = get_percent_burned ( forest_size, forest );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Percentage of forest burned = %g\n', percent );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'FIRE_SERIAL:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end
function [ value, seed ] = fire_spreads ( seed, prob_spread )

%*****************************************************************************80
%
%% FIRE_SPREADS determines whether the fire spreads.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 July 2013
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Input/output, integer SEED, a seed for the random
%    number generator.
%
%    Input, real PROB_SPREAD, the probability of spreading.
%
%    Output, logical VALUE, is TRUE if the fire spreads.
%
  [ u, seed ] = r8_uniform_01 ( seed );

  if ( u < prob_spread )
    value = 1;
  else
    value = 0;
  end

  return
end
function [ forest, seed ] = forest_burns ( forest_size, forest, seed, ...
  prob_spread )

%*****************************************************************************80
%
%% FOREST_BURNS models a single time step of the burning forest.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 July 2013
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Input, integer FOREST_SIZE, the linear dimension of the forest.
%
%    Input/output, integer FOREST(FOREST_SIZE,FOREST_SIZE), an
%    array with an entry for each tree in the forest.
%
%    Input/output, integer SEED, a seed for the random
%    number generator.
%
%    Input, real PROB_SPREAD, the probability that the fire will
%    spread from a burning tree to an unburnt one.
%
  BURNING = 2;
  BURNT = 3;
  SMOLDERING = 1;
  UNBURNT = 0;
%
%  Burning trees burn down;
%  Smoldering trees ignite;
%
  for j = 1 : forest_size
    for i = 1 : forest_size
      if ( forest(i,j) == BURNING )
        forest(i,j) = BURNT;
      elseif ( forest(i,j) == SMOLDERING )
        forest(i,j) = BURNING;
      end
    end
  end
%
%  Unburnt trees might catch fire.
%
  for j = 1 : forest_size
    for i = 1 : forest_size

      if ( forest(i,j) == BURNING )
%
%  North.
%
        if ( 1 < i )
          if ( forest(i-1,j) == UNBURNT )
            [ value, seed ] = fire_spreads ( seed, prob_spread );
            if ( value )
              forest(i-1,j) = SMOLDERING;
            end
          end
        end
%
%  South.
%
        if ( i < forest_size )
          if ( forest(i+1,j) == UNBURNT )
            [ value, seed ] = fire_spreads ( seed, prob_spread );
            if ( value )
              forest(i+1,j) = SMOLDERING;
            end
          end
        end
%
%  West.
%
        if ( 1 < j )
          if ( forest(i,j-1) == UNBURNT )
            [ value, seed ] = fire_spreads ( seed, prob_spread );
            if ( value )
              forest(i,j-1) = SMOLDERING;
            end
          end
        end
%
%  East.
%
        if ( j < forest_size )
          if ( forest(i,j+1) == UNBURNT )
            [ value, seed ] = fire_spreads ( seed, prob_spread );
            if ( value )
              forest(i,j+1) = SMOLDERING;
            end
          end
        end

      end

    end
  end

  return
end
function forest = forest_initialize ( forest_size )

%*****************************************************************************80
%
%% FOREST_INITIALIZE initializes the forest values.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 July 2013
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Input, integer FOREST_SIZE, the linear dimension of the forest.
%
%    Output, integer FOREST(FOREST_SIZE,FOREST_SIZE), an array
%    with an entry for each tree in the forest.
%
  UNBURNT = 0;

  forest(1:forest_size,1:forest_size) = UNBURNT;

  return
end
function value = forest_is_burning ( forest_size, forest )

%*****************************************************************************80
%
%% FOREST_IS_BURNING reports whether any trees in the forest are burning.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 July 2013
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Input, integer FOREST_SIZE, the linear dimension of the forest.
%
%    Input, integer FOREST(FOREST_SIZE,FOREST_SIZE), an array
%    with an entry for each tree in the forest.
%
%    Output, logical FOREST_IS_BURNING, is TRUE if any tree in the forest
%    is in the SMOLDERING or BURNING state.
%
  BURNING = 2;
  SMOLDERING = 1;

  value = 0;

  for j = 1 : forest_size
    for i = 1 : forest_size
      if ( forest(i,j) == SMOLDERING || forest(i,j) == BURNING )
        value = 1;
      end
    end
  end

  return
end
function forest_print ( forest_size, forest, i_ignite, j_ignite )

%*****************************************************************************80
%
%% FOREST_PRINT prints the state of the trees in the forest.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 July 2013
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Input, integer FOREST_SIZE, the linear dimension of the forest.
%
%    Input, integer FOREST(FOREST_SIZE,FOREST_SIZE), an array
%    with an entry for each tree in the forest.
%
%    Input, integer I_IGNITE, J_IGNITE, the location of the start
%    of the fire.
%
  BURNT = 3;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Map of fire damage.\n' );
  fprintf ( 1, '  Fire started at "*".\n' );
  fprintf ( 1, '  Burned trees are indicated by ".".\n' );
  fprintf ( 1, '  Unburned trees are indicated by "X".\n' );
  fprintf ( 1, '\n' );

  for i = 1 : forest_size
    fprintf ( 1, '  ' );
    for j = 1 : forest_size
      if ( i == i_ignite && j == j_ignite )
        fprintf ( 1, '*' );
      elseif ( forest(i,j) == BURNT )
        fprintf ( 1, '.' );
      else
        fprintf ( 1, 'X' );
      end
    end
    fprintf ( 1, '\n' );
  end

  return
end
function percent = get_percent_burned ( forest_size, forest )

%*****************************************************************************80
%
%% GET_PERCENT_BURNED computes the percentage of the forest that burned.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 July 2013
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Input, integer FOREST_SIZE, the linear dimension of the forest.
%
%   Input, integer FOREST(FOREST_SIZE,FOREST_SIZE), an array
%    with an entry for each tree in the forest.
%
%    Output, real PERCENT, the percentage of the forest
%    that burned.
%
  BURNT = 3;

  total = 0;
  for j = 1 : forest_size
    for i = 1 : forest_size
      if ( forest(i,j) == BURNT )
        total = total + 1;
      end
    end
  end

  percent = total / forest_size / forest_size;

  return
end
function seed = get_seed ( )

%*****************************************************************************80
%
%% GET_SEED returns a seed for the random number generator.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 November 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, integer SEED, a random seed value.
%
  I_MAX = 2147483647;

  time_array = clock;

  hour = time_array(4);
  minute = time_array(5);
  second = time_array(6);

  temp = ( second + 60 * ( minute + 60 * hour ) ) / ( 60.0 * 60.0 * 24.0 );

  if ( temp <= 0.0 )
    temp = temp + 1.0;
  end

  if ( 1.0 < temp )
    temp = temp - 1.0;
  end

  seed = 1 + floor ( I_MAX * temp );

  return
end
function [ c, seed ] = i4_uniform_ab ( a, b, seed )

%*****************************************************************************80
%
%% I4_UNIFORM_AB returns a scaled pseudorandom I4.
%
%  Discussion:
%
%    The pseudorandom number will be scaled to be uniformly distributed
%    between A and B.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 November 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Paul Bratley, Bennett Fox, Linus Schrage,
%    A Guide to Simulation,
%    Second Edition,
%    Springer, 1987,
%    ISBN: 0387964673,
%    LC: QA76.9.C65.B73.
%
%    Bennett Fox,
%    Algorithm 647:
%    Implementation and Relative Efficiency of Quasirandom
%    Sequence Generators,
%    ACM Transactions on Mathematical Software,
%    Volume 12, Number 4, December 1986, pages 362-376.
%
%    Pierre L'Ecuyer,
%    Random Number Generation,
%    in Handbook of Simulation,
%    edited by Jerry Banks,
%    Wiley, 1998,
%    ISBN: 0471134031,
%    LC: T57.62.H37.
%
%    Peter Lewis, Allen Goodman, James Miller,
%    A Pseudo-Random Number Generator for the System/360,
%    IBM Systems Journal,
%    Volume 8, Number 2, 1969, pages 136-143.
%
%  Parameters:
%
%    Input, integer A, B, the minimum and maximum acceptable values.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, integer C, the randomly chosen integer.
%
%    Output, integer SEED, the updated seed.
%
  i4_huge = 2147483647;

  if ( seed == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4_UNIFORM_AB - Fatal error!\n' );
    fprintf ( 1, '  Input SEED = 0!\n' );
    error ( 'I4_UNIFORM_AB - Fatal error!' );
  end

  seed = floor ( seed );
  a = round ( a );
  b = round ( b );

  seed = mod ( seed, i4_huge );

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

  k = floor ( seed / 127773 );

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

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

  r = seed * 4.656612875E-10;
%
%  Scale R to lie between A-0.5 and B+0.5.
%
  r = ( 1.0 - r ) * ( min ( a, b ) - 0.5 ) ...
    +         r   * ( max ( a, b ) + 0.5 );
%
%  Use rounding to convert R to an integer between A and B.
%
  value = round ( r );

  value = max ( value, min ( a, b ) );
  value = min ( value, max ( a, b ) );

  c = value;

  return
end
function [ r, seed ] = r8_uniform_01 ( seed )

%*****************************************************************************80
%
%% R8_UNIFORM_01 returns a unit pseudorandom R8.
%
%  Discussion:
%
%    This routine implements the recursion
%
%      seed = 16807 * seed mod ( 2^31 - 1 )
%      r8_uniform_01 = seed / ( 2^31 - 1 )
%
%    The integer arithmetic never requires more than 32 bits,
%    including a sign bit.
%
%    If the initial seed is 12345, then the first three computations are
%
%      Input     Output      R8_UNIFORM_01
%      SEED      SEED
%
%         12345   207482415  0.096616
%     207482415  1790989824  0.833995
%    1790989824  2035175616  0.947702
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Paul Bratley, Bennett Fox, Linus Schrage,
%    A Guide to Simulation,
%    Springer Verlag, pages 201-202, 1983.
%
%    Pierre L'Ecuyer,
%    Random Number Generation,
%    in Handbook of Simulation,
%    edited by Jerry Banks,
%    Wiley Interscience, page 95, 1998.
%
%    Bennett Fox,
%    Algorithm 647:
%    Implementation and Relative Efficiency of Quasirandom
%    Sequence Generators,
%    ACM Transactions on Mathematical Software,
%    Volume 12, Number 4, pages 362-376, 1986.
%
%    Peter Lewis, Allen Goodman, James Miller,
%    A Pseudo-Random Number Generator for the System/360,
%    IBM Systems Journal,
%    Volume 8, pages 136-143, 1969.
%
%  Parameters:
%
%    Input, integer SEED, the integer "seed" used to generate
%    the output random number.  SEED should not be 0.
%
%    Output, real R, a random value between 0 and 1.
%
%    Output, integer SEED, the updated seed.  This would
%    normally be used as the input seed on the next call.
%
  if ( seed == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_UNIFORM_01 - Fatal error!\n' );
    fprintf ( 1, '  Input SEED = 0!\n' );
    error ( 'R8_UNIFORM_01 - Fatal error!' );
  end

  seed = floor ( seed );

  seed = mod ( seed, 2147483647 );

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

  k = floor ( seed / 127773 );

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

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

  r = seed * 4.656612875E-10;

  return
end
function timestamp ( )

%*****************************************************************************80
%
%% TIMESTAMP prints the current YMDHMS date as a timestamp.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  t = now;
  c = datevec ( t );
  s = datestr ( c, 0 );
  fprintf ( 1, '%s\n', s );

  return
end


function element_node = grid_element ( code, element_order, nelemx, nelemy )

%*****************************************************************************80
%
%% GRID_ELEMENT returns the grid associated with any available element.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 April 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, character CODE(*), identifies the element desired.
%    Legal values include 'Q4', 'Q8', 'Q9', 'Q12', 'Q16', 'QL', 'T3',
%    'T6' and 'T10'.
%
%    Input, integer ELEMENT_ORDER, the order of the element.
%
%    Input, integer NELEMX, NELEMY, the number of quadrilaterals along the
%    X and Y directions.  The number of elements generated will be
%    NELEMX * NELEMY for quadrilaterals, or 2 * NELEMX * NELEMY for
%    triangles.
%
%    Output, integer ELEMENT_NODE(ELEMENT_ORDER,ELEMENT_NUM), the nodes
%    that form each element.
%
  if ( s_eqi ( code, 'Q4' ) )
    element_node = grid_q4_element ( nelemx, nelemy );
  elseif ( s_eqi ( code, 'Q8' ) )
    element_node = grid_q8_element ( nelemx, nelemy );
  elseif ( s_eqi ( code, 'Q9' ) )
    element_node = grid_q9_element ( nelemx, nelemy );
  elseif ( s_eqi ( code, 'Q12' ) )
    element_node = grid_q12_element ( nelemx, nelemy );
  elseif ( s_eqi ( code, 'Q16' ) )
    element_node = grid_q16_element ( nelemx, nelemy );
  elseif ( s_eqi ( code, 'QL' ) )
    element_node = grid_ql_element ( nelemx, nelemy );
  elseif ( s_eqi ( code, 'T3' ) )
    element_node = grid_t3_element ( nelemx, nelemy );
  elseif ( s_eqi ( code, 'T4' ) )
    element_node = grid_t4_element ( nelemx, nelemy );
  elseif ( s_eqi ( code, 'T6' ) )
    element_node = grid_t6_element ( nelemx, nelemy );
  elseif ( s_eqi ( code, 'T10' ) )
    element_node = grid_t10_element ( nelemx, nelemy );

  else

    fprintf ( 1, '\n' );
    fprintf ( 1, 'GRID_ELEMENT - Fatal error!\n' );
    fprintf ( 1, '  Illegal value of CODE = %s\n', code );
    error ( 'GRID_ELEMENT - Fatal error!' );

  end

  return
end


clc;
pzstat = input('enter a puzzle in 3 x 3 form (use 0 for space) ..........:   ');
pzrstat = [1 2 3;4 5 6;7 8 0];
intv000 = size(pzstat);
if not(all(intv000 == [3 3]))
    msgbox('Input is not in correct format.............. The format must be 3 x 3 matrix. The blank space should be replaced by 0.','8-Puzzle suresh Kumar Gadi');
else
    pzinl = [];
    pzhis = [];
    pzrhis = [];
    intv00g = 0;
    [intv0h1,intv0h2] = gskcostastar(pzstat,pzrstat);
    intv00h = intv0h1 + intv0h2;
    intv00f = intv00h + intv00g;
    pzinl = gskpzjoin(pzinl,pzstat,intv00f,intv00g,intv00h);
    pzhis = gskpzjoin(pzhis,pzstat,intv00f,intv00g,intv00h);
    pzhis(:,13)=0;
    pzhis(1,14)=0;
    pzhis(1,15)=1;
    intv001 = 1;
    intv0ct = 0;
    intv0nd = 1;
    intv1nd = 1;
    while intv001 == 1;
        intv0ct = intv0ct + 1;
        [pzout, intv00f, intv00g, intv00h] =  gskpzget(pzinl,1);
        [intv002, intv003] = find(pzout==0);
        pzinl = [];
        if intv00h ~= 0
% % % % % % left
            if intv003 > 1
                pzout1 = pzout;
                pzout1(intv002,intv003) = pzout(intv002,intv003 - 1);
                pzout1(intv002,intv003 - 1) = 0;
                [intv0h1,intv0h2] = gskcostastar(pzout1,pzrstat);
                intv00h = intv0h1 + intv0h2;
                intv0g1 = intv00g + 1;
                intv00f = intv0g1 + intv00h;
                [pzinl] =  gskpzjoin(pzinl,pzout1,intv00f,intv0g1,intv00h);
            end
% % % % % % top
            if intv002 > 1
                pzout1 = pzout;
                pzout1(intv002,intv003) = pzout(intv002-1,intv003);
                pzout1(intv002-1,intv003) = 0;
                [intv0h1,intv0h2] = gskcostastar(pzout1,pzrstat);
                intv00h = intv0h1 + intv0h2;
                intv0g1 = intv00g + 1;
                intv00f = intv0g1 + intv00h;
                [pzinl] =  gskpzjoin(pzinl,pzout1,intv00f,intv0g1,intv00h);
            end
% % % % % % right
            if intv003 < 3
                pzout1 = pzout;
                pzout1(intv002,intv003) = pzout(intv002,intv003 + 1);
                pzout1(intv002,intv003 + 1) = 0;
                [intv0h1,intv0h2] = gskcostastar(pzout1,pzrstat);
                intv00h = intv0h1 + intv0h2;
                intv0g1 = intv00g + 1;
                intv00f = intv0g1 + intv00h;
                [pzinl] =  gskpzjoin(pzinl,pzout1,intv00f,intv0g1,intv00h);
            end
% % % % % % down
            if intv002 < 3
                pzout1 = pzout;
                pzout1(intv002,intv003) = pzout(intv002 + 1,intv003);
                pzout1(intv002 + 1,intv003) = 0;
                [intv0h1,intv0h2] = gskcostastar(pzout1,pzrstat);
                intv00h = intv0h1 + intv0h2;
                intv0g1 = intv00g + 1;
                intv00f = intv0g1 + intv00h;
                [pzinl] =  gskpzjoin(pzinl,pzout1,intv00f,intv0g1,intv00h);
            end
% % % % % % compare with history
            [intv003, intv004] = size(pzinl);
            for intv004 = 1 : intv003
                intv005 = gsksearch(pzhis,pzinl(intv004,:));
                if ~isempty(intv005)
%                     pzhis(intv005,13) = 1;
                else
                    [intv007 , intv008] = size(pzhis);
                    intv008 = pzinl(intv004,1:12);
                    intv008(13) = 0;
                    intv008(14) = intv0nd;
                    intv1nd = intv1nd + 1;
                    intv008(15) = intv1nd;
                    pzhis(intv007+1,:)=intv008;
                end
            end
% % % % % % selecting new node
            intv005 = find(pzhis(:,13) == 1);
            intv006 = length(intv005);
            pzrhis = pzhis;
            for intv007 = 1:intv006
                pzrhis(intv005(intv007)-(intv007-1),:)=[];
            end
            intv003 = min(pzrhis,[],1);
            [pzout, intv00f, intv00g, intv00h] =  gskpzget(intv003,1);
            [intv002, intv003] = find((pzrhis(:,10)==intv00f) & (pzrhis(:,13) == 0));
%             intv002 = intv002(1,1);
            [pzout, intv00f, intv00g, intv00h] =  gskpzget(pzrhis,intv002(1,1));
            intv0nd = pzrhis(intv002(1,1),15);
            pzinl = [];
            [pzinl] =  gskpzjoin(pzinl,pzout,intv00f,intv00g,intv00h);
            intv005 = gsksearch(pzhis,pzinl);
            pzhis(intv005,13) = 1;
            intv001 = 1;
        else
            intv001 = 2;
        end
    end
% % Display of steps
    [intv000, intv001] = size(pzhis);
    intv002 = 1;
    pzshow = [];
    intvcnt = 0;
    while intv002 ==1
        intvcnt = intvcnt + 1;
        pzshow(intvcnt) = intv000;
        intv000 = pzhis(intv000,14);
        if intv000 == 0
            intv002 = 2;
        else
            intv002 = 1;
        end
    end
    for intv000 = 1 : intvcnt
        [pzout, intv00f, intv00g, intv00h] =  gskpzget(pzhis,pzshow(intvcnt-intv000+1));
        intv000
        pzout
    end
end


% =========================================================================
% This code is part of the Matlab-based toolbox
% LagLDDDM - A Lagrangian Gauss--Newton--Krylov Solver for Mass- and
%                        Intensity-Preserving Diffeomorphic Image Registration
%
% For details and license info see
% - https://github.com/C4IR/FAIR.m/tree/master/add-ons/LagLDDMM
%
% 3D Hyperelastic registration of NIREP data to compare with LDDMM
% as described in Sec. 4 of the paper:
%
% @article{MangRuthotto2017,
%   Title = {A {L}agrangian {G}auss--{N}ewton--{K}rylov solver for mass- and intensity-preserving diffeomorphic image registration},
%   Year = {2017},
%   Journal = {SIAM Journal on Scientific Computing},
%   Author = {A. Mang, L. Ruthotto},
% }
%
% =========================================================================

close all; clc; clear all;

setupNIREPDataNA02;
%setupNIREPDataNA03;
%setupNIREPDataNA04;
%setupNIREPDataNA05;

imgModel('reset','imgModel','linearInterMex');
trafo('reset','trafo','rigid3D');
distance('reset','distance','SSD');
viewImage('reset','viewImage','viewOrthoSlices2D');

% example specific parameters
switch example
    case ['3D-nirep-',dataset]
        alpha(1) = 100;
        alpha(2) = 1;
        alpha(3) = .1;
        alpha(4) = 1;
        pad = 0;
        minLevel = 5;
        maxLevel = 7;
        maxIterNPIR = 50;
    otherwise
        error('MP-LDDMM example %s - nyi');
end

Dshow = @(T,R,omega,m) viewIP(abs(T-R),omega,m,'colormap',gray(256));
FAIRplots('set','Dshow',Dshow);

solve = true;
postproc = true;

% 2) setup regularizer
regularizer('reset','regularizer','mfHyperElastic','alpha',alpha(1),...
    'alphaLength',alpha(2),'alphaArea',alpha(3),'alphaVolume',alpha(4));

filename = ['HyperElastic-',example,'-',regularizer,'-alpha-',num2str(alpha(1)),'-',num2str(alpha(2)),'-',num2str(alpha(3)),'-',num2str(alpha(4))];

if (solve)
    diary(fullfile('results',[filename,'.log']));
    % finally: run the MultiLevel Non-Parametric Image Registration
    [yc,wc,his] = MLIR(ML,'parametric',0,'NPIRLS',@ArmijoBacktrack,'minLevel',minLevel,'maxLevel',maxLevel,'maxIterNPIR',maxIterNPIR);
    diary('off')
    [~,interOpts] = inter;
    [~,distOpts ] = distance;
    [~,regOpts  ] = regularizer;

    save(fullfile('results',[filename,'.mat']),'yc','wc','his','m','minLevel','maxLevel','interOpts','distOpts','regOpts')
end

if postproc
    % generate figures
    close all;
    load(fullfile('results',[filename,'.mat']));

    computeOverlap;

    % generate figures
    figDir = ['results/',filename];
    if ~exist(figDir,'dir'), mkdir(figDir); end;
    dim  = numel(omega)/2;

    viewImage('reset','viewImage','viewOrthoSlices2D','colormap','gray');
    fig = figure(1); clf;
    fig.Name = 'dataR';
    viewImage(dataR,omega,m);
    cax = caxis;

    fig = figure(2); clf;
    fig.Name = 'dataT';
    viewImage(dataT,omega,m);
    caxis(cax);

    fig = figure(3); clf;
    fig.Name = 'res0';
    % viewImage(abs(dataR-dataT),omega,m)
    viewOrthoSlices2D(abs(dataR-dataT),omega,m,'color1',[0 0 0],'color2',[0 0 0]);
    colormap gray;
    colormap(flipud(colormap));
    % caxd = caxis;
    caxd = cax;

    Topt = imgModel(dataT,omega,center(yc,m));
    fig = figure(4); clf;
    fig.Name = 'Topt';
    viewImage(Topt,omega,m);
    % colormap gray
    caxis(cax);

    fig = figure(5); clf;
    fig.Name = 'resOpt';
    % viewImage(abs(dataR(:)-Topt),omega,m)
    viewOrthoSlices2D(abs(dataR(:)-Topt),omega,m,'color1',[0 0 0],'color2',[0 0 0]);
    caxis(caxd);
    colormap gray;
    colormap(flipud(colormap));

    fig = figure(6); clf;
    fig.Name = 'vol';
    Jac = geometry(yc,m,'Jac','matrixFree',1,'omega',omega);
    viewOrthoSlices2D(Jac,omega,m,'color1',[0 0 0],'color2',[0 0 0]);
    colormap jet;
    cb = colorbar('East');
    cpos = cb.Position;
    cb.Position(4) = cpos(4)*.4;
    cb.Position(2) = .56;
    cb.Ticks = [min(Jac) max(Jac)];
    caxis([min(Jac) max(Jac)]);
    cb.TickLabels = {sprintf('%1.2f',min(Jac)), sprintf('%1.2f',max(Jac))};
    cb.FontSize = 50;
    cb.Color = 'k';

    %str = {'dataR','dataT','res0','Topt','resOpt','yOpt','yInv','vol','T+yc'};
    str = {'dataR','dataT','res0','Topt','resOpt','vol'};
    for k=1:6
        figure(k);
        axis off;
        title([]);
        printFigure(gcf,fullfile(figDir,['LDDMM-' str{k} '_' regularizer '.png']),...
            'printOpts','-dbmp','printFormat','.bmp');
    end
end


function D = driving_function_mono_nfchoa(x0,xs,src,f,conf)
%DRIVING_FUNCTION_MONO_NFCHOA driving signal for NFC-HOA
%
%   Usage: D = driving_function_mono_nfchoa(x0,xs,src,f,conf)
%
%   Input parameters:
%       x0          - position and direction of the secondary source / m [nx7]
%       xs          - position of virtual source or direction of plane
%                     wave / m [1x3] or [1x6]
%                     OR position and orientation of a line source [1x6]
%       src         - source type of the virtual source
%                         'pw' - plane wave (xs is the direction of the
%                                plane wave in this case)
%                         'ls' - line source
%                         'ps' - point source
%                         'fs' - focused source
%       f           - frequency of the monochromatic source / Hz
%       conf        - configuration struct (see SFS_config)
%
%   Output parameters:
%       D           - driving function signal [nx1]
%
%   See also: plot_sound_field, sound_field_mono_nfchoa,
%             driving_function_imp_nfchoa

%*****************************************************************************
% The MIT License (MIT)                                                      *
%                                                                            *
% Copyright (c) 2010-2019 SFS Toolbox Developers                             *
%                                                                            *
% Permission is hereby granted,  free of charge,  to any person  obtaining a *
% copy of this software and associated documentation files (the "Software"), *
% to deal in the Software without  restriction, including without limitation *
% the rights  to use, copy, modify, merge,  publish, distribute, sublicense, *
% and/or  sell copies of  the Software,  and to permit  persons to whom  the *
% Software is furnished to do so, subject to the following conditions:       *
%                                                                            *
% The above copyright notice and this permission notice shall be included in *
% all copies or substantial portions of the Software.                        *
%                                                                            *
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
% IMPLIED, INCLUDING BUT  NOT LIMITED TO THE  WARRANTIES OF MERCHANTABILITY, *
% FITNESS  FOR A PARTICULAR  PURPOSE AND  NONINFRINGEMENT. IN NO EVENT SHALL *
% THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
% LIABILITY, WHETHER  IN AN  ACTION OF CONTRACT, TORT  OR OTHERWISE, ARISING *
% FROM,  OUT OF  OR IN  CONNECTION  WITH THE  SOFTWARE OR  THE USE  OR OTHER *
% DEALINGS IN THE SOFTWARE.                                                  *
%                                                                            *
% The SFS Toolbox  allows to simulate and  investigate sound field synthesis *
% methods like wave field synthesis or higher order ambisonics.              *
%                                                                            *
% https://sfs.readthedocs.io                            sfstoolbox@gmail.com *
%*****************************************************************************

%% ===== Checking of input  parameters ==================================
nargmin = 5;
nargmax = 5;
narginchk(nargmin,nargmax);
isargsecondarysource(x0);
isargxs(xs);
isargpositivescalar(f);
isargchar(src);
isargstruct(conf);

%% ===== Computation ====================================================

% Secondary source positions
x0 = x0(:,1:3);

% Get maximum order of spherical harmonics
N = nfchoa_order(size(x0,1),conf);

% Source position/direction/orientation
xs = repmat(xs,[size(x0,1) 1]);

% Get driving signals
if strcmp('pw',src)
    % === Plane wave =====================================================
    % Direction of plane wave
    nk = bsxfun(@rdivide,xs,vector_norm(xs,2));
    % Driving signal
    D = driving_function_mono_nfchoa_pw(x0,nk,f,N,conf);

elseif strcmp('ps',src)
    % === Point source ===================================================
    % Driving Signal
    D = driving_function_mono_nfchoa_ps(x0,xs,f,N,conf);

elseif strcmp('ls',src)
    % === Line source ====================================================
    % Driving signal
    D = driving_function_mono_nfchoa_ls(x0,xs,f,N,conf);

elseif strcmp('fs',src)
    % === Focused source =================================================
    % Driving Signal
    D = driving_function_mono_nfchoa_fs(x0,xs,f,N,conf);

else
    error('%s: %s is not a known source type.',upper(mfilename),src);
end


function itc_ds=cosmo_phase_itc(ds,varargin)
% compute phase inter trial coherence
%
% itc_ds=cosmo_phase_itc(ds,varargin)
%
% Inputs:
%   ds                      dataset struct with fields:
%       .samples            PxQ complex matrix for P samples (trials,
%                           observations) and Q features (e.g. combinations
%                           of time points, frequencies and channels)
%       .sa.targets         Px1 array with trial conditions. Each condition
%                           must occur equally often; that is, the
%                           samples must be balanced.
%                           In the typical case of two conditions,
%                           .sa.targets must have exactly two unique
%                           values.
%       .sa.chunks          Px1 array indicating which samples can be
%                           considered to be independent. It is required
%                           that all samples are independent, therefore
%                           all values in .sa.chunks must be different from
%                           each other
%       .fa                 } optional feature attributes
%       .a                  } optional sample attributes
%  'samples_are_unit_length',u  (optional, default=false)
%                           If u==true, then all elements in ds.samples
%                           are assumed to be already of unit length. If
%                           this is indeed true, this can speed up the
%                           computation of the output.
%  'check_dataset',c        (optional, default=true)
%                           if c==false, there is no check for consistency
%                           of the ds input.
%
% Output:
%   itc_ds                  dataset struct with fields:
%       .samples            (N+1)xQ array with inter-trial coherence
%                           measure, where U=unique(ds.sa.targets) and
%                           N=numel(U). The first N rows correspond to the
%                           inter trial coherence for each condition. The
%                           last row is the inter trial coherence for all
%                           samples together.
%       .sa.targets         (N+1)x1 vector containing the values
%                           [U(:);NaN]' with trial conditions
%       .a                  } if present in the input, then the output
%       .fa                 } contains these fields as well
%
% #   For CoSMoMVPA's copyright information and license terms,   #
% #   see the COPYING file distributed with CoSMoMVPA.           #

    defaults=struct();
    defaults.samples_are_unit_length=false;
    defaults.check_dataset=true;

    opt=cosmo_structjoin(defaults,varargin{:});

    check_input(ds,opt);

    samples=ds.samples;
    if opt.samples_are_unit_length
        quick_check_some_samples_being_unit_length(samples);
    else
        % normalize
        samples=samples./abs(samples);
    end

    % split based on .sa.targets
    [idxs,classes]=cosmo_index_unique(ds.sa.targets);
    nclasses=numel(classes);
    nfeatures=size(samples,2);

    % allocate space for output
    itc=zeros(nclasses+1,nfeatures);

    % ITC for each class
    for k=1:nclasses
        samples_k=samples(idxs{k},:);
        itc(k,:)=itc_on_unit_length_elements(samples_k);
    end

    % overall ITC
    itc(nclasses+1,:)=itc_on_unit_length_elements(samples);

    % set output
    itc_ds=set_output(itc,ds,classes);

function itc_ds=set_output(itc,ds,classes)
    % store results
    itc_ds=struct();
    itc_ds.samples=itc;
    itc_ds.sa.targets=[classes(:); NaN];

    % copy .a and .fa fields, if present
    if isfield(ds,'a')
        itc_ds.a=ds.a;

        if isfield(ds.a,'sdim')
            % remove sample dimensions if present
            itc_ds.a=rmfield(itc_ds.a,'sdim');
        end
    end

    if isfield(ds,'fa')
        itc_ds.fa=ds.fa;
    end

function itc=itc_on_unit_length_elements(samples)
    % computes inter-trial coherence for each column seperately
    itc=abs(sum(samples,1)/size(samples,1));

function quick_check_some_samples_being_unit_length(samples)
    % instead of checking all values, only verify for a subset of values.
    % This should prevent most use cases where the user accidentally
    % uses non-normalized data, whereas checking all values would be
    % equivalent to actually computing their length for each of them.
    count_to_check=10;

    % generate random positions to check for unit length
    nelem=numel(samples);
    pos=ceil(rand(1,count_to_check)*nelem);

    samples_subset=samples(pos);
    lengths=abs(samples_subset);

    % safety margin
    delta=10*eps('single');
    if any(lengths+delta<1 | lengths-delta>1)
        error('.samples input is not of unit length');
    end

function check_input(ds,opt)
    % must be a proper dataset
    if opt.check_dataset
        raise_exception=true;
        cosmo_check_dataset(ds,raise_exception);

        % must have targets and chunks
        cosmo_isfield(ds,{'sa.targets','sa.chunks'},raise_exception);
    end

    % all chunks must be unique
    if ~isequal(sort(ds.sa.chunks),unique(ds.sa.chunks))
        error(['All values in .sa.chunks must be different '...
                    'from each other']);
    end

    % trial counts must be balanced
    [idxs,classes]=cosmo_index_unique(ds.sa.targets);
    class_count=cellfun(@numel,idxs);
    unequal_pos=find(class_count~=class_count(1),1);
    if ~isempty(unequal_pos)
        error(['.sa.targets indicates unbalanced targets, with '...
                        '.sa.targets==%d occuring %d times, and '...
                        '.sa.targets==%d occuring %d times.\n'...
                        'To obtain balanced targets, consider '...
                        'using cosmo_balance_dataset.'],...
                    classes(1),class_count(1),...
                    classes(unequal_pos),class_count(unequal_pos));
    end

    % input must be complex
    if isreal(ds.samples)
        error('.samples must be complex');
    end

    v=opt.samples_are_unit_length;
    if ~(islogical(v) ...
            && isscalar(v))
        error('option ''samples_are_unit_length'' must be logical scalar');
    end


function varargout = plot(f, varargin)
%PLOT   Plot a BALLFUN on the ball
%   PLOT(F) creates a plot showing F on the ball.
%
%   PLOT(f, 'WedgeAz') plot a BALLFUN with a wedge in the azimuthal
%   (longitude) direction removed.
%
%   PLOT(f, 'WedgePol') plot a BALLFUN with a wedge in the polar
%   (latitude) direction removed.
%
% EXAMPLES:
%   f = cheb.galleryball;
%   plot(f)
%   plot(f, 'WedgeAz')
%   plot(f, 'WedgePol')
%
% See also BALLFUN/SURF, BALLFUN/SLICE.

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

% Check if the function is empty
if isempty(f)
    error('CHEBFUN:BALLFUN:plot:isempty','Function is empty.');
end

% Add a warning of the function is not real
if (f.isReal == 0)
    warning('CHEBFUN:BALLFUN:plot:isReal','Function is not real, plotting the real part.');
end

if ( nargin == 1 )
    h = plotBall(f);
elseif ( nargin == 2 ) && ( strcmpi(varargin{1},'wedgeaz') )
    h = plotWedgeAz(f);
elseif ( nargin == 2 ) && ( strcmpi(varargin{1},'wedgepol') )
    h = plotWedgePol(f);
else
    error('CHEBFUN:BALLFUN:plot:input Invalid input arguments')
end

if ( nargout > 0 )
    varargout = { h };
end
end

function h = plotBall(f)
% Plot a BALLFUN function on the ball

% Is the plot currently being held?
plotOnHold = ishold;
% Default plotting options
defaultOpts = {'facecolor', 'interp','edgecolor', 'none'};

% Define the size of F:
[m,n,p] = size(f);

% m >= 25 and n, p >= 28
m = 25*(m < 25) + m*(m >= 25);
n = 28*(n < 28) + n*(n>=28);
p = 28*(p < 28) + p*(p>=28);

% Impose m = 1 [6] and n, p = 0 [4] to avoid errors in the plot
m = m + mod(1-mod(m,6),6);
n = n + mod(4-mod(n,4),4);
p = p + mod(4-mod(p,4),4);

% Get the coeffs of the ballfun function F(r,lam,th), lam is the
% azimuthal variable in [-pi,pi] and theta the polar variable in [0,pi]
F = coeffs3(f, m, n, p);

% Convert to values
ff = real(ballfun.coeffs2vals(F));

% Permute lambda and theta
ff = permute(ff,[1 3 2]);

% Evaluation points
r   = chebpts( m );
lam  = [pi*trigpts( n ); pi];
th = [pi*trigpts( p );pi]-pi/2;

% Remove doubled-up data
r = r(floor(m/2)+1:end);
th = th(floor(p/2)+1:end);

% Reverse theta : 1st element of the array is theta = pi (South Pole), last element is
% th = 0 (not included) (North Pole)
ff  = ff(floor(m/2)+1:end,[1 end:-1:floor(p/2)+1],:);
ff(:,:,end+1) = ff(:,:,1);

% Define the meshgrid
[tt, rr, ll] = meshgrid(th, r, lam);

% Slices in the cylinder to plot
% Find the indice of r = 0.5
[~,idr]=min(abs(r-0.5));
rslice = rr(idr,1,1);
tslice = tt(1,[1,floor(p/4)+1],1);
lslice = ll(1,1,[1,floor(n/4)+1]);

hslicer = slice(tt,rr,ll,ff,tslice,rslice,lslice);

hold on
for j = 1:numel(hslicer)
    h = hslicer(j);
    [xs,ys,zs] = sph2cart(h.ZData,h.XData,h.YData);
    h = surf(xs,ys,zs,h.CData,defaultOpts{:});
end
delete(hslicer);

if ~plotOnHold
    hold off;
end

axis([-1 1 -1 1 -1 1])
daspect([1 1 1])

camlight('headlight');
lighting phong;
material dull;
end

function h = plotWedgeAz(f)
% Plot a BALLFUN with a wedge in the azimuthal direction removed

% Is the plot currently being held?
plotOnHold = ishold;
% Default plotting options
defaultOpts = {'facecolor', 'interp','edgecolor', 'none'};

% Azimuthal (longitude) values to include.  TODO: Make these optional inputs
az_intvl = [-pi/2 pi];

% Define the size of f:
[m,n,p] = size(f);

% m >= 25 and n, p >= 28
m = 25*(m < 25) + m*(m >= 25);
n = 28*(n < 28) + n*(n>=28);
p = 28*(p < 28) + p*(p>=28);

% Impose m = 1 [6] and n, p = 0 [4] to avoid errors in the plot
m = m + mod(1-mod(m,6),6);
n = n + mod(4-mod(n,4),4);
p = p + mod(4-mod(p,4),4);

% Construct the values of lambda and theta to plot on the outer sphere (r=1)
lam = linspace(az_intvl(1),az_intvl(2),n);
th = linspace(0,pi,p)';

% Evaluate the function on the outer sphere
ff = permute(fevalm(f,1,lam,th),[3 2 1]);

% Plot the result
h = surf(sin(th)*cos(lam),sin(th)*sin(lam),cos(th)*ones(1,n),ff,defaultOpts{:});
hold on

% Construct the values of r and theta to plot from the origin to the outer
% sphere (r=1).
r = chebpts(m); r = r(floor(m/2)+1:end); th = th';

% Evaluate the function on the wedge from the origin along the az_intvl(1).
ff = permute(fevalm(f,r,lam(1),th),[1 3 2]);
% Plot the result
surf(r*sin(th)*cos(lam(1)),r*sin(th)*sin(lam(1)),r*cos(th),ff,defaultOpts{:})

% Evaluate the function on the wedge from the origin along the az_intvl(2).
ff = permute(fevalm(f,r,lam(end),th),[1 3 2]);
% Plot the result
surf(r*sin(th)*cos(lam(end)),r*sin(th)*sin(lam(end)),r*cos(th),ff,defaultOpts{:})

if ~plotOnHold
    hold off;
end

camlight('headlight');
lighting phong;
material dull;

axis([-1 1 -1 1 -1 1])
daspect([1 1 1])
end

function h = plotWedgePol(f)
% Plot a BALLFUN with a wedge in the polar direction removed

% Is the plot currently being held?
plotOnHold = ishold;
% Default plotting options
defaultOpts = {'facecolor', 'interp','edgecolor', 'none'};

% Polar (latitude) values to include.  TODO: Make these optional inputs
pol_intvl = [pi/2 pi];
% Azimuthal (longitude) values to include.  TODO: Make these optional inputs
az_intvl = [0 pi];

% Define the size of f:
[m,n,p] = size(f);

% m >= 25 and n, p >= 28
m = 25*(m < 25) + m*(m >= 25);
n = 28*(n < 28) + n*(n>=28);
p = 28*(p < 28) + p*(p>=28);

% Impose m = 1 [6] and n, p = 0 [4] to avoid errors in the plot
m = m + mod(1-mod(m,6),6);
n = n + mod(4-mod(n,4),4);
p = p + mod(4-mod(p,4),4);

% Construct the values of lambda and theta to plot on the outer sphere (r=1)
% where the sphere is closed
lam = linspace(-pi,pi,n);
th = linspace(pol_intvl(1),pol_intvl(2),p)';

% Evaluate the function on the outer sphere
ff = permute(fevalm(f,1,lam,th),[3 2 1]);

% Plot the result
h = surf(sin(th)*cos(lam),sin(th)*sin(lam),cos(th)*ones(1,n),ff,defaultOpts{:});
hold on

% Construct the values of lambda and theta to plot on the outer sphere (r=1)
% where the sphere is open at the wedge
lam = linspace(az_intvl(1),az_intvl(2),n);
th = linspace(0,pol_intvl(1),p)';

% Evaluate the function on the outer sphere
ff = permute(fevalm(f,1,lam,th),[3 2 1]);

% Plot the result
surf(sin(th)*cos(lam),sin(th)*sin(lam),cos(th)*ones(1,n),ff,defaultOpts{:})

% Construct the values of r and theta to plot from the origin to the outer
% sphere (r=1).
r = chebpts(m); r = r(floor(m/2)+1:end); th = linspace(0,pol_intvl(1),p);

% Evaluate the function on the wedge from the origin along the az_intvl(1).
ff = permute(fevalm(f,r,lam(1),th),[1 3 2]);
% Plot the result
surf(r*sin(th)*cos(lam(1)),r*sin(th)*sin(lam(1)),r*cos(th),ff,defaultOpts{:})

% Construct the values of r and theta to plot from the origin to the outer
% sphere (r=1).
r = chebpts(m); r = r(floor(m/2)+1:end); th = linspace(0,pol_intvl(1),p);

% Evaluate the function on the wedge from the origin along the az_intvl(1).
ff = permute(fevalm(f,r,lam(end),th),[1 3 2]);
% Plot the result
surf(r*sin(th)*cos(lam(end)),r*sin(th)*sin(lam(end)),r*cos(th),ff,defaultOpts{:})

% Construct the values of r and lambda to plot from the origin to the outer
% sphere (r=1): slice along
r = chebpts(m); r = r(floor(m/2)+1:end); lam = linspace(-pi,pi,p);

% Evaluate the function on the wedge from the origin along the az_intvl(2).
ff = fevalm(f,r,lam,pol_intvl(1));
% Plot the result
surf(r*sin(pol_intvl(1))*cos(lam),r*sin(pol_intvl(1))*sin(lam),r*cos(pol_intvl(1))*ones(size(lam)),ff,defaultOpts{:})

if ~plotOnHold
    hold off;
end

camlight('headlight');
lighting phong;
material dull;

axis([-1 1 -1 1 -1 1])
daspect([1 1 1])
end


function [pvec, pstruct] = tapas_hgf_categorical_transp(r, ptrans)
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2013 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Number of states whose contingencies have to be learned
no = r.c_prc.n_outcomes;

pvec    = NaN(1,length(ptrans));
pstruct = struct;

pvec(1:no)       = ptrans(1:no);                           % mu2_0
pstruct.mu2_0    = pvec(1:no);
pvec(no+1:2*no)  = exp(ptrans(no+1:2*no));                 % sa2_0
pstruct.sa2_0    = pvec(no+1:2*no);
pvec(2*no+1)     = ptrans(2*no+1);                         % mu3_0
pstruct.mu3_0    = pvec(2*no+1);
pvec(2*no+2)     = exp(ptrans(2*no+2));                    % sa3_0
pstruct.sa3_0    = pvec(2*no+2);
pvec(2*no+3)     = tapas_sgm(ptrans(2*no+3),r.c_prc.kaub); % ka
pstruct.ka       = pvec(2*no+3);
pvec(2*no+4)     = ptrans(2*no+4);                         % om
pstruct.om       = pvec(2*no+4);
pvec(2*no+5)     = tapas_sgm(ptrans(2*no+5),r.c_prc.thub); % th
pstruct.th       = pvec(2*no+5);

return;


function [F,L,KL] = spm_vb_Fn(Y,block)
% Compute voxel-wise contributions to model evidence
% FORMAT [F,L,KL] = spm_vb_Fn(Y,block)
%
% Y          - [T x N] time series
% block      - data structure (see spm_vb_glmar)
%
% F          - [N x 1] vector where nth entry is unique contribution to
%              model evidence from voxel n
% L          - [N x 1] Average Likelihood
% KL.w       - [N x 1] KL w - unique contribution
% KL.a       - [N x 1] KL a - unique contribution
% KL.lam     - [N x 1] KL Lambda
% KL.alpha   - Scalar
% KL.beta    - Scalar
%__________________________________________________________________________
% Copyright (C) 2005-2014 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: spm_vb_Fn.m 6079 2014-06-30 18:25:37Z spm $

T  = block.T;
p  = block.p;
k  = block.k;
N  = block.N;
X  = block.X;

C2 = block.C2;

Bk = kron(diag(block.mean_alpha),block.Dw);
B  = block.Hw*Bk*block.Hw';

if p>0
    if ~strcmp(block.priors.A,'Discrete')
        Jk = kron(diag(block.mean_beta),block.Da);
        J  = block.Ha*Jk*block.Ha';
    end
end

%-Get KL-alpha
%--------------------------------------------------------------------------
KL.alpha = 0;
for j=1:k
    KL.alpha = KL.alpha + spm_kl_gamma(block.b_alpha(j),block.c_alpha(j),block.b_alpha_prior(j),block.c_alpha_prior(j));
end

% Get KL-beta
%--------------------------------------------------------------------------
KL.beta = 0;
if p > 0
    if strcmp(block.priors.A,'Discrete')
        for j=1:p
            for s=1:block.priors.S
                KL.beta = KL.beta + spm_kl_gamma(block.b_beta(j,s),block.c_beta(j,s),block.b_beta_prior(j,s),block.c_beta_prior(j,s));
            end
        end
    else
        for j = 1:p
            KL.beta = KL.beta + spm_kl_gamma(block.b_beta(j),block.c_beta(j),block.b_beta_prior(j),block.c_beta_prior(j));
        end
    end
end

% Get average Likelihood, KL-Lambda and terms for KL-w, KL-a
%--------------------------------------------------------------------------
for n=1:N
    subblock_n = [(n-1)*k+1:n*k];

    if p>0
        Gn = spm_vb_get_Gn(Y,block,n);
    else
        wc = block.w_cov{n};
        en = (Y(:,n)-X*block.w_mean(subblock_n,1));
        Gn = trace(wc*block.XTX)+en'*en;
    end

    L(n)   = -0.5*block.mean_lambda(n)*Gn;
    L(n)   = L(n) + 0.5*(T-p)*(psi(block.c_lambda(n)) + log(block.b_lambda(n)));
    L(n)   = L(n)-0.5*block.C2/N;

    KL.lam(n) = spm_kl_gamma(block.b_lambda(n),block.c_lambda(n),block.b_lambda_prior(n),block.c_lambda_prior(n));

    KL_w1  = -0.5*sum(log(block.mean_alpha))-0.5*log(det(block.w_cov{n}));
    KL_w1  = KL_w1-0.5*k*block.log_det_Dw/N;
    KL_w2  = 0.5*trace(B(subblock_n,subblock_n)*block.w_cov{n});

    subblock_ni = [1:N*k];
    subblock_ni(subblock_n) = [];
    Bnn         = B(subblock_n,subblock_n);
    Bni         = B(subblock_n,subblock_ni);
    Bin         = B(subblock_ni,subblock_n);

    w_mean  = block.w_mean(subblock_n,1);
    KL_w3   = 0.5*w_mean'*Bnn*w_mean+0.5*w_mean'*Bni*block.w_mean(subblock_ni,1);
    KL_w3   = KL_w3-0.5*k;
    KL.w(n) = KL_w1+KL_w2+KL_w3;

    if p>0
        asubblock_n = [(n-1)*p+1:n*p];
        a_mean      = block.a_mean(asubblock_n,1);
        if strcmp(block.priors.A,'Discrete')
            ibeta_n = diag(block.priors.gamma(n,:)*(1./block.mean_beta'));
            a_n     = block.priors.gamma(n,:)*block.as';
            KL.a(n) = spm_kl_normal(a_mean,block.a_cov{n},a_n,ibeta_n);
%             a_mean
%             sqrt(block.a_cov{n})
%             a_n
%             sqrt(ibeta_n)
        else
            asubblock_ni = [1:N*p];
            asubblock_ni(asubblock_n) = [];
            Jnn = J(asubblock_n,asubblock_n);
            Jni = J(asubblock_n,asubblock_ni);
            KL_a1 = -0.5*sum(log(block.mean_beta)) - 0.5*log(det(block.a_cov{n}));
            KL_a1 = KL_a1-0.5*p*block.log_det_Da/N;
            KL_a2 = 0.5*trace(Jnn*block.a_cov{n});
            KL_a3 = 0.5*a_mean'*Jnn*a_mean+0.5*a_mean'*Jni*block.a_mean(asubblock_ni,1);
            KL_a3 = KL_a3-0.5*p;
            KL.a(n) = KL_a1 + KL_a2 + KL_a3;
        end
    else
        KL.a(n) = 0;
    end
end

F = L -KL.w  -KL.lam -KL.a -KL.alpha/N -KL.beta/N;


% Locate place fields in a 1D firing map.
%
% Locates the place fields and calculates the field sizes and spacing
% between fields.
%
%  USAGE
%   [fieldsCurve, fields, spacing] = analyses.placefield1D(map, <options>)
%   map           Firing rate map either structure obtained using analyses.map
%                 or a matrix that represents firing map. Note that if you want to filter place fields based on number
%                 of spikes, then you should use map structure.
%   <options>     optional list of property-value pairs (see table below)
%
%   ======================================================================================
%    Properties     Values
%   --------------------------------------------------------------------------------------
%    'thresholdType' Type of the firing rate threshold. '%' specifies that 'threshold'
%                    value is in range [0; 1] and rates lower than 'threshold * curveAmplitude'
%                    are not considered as place fields. curveAmplitude is the range of a firing
%                    curve, i.e. max(curve) - min(curve).
%                    'rate' specifies that 'threshold' value is an absolute rate. Values lower than
%                    'threshold' are not considered as place fields. Default it '%'.
%
%    'threshold'    Threshold for a firing rate. How the value is treated depends on options
%                   'thresholdType'. Default = 0.2 and 'thresholdType' = '%'.
%
%    'minRows'      Minimum number of row bins in a place field. Fields with
%                   fewer bins are not considered as place fields. Remember to
%                   adjust this value when you change the bin width. Default = 3.
%                   If minRows == 0, then minimum number of bins is not checked.
%
%    'binWidth'     Bin length in centimetres. Used to calculate size of fields in cm.
%                   Default is 1.
%
%    'minSpikes'    Minimum number of spikes in a place field. Default is 0, which means
%                   that minimum number of spikes is not checked. To use this value 'map'
%                   must be a result of function analyses.map.
%
%    'minDistance'  Minimum number of bins between adjacent fields. Distance is calculated between
%                   end of one field and start of another field. If distance is smaller or
%                   equals to 'minDistance', then two fields are merged together. Note that more
%                   than two fields can be merged if they are all adjacent. Default is 0, which
%                   means that fields should have a common border.
%
%    'debug'        'on' plot turning curve, place fields and their centres of mass. 'off' do not
%                   plot anything. Default is 'off'.
%
%    'pos'          Position samples. Used to calculate posInd. If not provided,
%                   then posInd will be an empty matrix.
%   ======================================================================================
%
%   fieldsCurve     Vector of the same size as underline firing rate curve. The elements of fieldsCurve
%                   are integer values greater or equal to 0. 0 elements correspond to background (not
%                   a place field). Values of 1 constitute first field. Values of 2 constitute second
%                   field; and so on.
%   fields          Structure with information about each field. Structure fields are:
%       col         vector of columns that constitute this field.
%       row         vector of rows (artificial). Same size as col, all ones;
%       peak        field peak;
%       x, y        field centre of mass point. See NOTES for details about centre of mass calculation;
%       meanRate    mean firing rate;
%       width       field width in cm;
%       size        size of the field. Do not rely on it! It's from old code and could be misleading.
%                   Calculated as width * <mean firing rate of a field>.
%       posInd      Indices of position samples that correspond to this field. In case position sample
%                   matrix is of size Nx5 ([t x y x1 y1]), posInd corresponds to the left most position
%                   columns ([x y]). If pos argument is not provided, then posInd will be an empty matrix.
%   spacing         Matrix of spacing between fields. Entry (i, j) equals to distance between fields i and j.
%
%  NOTE
%   Since centre of mass for a curve is somewhat undefined, it is calculated for plate bounded by a firing curve
%   and a function y = 0 for all x. See http://tutorial.math.lamar.edu/Classes/CalcII/CenterOfMass.aspx
%
function [fieldsCurve, fields, spacing] = placefield1D(mapS, varargin)
    if nargin < 1 || mod(length(varargin), 2) ~= 0
        error('BNT:numArgs', 'Incorrect number of parameters (type ''help analyses.placefield1D'' for details).');
    end

    % Default values
    threshold = 0.1;
    minRows = 2;
    binWidth = 1; % [cm];
    minSpikes = 0;
    minFieldDistance = 0; % [bins]
    debug = false;
    isThrshPercentage = true;
    pos = [];

    % Parse parameter list
    i = 1;
    while i < length(varargin)
        if ~ischar(varargin{i}),
            error(['Parameter ' num2str(i+2) ' is not a property (type ''help analyses.placefield1D'' for details).']);
        end

        switch(lower(varargin{i})),
            case 'threshold'
                threshold = varargin{i+1};
                i = i + 2;

            case 'minrows'
                minRows = varargin{i+1};
                if ~helpers.isdscalar(minRows, '>=0')
                    error('Incorrect value for property ''minRows'' (type ''help analyses.placefield1D'' for details).');
                end
                i = i + 2;

            case 'binwidth'
                binWidth = varargin{i+1};
                if ~helpers.isdscalar(binWidth, '>0')
                    error('Incorrect value for property ''binWidth'' (type ''help analyses.placefield1D'' for details).');
                end
                i = i + 2;

            case 'minspikes'
                minSpikes = varargin{i+1};
                if ~helpers.isdscalar(minSpikes, '>=0')
                    error('Incorrect value for property ''minSpikes'' (type ''help analyses.placefield1D'' for details).');
                end
                if minSpikes > 0 && ~isstruct(mapS)
                    error('You should only use minSpikes with a structure-like firing curve (type ''help analyses.placefield1D'' for details).');
                end
                i = i + 2;

            case 'mindistance'
                minFieldDistance = varargin{i+1};
                if ~helpers.isdscalar(minFieldDistance, '>=0')
                    error('Incorrect value for property ''minDistance'' (type ''help analyses.placefield1D'' for details).');
                end
                i = i + 2;

            case 'debug'
                debug = strcmpi(varargin{i+1}, 'on');
                i = i + 2;

            case 'thresholdtype'
                isThrshPercentage = strcmpi(varargin{i+1}, '%');
                if ~helpers.isstring(varargin{i+1}, '%', 'rate'),
                    error('Incorrect value for property ''thresholdType'' (type ''help analyses.placefield1D'' for details).');
                end
                i = i + 2;

            case 'pos'
                pos = varargin{i+1};
                if ~ismatrix(pos) || size(pos, 2) < 2
                    error('Incorrect value for property ''pos'' (type ''help analyses.placefield1D'' for details).');
                end
                i = i + 2;

            otherwise,
                error(['Unknown property ''' num2str(varargin{i}) ''' (type ''help analyses.placefield1D'' for details).']);
        end
    end
    if isThrshPercentage
        if ~helpers.isdscalar(threshold, '>=0', '<=1')
            error('Incorrect value for property ''threshold'' (type ''help analyses.placefield1D'' for details).');
        end
    else
        if ~helpers.isdscalar(threshold, '>=0')
            error('Incorrect value for property ''threshold'' (type ''help analyses.placefield1D'' for details).');
        end
    end

    if isstruct(mapS)
%         mapAxis = mapS.x;
        if size(mapS.z, 1) > 1 && size(mapS.z, 2) > 1
            error('Incorrect value for property ''map''. Firing curve is 2D, but should be a vector (type ''help analyses.placefield1D'' for details).');
        end

        curve = mapS.z;
    else
        if size(mapS, 1) > 1 && size(mapS, 2) > 1
            error('Incorrect value for property ''map''. It should be a vector (type ''help analyses.placefield1D'' for details).');
        end
%         mapAxis = 1:length(mapS);

        curve = mapS;
    end
    curveLen = length(curve);

    fieldsCurve = zeros(1, curveLen);
    fields = struct('row', {}, 'col', {}, ...
        'size', {}, 'peak', {}, ...
        'x', {}, 'y', {}, ...
        'meanRate', {}, 'width', {}, ...
        'posInd', {} ...
        );
    spacing = [];

    if curveLen == 0
        return
    end

    % extend curve, so that findpeaks function finds all peaks
    map = zeros(1, curveLen + 2);
    map(2:2+curveLen-1) = curve;

    [~, locs] = findpeaks(map);
    dMap = diff(map);
    dMap(end+1) = dMap(end);

    if isThrshPercentage
        threshold = min(curve) + ((max(curve) - min(curve)) * threshold);
    end

    startField = zeros(1, 100);
    stopField = zeros(1, 100);
    fieldWidth = zeros(1, 100);
    fieldSize = zeros(1, 100);
    curField = 1;

    for i = 1:length(locs)
        if map(locs(i)) < threshold
            continue;
        end
        curStopField = locs(i);
        while curStopField < length(dMap) && dMap(curStopField) < 0 && map(curStopField) >= threshold
            curStopField = curStopField + 1;
        end
        % substract one because of the loop
        curStopField = curStopField -1;

        curStartField = locs(i) - 1;
        while curStartField > 0 && dMap(curStartField) > 0 && map(curStartField) >= threshold
            curStartField = curStartField - 1;
        end
        if curStartField == 0
            curStartField = 1;
        end
        curStopField = curStopField - 1; % substract one cause of map -> curve translation

        if curStopField > length(curve)
            curStopField = length(curve);
        end

        startField(curField) = curStartField;
        stopField(curField) = curStopField;

        fieldWidth(curField) = (curStopField - curStartField + 1) * binWidth;
        fieldSize(curField) = fieldWidth(curField) * nanmean(curve(curStartField:curStopField));
        curField = curField + 1;
    end

    startField(curField:end) = [];
    stopField(curField:end) = [];
    fieldWidth(curField:end) = [];
    fieldSize(curField:end) = [];

    nFields = length(fieldWidth);
    removeField = false(1, nFields);

    fieldsCurve = zeros(1, curveLen);
    if nFields == 0
        fields = struct();
        fields(1) = [];
        spacing = [];
        return;
    end

    % merge neighbouring fields
    curField = 1;
    for i = 1:nFields
        if i + 1 > nFields
            break;
        end

        fieldsDist = abs(startField(i+1) - stopField(i));
        if fieldsDist <= minFieldDistance
            stopField(curField) = stopField(i+1);
            removeField(i+1) = true;
        else
            curField = i + 1;
        end
    end
    startField(removeField) = [];
    stopField(removeField) = [];
    fieldWidth(removeField) = [];
    fieldSize(removeField) = [];

    if ~isempty(find(removeField, 1))
        % adjust fields width and size after merge
        nFields = length(startField);
        for i = 1:nFields
            fieldWidth(i) = (stopField(i) - startField(i) + 1) * binWidth;
            fieldSize(i) = fieldWidth(i) * nanmean(curve(startField(i):stopField(i)));
        end
    end

%     fields(nFields) = struct('row', {}, 'col', {}, ...
%         'size', {}, 'peak', {}, ...
%         'x', {}, 'y', {}, ...
%         'meanRate', {}, 'width', {}, ...
%         'posInd', {} ...
%         );
%     fields(nFields) = struct('row', [], 'peak', 0, 'x', -1, 'y', -1, 'meanRate', -1, 'posInd', []);

    curField = 1;
    removeField = false(1, nFields);
    for i = 1:nFields
        fieldData = curve(startField(i):stopField(i));
        peak = max(fieldData);

        % check field peak rate
        if peak < threshold
            removeField(i) = true;
            continue;
        end

        % Number of spikes in the field
        if minSpikes > 0
            numSpikes = sum(mapS.Nspikes(startField(i):stopField(i)));
            if numSpikes < minSpikes
                % Marks field as having to few spikes
                removeField(i) = true;
                continue;
            end
        end

        % check number of bins
        if length(startField(i):stopField(i)) <= minRows
            removeField(i) = true;
            continue;
        end

        fields(curField).col = startField(i):stopField(i);
        fields(curField).row = ones(1, length(fields(curField).col));
        fields(curField).peak = peak;
        fields(curField).meanRate = nanmean(fieldData);
        fields(curField).width = fieldWidth(i);
        fields(curField).size = fieldSize(i);
        fieldsCurve(startField(i):stopField(i)) = curField;

        if ~isempty(pos)
            if isstruct(mapS)
                xSpace = mapS.x;
            else
                nBins = length(curve);
                limitsX = [nanmin(pos(:, bntConstants.PosX)) nanmax(pos(:, bntConstants.PosX))];
                xSpace = linspace(limitsX(1), limitsX(2), nBins);
            end
            xMin = xSpace(startField(i));
            xMax = xSpace(stopField(i));
            posIndX = pos(:, bntConstants.PosX) >= xMin & pos(:, bntConstants.PosX) <= xMax;
            fields(curField).posInd = find(posIndX);
        end

        curField = curField + 1;
    end

    fields(curField:end) = [];
    nFields = length(fields);

    if debug && nFields > 0
        figure, plot(curve);
        v = axis();
        ymin = v(3);
        ymax = v(4);
        color = [1 0 0];
        fieldSize = zeros(1, nFields);
        for i = 1:nFields
            startField = fields(i).col(1);
            stopField = fields(i).col(end);

            p = patch([startField startField stopField stopField], [ymax ymin ymin ymax], color);
            set(p, 'FaceAlpha', 0.5);
            color(1) = color(1) - 0.1;
            color(2) = color(2) + 0.2;
%             color(3) = color(3) + 0.2;
            if color(1) < 0
                color(1) = 0;
            end
            if color(2) > 1
                color(2) = 1;
            end

            fieldSize(i) = fields(i).size;
        end
        xlabel('Bins'), ylabel('Rate');
        titleStr = sprintf('Field(s) with size(s) %s', sprintf('%f, ', fieldSize));
        titleStr(end-1:end) = [];
        title(titleStr);
    end

    % Calculate the spacing using centre of mass (COM). Since centre of mass for a curve is somewhat
    % undefined I'll calculate COM for palte bounded by curve and a function y = 0 for all x.
    % http://tutorial.math.lamar.edu/Classes/CalcII/CenterOfMass.aspx
    distMat = zeros(nFields, 2);

    % store points for debug purposes
    pointsX = zeros(1, nFields);
    pointsY = zeros(1, nFields);

    for i = 1:nFields
        x = fields(i).col(:);
        y = curve(x);
        y = y - min(y); % make it from 0. This gives better estimation

        y1 = x .* y;
        y2 = (y.^2)/2;
        A = trapz(x, y);

        Mx = trapz(x, y1)/A;
        My = min(curve(x)) + trapz(x, y2)/A;

        pointsX(i) = Mx;
        pointsY(i) = My;

        fields(i).x = Mx;
        fields(i).y = My;

        distMat(i, :) = [Mx My];

%             % Raymond's code
%             posMass = sum(mapAxis(x).*curve(x));
%             mass = sum(curve(x));
%             com(i) = posMass/mass;
    end

    if debug
        hold on;
        plot(pointsX, pointsY, 'o');
    end

    if nFields <= 1
        spacing = [];
    else
        D = pdist(distMat);
        spacing = squareform(D);
    end
end


% op_alignAverages.m
% Jamie Near, McGill University 2014.
%
% USAGE:
% [out,fs,phs]=op_alignAverages(in,tmax,med,ref);
%
% DESCRIPTION:
% Perform spectral registration in the time domain to correct frequency and
% phase drifts.  As described in Near et al.  Frequency and phase drift
% correction of magnetic resonance spectroscopy data by spectral
% registration in the time domain. Magn Reson Med 2015; 73(1):44-50.
%
% June 15th 2017:  Made the tmax and med arguments optional.  If tmax is
% not specified, the value is determined automatically by finding the time
% at which the SNR of the FID drops permanently below 5.  This idea
% was suggested by Mark Mikkelsen.  Thanks Mark!!
%
% INPUTS:
% in        = Input data structure.
% tmax      = Maximum time (s) in time domain to use for alignment.
%             (Optional.  Default is the time at which SNR drops below 5)
% med       = Align averages to the median of the averages? ('y','n', 'a', or
%             'r').  (Optional.  Default = 'n').  If you select 'n', all
%             averages will be aligned to a single average.  The average
%             chosen as the reference average will be the one with the
%             lowest 'unlikeness' metric (see 'op_rmbadaverages.m').  If
%             select 'y', all averages will be alinged to the median of
%             the averages.  If you select 'a', all averages will be
%             aligned to the average of the averages.  If you select 'r',
%             all averages will be aligned to an externally provided
%             reference spectrum.
% ref       = An externally provided reference spectrum that you would like
%             to align everything to (Required only if med = 'r').
%
% OUTPUTS:
% out       = Output following alignment of averages.
% fs        = Vector of frequency shifts (in Hz) used for alignment.
% phs       = Vector of phase shifts (in degrees) used for alignment.

function [out,fs,phs]=op_alignAverages(in,tmax,med,initPars)

if ~in.flags.addedrcvrs
    error('ERROR:  I think it only makes sense to do this after you have combined the channels using op_addrcvrs.  ABORTING!!');
end

if in.dims.averages==0
    %DO NOTHING
    disp('WARNING:  No averages found.  Returning input without modification!');
    out=in;
    fs=0;
    phs=0;

else

    parsFit=[0,0];

    if nargin<3
        med='n'
        if nargin<2
            %if tmax is not specified, find the time at which the SNR
            %drops below 5
            disp('tmax not supplied.  Calculating tmax....');
            sig=abs(in.fids);
            noise=std(real(in.fids(ceil(0.75*end):end,:,:)),[]);
            noise=mean(mean(mean(noise,2),3),4);
            snr=sig/noise;

            for n=1:(numel(snr)/size(snr,1))
                N=find(snr(:,n)>5);
                tmax_est(n)=in.t(N(end));
            end
            tmax=median(tmax_est);
            disp(['tmax = ' num2str(tmax*1000) 'ms.']);
        end
    end

    if (strcmp(med,'r') || strcmp(med,'R'))
        if nargin<4
            error('ERROR:  If using the ''r'' option for input variable ''med'', then a 4th input argument must be provided');
        end
    else
        if nargin<4
            ref=struct();
        end
    end

    if in.dims.subSpecs==0
        B=1;
    else
        B=in.sz(in.dims.subSpecs);
    end

    fs=zeros(in.sz(in.dims.averages),B);
    phs=zeros(in.sz(in.dims.averages),B);
    fids=zeros(in.sz(in.dims.t),1,B);
    for m=1:B
        if med=='y' || med=='Y'
            disp('Aligning all averages to the median of the averages.');
            base=op_median(in);
            base=[real(base.fids( in.t>=0 & in.t<tmax ,m));imag(base.fids( in.t>=0 & in.t<tmax ,m))];
            ind_min=0;
        elseif med=='a' || med=='a'
            disp('Aligning all averages to the average of the averages.');
            base=op_averaging(in);
            base=[real(base.fids( in.t>=0 & in.t<tmax ,m));imag(base.fids( in.t>=0 & in.t<tmax ,m))];
            ind_min=0;
        elseif med=='n' || med=='N'
            %First find the average that is most similar to the total average:
            inavg=op_median(in);
            for k=1:in.sz(in.dims.averages)
                for l=1:B
                    metric(k,l)=sum((real(in.fids(in.t>=0 & in.t<=tmax,k,l))-(real(inavg.fids(inavg.t>=0 & inavg.t<=tmax,l)))).^2);
                end
            end
            [temp,ind_min]=min(metric(:,m));

            %Now set the base function using the index of the most similar
            %average:
            disp(['Aligning all averages to average number ' num2str(ind_min) '.']);
            base=[real(in.fids(in.t>=0 & in.t<tmax,ind_min,m));imag(in.fids(in.t>=0 & in.t<tmax,ind_min,m))];
            fids(:,ind_min,m)=in.fids(:,ind_min,m);
        elseif med=='r' || med=='R'
            disp('Aligning all averages to an externally provided reference spectrum.');
            base=ref;
            base=[real(base.fids( in.t>=0 & in.t<tmax ,m));imag(base.fids( in.t>=0 & in.t<tmax ,m))];
            ind_min=0;
        end
        for n=1:in.sz(in.dims.averages)
            if n~=ind_min
                parsGuess=parsFit;
                %disp(['fitting subspec number ' num2str(m) ' and average number ' num2str(n)]);
                parsFit=nlinfit(in.fids(in.t>=0 & in.t<tmax,n,m),base,@op_freqPhaseShiftComplexNest,parsGuess);
                fids(:,n,m)=op_freqPhaseShiftNest(parsFit,in.fids(:,n,m));
                fs(n,m)=parsFit(1);
                phs(n,m)=parsFit(2);
                %plot(in.ppm,fftshift(ifft(fids(:,1,m))),in.ppm,fftshift(ifft(fids(:,n,m))));
            end
        end
    end

    %re-calculate Specs using fft
    specs=fftshift(ifft(fids,[],in.dims.t),in.dims.t);

    %FILLING IN DATA STRUCTURE
    out=in;
    out.fids=fids;
    out.specs=specs;

    %FILLING IN THE FLAGS
    out.flags=in.flags;
    out.flags.writtentostruct=1;
    out.flags.freqcorrected=1;

end

    function y=op_freqPhaseShiftComplexNest(pars,input)
        f=pars(1);     %Frequency Shift [Hz]
        p=pars(2);     %Phase Shift [deg]

        dwelltime=in.dwelltime;
        t=0:dwelltime:(length(input)-1)*dwelltime;
        fid=input(:);

        shifted=addphase(fid.*exp(1i*t'*f*2*pi),p);

        y=[real(shifted);imag(shifted)];
        %y=real(fid.*exp(-1i*t'*f*2*pi));

    end

    function y=op_freqPhaseShiftNest(pars,input)
        f=pars(1);     %Frequency Shift [Hz]
        p=pars(2);     %Phase Shift [deg]

        dwelltime=in.dwelltime;
        t=0:dwelltime:(length(input)-1)*dwelltime;
        fid=input(:);

        y=addphase(fid.*exp(1i*t'*f*2*pi),p);
        %y=real(fid.*exp(-1i*t'*f*2*pi));

    end

end