function [signal, state] = flt_dynamicloreta(varargin)
% Return the current source density for a given head model and data using
% the cortically-constrained LORETA (low resolution electrical
% tomographic analysis) with a Bayesian update scheme for hyperparameters.
% The reconstructed CSD time-series (or source potential maps) will be
% stored in signal.srcpot. This matrix has dimension [num_voxels x num_samples].
%
% Author: Tim Mullen, Jan 2013, SCCN/INC/UCSD
%         Alejandro Ojeda, Jan 2013, SCCN/INC/UCSD
%         Christian Kothe, Jan 2013, SCCN/INC/UCSD

if ~exp_beginfun('filter'), return; end

declare_properties('name','Dynamic LORETA', 'experimental',true, 'independent_channels',false, 'independent_trials',false);

arg_define(varargin, ...
    arg_norep({'signal','Signal'}), ...
    arg_nogui({'K','ForwardModel'},[],[],'Forward model (matrix)','shape','matrix'), ...
    arg_nogui({'L','LaplacianOperator'},[],[],'Laplacian operator. Sparse matrix of N sources x N sources, this is matrix is used as the square root of the precision matrix of the sources.'), ...
    arg_sub({'options','LoretaOptions'},{},...
        { ...
        arg({'maxTol','MaxTolerance'},1e-12,[0 Inf],'Tolerance for hyperparameter update loop','cat','Loreta Options'), ...
        arg({'maxIter','MaxIterations'},100,[1 Inf],'Maximum iterations for hyperparameter update loop','cat','Loreta Options'), ...
        arg({'gridSize','GridSize'},100,[1 Inf],'Lambda grid size.'), ...
        arg({'history','TrackHistory'},false,[],'Track history for hyperparameters'), ...
        arg({'verbose','VerboseOutput'},false,[],'Verbosity','cat','Loreta Options'), ...
        arg({'initNoiseFactor','InitialNoiseFactor'},0.001,[0 Inf],'Fraction of noise level. Used for initializing alpha parameter','cat','Loreta Options') ...
        arg({'block_size','BlockSize'},5, [], 'Block granularity for processing. The inverse operator will be updated using blocks of this many samples. This assumes that the inverse solution is spatially stationary over this many samples.'), ...
        arg({'skipFactor','SkipFactor'},0,[0 Inf],'Number of blocks to skip'), ...
        arg({'maxblocks','MaxBlocks'},Inf,[0 Inf],'Maximum number of blocks'), ...
        arg({'standardize','Standardize'},'all',{'none','channels','all'},'Rescale data to unit variance. If ''channels'', standardization is carried out across channels for each time point. If ''all'' each data sample is normalized by the standard deviation taken over all data.'), ...
        arg({'useGPU','UseGPU'},false,[],'Use GPU to accelerate computation.'), ...
        },'Additional options for Loreta function'), ...
    arg({'verb','Verbosity'},false,[],'Verbose output'), ...
    arg_nogui({'state','State'},[],[],'State object. When provided, hyperparameters will be estimated adaptively from prior state'));
if verb
    fprintf('Estimating current source density using cLORETA (%s)\n',mfilename);
end

[nchs, npnts, ntrs] = size(signal.data);
if isempty(block_size) || block_size > npnts
    block_size = npnts;
end
numsplits    = floor(npnts/block_size);

% if necessary, cast to double-precision
if ~strcmpi(class(signal.data),'double')
    signal.data = double(signal.data);
end

% normData the data
if ~strcmpi(normData,'none')
    switch normData
        case 'channels'
            scale = std(signal.data,[],1);
        case 'time'
            scale = std(signal.data,[],2);
        case 'all'
            scale = std(signal.data(:));
    end
    signal.data = bsxfun(@rdivide,signal.data,scale);
%     scale = std(signal.data(:));
%     signal.data = signal.data./scale;
end

if isempty(state) || ~isfield(state,'iLV') || isempty(state.iLV)
    if verb
        fprintf('...computing SVD of LFM.\n');
    end
    % mode is offline or we are initializing online filter
    % perform one-time SVD for faster computation.
    [U,S,V]      = svd(K/L,'econ');
    state.iLV    = L\V;
    state.s2     = diag(S).^2; %s^2
    state.Ut     = U';
    state.sigma2 = repmat({options.sigma2},1,ntrs);
    state.tau2   = repmat({options.tau2},1,ntrs);
end

if npnts == 0
    % no data
    signal.srcpot    = [];
    state.srcweights = [];
    exp_endfun; return;
end

signal.srcpot    = zeros([size(K,2), npnts, ntrs]);
state.srcweights = zeros(size(L,1),nchs);
sum_srcweights   = zeros(size(L,1),nchs);
signal.loretaHistory = struct([]);

if verb
    fprintf('...assuming %d stationary blocks of length %d\n',numsplits,block_size);
end

% loop over all trials
for tr=1:ntrs
    if verb
        fprintf('\nTrial (%d\%d).',tr,ntrs);
    end
    k = 0;
    % loop over sub-blocks and estimate CSD for each block
    for i=0:skipFactor+1:numsplits-1
        if verb
            if i+1 >= floor(numsplits*(k+1)/10)
                k = k + 1;
                fprintf('%0.3g%%...',round((i/numsplits)*100));
            end
        end
        range = 1+floor(i*npnts/numsplits) : min(npnts,floor((i+1)*npnts/numsplits));
        % call (dynamic bayesian) loreta estimator
        [signal.srcpot(:,range,tr), state.sigma2{tr}, state.tau2{tr}, state.srcweights, tmpHist] ...
            = dynamicLoreta( signal.data(:,range,tr), state.Ut, state.s2, state.iLV,...
                             state.sigma2{tr}, state.tau2{tr}, options);
        if ~isempty(tmpHist)
            signal.loretaHistory{tr} = [signal.loretaHistory{tr},tmpHist];
        end

        if skipFactor > 0
            % estimate CSD for samples between blocks using current inverse operator
            range = 1+floor((i+1)*npnts/numsplits) : min(npnts,floor((i+skipFactor+1)*npnts/numsplits));
            signal.srcpot(:,range,tr) = state.srcweights*signal.data(:,range,tr);
        end

        % running sum
        sum_srcweights = sum_srcweights + state.srcweights;
    end
end

if numsplits > 1
    % store the mean inverse operator over all splits
    state.srcweights = sum_srcweights/(numsplits*ntrs);
end

if ~strcmpi(normData,'none')
    % recale data to original units
%     signal.srcpot     = signal.srcpot*scale;
%     state.srcweights  = state.srcweights/scale;
    signal.srcpot = bsxfun(@times,signal.srcpot,scale);
%     signal.srcpot = bsxfun(@rdivide,signal.srcpot,std(signal.srcpot,[],1));
%     state.srcweights  = bsxfun(@times,state.srcweights,scale'); %state.srcweights/mean(scale);
end

if verb
    fprintf('done.\n');
end

exp_endfun;


function L = nodesInLen(nodeNo,inLen,doNoExt,wt)
%NODESINLEN Length of the node input signal
%   Usage:  L = nodesInLen(nodeNo,inLen,doExt,treeStruct);
%
%   Input parameters:
%         nodeNo     : Node index.
%         inLen      : Filter thee input signal length.
%         doNoExt    : Expansive representation indicator.
%         wt         : Structure containing description of the filter tree.
%
%   Output parameters:
%         Lin        : Length of the node input signal
%
%   `nodesInLen(nodeNo,inLen,doExt,treeStruct)` return length of the input
%   signal of the node `nodeNo`. For definition of the structure see `wfbinit`.
%
%   See also: wfbtinit
%

L = zeros(numel(nodeNo),1);
for nn=1:length(nodeNo)
    subPat = [];
    filtLenPat = [];
    tmpNodeNo = nodeNo(nn);

    while(wt.parents(tmpNodeNo))
       parentNo = wt.parents(tmpNodeNo);
       tmpIdx = find(wt.children{parentNo}==tmpNodeNo);
       subPat(end+1) = wt.nodes{parentNo}.a(tmpIdx);
       filtLenPat(end+1) = length(wt.nodes{parentNo}.g{tmpIdx}.h);
       tmpNodeNo=parentNo;
    end

    subPat = subPat(end:-1:1);
    filtLenPat = filtLenPat(end:-1:1);

    L(nn) = inLen;
    if(~doNoExt)
        for ii=1:length(subPat)
            L(nn) = floor((L(nn)+filtLenPat(ii)-1)/subPat(ii));
        end
    else
        for ii=1:length(subPat)
            L(nn) = ceil(L(nn)/subPat(ii));
        end
    end
end


function ar=lpczz2ar(zz)
%LPCZZ2AR Convert z-place poles to ar coefficients AR=(ZZ)
% The complex poles must occur in complex conjugate pairs
% but the order is unimportant.

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

[nf,p]=size(zz);
ar=zeros(nf,p+1);
for k=1:nf
  ar(k,:)=real(poly(zz(k,:)));
end


function model = expvarMeanCreate(inputDim, outputDim, options)

% EXPVARMEANCREATE Creates an the mean function for the EXP kernel.
% FORMAT
% DESC creates a model for returning the first moment of an
% 'exponentiated Gaussian process'. If the output of a Gaussian
% process is exponentiated the resulting process is no longer
% Gaussian. However, it can be approximated by a Gaussian process
% by matching its first and second moments. This function returns
% the mean of that approximating process for a given kernel
% (specified in the options vector). It should be used in tandem
% with the EXP kernel for approximating these Gaussian processes.
% ARG inputDimension : dimension of input to function.
% ARG outputDim : dimension of output from mean function data.
% ARG options : options structure. The structure contains the type
% of kernel that the function is based on. A set of default options
% are given by the file expvarMeanOptions.
% RETURN model : model structure containing the mapping.
%
% COPYRIGHT : Neil D. Lawrence, 2005, 2006
%
% SEEALSO : expvarMeanOptions

% SHEFFIELDML

model.type = 'expvarMean';
if isstruct(options.kern)
  model.kern = options.kern;
else
  model.kern = kernCreate(inputDim, options.kern);
end
model.q = inputDim;
model.d = outputDim;
model.numParams = model.kern.nParams;


%
% Usage:   [val [paths]]=mexEvalPathCoding(U,DAG,param);
%
% Name: mexEvalPathCoding
%
% Description: mexEvalPathCoding evaluate the path coding penalies
%         of http://arxiv.org/abs/1204.4539 and provides a path
%         decomposition of a vector W.
%
%         Given an input matrix U=[u^1,\ldots,u^n],
%
%
% Inputs: U:  double p x n matrix   (input signals)
%               m is the signal size
%         DAG:  struct
%               with three fields, weights, start_weights, stop_weights
%         for a graph with |V| nodes and |E| arcs,
%         DAG.weights: sparse double |V| x |V| matrix. Adjacency
%               matrix. The non-zero entries represent costs on arcs
%               linking two nodes.
%         DAG.start_weights: dense double |V| vector. Represent the costs
%               of starting a path from a specific node.
%         DAG.stop_weights: dense double |V| vector. Represent the costs
%               of ending a path at a specific node.
%
%         if param.regul='graph-path-l0', non-convex penalty
%         if param.regul='graph-path-conv', convex penalty
%
%         param: struct
%               param.regul (choice of regularization, see above)
%               param.verbose (optional, verbosity level, false by default)
%               param.precision (optional, by default a very large integer.
%                 It returns approximate proximal operator by choosing a small integer,
%                 for example, 100 or 1000.
%               param.numThreads (optional, number of threads for exploiting
%                 multi-core / multi-cpus. By default, it takes the value -1,
%                 which automatically selects all the available CPUs/cores).
%
% Output: V: double 1 x n vector (values of the objective function)
%         paths: optional, double sparse p x k matrix. selected paths for the
%                first column of U
%
% Author: Julien Mairal, 2012


function [u, V, exitflag, output] = snd_solveOptimalControlProblem (snd,  varargin)
%UNTITLED2 Summary of this function goes here
%   solves the optimal control problem of the

    % Set control and linear bounds
    A = [];
    b = [];
    Aeq = [];
    beq = [];
    lb = [];
    ub = [];
    for k=1:snd.horizon %Aggregation
        [Anew, bnew, Aeqnew, beqnew, lbnew, ubnew] = ...
               snd.l_constraints( k, snd.net_load, snd.battery, snd.u0_ref);
        A = blkdiag(A,Anew);
        b = [b, bnew];
        Aeq = blkdiag(Aeq,Aeqnew);
        beq = [beq, beqnew];
        lb = [lb, lbnew];
        ub = [ub, ubnew];
    end

    % Solve optimization problem
    [u, V, exitflag, output] = fmincon( @(u) snd.costfunction( snd, u ), ...
        snd.u0, A, b, Aeq, beq, lb, ub, ...
        @(u) snd.nonlinearconstraints(snd, u ), snd.option);
end


function [means,stats] = weighted_reg(Y,varargin)
% Calculate weighted average using weighted linear least squares
% See examples below for usage
%
% :Model:
%
%   Y_i = 1*Ypop + noise
%
% :Inputs:
%
%   **Y:**
%        data matrix (nsub x T)
%
%   **w:**
%        weights
%
%   **varY:**
%        variance of data at each time point (nsub x T) + var between
%
% :Outputs:
%
%   **Ymean:**
%        weighted mean of each column of Y
%
%   **dfe:**
%        error degrees of freedom, adjusted for inequality of variance
%        (Sattherwaite) and pooled across data columns
%
% Extended output in stats structure:
%
%   **stats.t:**
%        t-values for weighted t-test
%
%   **stats.p:**
%        2-tailed p-values for weighted t-test
%
%
%   **r:**
%        weighted correlation coeff across columns of Y
%
%   **xy:**
%        weighted covariance matrix
%
%   **v:**
%        weighted variance estimates for each column of Y
%      - sqrt(v) is the standard error of the mean (or grp difference)
%
%
%   **stats.fits:**
%        fits  for each group (Ymean by group), low contrast weight group then high
%        Fastest if no stats are asked for.
%
% Computation time:
% For FULL stats report
%  - Triples from 500 -> 1000 columns of Y, continues to increase
%
% For mean/dfe only, fast for full dataset (many columns of Y)
%
% :Examples:
% ::
%
%    % Basic multivariate stats for 1000 columns of dat, no weighting
%    % Multivariate covariances are meaningful if cols of Y are organized, e.g., timeseries
%    [means,stats] = weighted_reg(dat(:,1:1000));
%
%    % The same, but return univariate stats only (good for large Y)
%    [means,stats] = weighted_reg(dat,'uni');
%
% ..
%    NOTE: TOR CHANGED INPUT TO ASSUME THAT WE SHOULD ENTER VARWI + VARBETWEEN
% ..

% ..
%    Set up arguments
% ..

if nargin == 0, error('Must at least enter data as 1st argument.'); end

domultivariate = 1;     % multivariate covariance est for Y
dobtwn = 0;             % between-subjects contrast

bcon = []; Ydiff = []; w = []; varY = [];

for i = 1:length(varargin)
    arg = varargin{i};
    if ischar(arg)
        switch lower(arg)
            case 'w', w = varargin{i+1};
            case 'btwn', bcon = contrast_code(varargin{i+1});
            case 'vary', varY = varargin{i+1};
            case 'uni', domultivariate = 0;
        end
    end
end

[m,n] = size(Y);

% fill in missing inputs with default values

if ~is_entered(w), w = ones(m,1);  end
if ~is_entered(varY), varY = ones(m,1); end
if is_entered(bcon), dobtwn = 1; end

% --------------------------------------
% * Weights and computational steps
% --------------------------------------

W = diag(w);                    % Weight matrix

X = repmat(1,m,1);              % Design matrix - 1 column of all ones to calculate average
% and, separately, use bcon if that's entered

invxwx = inv(X'*W*X);
hat = invxwx * X'* W;         % hat matrix

% Between-observations, if entered
% ----------------------------------------
if dobtwn
    invxwx_diff = inv(bcon'*W*bcon);
    hatdiff = invxwx_diff*bcon'*W;
else
    hatdiff = [];
end

% --------------------------------------
% * Means and contrast
% --------------------------------------

Ymean = hat*Y;

% Output: weighted population mean
means.Ymean = Ymean;

% Between-observation contrast, if entered
% ----------------------------------------
if dobtwn

    Ydiff = hatdiff*Y;

    % for output; fits for each group; low then high
    grpfits = repmat(Ymean,2,1) + repmat(sort(unique(bcon)),1,n) .* repmat(Ydiff,2,1);

    means.Ydiff = Ydiff;
    means.grpmeans = grpfits;
end

if nargout == 1, return, end

% --------------------------------------
% * Residuals
% --------------------------------------

e = Y - repmat(Ymean,m,1);         % residuals

if dobtwn

    % fitted values depending on group
    fits = repmat(Ymean,m,1) + repmat(bcon,1,n) .* repmat(Ydiff,m,1);
    ediff = Y - fits;
end

% --------------------------------------
% * Degrees of freedom
% --------------------------------------

[dfe,dfediff] = get_dfe(m,n,X,hat,varY,dobtwn,hatdiff,bcon);

if ~domultivariate
    % ======================================
    %
    %
    % Univariate stats: MSE, t, and p-values
    %
    %
    % ======================================

    % --------------------------------------
    % * Mean squared error
    % --------------------------------------
    % Loop version of MSE: avoids out of memory errors for large voxel sets
    MSE = zeros(1,n); for i=1:n, MSE(i) = e(:,i)'*W*e(:,i); end, MSE = MSE/dfe;
    v = invxwx * MSE;       % variances for mean

    if dobtwn
        MSEdiff = zeros(1,n);
        for i=1:n, MSEdiff(i) = ediff(:,i)'*W*ediff(:,i); end, MSEdiff = MSEdiff/dfediff;
        vdiff = invxwx_diff * MSEdiff;       % variances for mean
    end

    % output
    stats.descrip1 = 'Univariate stats for test against zero:';
    stats.v = v;
    stats.v_descrip = 'V = ste^2; variance of mean estimate';
    stats.t = Ymean ./ sqrt(v);
    stats.p = 2 * ( 1 - tcdf(abs(stats.t),dfe) );
    stats.dfe = dfe;

    if dobtwn
        stats.descrip2 = 'Univariate stats for between-case contrast:';
        stats.bcon = bcon;
        stats.vdiff = vdiff;
        stats.tdiff = fits ./ sqrt(v);
        stats.pdiff = 2 * ( 1 - tcdf(abs(stats.tdiff),dfediff) );
        stats.dfediff = dfediff;
    end

else
    % ======================================
    %
    %
    % Multivariate stats: MSE, cov(Y), r(Y)
    % Useful for simulating t-values under dependence
    %
    % ======================================

    % --------------------------------------
    % * Mean squared error
    % --------------------------------------

    % additional output: covariance matrix for Ymean and zdiff across time
    % (columns)
    % and correlation matrix for Ymean and zdiff
    % used in Monte Carlo simulations for controlling false positives
    % across columns

    MSE = (e'*W*e)/dfe;             % Mean square error

    if dobtwn
        MSEdiff = (ediff'*W*ediff)/dfediff;
    end

    % --------------------------------------
    % * Estimated covariance and correlation
    %   Estimated between-subjects variance (v)
    % --------------------------------------

    xy = invxwx * MSE;           % Covariance matrix for Ymean;

    xy = 0.5*(xy+xy');              % Remove rounding error

    if dobtwn
        xydiff = inv(bcon'*W*bcon)*MSEdiff;           % Covariance matrix for Ymean;
        xydiff = 0.5*(xydiff+xydiff');
    end

    v = diag(xy);                   % Variance for Ymean

    if dobtwn
        vdiff = diag(xydiff);
    end

    r = xy./sqrt(v*v');             % Correlation matrix for Ymean

    if dobtwn
        rdiff = xydiff./sqrt(vdiff*vdiff');         % Correlation matrix for Ymean
    end

    stats.descrip1 = 'Multivariate stats for test against zero:';
    stats.r = r;
    stats.v = v;
    stats.xy = xy;

    stats.dfe = dfe;
    stats.t = Ymean ./ sqrt(v');
    stats.p = 2 * ( 1 - tcdf(abs(stats.t),dfe) );

    if dobtwn
        stats.descrip2 = 'Univariate stats for between-case contrast:';
        stats.rdiff = rdiff;
        stats.vdiff = vdiff;
        stats.xydiff = xydiff;

        stats.dfediff = dfediff;

        stats.tdiff = Ydiff ./ sqrt(vdiff');
        stats.p = 2 * ( 1 - tcdf(abs(stats.tdiff),dfediff) );
    end
end

return

function [dfe,dfediff] = get_dfe(m,n,X,hat,varY,dobtwn,hatdiff,bcon)

dfediff = [];

% Set up residual-forming matrix
% --------------------------------------
dfe_v = zeros(n,1);
R = eye(m) - X*hat;    % residual inducing matrix

% contrast, if entered
if dobtwn
    dfe_vdiff = zeros(n,1);
    Rdiff = eye(m) - bcon * hatdiff;
end

% Calculate effective degrees of freedom
% --------------------------------------

have_unique_vars = size(varY,2) == n;

if ~have_unique_vars

    % Only one (pooled?) vector of variance estimates
    % --------------------------------------
    V = diag(varY(:,1));
    dfe = (trace(R*V)^2)/trace(R*V*R*V);       % Satherwaite approximation
    if dobtwn, dfediff = (trace(Rdiff*V)^2)/trace(Rdiff*V*Rdiff*V); end
else
    % Variance estimates for each data vector
    % --------------------------------------
    for i=1:n,

        % make diagonal matrix of variances
        V = diag(varY(:,i));

        dfe_v(i) = (trace(R*V)^2)/trace(R*V*R*V);       % Satherwaite approximation

        if dobtwn
            dfe_vdiff(i) = (trace(Rdiff*V)^2)/trace(Rdiff*V*Rdiff*V);
        end

    end

    dfe = mean(dfe_v);               % Calculate average df over all columns (pool over data vectors)
    if dobtwn
        dfediff = mean(dfe_vdiff);
    end

end

return

function bool = is_entered(x)

bool = exist('x','var') && ~isempty(x);

return


function denu=tide_oload(tut,odisp)

D2R=pi/180;
args=[1.40519E-4, 2.0,-2.0, 0.0, 0.00;  % M2
      1.45444E-4, 0.0, 0.0, 0.0, 0.00;  % S2
      1.37880E-4, 2.0,-3.0, 1.0, 0.00;  % N2
      1.45842E-4, 2.0, 0.0, 0.0, 0.00;  % K2
      0.72921E-4, 1.0, 0.0, 0.0, 0.25;  % K1
      0.67598E-4, 1.0,-2.0, 0.0,-0.25;  % O1
      0.72523E-4,-1.0, 0.0, 0.0,-0.25;  % P1
      0.64959E-4, 1.0,-3.0, 1.0,-0.25;  % Q1
      0.53234E-5, 0.0, 2.0, 0.0, 0.00;  % Mf
      0.26392E-5, 0.0, 1.0,-1.0, 0.00;  % Mm
      0.03982E-5, 2.0, 0.0, 0.0, 0.00];  % Ssa
ep1975=[1975,1,1,0,0,0];
dp=zeros(3,1);

%angular argument
ep=time2epoch(tut);
fday=ep(4)*3600.0+ep(5)*60.0+ep(6);
ep(4)=0;ep(5)=0;ep(6)=0.0;
days=timediff(epoch2time(ep),epoch2time(ep1975))/86400.0+1.0;
t=(27392.500528+1.000000035*days)/36525.0;
t2=t*t; t3=t2*t;
a(1)=fday;
a(2)=(279.69668+36000.768930485*t+3.03E-4*t2)*D2R;% H0
a(3)=(270.434358+481267.88314137*t-0.001133*t2+1.9E-6*t3)*D2R;% S0
a(4)=(334.329653+4069.0340329577*t-0.010325*t2-1.2E-5*t3)*D2R;% P0
a(5)=2.0*pi;

%dispalcements by 11 constituents
for i=1:11
    ang=0;
    for j=1:5
        ang=ang+a(j)*args(i,j);
    end
    for j=1:3
        dp(j)=dp(j)+odisp(i,j)*cos(ang-odisp(i,j+3)*D2R);
    end
end

denu(1)=-dp(2);
denu(2)=-dp(3);
denu(3)=dp(1);

return


function Y = invariantMultiply( psfMatData, X, padsize );
%
%           Y = invariantMultiply( psfMatData, X, padsize );
%
%  This function computes the multiplication of a spatially invariant
%  point spread function (PSF) times an image (i.e., convolution):
%                y = A*x
%
%  Here we assume A is made up of a single PSF whose extent may be
%  (much) smaller than the image.
%
%  Input:
%   psfMatData  -  complex array containing the matrix data, usually
%                  computed from onePsfMatrix.m
%            X  -  array containing the image to which the psfMatrix
%                  is to be multiplied.
%
%  Output:
%            Y  -  contains the result after PSF multiplication.
%

%  J. Nagy  1/7/02

imsize = size( X ) - 2*padsize;

%
%  In order for this to be consistent for 2-D and 3-D images, we need to make
%  sure there is a third dimension ...
%
if length(imsize) == 1
  imsize = [imsize, 1, 1];
  padsize = [padsize, 0, 0];
elseif length(imsize) == 2
  imsize = [imsize, 1];
  padsize = [padsize, 0];
end

%
%  partition_info computes number of subregions, and their sizes
%
[nregions, rsize] = partition_info(imsize, padsize);

%
%  Coding the rest of this will be easier if all of the image subregions
%  have the same dimensions.  If it's not, we pad with a few zeros to make
%  it so ...
%
padsize1 = rsize .* nregions - imsize;
if any( padsize1 < 0 )
  error('Something is wrong here ...')
end
X = padarray(X, padsize1, 'post');

Y = zeros( size(X) );

%
%  Now we get information about beginning and ending indices of subregions
%  so we can "put" and "get" subregions correctly ...
%
[RIidx, RJidx, RKidx] = region_indices( nregions, rsize );
[EIidx, EJidx, EKidx] = eregion_indices( RIidx, RJidx, RKidx, 2*padsize );
Tidx = [padsize+1; padsize+rsize];

%
%  Now loop over all the subregions ...
%
for k = 1:nregions(3)
  for j = 1:nregions(2)
    for i = 1:nregions(1)
      Xt = X(EIidx(i,1):EIidx(i,2), EJidx(j,1):EJidx(j,2), EKidx(k,1):EKidx(k,2));

      Yt = multiplyOneRegion( psfMatData, Xt );

      Y(RIidx(i,1):RIidx(i,2), RJidx(j,1):RJidx(j,2), RKidx(k,1):RKidx(k,2)) = ...
           Yt(Tidx(1,1):Tidx(2,1), Tidx(1,2):Tidx(2,2), Tidx(1,3):Tidx(2,3));
    end
  end
end

Y = Y(1:imsize(1), 1:imsize(2), 1:imsize(3));


function [cm] = km2cm(km)
% Convert length from kilometers to centimeters.
% Chad A. Greene 2012
cm = km*100000;


function airy_bi_values_test ( )

%*****************************************************************************80
%
%% AIRY_BI_VALUES_TEST demonstrates the use of AIRY_BI_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 April 2007
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'AIRY_BI_VALUES_TEST:\n' );
  fprintf ( 1, '  AIRY_BI_VALUES stores values of \n' );
  fprintf ( 1, '  the Airy function Bi(X).\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      X           Bi(X)\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, x, bi ] = airy_bi_values ( n_data );

    if ( n_data == 0 )
      break
    end

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

  end

  return
end


%[2006]-"A GA-based feature selection and parameters optimization for
%support vector machines"

% (9/12/2020)

function GA = jGeneticAlgorithmTour(feat,label,opts)
% Parameters
CR        = 0.8;   % crossover rate
MR        = 0.01;  % mutation rate
Tour_size = 3;     % tournament size

if isfield(opts,'N'), N = opts.N; end
if isfield(opts,'T'), max_Iter = opts.T; end
if isfield(opts,'CR'), CR = opts.CR; end
if isfield(opts,'MR'), MR = opts.MR; end
if isfield(opts,'Ts'), Tour_size = opts.Ts; end

% Objective function
fun = @jFitnessFunction;
% Number of dimensions
dim = size(feat,2);
% Initial
X   = jInitialization(N,dim);
% Fitness
fit  = zeros(1,N);
fitG = inf;
for i = 1:N
  fit(i) = fun(feat,label,X(i,:),opts);
  % Best update
  if fit(i) < fitG
    fitG = fit(i);
    Xgb  = X(i,:);
  end
end
% Pre
curve = zeros(1,max_Iter);
curve(1) = fitG;
t = 2;
% Generations
while t <= max_Iter
  % Preparation
  Xc1   = zeros(1,dim);
  Xc2   = zeros(1,dim);
  fitC1 = ones(1,1);
  fitC2 = ones(1,1);
  z     = 1;
  for i = 1:N
    if rand() < CR
      % Select two parents
      k1 = jTournamentSelection(fit,Tour_size,N);
      k2 = jTournamentSelection(fit,Tour_size,N);
      % Store parents
      P1 = X(k1,:);
      P2 = X(k2,:);
      % Single point crossover
      ind = randi([1, dim - 1]);
      % Crossover between two parents
      Xc1(z,:) = [P1(1:ind), P2(ind + 1:dim)];
      Xc2(z,:) = [P2(1:ind), P1(ind + 1:dim)];
      % Mutation
      for d = 1:dim
        % First child
        if rand() < MR
          Xc1(z,d) = 1 - Xc1(z,d);
        end
        % Second child
        if rand() < MR
          Xc2(z,d) = 1 - Xc2(z,d);
        end
      end
      % Fitness
      fitC1(1,z) = fun(feat,label,Xc1(z,:),opts);
      fitC2(1,z) = fun(feat,label,Xc2(z,:),opts);
      z = z + 1;
    end
  end
  % Merge population
  XX = [X; Xc1; Xc2];
  FF = [fit, fitC1, fitC2];
  % Select N best solution
  [FF, idx] = sort(FF,'ascend');
  X         = XX(idx(1:N),:);
  fit       = FF(1:N);
  % Best agent
  if fit(1) < fitG
    fitG = fit(1);
    Xgb  = X(1,:);
  end
  % Save
  curve(t) = fitG;
  fprintf('\nGeneration %d Best (GA Tournament)= %f',t,curve(t))
  t = t + 1;
end
% Select features based on selected index
Pos   = 1:dim;
Sf    = Pos(Xgb == 1);
sFeat = feat(:,Sf);
% Store results
GA.sf = Sf;
GA.ff = sFeat;
GA.nf = length(Sf);
GA.c  = curve;
GA.f  = feat;
GA.l  = label;
end

%// Tournament Selection //
function Index = jTournamentSelection(fit,Tour_size,N)
% Random positions based on position & Tournament Size
Tour_idx  = randsample(N,Tour_size);
% Select ftiness value based on position selected by tournament
Tour_fit  = fit(Tour_idx);
% Get position of best ftiness value (win tournament)
[~, idx]  = min(Tour_fit);
% Store the position
Index     = Tour_idx(idx);
end

function X = jInitialization(N,dim)
% Initialize X vectors
X = zeros(N,dim);
for i = 1:N
  for d = 1:dim
    if rand() > 0.5
      X(i,d) = 1;
    end
  end
end
end


function [Btuph] = hpe2Btuph(hpe)
% Convert power from electric horsepower to British
% thermal units per hour.
% Chad A. Greene 2012
Btuph = hpe*2545.457658313;


function [y,sy,st] = spm_mci_sens_init (R,P,M,U)
% Compute sensitivity to initial state
% FORMAT [y,sy,st] = spm_mci_sens_init (R,P,M,U)
%
% R         Initial state
% P         Parameters
% M         Model structure
% U         Inputs  [Nin x N]
%
% y         Outputs     [N x Nout]
% sy        Output Sensitivity, dy/dP [N x Nout x Nparams]
% st        Status flag (0 for OK, -1 for problem)
%           ... evaluated at the N time points in M.t
%
% M.f       Flow function dx/dt=f(x,u,P,M)
% M.g       Observation function y=g(x,u,P,M)
%
% This function uses Matlab's ODE suite
%
% B. Sengupta, K. Friston and W. Penny (2014) Efficient Gradient
% Computation for Dynamical Models. Neuroimage,98, 521-527.
%__________________________________________________________________________
% Copyright (C) 2015 Wellcome Trust Centre for Neuroimaging

% Will Penny and Biswa Sengupta
% $Id: spm_mci_sens_init.m 6697 2016-01-27 14:57:28Z spm $

y=[];sy=[];x=[];sx=[];
st=0;

% Tolerances for ode15s
try, tol.rel=M.reltol; catch, tol.rel=1e-2; end
try, tol.abs=M.abstol; catch, tol.abs=1e-4; end

if isempty(U)
    U=zeros(1,M.N);
end

init_t=0;
final_t=M.T;
init_state=R;

% parameters for the integrator
options = odeset('AbsTol',tol.abs,'RelTol',tol.rel);

% Allocate matrices
x=zeros(M.N,M.n);
y=zeros(M.N,M.l);
sx=zeros(M.N,M.n,M.n);
sy=zeros(M.N,M.l,M.n);

% initialise state-sensitivites
Nx=length(M.x0);
Np=length(P);
S0=eye(M.n);
init_sens = S0(:);

% Use Klopfenstein-Shampine integrator from Matlab's ODE suite
[T,V] = ode15s(@(t,v) int_states_init(t,v,U,P,M),[init_t final_t], [init_state; init_sens], options);

% T     Times
% V     States and sensitivities

% Extract states
xm=V(:,1:Nx);

% Interpolate states to times M.t
x=interp1q(T,xm,M.t);

% Interpolate state sensitivities to times M.t
sxm=interp1q(T,V(:,Nx+1:end),M.t);
%sx=reshape(sxm,length(M.t),Nx,Np);
% Updated Sep 13 2014
sx=reshape(sxm,length(M.t),Nx,Nx);

% When computing output sensitivities, assume dydx=L, ie not a
% function of x. Generalise later
[tmp,L]=feval(M.g,M.x0,U(:,1),P,M);

% Compute output and output sensitivity
for n=1:M.N,
    yout = feval (M.g,x(n,:)',U(:,n),P,M);
    y(n,:) = yout';
    sy(n,:,:)=L*squeeze(sx(n,:,:));
end

end

%--------------------------------------------------------------------------
function DvDt = int_states_init (t,v,U,P,M)
% Integrate states and sensitivities to initial conditions

Nx=length(M.x0);
Nt=Nx+Nx*Nx;

state_ind=1:Nx;
sens_ind=Nx+1:Nt;

% Find nearest time point for which we have pre-computed input
if isempty(U)
    ut=[];
else
    [tmp,ind]=min(abs(t-M.t));
    ut=U(:,ind);
end

% initialise state matrix
DvDt = zeros(Nt,1);

% Flow
DvDt(1:Nx)=feval(M.f,v(state_ind),ut,P,M);

if isfield(M,'dfdx')
    Fx = feval(M.dfdx,v(1:Nx),ut,P,M);
else
    Fx = spm_diff(M.f,v(1:Nx),ut,P,M,1);
end

Sx_old = v(sens_ind);
Sx_old = reshape(Sx_old,Nx,Nx);

% Compute change in sensitivity from old - note absence of Fp term
Sx = Fx*Sx_old;

% Sensitivities
DvDt(sens_ind)=Sx(:);

end


function [err,XYZdic] = AFNI_Index2XYZcontinuous (Indx, Info, CoordCode)
%
%   [err,XYZdic] = AFNI_Index2XYZcontinuous (Indx, Info, [CoordCode])
%
%Purpose:
%   Change from voxel XYZindex (called Voxel Coords in AFNI) to XYZ in mm
%   The mm and voxel coordinates refer to the values displayed
%   on the top left corner of AFNI controller.
%   CoordCode is the one you'd set from the Coord Order plugin
%
%
%Input Parameters:
%   Indx an Mx3 matrix or an  Mx1 vector containing the voxel indices to be
%        transformed to voxel coordinates.  (indices start at 0)
%   Info is the output of BrikInfo
%   CoordCode is an optional parameter used to specify the coordinates system of the output
%      if empty or not specified, the default is 'RAI'. The code can be either a string or a vector
%      of numbers (see AFNI_CoordChange for more on that)
%
%Output Parameters:
%   err : 0 No Problem
%       : 1 Mucho Problems
%   XYZdic : The continuous coordinates corresponding to Indx
%       The coordnate system output is in RAI (DICOM)
%       unless otherwise specified by CoordCode
%
%
%Key Terms:
%
%More Info :
%   BrikInfo
%   Test_AFNI_Index2XYZcontinuous
%   AFNI_XYZcontinuous2Index
%   Test_AFNI_XYZcontinuous2Index
%
% You can also go from index to XYZ using the header field IJK_TO_DICOM_REAL
%  For instance, say you have voxel indices 12, 2, 4 (matlab indexing 13, 3, 5)
%  to go from AFNI index to AFNI DICOM RAI you can do:
%     M = [reshape(Info.IJK_TO_DICOM_REAL, 4, 3)' ; 0 0 0 1];
%     I = [12 2 4 1]';
%     X = M*I;
%     To go from AFNI DICOM RAI to AFNI indices:
%     I = inv(M)*X;
%
%     Author : Ziad Saad
%     Date : Tue Sep 5 21:48:06 PDT 2000           Latest Modification: Feb 18 04
%     LBC/NIMH/ National Institutes of Health, Bethesda Maryland

%Define the function name for easy referencing
FuncName = 'AFNI_Index2XYZcontinuous';

%Debug Flag
DBG = 1;

ChangeCoord = 0;
if (nargin == 3)
	if (~isempty(CoordCode)),
		ChangeCoord = 1;
	end
end

%initailize return variables
err = 1;
XYZmm = [];

%make sure Indx is the right size
switch size(Indx,2),
	case 1, %change 1D index to XYZ index
		[err, Indx] = AfniIndex2AfniXYZ (Indx, Info.DATASET_DIMENSIONS(1), Info.DATASET_DIMENSIONS(2))
	case 3, %OK
	otherwise,
		err = ErrEval(FuncName,'Err_Bad dimension for Indx');
		return
end

XYZmm = Indx;

	%The equations that would change the indices to coordinate system result in a coordinate system that
	% may be any permutation of RAI (like IRA or AIR or IAR or RIA or ARI) so one only needs to find the
	%dimension permutation needed to bring the final result to RAI.

	%determine the ordering map to go from any permutation of RAI to RAI
		%[maploc(1),jnk] = find(Info.Orientation == 'R');
		%[maploc(2),jnk] = find(Info.Orientation == 'A');
		%[maploc(3),jnk] = find(Info.Orientation == 'I');

	%pre - Wed May 23 18:20:56 PDT 2001 - WRONG !
		%XYZmm(:, maploc(1)) = Info.ORIGIN(1) + Indx(:,1) .* Info.DELTA(1);
		%XYZmm(:, maploc(2)) = Info.ORIGIN(2) + Indx(:,2) .* Info.DELTA(2);
		%XYZmm(:, maploc(3)) = Info.ORIGIN(3) + Indx(:,3) .* Info.DELTA(3);

	%post - Wed May 23 18:20:56 PDT 2001 - WRONG!
		%XYZmm(:, 1) = Info.ORIGIN(maploc(1)) + Indx(:,maploc(1)) .* Info.DELTA(maploc(1));
		%XYZmm(:, 2) = Info.ORIGIN(maploc(2)) + Indx(:,maploc(2)) .* Info.DELTA(maploc(2));
		%XYZmm(:, 3) = Info.ORIGIN(maploc(3)) + Indx(:,maploc(3)) .* Info.DELTA(maploc(3));

   %Feb 18 04, back to the original
      XYZmm(:, 1) = Info.ORIGIN(1) + Indx(:,1) .* Info.DELTA(1);
		XYZmm(:, 2) = Info.ORIGIN(2) + Indx(:,2) .* Info.DELTA(2);
		XYZmm(:, 3) = Info.ORIGIN(3) + Indx(:,3) .* Info.DELTA(3);
      %Now this is in the axis orientation which is Info.Orientation(:,1)' called 3dmm in thd_coords.c
      [err,XYZdic, map] = THD_3dmm_to_dicomm (Info, XYZmm);

if (ChangeCoord),
	[err, maplocation, mapsign, XYZdic] = AFNI_CoordChange ('RAI', CoordCode, XYZdic);
end

err = 0;
return;


function W = weightingscheme(cases,wfct,varargin)

%
% INTERNAL FUNCTION
%

% Copyright (c) 2011,  KULeuven-ESAT-SCD, License & help @ http://www.esat.kuleuven.be/sista/lssvmlab

switch wfct

    case {'WHuber','whuber'}
        b = varargin{1};
        cases = abs(cases);
        temp = 1;
        W = temp.*(cases<b) + temp.*(cases>=b).*(b./cases);

    case {'WLogistic','wlogistic'}
        W = tanh(cases)./cases;

    case {'WHampel','whampel'}
       cases = abs(cases);
       % defaults for c1 and c2
       c1=2.5; c2=3;
       %[c1,c2] = adaptweight(cases);
       dc = c2-c1;
       temp = 1;
       W = temp.*(cases<=c1) + ...
           temp.*(cases<=c2 & cases>c1).*((c2-cases)./dc) + ...
           temp.*(cases>c2).*10e-8;

    case {'WMyriad','wmyriad'}
        %K = 0.5*iqr(cases);
        K = varargin{1};
        W = K^2./(K^2+cases.^2);
end


function [xn] = normalize(x_kk,fc,cc,kc,alpha_c)

%normalize
%
%[xn] = normalize(x_kk,fc,cc,kc,alpha_c)
%
%Computes the normalized coordinates xn given the pixel coordinates x_kk
%and the intrinsic camera parameters fc, cc and kc.
%
%INPUT: x_kk: Feature locations on the images
%       fc: Camera focal length
%       cc: Principal point coordinates
%       kc: Distortion coefficients
%       alpha_c: Skew coefficient
%
%OUTPUT: xn: Normalized feature locations on the image plane (a 2XN matrix)
%
%Important functions called within that program:
%
%comp_distortion_oulu: undistort pixel coordinates.

if nargin < 5,
   alpha_c = 0;
   if nargin < 4;
      kc = [0;0;0;0;0];
      if nargin < 3;
         cc = [0;0];
         if nargin < 2,
            fc = [1;1];
         end;
      end;
   end;
end;

% First: Subtract principal point, and divide by the focal length:
x_distort = [(x_kk(1,:) - cc(1))/fc(1);(x_kk(2,:) - cc(2))/fc(2)];

% Second: undo skew
x_distort(1,:) = x_distort(1,:) - alpha_c * x_distort(2,:);

if norm(kc) ~= 0,
	% Third: Compensate for lens distortion:
	% xn = comp_distortion_oulu(x_distort,kc);
  xn = undistort(x_distort, kc);
else
   xn = x_distort;
end;


function rf=lpcla2rf(la)
%LPCLA2RF Convert log areas to reflection coefficients RF=(LA)

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

[nf,p2]=size(la);
rf=-tanh((la(:,1:p2-1)-la(:,2:p2))/2);


function f = perform_wavortho_transf(f,Jmin,dir,options)

% perform_wavortho_transf - compute orthogonal wavelet transform
%
%   fw = perform_wavortho_transf(f,Jmin,dir,options);
%
%   You can give the filter in options.h.
%
%   Works in arbitrary dimension.
%
%   Copyright (c) 2009 Gabriel Peyre

options.null = 0;
h = getoptions(options,'h', compute_wavelet_filter('Daubechies',4) );
g = [0 h(length(h):-1:2)] .* (-1).^(1:length(h));

n = size(f,1);
Jmax = log2(n)-1;

if dir==1
    %%% FORWARD %%%
    for j=Jmax:-1:Jmin
        sel = 1:2^(j+1);
        a = subselect(f,sel);
        for d=1:nb_dims(f)
            a = cat(d, subsampling(cconv(a,h,d),d), subsampling(cconv(a,g,d),d) );
        end
        f = subassign(f,sel,a);
    end
else
    %%% FORWARD %%%
    for j=Jmin:Jmax
        sel = 1:2^(j+1);
        a = subselect(f,sel);
        for d=1:nb_dims(f)
            w = subselectdim(a,2^j+1:2^(j+1),d);
            a = subselectdim(a,1:2^j,d);
            a = cconv(upsampling(a,d),reverse(h),d) + cconv(upsampling(w,d),reverse(g),d);
        end
        f = subassign(f,sel,a);
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function f = subselect(f,sel)
switch nb_dims(f)
    case 1
        f = f(sel);
    case 2
        f = f(sel,sel);
    case 3
        f = f(sel,sel,sel);
    case 4
        f = f(sel,sel,sel,sel);
    case 5
        f = f(sel,sel,sel,sel,sel);
    case 6
        f = f(sel,sel,sel,sel,sel,sel);
    case 7
        f = f(sel,sel,sel,sel,sel,sel,sel);
    case 8
        f = f(sel,sel,sel,sel,sel,sel,sel,sel);
    otherwise
        error('Not implemented');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function f = subselectdim(f,sel,d)
switch d
    case 1
        f = f(sel,:,:,:,:,:,:,:);
    case 2
        f = f(:,sel,:,:,:,:,:,:);
    case 3
        f = f(:,:,sel,:,:,:,:,:);
    case 4
        f = f(:,:,:,sel,:,:,:,:);
    case 5
        f = f(:,:,:,:,sel,:,:,:);
    case 6
        f = f(:,:,:,:,:,sel,:,:);
    case 7
        f = f(:,:,:,:,:,:,sel,:);
    case 8
        f = f(:,:,:,:,:,:,:,sel);
    otherwise
        error('Not implemented');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function f = subassign(f,sel,g)
switch nb_dims(f)
    case 1
        f(sel) = g;
    case 2
        f(sel,sel) = g;
    case 3
        f(sel,sel,sel) = g;
    case 4
        f(sel,sel,sel,sel) = g;
    case 5
        f(sel,sel,sel,sel,sel) = g;
    case 6
        f(sel,sel,sel,sel,sel,sel) = g;
    case 7
        f(sel,sel,sel,sel,sel,sel,sel) = g;
    case 8
        f(sel,sel,sel,sel,sel,sel,sel,sel) = g;
    otherwise
        error('Not implemented');
end


function [rar,yclean] = spm_rar (Z,p,m,verbose)
% Bayesian autoregressive modelling with zero-mean Gaussian mixture noise
% function [rar,yclean] = spm_rar (Z,p,m,verbose)
%
% Z          [N x 1] vector of data points
% p          Number of AR coefficients
% m          Number of mixture components (default=2)
% verbose    0/1 to printout inner workings (default=0)
%
% rar        Returned model
% yclean     'Clean' data (ie. with outlier errors removed)
%
% -------------------------------------------------------
% The fields in rar are:
%
% p                The number of AR coefficients
% m                The number of components
% fm               The negative free energy
%
%                  In the field priors:
% lambda_0         Dirichlet parameters for mixing coeffs
% b_0,c_0          Gamma parameters for precisions
%
%                  In the field posts:
% lambda           Dirichlet parameters  for mixing coeffs
% b,c              Gamma parameters for precisions
% a_mean           AR parameters (posterior mean)
% a_cov            AR parameters (posterior cov)
% b_alpha,c_alpha  Gamma parameters for weight precisions
%
%                  Mean posterior values:
% pi               mixing coefficients (lambda/sum(lambda))
% variances        variances (1./(b.*c))
%
% gamma            the responsibilities of each noise component
%
% For details of algorithm see:
%
% S.J. Roberts and W.D. Penny. Variational Bayes for Generalised Autoregressive
% models. IEEE Transactions on Signal Processing, 50(9):2245-2257, 2002
%___________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: spm_rar.m 1276 2008-03-28 18:29:19Z guillaume $

if nargin < 3 | isempty(m)
    m=2;
end

if nargin < 4 | isempty(verbose)
  verbose=0;
end

if (m==1)
  rar=spm_ar(Z,p);
  return;
end

N=length(Z);

% Initialise AR coefficients to maximum likelihood solution
Z=Z(:);
y=Z(p+1:end);
for i=1:p,
    x(:,i)=Z(p-i+1:end-i);
end
x=-x;
y2=y.^2;
xt=x';
N=size(x,1);
wun=ones(N,1);

a_mean = pinv(x)*y;
y_pred = x*a_mean;
err=y-y_pred;
v=mean((y-y_pred).^2);
a_cov = v*inv(x'*x);

% Set mixing priors
lambda_0=5;

% Setting to these values gives updates for mean_alpha
% v. close to evidence framework
bishop_prior=1;
if bishop_prior
  b_alpha_prior=1000;
  c_alpha_prior=0.001;
  b_0=1000;
  c_0=0.001;
end
mean_alpha_prior=b_alpha_prior*c_alpha_prior;

% Cluster on absolute difference from mean
zmix=spm_kmeans1(abs(err-mean(err)),m);
% Posterior for mixers
lambda=100*zmix.pi;
zmean=[zmix.m].^2;
% Posterior for precisions
var_precision=1/std(zmean)^2;
for s=1:m,
      % Set so that b*c=precision and  b^2*c=var_precision
      precision=1/zmean(s);
      b(s)=var_precision/precision;
      c(s)=(precision^2)/var_precision;
end

% Initialise weight precision posterior
E_w=a_mean'*a_mean;
b_alpha=0.5*E_w+0.5*trace(a_cov)+(1/b_alpha_prior);
b_alpha=1/b_alpha;
c_alpha=0.5*p+c_alpha_prior;
mean_alpha=b_alpha*c_alpha;

if verbose
  disp('Init');
  disp('AR Coefficients');
  a_mean(:)'
  for s=1:m,
    disp(sprintf('State %d mix=%1.2f var=%1.2f',s,lambda(s)/sum(lambda),1/(b(s)*c(s))));
  end
end

lik=[];
tol=0.0001;
max_loops=32;
WLOOPS=5;
for loops=1:max_loops,

    % E-step
    lambda_tot=sum(lambda);
    ypred=x*a_mean;
    ypred2=ypred.^2;
    y_err=sum(xt.*(a_cov*xt));
    tv=y2-2*ypred.*y+y_err'+ypred2;
    tv=tv';
    for s=1:m,
        log_tilde_pi(s)=psi(lambda(s))-psi(lambda_tot);
        log_tilde_beta(s)=psi(c(s))+log(b(s));
        tilde_pi(s)=exp(log_tilde_pi(s));
        tilde_beta(s)=exp(log_tilde_beta(s));
        mean_beta(s)=c(s)*b(s);
        tilde_var(s,:)=tv;
        gamma(s,:)=tilde_pi(s)*(tilde_beta(s)^0.5)*exp(-0.5*mean_beta(s)*tv);
    end
    gamma_n=sum(gamma);
    for s=1:m,
        if mean(gamma_n) > eps
            % If component still exists
            gamma(s,:)=gamma(s,:)./gamma_n;
        end
    end

    % M-step
    % Part I
    for s=1:m,
        pi_bar(s)=mean(gamma(s,:));
        N_bar(s)=N*pi_bar(s);
        mean_bar(s)=mean(gamma(s,:)'.*y);
        dg=diag(gamma(s,:));
        x_bar(s,:)=mean(dg*x);
        var_bar_mu(s)=mean(gamma(s,:).*tilde_var(s,:));
    end

    % CALCULATE THE FREE ENERGY
    avg_likelihood=sum(N_bar.*(log_tilde_pi+0.5*log_tilde_beta));
    fit=-0.5*N*sum(mean_beta.*var_bar_mu);
    % Ensure that 0 log 0 = 0
    ent_s=sum(sum(-gamma.*log(gamma+eps)));
    avg_likelihood=avg_likelihood+fit+ent_s;
    lambda_p=lambda_0*ones(1,m);
    kl_dir=spm_kl_dirichlet(lambda,lambda_p,log_tilde_pi);
    kl_gamm=0;
    for s=1:m,
        kl_gamm=kl_gamm+spm_kl_gamma(b(s),c(s),b_0,c_0);
    end
    kl_weights=spm_kl_normal(a_mean,a_cov,zeros(1,p),(1/mean_alpha)*eye(p));
    kl_alpha=spm_kl_gamma(b_alpha,c_alpha,b_alpha_prior,c_alpha_prior);
    fm= avg_likelihood - kl_dir - kl_gamm - kl_weights - kl_alpha;

    % Convergence criterion
    oldlik=lik;
    lik=fm;

    if (mod(loops-1,WLOOPS)==0)
        if (loops>1)
            if abs((lik-oldlik)/lik) < tol
                break;
            end
        end
    end

    % M-Step: Part II
    for s=1:m,
        % Mixers
        lambda(s)=N_bar(s)+lambda_0;
        % Precisions
        b(s)=1/(0.5*N*var_bar_mu(s)+1/b_0);
        c(s)=0.5*N_bar(s)+c_0;
        mean_beta(s)=c(s)*b(s);
    end

    if mod(loops,WLOOPS)==0
        % Weight precisions
        E_w=0.5*a_mean'*a_mean;
        b_alpha=E_w+0.5*trace(a_cov)+(1/b_alpha_prior);
        b_alpha=1/b_alpha;
        c_alpha=0.5*p+c_alpha_prior;
        mean_alpha=b_alpha*c_alpha;

        % AR coefficients
        cc=zeros(p,p);
        cw=zeros(p,1);
        for s=1:m,
            dg=diag(gamma(s,:));
            cc=cc+mean_beta(s)*x'*dg*x;
            cw=cw+mean_beta(s)*x'*dg*y;
        end
        cc=cc+mean_alpha*eye(p);
        a_cov=inv(cc);
        a_mean=a_cov*cw;
    end

    if verbose
        disp(sprintf('It=%d, L_AV =%1.2f, KL Mix=%1.2f, KL Prec=%1.2f, KL-AR=%1.2f, KL-alpha=%1.2f, Fm=%1.2f',loops,avg_likelihood,kl_dir,kl_gamm,kl_weights,kl_alpha,fm));
    end

end

% Put variables into data structure
rar.posts.a_mean=a_mean;
rar.posts.a_cov=a_cov;
rar.m=m;
rar.fm=fm;
rar.priors.lambda_0=lambda_0;
rar.priors.c_0=c_0;
rar.priors.b_0=b_0;

for k=1:m,
  rar.posts.lambda(k)=lambda(k);
end

for k=1:m,
  rar.posts.c(k)=c(k);
  rar.posts.b(k)=b(k);
end

for k=1:m,
  rar.pi(k)=lambda(k)/sum(lambda);
end
rar.variances=1./(b.*c);

% Pre-pad gamma with zeros to get original length time series
rar.gamma=[zeros(m,p),gamma];

% Get 'clean' data
if m > 1
    [tmp,outlier_class]=min(rar.pi);
    e=y-ypred;
    outlier_error=gamma(outlier_class,:)'.*e;
    yclean=ypred+e-outlier_error;
    yclean=[Z(1:p);yclean];  % Pre-padding
else
    yclean=Z;
end


classdef OptimizerAugmentedLagrangian < Optimizer

    properties (GetAccess = public, SetAccess = protected)
        type = 'Augmented Lagrangian';
    end

    properties (Access = private)
        tau
        lineSearchTrials
        lineSearch
        costOld
        upperBound
        lowerBound
        tol = 1e-8
        nX
        nConstr
        hasConverged
        acceptableStep
        oldDesignVariable
        oldCost
        incrementalScheme
        hasFinished
        mOld
        meritNew
        penalty
        meritGradient

        globalCost
        globalConstraint
        globalCostGradient
        globalMerit
        globalLineSearch
        globalDual
        globalDesignVar
    end

    methods (Access = public)

        function obj = OptimizerAugmentedLagrangian(cParams)
            obj.initOptimizer(cParams);
            obj.init(cParams);
            obj.outputFunction.monitoring.create(cParams);
            obj.createPrimalUpdater(cParams);
            obj.createDualUpdater(cParams);
            obj.prepareFirstIter();
        end

        function obj = solveProblem(obj)
            obj.hasConverged = false;
            obj.cost.computeFunctionAndGradient();
            obj.constraint.computeFunctionAndGradient();
%             obj.saveVariablesForAnalysis();
            obj.hasFinished = 0;
            obj.printOptimizerVariable();
            while ~obj.hasFinished
%             while ~obj.hasConverged
                obj.update();
                obj.updateIterInfo();
                obj.updateMonitoring();
                obj.checkConvergence();
                obj.printOptimizerVariable();
%                 obj.saveVariablesForAnalysis();
            end
        end

    end

    methods(Access = private)

        function init(obj,cParams)
            obj.upperBound             = cParams.uncOptimizerSettings.ub;
            obj.lowerBound             = cParams.uncOptimizerSettings.lb;
            obj.cost                   = cParams.cost;
            obj.constraint             = cParams.constraint;
            obj.nConstr                = cParams.constraint.nSF;
            obj.designVariable         = cParams.designVar;
            obj.dualVariable           = cParams.dualVariable;
            obj.incrementalScheme      = cParams.incrementalScheme;
            obj.nX                     = length(obj.designVariable.value);
            obj.maxIter                = cParams.maxIter;
            obj.hasConverged           = false;
            obj.nIter                  = 0;
        end

        function prepareFirstIter(obj)
            obj.cost.computeFunctionAndGradient();
            obj.costOld = obj.cost.value;
            obj.designVariable.updateOld();
            obj.dualVariable.value = zeros(obj.nConstr,1);
            obj.penalty            = 10;
        end

        function obj = update(obj)
            x0 = obj.designVariable.value;
            obj.designVariable.update(x0);
            obj.saveOldValues(x0);
            obj.mOld = obj.computeMeritFunction(x0);
            obj.calculateInitialStep();
            obj.acceptableStep   = false;
            obj.lineSearchTrials = 0;
            obj.computeMeritGradient();
            while ~obj.acceptableStep
                x = obj.updatePrimal();
                obj.checkStep(x,x0);
            end
            obj.updateOldValues(x);
        end

        function displayIter(obj,x)
            m = obj.designVariable.mesh;
            bm = m.createBoundaryMesh();
            s.backgroundMesh = m;
            s.boundaryMesh   = bm;
            um = UnfittedMesh(s);
            um.compute(x);
            figure()
            um.plot();
        end

        function obj = calculateInitialStep(obj)
            obj.cost.computeFunctionAndGradient();
            obj.constraint.computeFunctionAndGradient();
            x       = obj.designVariable.value;
            l       = obj.dualVariable.value;
            DJ      = obj.cost.gradient;
            Dg      = obj.constraint.gradient;
            g       = obj.constraint.value;
            p       = obj.penalty;
            DmF     = DJ + Dg*(l + p*g);
            if obj.nIter == 0
                factor = 1;
                obj.primalUpdater.computeFirstStepLength(DmF,x,factor);
            else
                factor = 1.05;
                obj.primalUpdater.increaseStepLength(factor);
            end
        end

        function x = updatePrimal(obj)
            x   = obj.designVariable.value;
            g   = obj.meritGradient;
            x   = obj.primalUpdater.update(g,x);
        end

        function computeMeritGradient(obj)
            Dh    = obj.constraint.gradient;
            DJ    = obj.cost.gradient;
            l     = obj.dualVariable.value;
            p     = obj.penalty;
            gPlus = obj.defineConstraintValue();
            g     = (DJ + Dh*(l + p*gPlus));
            obj.meritGradient = g;
        end

        function mF = computeMeritFunction(obj,x)
            obj.designVariable.update(x)
            obj.cost.computeFunctionAndGradient();
            obj.constraint.computeFunctionAndGradient();
            J      = obj.cost.value;
            gPlus  = obj.defineConstraintValue();
            l      = obj.dualVariable.value;
            rho    = obj.penalty;
            mF     = J + l'*gPlus + 0.5*rho*(gPlus'*gPlus);
        end

        function c = defineConstraintValue(obj)
            c   = obj.constraint.value;
            l   = obj.dualVariable.value;
            rho = obj.penalty;
            for i = 1:obj.nConstr
                switch obj.constraintCase{i}
                    case 'EQUALITY'

                    case 'INEQUALITY'
                        c(i) = max(c(i),-l/rho);
                end
            end
        end

        function checkStep(obj,x,x0)
            mNew = obj.computeMeritFunction(x);
            if mNew < obj.mOld
                obj.acceptableStep = true;
                obj.dualUpdater.updatePenalty(obj.penalty);
                obj.dualUpdater.update();
                obj.meritNew = mNew;
            elseif obj.primalUpdater.isTooSmall()
%                 error('Convergence could not be achieved (step length too small)')
                warning('Convergence could not be achieved (step length too small)')
                obj.acceptableStep = true;
                obj.meritNew = mNew; % Provisional value
            else
                obj.primalUpdater.decreaseStepLength();
                obj.designVariable.update(x0);
                obj.lineSearchTrials = obj.lineSearchTrials + 1;
            end
        end

        function obj = saveOldValues(obj,x)
            obj.designVariable.update(x);
            obj.cost.computeFunctionAndGradient();
            obj.constraint.computeFunctionAndGradient();
            obj.oldCost            = obj.cost.value;
            obj.oldDesignVariable  = x;
        end

        function obj = updateOldValues(obj,x)
            obj.designVariable.update(x);
            obj.cost.computeFunctionAndGradient();
            obj.constraint.computeFunctionAndGradient();
        end

        function obj = checkConvergence(obj)
           if abs(obj.meritNew - obj.mOld) < obj.tol && obj.checkConstraint()
               obj.hasConverged = true;
           else

           end

        end

        function obj = updateMonitoring(obj)
            s.nIter            = obj.nIter;
            s.tau              = obj.primalUpdater.tau;
            s.lineSearch       = obj.lineSearch;
            s.lineSearchTrials = obj.lineSearchTrials;
            s.oldCost          = obj.oldCost;
            s.hasFinished      = obj.hasFinished;
            s.meritNew         = obj.meritNew;
            obj.outputFunction.monitoring.compute(s);
        end

        function updateIterInfo(obj)
            obj.increaseIter();
            obj.updateStatus();
        end

        function increaseIter(obj)
            obj.nIter = obj.nIter + 1;
        end

        function updateStatus(obj)
            obj.hasFinished = obj.hasConverged || obj.hasExceededStepIterations();
        end

        function itHas = hasExceededStepIterations(obj)
            iStep = obj.incrementalScheme.iStep;
            nStep = obj.incrementalScheme.nSteps;
            itHas = obj.nIter >= obj.maxIter*(iStep/nStep);
        end

        function saveVariablesForAnalysis(obj)
            i                           = obj.nIter + 1;
            obj.globalCost(i)           = obj.cost.value;
            obj.globalConstraint(:,i)   = obj.constraint.value;
            obj.globalCostGradient(i)   = norm(obj.cost.gradient);
            obj.globalMerit(i)          = obj.meritNew;
            obj.globalLineSearch(i)     = obj.primalUpdater.tau;
            obj.globalDual(:,i)         = obj.dualVariable.value;
            obj.globalDesignVar(:,i)    = obj.designVariable.value;
            if obj.hasConverged
                c = obj.globalCost;
                h = obj.globalConstraint;
                g = obj.globalCostGradient;
                m = obj.globalMerit;
                t = obj.globalLineSearch;
                d = obj.globalDual;
                v = obj.globalDesignVar;
                save('name.mat',"c","g","h","d","v");
            end
        end

    end

end


function chebyshev_polynomial_test16 ( )

%*****************************************************************************80
%
%% CHEBYSHEV_POLYNOMIAL_TEST16 tests W_POLYNOMIAL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 April 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CHEBYSHEV_POLYNOMIAL_TEST16:\n' );
  fprintf ( 1, '  W_POLYNOMIAL_VALUES stores values of\n' );
  fprintf ( 1, '  the Chebyshev polynomials.\n' );
  fprintf ( 1, '  W_POLYNOMIAL evaluates the polynomial.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '                        Tabulated                 Computed\n' );
  fprintf ( 1, '     N        X           W(n,x)                    W(n,x)                     Error\n' );

  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, n, x, fx1 ] = w_polynomial_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fx2_vec = w_polynomial ( 1, n, x );
    fx2 = fx2_vec(1,n+1);
    e = fx1 - fx2;

    fprintf ( 1, '  %4d  %12f  %24.16e  %24.16e  %8.2g\n', n, x, fx1, fx2, e );

  end

  return
end


function stroud_test045 ( )

%*****************************************************************************80
%
%% TEST045 tests BALL_UNIT_VOLUME_3D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 April 2009
%
%  Author:
%
%    John Burkardt
%
  n = 3;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST045\n' );
  fprintf ( 1, '  In 3 dimensions:\n' );
  fprintf ( 1, '  BALL_UNIT_VOLUME_3D gets the volume of the unit ball.\n' );
  fprintf ( 1, '  BALL_UNIT_VOLUME_ND will be called for comparison.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  N    Volume    Method\n' );
  fprintf ( 1, '\n' );

  fprintf ( 1, '  %1d  %12f  %s\n', n, ball_unit_volume_3d ( ), ...
    'BALL_UNIT_VOLUME_3D' );

  fprintf ( 1, '  %1d  %12f  %s\n', n, ball_unit_volume_nd ( n ), ...
    'BALL_UNIT_VOLUME_ND' );

  return
end


function [pvec, pstruct] = tapas_hgf_ar1_transp(r, ptrans)
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2012-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/>.

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

l = r.c_prc.n_levels;

pvec(1:l)         = ptrans(1:l);                           % mu_0
pstruct.mu_0      = pvec(1:l);
pvec(l+1:2*l)     = exp(ptrans(l+1:2*l));                  % sa_0
pstruct.sa_0      = pvec(l+1:2*l);
pvec(2*l+1:3*l)   = tapas_sgm(ptrans(2*l+1:3*l),1);        % phi
pstruct.phi       = pvec(2*l+1:3*l);
pvec(3*l+1:4*l)   = ptrans(3*l+1:4*l);                     % m
pstruct.m         = pvec(3*l+1:4*l);
pvec(4*l+1:5*l-1) = exp(ptrans(4*l+1:5*l-1));              % ka
pstruct.ka        = pvec(4*l+1:5*l-1);
pvec(5*l:6*l-1)   = ptrans(5*l:6*l-1);                     % om
pstruct.om        = pvec(5*l:6*l-1);
pvec(6*l)         = exp(ptrans(6*l));                      % al
pstruct.al        = pvec(6*l);

return;


function rr = ARS(rel_data, wins, up)
%ARS calculates the power spectral density of a signal using
% auto-regressive modelling, and finds the RR
%
%	            ARS(option, up)
%
%	Inputs:
%       rel_data    .t  vector of times
%                   .v  vector of resp Sig values
%                   .fs sampling freq
%       up              universal parameters structure
%       wins        .t  vector of start times
%
%	Outputs:
%       rr          .t  vector of times of estimated RRs
%                   .v  vector of estimated RRs
%                   .f  vector of freqs of power spectrum
%                   .p  vector of powers of power spectrum
%

%% Setup
downsample_freq = up.paramSet.ar_resample_freq;    % it would be worth changing this - it changes the answer (try 1 Hz e.g.)
true_fs = rel_data.fs;

%% Cycle through windows
rr.t = mean([wins.t_start(:)' ; wins.t_end(:)']); rr.t = rr.t(:);
rr.v = nan(length(rr.t),1);
rr.p = cell(length(wins.t_start),1);
rr.f = cell(length(wins.t_start),1);

for win_no = 1 : length(wins.t_start)

    % extract relevant data
    rel_els = find(rel_data.t >= wins.t_start(win_no) & rel_data.t < wins.t_end(win_no));
    data.v = rel_data.v(rel_els);
    data.t = rel_data.t(rel_els);

    good_els = ~isnan(data.v);
    data.v = data.v(good_els);
    data.t = data.t(good_els);

    % Downsample
    data.filt.t = downsample(data.t, true_fs/downsample_freq);
    data.filt.v = decimate(data.v, true_fs/downsample_freq);
    data.filt.v = detrend(data.filt.v);

    data.filt.v = data.filt.v(:);
    data.filt.t = data.filt.t(:);

    % AR modelling
    [Yy,Xx]=pburg(data.filt.v,up.paramSet.ar_model_order,[],downsample_freq);
    data.freqs=Xx;
    data.power=Yy;

    % Find spectral peak
    [rr.v(win_no), rr.f{win_no}, rr.p{win_no}] = find_spectral_peak(data, up);

    clear data

end

end


%% Housekeeping
close all
clc
%% load RISE before proceeding if you have not done so already

%% read the models and their calibrations
sw0=rise('usmodel','solve_linear',true);

%% solve the model
sw=solve(sw0,'solver','mn');

%% print results
sw.print_solution()

%% print solution for a subset of variables only
sw.print_solution({'a','b','c','cf','dc','dinve','dw','dy'})

%% compute regime-specific impulse responses
myirfs0=irf(sw,'irf_periods',20);

%% compute generalized impulse responses
myirfs1=irf(sw,'irf_periods',20,'irf_type','girf');

%% plot the impulse responses
close all
var_list={'dc','dinve','dw','dy','lab','pinf','r'};
figure('name','Impulse responses to a wage markup shock');
for ii=1:numel(var_list)
    subplot(3,3,ii)
	v=var_list{ii};
	aggregate=[myirfs0.ew.(v),myirfs1.ew.(v)];
    plot(aggregate,'linewidth',2);
    title(var_list{ii})
    if ii==1
        legend(aggregate.varnames)
    end
    axis tight
end


function H = cross(F, G)
%CROSS   Vector cross product.
%   CROSS(F, G) returns a SPHEREFUNV representing the 3D cross product of
%   the SPHEREFUNV objects F and G.
%
%   See also SPHEREFUNV/DOT.

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

% [TODO]: Implement the following option
%   CROSS(F, G, 'n') returns a SPHEREFUN representing the normal component
%   of the cross product of the SPHEREFUNV objects F and G.
%   For two vector fields tangent to the sphere, the cross product is a
%   a vector that points in the normal direction.  CROSS returns a
%   SPHEREFUN object representing the component in the normal (radial)
%   direction.  Mathematically, this is N dot (F x G), where N is the
%   normal to the sphere.

% Empty check:
if ( isempty(F) || isempty(G) )
    H = spherefunv;
    return
end

% Get the components:
Fc = F.components;
Gc = G.components;

% Do cross:
H = [ Fc{2} .* Gc{3} - Fc{3} .* Gc{2} ; ...
      Fc{3} .* Gc{1} - Fc{1} .* Gc{3} ; ...
      Fc{1} .* Gc{2} - Fc{2} .* Gc{1} ];

end


% plots a basic tire model given via the vector PacParam
function plotTireModel(PacParam, alpha_data, F_data)
  figure;
  grid on; hold on;
  if(nargin > 2)
    % if tire model data is available, plot it
    scatter(alpha_data, F_data, 10);
    alphaMax = max(alpha_data);
    alphaMin = min(alpha_data);
  else
    alphaMax = 0.5;
    alphaMin = -0.5;
  end
  alpha_sample = alphaMin:0.002:alphaMax;
  FyF = PacParam(3).*sin(PacParam(2).*atan(PacParam(1).*alpha_sample - PacParam(4).*(PacParam(1).*alpha_sample - atan(PacParam(1).*alpha_sample))));
  scatter(alpha_sample, FyF, 'LineWidth', 2);
  xlabel('Side slip angle in rad');
  ylabel('Tire force in N');
  ylim([-6000, 6000]);
  hold on;
  plot(alpha_sample, PacParam(3).*sin(PacParam(2).*atan(PacParam(1).*alpha_sample)));
  legend('Detailed Model', 'Simple Model for Control');
end


function [beta, bo] = svm_multi_predK(X,Y,C,K)
% SVM_MULTI_PREDK
%
% Support Vector Multi Classification
%
% USAGE: [beta, bo] = svm_multi_pred(X,Y,C,K)
%
% PARAMETERS:  X      - (m,d) matrix of m Training inputs in R^d
%
%              Y      - m vector of m Training targets in {1,..,Q}
%
%              C      - Trade-off between regularization and empirical error
%
%              K      - (m,m) Gram matrix
%
%              beta   - (m,Q) Coefficients matrix of the expansion of
%                       the vectors w_i over the x_p's
%                       w_i = \sum_p beta(p,i)x_p
%
%              bo     - (Q,1) bias terms
%
% SUBROUTINES: svm_multi_init.m -> initialization of the optimization problem
%             squadsolve.m          -> optimization
%             compute_kernel.m -> computation of the gram matrix
%
% DESCRIPTION:
%
%              This procedure is the implementation of a multiclass SVM corresponding to
%              the following problem
%
%              \min_{w_i,w_j} \sum_{i\neq j} \|w_i - w_j\|^2 + C\sum_{p,j} \xi_{pj}
%
%              subject to :
%               for all p=1,..,m   ( w_{c(p)} - w_j ).x_p + b_{c(p)} - b_j \geq 1 - \xi_{pj}
%                                  \xi_{pj} \geq 0
%
%      where:  m is the number of training points
%              Q is the number of classes
%              i and j are indices between 1 and Q
%              p is an index between 1 and m
%              w.x is the dot product between w and x
%
%              To solve this problem, a direct approach is used. All the gram matrix K (K_{i,j} =x_i.x_j)
%              is computed before beginning the optimization. The memory cost is thus proportionnal to Q^2*m^2.
%              The solution is not approximated but is provided by a quadratic programming method implemented in
%              squadsolve.m. Instead of minimizing the primal objective function, we maximize the dual. Its
%              formulation is not given here.
%
%
% ERRORS AND BUGS:
%              It is recommended not to use this code for large problems: unless your computer has a huge memory,
%              the procedure will stop with the message: 'memory exhausted'. Use the code msvm_fw.m or chunk_msvm.m
%              instead. Sometimes, the bias may not be computable because the term 'bias_eps' (see below) is too large.
%              In that case, decrease 'bias_eps' and it should work. In all cases, the program will output the bias and
%              try to give the best solution.
%
% EXAMPLE OF USE:
%
%      > [beta,bo] = svm_multi_pred(inputs,targets,10,K);
%
%       -> solve the multiclass optimization problem for a learning set S = \cup_i {(inputs(i,:),targets(i))}
%          with C=10, and with the gram matrix K.
%
% Description of variables:
%
%      kernel = 0,1,2 (integer)    -> type of kernel
%      m              (integer)    -> number of input points
%      Q              (integer)    -> number of classes
%      bias_eps       (real)       -> precision for the computation of the bias
%      reg_eps        (real)       -> to avoid ill conditionning, a diagonal matrix
%                                     is added to the hessian of the minimization problem,
%                                     reg_eps scales this diagonal matrix. (usual < 10^(-6))
%      A              ((Q,Q*m) matrix) -> equality constraint matrix
%      H            ((Q*m,Q*m) matrix) -> hessian of the objective function
%      c               (Q*m vector)    -> linear part of the objectif function
%      alpha           (Q*m vector)    -> variables of the optimization problem
%      constraints     (Q*m vector)    -> store 1-(w_{c(p)} - w_j).x_p
%      beta            ((m,Q) matrix)  -> vector w_i is computed as w_i = sum_p beta_{pi} x_p
%      b               ((Q,Q) matrix)  -> store the differences between bias: b(i,j) = b_i - b_j
% Avoid warning messages
warning off;
disp('Optimizing...');
% Test if nargin is correct
if (nargin < 2) | (nargin > 6),
help svm_multi_pred;
else

% Init
 empty_list_elements_ok = 1;
 m = size(X,1);
 Q = max(Y);
 if (nargin<3),
     C=Inf;
 end;
 if (C==Inf)
    tol = 1e-5;
  else
    tol = C*1e-6;
 end;
 bias_eps=10^(-8)/C;
 reg_eps = 10^(-6);
 sv_eps=10^(-6);
% Message
 % disp(sprintf('\nMulti Support Vector Classification\n'));
 % disp(sprintf('-----------------------------------\n'));

% Set up the parameters for the Optimisation problem
%disp(sprintf('Initialization...\n'));
[H,A,c]=svm_multi_init(Y,K);
% Add small amount of zero order regularization to
% avoid problems when Hessian is badly conditioned.
H = 1/(2*Q^2)*H+reg_eps*eye(size(H));% the problem is to minimize (1/4Q^2)x'Hx - c.x
c = -c;
% Some variables are irrelevant. Consider only relevant variables
mul = Q*(0:m-1);
indx = mul' + Y;
s1 = (1:Q*m);
s2 = (indx);
indx = setdiff(s1,s2)';
clear s1;clear s2;clear mul;
% Solve the optimization problem
% the solution is stored in alpha
%disp(sprintf('Begin the optimization ...\n'));
bo = zeros(size(A,1),1);
[x1,y] = quadsolve(H(indx,indx),c(indx),A(:,indx),bo,C);
x1=x1(1:length(indx));
obj = 0.5*x1'*(H(indx,indx)-reg_eps*eye(length(indx)))*x1 + c(indx)'*x1;
clear H;clear c;
alpha = zeros(Q*m,1);
alpha(indx)=x1;
clear x1;
% Output the objective value
%disp(sprintf('Final objective function: %f \n',-obj));
% if the alpha's are all greater than C then, C can be considered as infinite
% (for the computation of the bias)
if (max(abs(alpha)) < C*0.95)
    C==Inf;
end;
% Compute the number of support vector
% Can be removed if not desired
M = max(alpha);
nsv=0;
for i=1:m,
  if ~(isempty(find(alpha(Q*(i-1)+1:Q*i)>100*eps)))
    nsv = nsv+1;
  end;
end;
%disp(sprintf('Support vectors : %d (%3.1f)\n',nsv,100*nsv/(m)));
% Compute the coefficients beta_ip of the vector w_i = sum_p beta_ip x_p
% clean alpha
beta=zeros(m,Q);
notvoid = find(alpha>=100*eps);
alpha_tmp=zeros(Q*m,1);
alpha_tmp(notvoid) = alpha(notvoid);
alpha=alpha_tmp;
clear notvoid;clear alpha_tmp;
% compute beta
for i =1:Q,
 for p=1:m,
  tmp=0;
  if (i==Y(p))
   tmp = tmp + sum(alpha((p-1)*Q + 1: (p-1)*Q + Q))/Q;
  end;
  beta(p,i) = tmp - alpha((p-1)*Q + i)/Q;
 end;
end;
% Computation of the bias
%disp(sprintf('\n Computation of the bias........\n'));
% Computation of the outputs 1-(w_{c(i)} - w_j).x_i
errorcache = zeros(Q*m,1);
for p=1:m,
  for i=1:Q,
    temp = ((beta(:,Y(p))-beta(:,i))'*K(:,p));
    errorcache((p-1)*Q+i)=temp;
  end;
end;

  % Computation of the output 1-(w_Y(p)-w_i).x_p
  constraints=1-errorcache;
   if C==Inf,
     b=-Inf*ones(Q,Q);
     for i=1:m,
         for k=1:Q,
            b(Y(i),k) = max([b(Y(i),k),(constraints(Q*(i-1)+k) - 0.05)]);
         end;
     end;
     for k=1:Q,
         b(k,k)=0;
         b(k,find(b(k,:)==-Inf))=0;
     end;
   end; %if C>max(alpha),
%%% For finite C
if C<Inf,
% computation of alpha_bias
  % here: computing of the bias that minimize the \sum_{ip} \xi_{pi}
  % subject to linear constraints:
  % (w_{c(p)} - w_i).x_p + b_{c(p)} - b_i >= 1 - \xi_{pi}
  % \xi_{pi}
  % This is a linear program whose dual is easy to compute. It can be
  % solved with the slinearsolve method.
  alpha_bias=zeros(Q*m,1);
  c_un = ones(Q*m,1);
  [alpha_bias(indx)] = slinearsolve(errorcache(indx)-c_un(indx),A(1:(Q-1),indx),zeros(Q-1,1),1);

  % clean alpha_bias
  tmp = 0:m-1;
  tmp = tmp*Q;
  tmp = tmp + Y';
  alpha_bias(tmp)=0;

  % if problem, consider the alpha instead
  if (flag~=1)
    alpha_bias=alpha;
  end;

  b=zeros(Q,Q); % matrix of the difference b_i-b_j
  % Computation of the output 1-(w_Y(p)-w_i).x_p
  constraints=1-errorcache;

  % consider the alpha_ip s.t. 0 < alpha_bias_ip < 1
  % here, i consider alpha_new, because i compute the bias
  % that minimize the l_1 error of the current solution
  % and not of the optimal.
  if C == Inf,
    Cm =1;
  else
    Cm=C;
  end;
  if (flag~=1)
   indice_fix = find(alpha_bias < C*(1-bias_eps) & alpha_bias > Cm*bias_eps);
  else
   indice_fix = find(alpha_bias < (1-bias_eps) & alpha_bias > bias_eps);
  end;

  % if there are no such alpha -> problem
  if isempty(indice_fix)
    disp('WARNING : Unable to compute the bias, use another method\n');
  end;
  % the bias are computed by averaging
  % the matrix 'compteur' counts when  b_i-b_j is computed
  compteur=zeros(Q,Q);
  for ind=indice_fix',
    % find the index Q and p corresponding to ind
    indQ = rem(ind,Q);
    if (indQ==0), indQ=Q;end;
    indp = (ind-indQ)/Q;
    compteur(Y(indp+1),indQ)=compteur(Y(indp+1),indQ)+1;
    b(Y(indp+1),indQ) = b(Y(indp+1),indQ) + constraints(ind);
  end;
  % average by dividing by compteur
  for i=1:Q,
    b(i,i) = 0;
    for j=1:Q,
      if (compteur(i,j))
       b(i,j)=b(i,j)/compteur(i,j);
      end;
    end;
  end;
  end; %if C...,
  clear indice_fix;clear constraints;

  % transform b in order to be symetric
  for i=1:Q,
    for j=i+1:Q,
      if (b(i,j)==0)
        b(i,j)=-b(j,i);
      end;
      if (b(j,i)==0)
        b(j,i)=-b(i,j);
      end;
    end;
  end;

  % average between both symetric elements
  if C==Inf,
   for i=1:Q,
    for j=i+1:Q,
        b(i,j)=max(b(i,j),-b(j,i));
        b(j,i)=-b(i,j);
    end;
  end;
  else,
  for i=1:Q,
    for j=i+1:Q,
        b(i,j)=(b(i,j)-b(j,i))/2;
        b(j,i)=-b(i,j);
    end;
  end;
  end;

  % compute the values of b_i s.t. sum_i b_i = 0
  ok=0;% ok > 0, if computation is possible otherwise ok=0
  b_guess = zeros(Q,1); % if no computation is possible still give a guess
  bo = zeros(Q,1); % the bias vector
  for i=1:Q,
    z = find(b(i,:)~=0);
    if (length(z)==Q-1)
      ok=ok+1;
      temp = sum(b(i,:))/Q;
      for j=1:Q,
        bo(j) =bo(j)+temp-b(i,j);
      end;
    else
      %disp(sprintf('Warning : bias %d -> problem... %d\n',i,length(z)));
      if length(z),
        b_guess(i) = sum(b(i,:))/length(z);
      end;
      for j=1:length(z),
        b_guess(z(j)) =b_guess(i) -b(i,z(j));
      end;
    end;
  end;

  if ~ok
    % then one should compute the bias with another method
    disp('WARNING : Problem with the bias, use another method');
    bo=b_guess;
  else
    bo =bo/ok;
  end;

end; %if nargin...


function [mmHg] = ftH2O2mmHg(ftH2O)
% Convert pressure from feet of water column at 4 degrees to millimeters of
% mercury at 0 degrees C
% Chad Greene 2012
mmHg = ftH2O*22.4198;


%BZreaction animation
%Belousov-Zhabotinsky Reaction animation
%This MATLAB code is converted from Processing code available in this link
%http://www.aac.bartlett.ucl.ac.uk/processing/samples/bzr.pdf

%version 2. Corrected the drift of pixels as suggested
%           by Jonh.

xres=75; %x resolution
yres=75; %y resolution

a=rand(xres,yres,2);
b=rand(xres,yres,2);
c=rand(xres,yres,2);
c_a = zeros(xres,yres);
c_b = zeros(xres,yres);
c_c = zeros(xres,yres);
p = 1;
q = 2;
img=zeros(xres,yres,3);

for k=1:100
         c_a = 0*c_a;
         c_b = 0*c_b;
         c_c = 0*c_c;
         for m=1:xres
            for n=1:yres
               for mm = m:m+2
                  for nn = n:n+2
                    c_a(m,n) =c_a(m,n)+ a( mod(mm+xres,xres)+1 ,mod(nn+yres,yres)+1 ,p);
                    c_b(m,n) =c_b(m,n)+ b( mod(mm+xres,xres)+1 ,mod(nn+yres,yres)+1 ,p);
                    c_c(m,n) =c_c(m,n)+ c( mod(mm+xres,xres)+1 ,mod(nn+yres,yres)+1 ,p);
                  end
               end
            end
         end

        %correction of pixel drift
        c_a = circshift(c_a,[2 2]);
        c_b = circshift(c_b,[2 2]);
        c_c = circshift(c_c,[2 2]);

        c_a =c_a/ 9.0;
        c_b =c_b/ 9.0;
        c_c =c_c/ 9.0;

        a(:,:,q) = double(uint8(255*(c_a + c_a .* (c_b - c_c))))/255;
        b(:,:,q) = double(uint8(255*(c_b + c_b .* (c_c - c_a))))/255;
        c(:,:,q) = double(uint8(255*(c_c + c_c .* (c_a - c_b))))/255;

        img(:,:,1)=c(:,:,q);
        img(:,:,2)=b(:,:,q);
        img(:,:,3)=a(:,:,q);

        if  p == 1
          p = 2; q = 1;
        else
          p = 1; q = 2;
        end

        image(uint8(255*hsv2rgb(img)))
        axis equal off
        drawnow
end


function [M,Id] = compute_texture_patchwork(H,n, options)

% compute_texture_patchwork - mix several textures
%
%   M = compute_texture_patchwork(H,n,options);
%
%   H can be a cell array of textures or a (n,n,3) matrix.
%   Handles up to 5 textures.
%
%   To generate a random partition, set options.patchwork_mode='random'.
%
%   Copyright (c) 2006 Gabriel Peyre

if not(iscell(H))
    H1 = H; H = {};
    for i=1:size(H1,3)
        H{i} = H1(:,:,i);
    end
end

options.null = 0;
mode = getoptions(options, 'patchwork_mode', 'deterministic');

if nargin<2
    n = size(H{1},1);
end

p = length(H); % number of textures

if strcmp(mode, 'random')

    sigma = getoptions(options, 'patchwork_sigma', 60);
    J = floor(log2(p));
    A = ones(n);
    for j=0:J-1
        B = A;
        for k=1:2^j
            I = find(B==k);
            U = perform_blurring(randn(n),sigma,options);
            s = median(U(I));
            I1 = find( (B==k) & (U>s) );
            I2 = find( (B==k) & (U<=s) );
            A(I1) = 2*k-1;
            A(I2) = 2*k;
        end
    end

    M = zeros(n);
    Id = zeros(n);
    for i=1:p
        B = H{i};
        if size(B,1)<n
            B = perform_image_extension(B,n);
        end
        B = B(1:n,1:n);
        M(A==i) = B(A==i);
        Id(A==i) = i;
    end
    return;

end

m = min(size(H{1},1),n);

M = zeros(n);
M(1:m,1:m) = H{1}(1:m,1:m);
Id = ones(n);

if p==2
    M(end/2+1:end,:) = H{2}(end-n/2+1:end,:);
    Id(end/2+1:end,:) = 2;
    return;
end

if p>1
    M(1:end/2,end/2+1:end) = H{2}(1:1:n/2,1:n/2);
    Id(1:end/2,end/2+1:end) = 2;
end
if p>2
    M(end/2+1:end,1:end/2) = H{3}(1:1:n/2,1:n/2);
    Id(end/2+1:end,1:end/2) = 3;
end
if p>3
    M(end/2+1:end,end/2+1:end) = H{4}(1:1:n/2,1:n/2);
    Id(end/2+1:end,end/2+1:end) = 4;
end
if p>4
    r = 1;
    A = H{5}(1:1:n/2,1:n/2);
    x = linspace(-1,1,n/2)';
    [Y,X] = meshgrid(x,x);
    J = find( X.^2 + Y.^2 <= r^2 );
    x  = [ones(n/4,1)*Inf; x; ones(n/4,1)*Inf];
    [Y,X] = meshgrid(x,x);
    I = find( X.^2 + Y.^2 <= r^2 );
    M(I) = A(J);
    Id(I) = 5;
end


function [SIG,BAK,OVL]= comp_fwseg_variant(cleanFile, enhancedFile);

% ----------------------------------------------------------------------
%      Frequency-variant fwSNRseg Objective Speech Quality Measure
%
%   This function implements the frequency-variant fwSNRseg measure [1]
%   (see also Chap. 10, Eq. 10.24)
%
%
%   Usage:  [sig,bak,ovl]=comp_fwseg_variant(cleanFile.wav, enhancedFile.wav)
%
%         cleanFile.wav - clean input file in .wav format
%         enhancedFile  - enhanced output file in .wav format
%         sig           - predicted rating [1-5] of speech distortion
%         bak           - predicted rating [1-5] of noise distortion
%         ovl           - predicted rating [1-5] of overall quality
%
%
%  Example call:  [s,b,o] =comp_fwseg_variant('sp04.wav','enhanced.wav')
%
%
%  References:
%     [1] S. R. Quackenbush, T. P. Barnwell, and M. A. Clements,
%	    Objective Measures of Speech Quality.  Prentice Hall
%	    Advanced Reference Series, Englewood Cliffs, NJ, 1988,
%	    ISBN: 0-13-629056-6.
%
%   Author: Philipos C. Loizou
%  (critical-band filtering routines were written by Bryan Pellom & John Hansen)
%
% Copyright (c) 2006 by Philipos C. Loizou
% $Revision: 0.0 $  $Date: 10/09/2006 $
% ----------------------------------------------------------------------

if nargin~=2
    fprintf('USAGE: [sig,bak,ovl]=comp_fwseg_variant(cleanFile.wav, enhancedFile.wav)\n');
    fprintf('For more help, type: help comp_fwseg_variant\n\n');
    return;
end

[data1, Srate1]= audioread(cleanFile);
[data2, Srate2]= audioread(enhancedFile);
% if ( Srate1~= Srate2) | ( Nbits1~= Nbits2)
%     error( 'The two files do not match!\n');
% end

len= min( length( data1), length( data2));
data1= data1( 1: len)+eps;
data2= data2( 1: len)+eps;

wss_dist_matrix= fwseg( data1, data2,Srate1);
wss_dist=mean(wss_dist_matrix);

% initialize  coefficients obtained from multiple linear
% regression analysis
%
b_sig=[0.021,-0.028,0.088,-0.031,0.048,-0.049,0.065,0.009,0.011,0.033,...
    -0.040,-0.002,0.041,-0.007,0.033,0.018,-0.007,0.044,-0.001,0.021,...
    -0.002,0.017,-0.03,0.073,0.043];
b_ovl=[-0.003,-0.026,0.066,-0.036,0.038,-0.023,0.037,0.022,0.014,0.009,...
    -0.03,0.004,0.044,-0.005,0.017,0.018,-0.001,0.051,0.009,0.011,...
    0.011,-0.002,-0.021,0.043,0.031];
b_bak=[-0.03,-0.022,0.03,-0.048,0.034,0.002,0.006,0.037,0.017,-0.016,-0.008,...
    0.019,0.024,-0.002,0.01,0.03,-0.018,0.046,0.022,0.005,0.03,-0.028,...
    -0.028,0.019,0.005];

SIG=0.567+sum(b_sig.*wss_dist);
SIG=max(1,SIG); SIG=min(5, SIG); % limit values to [1, 5]

BAK=1.013+sum(b_bak.*wss_dist);
BAK=max(1,BAK); BAK=min(5, BAK); % limit values to [1, 5]

OVL=0.446+sum(b_ovl.*wss_dist);
OVL=max(1,OVL); OVL=min(5, OVL); % limit values to [1, 5]

% ----------------------------------------------------------------------

function distortion = fwseg(clean_speech, processed_speech,sample_rate)

% ----------------------------------------------------------------------
% Check the length of the clean and processed speech.  Must be the same.
% ----------------------------------------------------------------------

clean_length      = length(clean_speech);
processed_length  = length(processed_speech);

if (clean_length ~= processed_length)
  disp('Error: Files  must have same length.');
  return
end

% ----------------------------------------------------------------------
% Global Variables
% ----------------------------------------------------------------------

winlength   = round(30*sample_rate/1000); 	   % window length in samples
skiprate    = floor(winlength/4);		   % window skip in samples
max_freq    = sample_rate/2;	   % maximum bandwidth
num_crit    = 25;		   % number of critical bands

n_fft       = 2^nextpow2(2*winlength);
n_fftby2    = n_fft/2;		   % FFT size/2

% ----------------------------------------------------------------------
% Critical Band Filter Definitions (Center Frequency and Bandwidths in Hz)
% ----------------------------------------------------------------------

cent_freq(1)  = 50.0000;   bandwidth(1)  = 70.0000;
cent_freq(2)  = 120.000;   bandwidth(2)  = 70.0000;
cent_freq(3)  = 190.000;   bandwidth(3)  = 70.0000;
cent_freq(4)  = 260.000;   bandwidth(4)  = 70.0000;
cent_freq(5)  = 330.000;   bandwidth(5)  = 70.0000;
cent_freq(6)  = 400.000;   bandwidth(6)  = 70.0000;
cent_freq(7)  = 470.000;   bandwidth(7)  = 70.0000;
cent_freq(8)  = 540.000;   bandwidth(8)  = 77.3724;
cent_freq(9)  = 617.372;   bandwidth(9)  = 86.0056;
cent_freq(10) = 703.378;   bandwidth(10) = 95.3398;
cent_freq(11) = 798.717;   bandwidth(11) = 105.411;
cent_freq(12) = 904.128;   bandwidth(12) = 116.256;
cent_freq(13) = 1020.38;   bandwidth(13) = 127.914;
cent_freq(14) = 1148.30;   bandwidth(14) = 140.423;
cent_freq(15) = 1288.72;   bandwidth(15) = 153.823;
cent_freq(16) = 1442.54;   bandwidth(16) = 168.154;
cent_freq(17) = 1610.70;   bandwidth(17) = 183.457;
cent_freq(18) = 1794.16;   bandwidth(18) = 199.776;
cent_freq(19) = 1993.93;   bandwidth(19) = 217.153;
cent_freq(20) = 2211.08;   bandwidth(20) = 235.631;
cent_freq(21) = 2446.71;   bandwidth(21) = 255.255;
cent_freq(22) = 2701.97;   bandwidth(22) = 276.072;
cent_freq(23) = 2978.04;   bandwidth(23) = 298.126;
cent_freq(24) = 3276.17;   bandwidth(24) = 321.465;
cent_freq(25) = 3597.63;   bandwidth(25) = 346.136;

bw_min      = bandwidth (1);	   % minimum critical bandwidth

% ----------------------------------------------------------------------
% Set up the critical band filters.  Note here that Gaussianly shaped
% filters are used.  Also, the sum of the filter weights are equivalent
% for each critical band filter.  Filter less than -30 dB and set to
% zero.
% ----------------------------------------------------------------------

min_factor = exp (-30.0 / (2.0 * 2.303));       % -30 dB point of filter

for i = 1:num_crit
  f0 = (cent_freq (i) / max_freq) * (n_fftby2);
  all_f0(i) = floor(f0);
  bw = (bandwidth (i) / max_freq) * (n_fftby2);
  norm_factor = log(bw_min) - log(bandwidth(i));
  j = 0:1:n_fftby2-1;
  crit_filter(i,:) = exp (-11 *(((j - floor(f0)) ./bw).^2) + norm_factor);
  crit_filter(i,:) = crit_filter(i,:).*(crit_filter(i,:) > min_factor);
end

% ----------------------------------------------------------------------
% For each frame of input speech, calculate the Weighted Spectral
% Slope Measure
% ----------------------------------------------------------------------

num_frames = floor(clean_length/skiprate-(winlength/skiprate)); % number of frames
start      = 1;					% starting sample
window     = 0.5*(1 - cos(2*pi*(1:winlength)'/(winlength+1)));

distortion=zeros(num_frames,num_crit);
for frame_count = 1:num_frames

   % ----------------------------------------------------------
   % (1) Get the Frames for the test and reference speech.
   %     Multiply by Hanning Window.
   % ----------------------------------------------------------

   clean_frame = clean_speech(start:start+winlength-1);
   processed_frame = processed_speech(start:start+winlength-1);
   clean_frame = clean_frame.*window;
   processed_frame = processed_frame.*window;

   % ----------------------------------------------------------
   % (2) Compute the magnitude Spectrum of Clean and Processed
   % ----------------------------------------------------------

       clean_spec     = abs(fft(clean_frame,n_fft));
       processed_spec = abs(fft(processed_frame,n_fft));

       % normalize so that spectra have unit area ----
        clean_spec=clean_spec/sum(clean_spec(1:n_fftby2));
        processed_spec=processed_spec/sum(processed_spec(1:n_fftby2));

   % ----------------------------------------------------------
   % (3) Compute Filterbank Output Energies (in dB scale)
   % ----------------------------------------------------------

   clean_energy=zeros(1,num_crit);
   processed_energy=zeros(1,num_crit);
   error_energy=zeros(1,num_crit);

   for i = 1:num_crit
      clean_energy(i) = sum(clean_spec(1:n_fftby2) ...
		            .*crit_filter(i,:)');
      processed_energy(i) = sum(processed_spec(1:n_fftby2) ...
			        .*crit_filter(i,:)');
      error_energy(i)=max((clean_energy(i)-processed_energy(i))^2,eps);
   end

   SNRlog=10*log10((clean_energy.^2)./error_energy);

   distortion(frame_count,:)=min(max(SNRlog,-10),35);

   start = start + skiprate;

end


%% GSA_GetSy: calculate the Sobol' sensitivity indices
%
% Usage:
%   [S eS pro] = GSA_GetSy(pro, iset, verbose)
%
% Inputs:
%    pro                project structure
%    iset               cell array or array of inputs of the considered set, they can be selected
%                       by index (1,2,3 ...) or by name ('in1','x',..) or
%                       mixed
%    verbose            if not empty, it shows the time (in hours) for
%                       finishing
%
% Output:
%    S                  sensitivity coefficient
%    eS                 error of sensitivity coefficient
%    pro                project structure
%
% ------------------------------------------------------------------------
% See also
%
% Author : Flavio Cannavo'
% e-mail: flavio(dot)cannavo(at)gmail(dot)com
% Release: 1.0
% Date   : 15-02-2011
%
% History:
% 1.0  15-04-2011  Added verbose parameter
% 1.0  15-01-2011  First release.
%%

function [S eS pro] = GSA_GetSy(pro, iset, verbose)

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

index = fnc_SelectInput(pro, iset);

if isempty(index)
    S = 0;
    eS = 0;
else
    S = 0;
    eS = 0;
    n = length(index);
    L = 2^n;

    if verbose
        tic
    end
    for i=1:(L-1)
        ii = fnc_GetInputs(i);
        si = fnc_GetIndex(index(ii));
        if isnan(pro.GSA.GSI(si))

            %-------
            if isnan(pro.GSA.Di(si))

                ixi = fnc_GetInputs(si);
                s = length(ixi);
                l = 2^s - 1;

                %======
                if isnan(pro.GSA.Dmi(si))
                    n = length(pro.Inputs.pdfs);
                    N = size(pro.SampleSets.E,1);
                    H = pro.SampleSets.E(:,:);
                    cii = fnc_GetComplementayInputs(si, n);
                    H(:,cii) = pro.SampleSets.T(:,cii);
                    ff = nan(N,1);

                    for j=1:N
                        ff(j) = pro.GSA.fE(j)*(pro.Model.handle(H(j,:))-pro.GSA.mfE);
                    end

                    pro.GSA.Dmi(si)  = nanmean(ff);
                    pro.GSA.eDmi(si) = 0.6745*sqrt((nanmean(ff.^2) - pro.GSA.Dmi(si)^2)/sum(~isnan(ff)));
                end
                %=======

                Di = pro.GSA.Dmi(si);
                eDi = pro.GSA.eDmi(si)^2;

                for j=1:(l-1)
                    sii = fnc_GetInputs(j);
                    k = fnc_GetIndex(ixi(sii));
                    s_r = s - length(sii);
                    Di = Di + pro.GSA.Dmi(k)*((-1)^s_r);
                    eDi = eDi + pro.GSA.eDmi(k)^2;
                end

                pro.GSA.Di(si) = Di + (pro.GSA.f0^2)*((-1)^s);
                pro.GSA.eDi(si) = sqrt(eDi + 2*(pro.GSA.ef0^2));

            end
            %------
            pro.GSA.GSI(si) = pro.GSA.Di(si)/pro.GSA.D;
            pro.GSA.eGSI(si) = pro.GSA.GSI(si)*pro.GSA.eDi(si)/pro.GSA.D;
        end
        S = S + pro.GSA.GSI(si);
        eS = eS + pro.GSA.eGSI(si);

        if verbose
            timelapse = toc;
            disp(timelapse*(L-1-i)/i/60/60);
        end
    end
end


function a = spiral(hyper)

%=================================================================================
% TOY SPIRAL data generation object
%=================================================================================
% A=spiral(H) returns a spiral toy data object initialized with hyperparameters H.
%
% This generates 2 spirals with 4*m points per spiral, where 1*m points are
% points on two perfect spiral and 3*m points are some additive noise on
% the first quarter.
%
%
% Hyperparameters, and their defaults
%  n=1           --  a parameter :-) play to explore
%  m=50          --  number of used points per winding
%
% Model
%
% Methods:
%  generate,train,test
%  Example :
%   d=gen(spiral({'n=1','m=50'}));
%   [r s0]=train(svm(kernel('rbf',1)),d)
%   plot(s0,[ -20 20 -20 20]);
%=================================================================================
% Reference :
% Author    :
% Link      :
%=================================================================================

  a.m=50;
  a.n=2;
  a.noise=1;

  p=algorithm('spiral');
  a= class(a,'spiral',p);

  if nargin==1
    eval_hyper;
  end


classdef MMF7 < PROBLEM
% <multi> <real> <multimodal>
% Multi-modal multi-objective test function

%------------------------------- Reference --------------------------------
% C. Yue, B. Qu, and J. Liang, A multi-objective particle swarm optimizer
% using ring topology for solving multimodal multiobjective Problems, IEEE
% Transactions on Evolutionary Computation, 2018, 22(5): 805-817.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    properties
        POS;    % Pareto optimal set for IGDX calculation
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 2;
            obj.D = 2;
            obj.lower    = [1,-1];
            obj.upper    = [3,1];
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,X)
            PopObj(:,1) = abs(X(:,1)-2);
            PopObj(:,2) = 1 - sqrt(PopObj(:,1)) + (X(:,2)-(0.3.*(PopObj(:,1).^2).*cos(24.*pi.*PopObj(:,1)+4.*pi)+0.6.*PopObj(:,1)).*sin(6.*pi.*PopObj(:,1)+pi)).^2;
        end
        %% Generate Pareto optimal solutions
        function R = GetOptimum(obj,N)
            % Generate points in Pareto optimal set
            obj.POS(:,1) = linspace(1,3,N)';
            obj.POS(:,2) = (0.3*(obj.POS(:,1)-2).^2.*cos(24*pi*abs(obj.POS(:,1)-2)+4*pi)+0.6*abs(obj.POS(:,1)-2)).*sin(6*pi*abs(obj.POS(:,1)-2)+pi);
            % Generate points on Pareto front
            R(:,1) = linspace(0,1,N)';
            R(:,2) = 1 - sqrt(R(:,1));
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            R(:,1) = linspace(0,1,100)';
            R(:,2) = 1 - sqrt(R(:,1));
        end
        %% Calculate the metric value
        function score = CalMetric(obj,metName,Population)
            switch metName
                case 'IGDX'
                    score = feval(metName,Population,obj.POS);
                otherwise
                    score = feval(metName,Population,obj.optimum);
            end
        end
        %% Display a population in the objective space
        function DrawObj(obj,Population)
            PopDec = Population.decs;
            temp   = PopDec(:,1) <= 2;
            Draw(Population(temp).objs,'o','MarkerSize',6,'Marker','o','Markerfacecolor',[1 .5 .5],'Markeredgecolor',[1 .2 .2],{'\it f\rm_1','\it f\rm_2',[]});
            Draw(Population(~temp).objs+0.1,'o','MarkerSize',6,'Marker','o','Markerfacecolor',[.5 .5 1],'Markeredgecolor',[.2 .2 1]);
            Draw(obj.PF,'-','LineWidth',1,'Color',[1 .2 .2]);
            Draw(obj.PF+0.1,'-','LineWidth',1,'Color',[.2 .2 1]);
        end
    end
end


function [P]=tesSmoothPosNeg(TES,V,IND_V,cPar)

%% CONTROL PARAMETERS

if isfield(cPar,'LambdaSmooth')
    LambdaSmooth=cPar.LambdaSmooth;
else
    LambdaSmooth=0.5; %DEFAULT
end

if isfield(cPar,'n')
    nMax=cPar.n;
else
    nMax=1; %DEFAULT
end

if isfield(cPar,'RigidConstraints')
    indRigid=cPar.RigidConstraints;
else
    indRigid=[]; %DEFAULT
end

if isfield(cPar,'Tolerance')
    SSQD_Tol=cPar.Tolerance;
else
    SSQD_Tol=[]; %DEFAULT
end

%%

if isempty(IND_V)
    [~,IND_V]=patchIND(TES,V);
end
logicValid=IND_V>0;

nDims=size(V,2); %Number of dimensions

VP=NaN(size(IND_V,1),size(IND_V,2),nDims);

P=V;
PP=V;
Q=V;
if ~isempty(SSQD_Tol)
    SSQD_old=[];
    SSQD_ratio=0;
end

for qIter=1:nMax;

    %% SIMPLE LAPLACIAN SMOOTHENING

    %Loop for all dimensions
    for qDim=1:1:nDims
        Xp=VP(:,:,qDim);
        Xp(logicValid)=P(IND_V(logicValid),qDim);
        Xp=gnanmean(Xp,2);
        PP(:,qDim)=Xp;
    end

    %Switch sign every iteration to partialy avoid shrinkage
    if iseven(qIter)
        wFac=1;
    else
        wFac=1;
    end

    P=P+wFac.*LambdaSmooth.*(PP-P);

    %%

    %Put back constrained points
    if ~isempty(indRigid)
       P(indRigid,:)=V(indRigid,:);
    end

    if ~isempty(SSQD_Tol)
        %Compute sum of squared differences with respect to previous iteration
        SSQD_new=gnansum((P(:)-Q(:)).^2);
        if ~isempty(SSQD_old)
            SSQD_ratio=SSQD_new./SSQD_old;
        end

        %Store current metrics
        Q=P;
        SSQD_old=SSQD_new;
        if abs(1-SSQD_ratio)<=SSQD_Tol
           break %STOP SMOOTHING LOOP IF TOLERANCE IS REACHED
        end
    end

end

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