function [  net,res,opts ] = sgd2(  net,res,opts )
% Stochastic gradient descent using second-order information.
%   Ye, C., Yang, Y., Fermuller, C., & Aloimonos, Y. (2017).
%   On the Importance of Consistency in Training Deep Neural Networks. arXiv preprint arXiv:1708.00631.

    if ~isfield(opts.parameters,'weightDecay')
        opts.parameters.weightDecay=1e-4;
    end
    if ~isfield(opts.parameters,'lambda_sgd2')
        opts.parameters.lambda_sgd2=1e0;
    end
    if ~isfield(opts.parameters,'large_matrix_inversion')
        opts.parameters.large_matrix_inversion=0;
    end
    if ~isfield(opts.parameters,'max_inv_size')
        opts.parameters.max_inv_size=500;
    end
    if ~isfield(opts.parameters,'clip')
        opts.parameters.clip=0;
    end
    if ~isfield(opts.parameters,'decorr_bias')
        opts.parameters.decorr_bias=1;
    end
    if ~isfield(net,'iterations')||(isfield(opts,'reset_mom')&&opts.reset_mom==1)
        net.iterations=0;
    end

    net.iterations=net.iterations+1;

    mom_factor=(1-opts.parameters.mom.^net.iterations);
    max_inv_size=opts.parameters.max_inv_size;
    lambda=opts.parameters.lambda_sgd2;

    for layer=1:numel(net.layers)
        if isfield(net.layers{layer},'weights')&&~isempty(net.layers{layer}.weights)
            if ~isfield(net.layers{layer},'momentum')||(isfield(opts,'reset_mom')&&opts.reset_mom==1)
                net.layers{layer}.momentum{1}=zeros(size(net.layers{layer}.weights{1}),'like',net.layers{layer}.weights{1});
                net.layers{layer}.momentum{2}=zeros(size(net.layers{layer}.weights{2}),'like',net.layers{layer}.weights{2});
            end

            dzdw=res(layer).dzdw;
            dzdb=res(layer).dzdb;

            if length(net.layers{layer}.weights)==2
                x=res(layer).x;
                batch_dim=length(size(x));%This assumes the batch size must be >1
                if batch_dim==4%2d cnn
                    x=permute(x,[3,1,2,4]);x=reshape(x,size(x,1),[]);
                    dzdw=permute(dzdw,[1,2,4,3]);new_size=size(dzdw);dzdw=reshape(dzdw,prod(new_size(1:3)),new_size(4));
                    K=size(dzdw,1)/numel(dzdb);dzdb=repelem(dzdb(:),K,1);
                end
                if batch_dim==3%1d cnn
                    x=permute(x,[2,1,3]);x=reshape(x,size(x,1),[]);
                    dzdw=permute(dzdw,[1,3,2]);new_size=size(dzdw);dzdw=reshape(dzdw,prod(new_size(1:2)),new_size(3));
                    K=size(dzdw,1)/numel(dzdb);dzdb=repelem(dzdb(:),K,1);
                end
                subsample=1;batch_size=size(x,2);
                if batch_size>1e4,subsample=ceil(min(50,batch_size/1e4));end
                if subsample>1,x=x(:,1:subsample:end);end
                if opts.parameters.decorr_bias==1
                    %insert bias
                    x=[ones(1,size(x,2),'like',x);x];
                    dzdw=[dzdb,dzdw];
                end
                if size(dzdw,2)<=max_inv_size %small scale inversion
                    dzdw=dzdw/(x*x'./size(x,2)+lambda*eye(size(x,1),'like',x));
                elseif opts.parameters.large_matrix_inversion %divide large scale into smaller scale
                    order=randperm(size(dzdw,2));
                    for i=1:max_inv_size:length(order) %could have been parallelized
                       block_size=min(max_inv_size,length(order)-i+1);
                       idx=order(i:i+block_size-1);x_tmp=x(idx,:);
                       dzdw(:,idx)=dzdw(:,idx)/(x_tmp*x_tmp'./size(x_tmp,2)+lambda*eye(size(x_tmp,1),'like',x));
                    end
                end
                if opts.parameters.decorr_bias==1
                    dzdb=dzdw(:,1);dzdw(:,1)=[];
                end
                if batch_dim==4,dzdw=reshape(dzdw,new_size);dzdw=permute(dzdw,[1,2,4,3]);end
                if batch_dim==3,dzdw=reshape(dzdw,new_size);dzdw=permute(dzdw,[1,3,2]);end
                if batch_dim>2%for cnn:
                    %dzdb is decorrelated with dzdw, take average to smooth the results.
                    dzdb=reshape(mean(reshape(dzdb(:),K,[]),1),size(res(layer).dzdb));
                end
            end

            if opts.parameters.clip>0
                mask=abs(res(layer).dzdw)>opts.parameters.clip;
                res(layer).dzdw(mask)=sign(res(layer).dzdw(mask)).*opts.parameters.clip;%%this type of processing seems to be very helpful
                mask=abs(res(layer).dzdb)>opts.parameters.clip;
                res(layer).dzdb(mask)=sign(res(layer).dzdb(mask)).*opts.parameters.clip;
            end

            %sgd updates
            net.layers{layer}.momentum{1}=opts.parameters.mom.*net.layers{layer}.momentum{1}-(1-opts.parameters.mom).*dzdw - opts.parameters.weightDecay * net.layers{layer}.weights{1};
            net.layers{layer}.weights{1}=net.layers{layer}.weights{1}+opts.parameters.lr*net.layers{layer}.momentum{1}./mom_factor;

            net.layers{layer}.momentum{2}=opts.parameters.mom.*net.layers{layer}.momentum{2}-(1-opts.parameters.mom).*dzdb;
            net.layers{layer}.weights{2}=net.layers{layer}.weights{2}+opts.parameters.lr*net.layers{layer}.momentum{2}./mom_factor;

        end
    end

    if ~isfield(opts,'reset_mom')||opts.reset_mom==1
        opts.reset_mom=0;
    end
end


function check = r8sp_check ( m, n, nz_num, row, col )

%*****************************************************************************80
%
%% R8SP_CHECK checks that a R8SP matrix data structure is properly sorted.
%
%  Discussion:
%
%    This routine assumes that the data structure has been sorted,
%    so that the entries of ROW are ascending sorted, and that the
%    entries of COL are ascending sorted, within the group of entries
%    that have a common value of ROW.
%
%    The R8SP storage format stores the row, column and value of each nonzero
%    entry of a sparse matrix.
%
%    The R8SP format is used by CSPARSE ("sparse triplet"), DLAP/SLAP
%    ("nonsymmetric SLAP triad"), by MATLAB, and by SPARSEKIT ("COO" format).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 July 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns of
%    the matrix.
%
%    Input, integer NZ_NUM, the number of nonzero elements in
%    the matrix.
%
%    Input, integer ROW(NZ_NUM), COL(NZ_NUM), the row and
%    column indices of the nonzero elements.
%
%    Output, logical CHECK, is TRUE if the matrix is properly defined.
%
  check = 1;
%
%  Check 1 <= ROW(*) <= M.
%
  for k = 1 : nz_num

    if ( row(k) < 1 | m < row(k) )
      check = 0;
      return
    end

  end
%
%  Check 1 <= COL(*) <= N.
%
  for k = 1 : nz_num

    if ( col(k) < 1 | n < col(k) )
      check = 0;
      return
    end

  end
%
%  Check that ROW(K) <= ROW(K+1).
%
  for k = 1 : nz_num - 1

    if ( row(k+1) < row(k) )
      check = 0;
      return
    end

  end
%
%  Check that, if ROW(K) == ROW(K+1), that COL(K) < COL(K+1).
%
  for k = 1 : nz_num - 1

    if ( row(k) == row(k+1) )
      if ( col(k+1) <= col(k) )
        check = 0;
        return
      end
    end

  end

  return
end


function [dataout] = ft_denoise_dssp(cfg, datain)

% FT_DENOISE_DSSP implements a dual signal subspace projection algorithm
% to suppress interference outside a predefined source region of
% interest. It is based on: Sekihara et al. J. Neural Eng. 2016 13(3), and
% Sekihara et al. J. Neural Eng. 2018 15(3).
%
% Use as
%   dataout = ft_denoise_dssp(cfg, datain)
% where cfg is a configuration structure that contains
%   cfg.channel          = Nx1 cell-array with selection of channels (default = 'all'), see FT_CHANNELSELECTION for details
%   cfg.trials           = 'all' or a selection given as a 1xN vector (default = 'all')
%   cfg.pertrial         = 'no', or 'yes', compute the temporal projection per trial (default = 'no')
%   cfg.sourcemodel      = structure, source model with precomputed leadfields, see FT_PREPARE_LEADFIELD
%   cfg.demean           = 'yes', or 'no', demean the data per epoch (default = 'yes')
%   cfg.dssp             = structure with parameters that determine the behavior of the algorithm
%   cfg.dssp.n_space     = 'all', or scalar. Number of dimensions for the
%                          initial spatial projection.
%   cfg.dssp.n_in        = 'all', or scalar. Number of dimensions of the
%                          subspace describing the field inside the ROI.
%   cfg.dssp.n_out       = 'all', or scalar. Number of dimensions of the
%                          subspace describing the field outside the ROI.
%   cfg.dssp.n_intersect = scalar (default = 0.9). Number of dimensions (if
%                          value is an integer>=1), or threshold for the
%                          included eigenvalues (if value<1), determining
%                          the dimensionality of the intersection.
%
% See also FT_DENOISE_PCA, FT_DENOISE_SYNTHETIC, FT_DENOISE_TSR

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

% these are used by the ft_preamble/ft_postamble function and scripts
ft_revision = '$Id$';
ft_nargin   = nargin;
ft_nargout  = nargout;

% do the general setup of the function
ft_defaults
ft_preamble init
ft_preamble debug
ft_preamble loadvar    datain
ft_preamble provenance datain

% the ft_abort variable is set to true or false in ft_preamble_init
if ft_abort
  % do not continue function execution in case the outputfile is present and the user indicated to keep it
  return
end

% check the input data
datain = ft_checkdata(datain, 'datatype', {'raw'}); % FIXME how about timelock and freq?

% ensure the external cellfunction toolbox is on the path
ft_hastoolbox('cellfunction', 1);

% check if the input cfg is valid for this function
cfg = ft_checkconfig(cfg, 'forbidden',  {'channels', 'trial'}); % prevent accidental typos, see issue 1729

% set the defaults
cfg.trials            = ft_getopt(cfg, 'trials',  'all', 1);
cfg.channel           = ft_getopt(cfg, 'channel', 'all');
cfg.pertrial          = ft_getopt(cfg, 'pertrial', 'no');
cfg.sourcemodel       = ft_getopt(cfg, 'sourcemodel');
cfg.demean            = ft_getopt(cfg, 'demean', 'yes');
cfg.dssp              = ft_getopt(cfg, 'dssp');         % sub-structure to hold the parameters
cfg.dssp.n_space      = ft_getopt(cfg.dssp, 'n_space', 'interactive'); % number of spatial components to retain from the Gram matrix
cfg.dssp.n_in         = ft_getopt(cfg.dssp, 'n_in',    'interactive'); % dimensionality of the Bin subspace to be used for the computation of the intersection
cfg.dssp.n_out        = ft_getopt(cfg.dssp, 'n_out',   'interactive'); % dimensionality of the Bout subspace to be used for the computation of the intersection
cfg.dssp.n_intersect  = ft_getopt(cfg.dssp, 'n_intersect', 'interactive'); % dimensionality of the intersection
cfg.output            = ft_getopt(cfg, 'output', 'original');

pertrial = istrue(cfg.pertrial);

% select channels and trials of interest, by default this will select all channels and trials
tmpcfg = keepfields(cfg, {'trials', 'channel', 'tolerance', 'showcallinfo', 'trackcallinfo', 'trackusage', 'trackdatainfo', 'trackmeminfo', 'tracktimeinfo', 'checksize'});
datain = ft_selectdata(tmpcfg, datain);
% restore the provenance information
[cfg, datain] = rollback_provenance(cfg, datain);

if istrue(cfg.demean)
  ft_info('demeaning the time series');
  tmpcfg = [];
  tmpcfg.demean = 'yes';
  datain = ft_preprocessing(tmpcfg, datain);
  % restore the provenance information
  [cfg, datain] = rollback_provenance(cfg, datain);
end

% match the input data's channels with the labels in the leadfield
sourcemodel = cfg.sourcemodel;
if ~isfield(sourcemodel, 'leadfield')
  ft_error('cfg.sourcemodel needs to contain leadfields');
end
[indx1, indx2] = match_str(datain.label, sourcemodel.label);
if ~isequal(indx1(:),(1:numel(datain.label))')
  ft_error('unsupported mismatch between data channels and leadfields');
end
if islogical(sourcemodel.inside)
  inside = find(sourcemodel.inside);
else
  inside = sourcemodel.inside;
end
for k = inside(:)'
  sourcemodel.leadfield{k} = sourcemodel.leadfield{k}(indx2,:);
end

% compute the Gram-matrix of the supplied forward model
lf = cat(2, sourcemodel.leadfield{:});
G  = lf*lf';

%dat     = cat(2,datain.trial{:});
[Bclean, subspace] = dssp(datain.trial, G, cfg.dssp.n_in, cfg.dssp.n_out, cfg.dssp.n_space, cfg.dssp.n_intersect, pertrial);
% datAe   = datain.trial*cellfun(@transpose, Ae, 'UniformOutput', false); % the projection is a right multiplication
% with a matrix (eye(size(Ae,1))-Ae*Ae'), since Ae*Ae' can become quite
% sizeable, it's computed slightly differently here.

% put some diagnostic information in the output cfg.
cfg.dssp.subspace = subspace;

% replace the input cfg values
cfg.dssp.n_space = subspace.S(1).n;
cfg.dssp.n_in    = subspace.Sin(1).n;
cfg.dssp.n_out   = subspace.Sout(1).n;
cfg.dssp.n_intersect = subspace.T(1).n;

% compute the cleaned data and put in a cell-array
switch cfg.output
  case 'original'
    trial = Bclean;
  case 'complement'
    trial = datain.trial-Bclean;
  otherwise
    ft_error(sprintf('cfg.output = ''%s'' is not implemented',cfg.output));
end

% create the output argument
dataout       = keepfields(datain, {'label', 'time', 'fsample', 'trialinfo', 'sampleinfo', 'grad', 'elec', 'opto'}); % grad can be kept and does not need to be balanced, since the cleaned data is a mixture over time, not space.
dataout.trial = trial;

% do the general cleanup and bookkeeping at the end of the function
ft_postamble debug
ft_postamble previous   datain
ft_postamble provenance dataout
ft_postamble history    dataout
ft_postamble savevar    dataout

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% subfunctions for the computation of the projection matrix
% kindly provided by Kensuke, and adjusted a bit by Jan-Mathijs
function [Bclean, subspace] = dssp(B, G, Nin, Nout, Nspace, Nintersect, pertrial)

% Nc: number of sensors
% Nt: number of time points
% inputs
% B(Nc,Nt):  interference overlapped sensor data
% G(Nc,Nc): Gram matrix of voxel lead field
% Nout and Nin: dimensions of the two row spaces
% recom_Nspace: recommended value for the dimension of the pseudo-signal subspace
% outputs
% Bclean(Nc,Nt): cleaned sensor data
% Nintersect: dimension of the intersection
% Nspace: dimension of the pseudo-signal subspace
%  ------------------------------------------------------------
%  programmed by K. Sekihara,  Signal Analysis Inc.
%  All right reserved by Signal Analysis Inc.
% -------------------------------------------------------------
%
% The code below is modified by Jan-Mathijs, no functional changes
% merely cosmetics, added the possibility to run the temporal subspace per
% trial

% eigen decomposition of the Gram matrix, matrix describing the spatial components of the defined 'in' compartment
fprintf('Computing the spatial subspace projection\n');
fprintf('Eigenvalue decomposition of the Gram matrix\n');
[Uspace,S] = eig(G);
Sspace     = abs(diag(S));

[Sspace, iorder] = sort(-Sspace);
Sspace           = -Sspace;
Uspace(:,:)      = Uspace(:,iorder);
Nspace           = getN(Nspace, Sspace, 'spatial');

% spatial subspace projector
Us   = Uspace(:,1:Nspace);

%USUS = Us*Us';
% % Bin and Bout creation
%Bin  =                  USUS  * B;
%Bout = (eye(size(USUS))-USUS) * B;

% computationally more efficient than the above
fprintf('Applying the spatial subspace projector\n');
Bin  = Us*(Us'*B);
Bout = B - Bin;

fprintf('Computing the subspace projector based on signal correlations\n');
[Ae, subspace] = CSP01(Bin, Bout, Nin, Nout, Nintersect, pertrial);

% add the first spatial subspace projection information as well
subspace.S.U = Uspace;
subspace.S.S = Sspace;
subspace.S.n = Nspace;
subspace.trial = Ae;

fprintf('Applying the subspace projector\n');
%Bclean    = B - (B*Ae)*Ae'; % avoid computation of Ae*Ae'
Bclean = B - (B*cellfun(@transpose, Ae, 'UniformOutput', false))*Ae;

function [Ae, subspace] = CSP01(Bin, Bout, Nin, Nout, Nintersect, pertrial)
%
% interference rejection by removing the common temporal subspace of the two subspaces
% K. Sekihara,  March 28, 2012
% Golub and Van Loan, Matrix computations, The Johns Hopkins University Press, 1996
%
%  Nc: number of channels
%  Nt: number of time points
% inputs
%  Bout(1:Nc,1:Nt): interference data
%  Bin(1:Nc,1:Nt): signal plus interference data
%  Nout: dimension of the interference subspace
%  Nin: dimension of the signal plus interference subspace
%  Nintersect: dimension of the intersection of the two subspaces
% outputs
% Ae = matrix from which the projector onto the intersection can
%      be obtained:
% subspace: struct containing information about the different subspace
%      projections
%  ------------------------------------------------------------
%  programmed by K. Sekihara,  Signal Analysis Inc.
%  All right reserved by Signal Analysis Inc.
% -------------------------------------------------------------
%

if ~pertrial
  % compute the projection across trials
  trllist = 1:numel(Bout);
else
  % compute the projection per trial
  trllist = (1:numel(Bout))';
end

Ae = cell(size(Bin));
for k = 1:size(trllist,1)
  indx = trllist(k,:); % this is either a scalar, or a vector
  [Uout,Sout,Vout] = svd(cat(2, Bout{indx}),'econ');
  [Uin, Sin, Vin]  = svd(cat(2, Bin{indx}), 'econ');
  Sout = diag(Sout);
  Sin  = diag(Sin);

  Nout = getN(Nout, Sout, 'outside');
  Nin  = getN(Nin,  Sin,  'inside');

  % compute unit-norm orthogonal time courses
  Qout = diag(1./Sout(1:Nout))*Uout(:,1:Nout)'*Bout(indx); % keep it in cell representation
  Qin  = diag(1./Sin(1:Nin)  )* Uin(:,1:Nin)' *Bin(indx);
  C    = Qin * cellfun(@transpose, Qout, 'UniformOutput', false);
  C    = sum(cat(3, C{:}), 3);

  % store the subspace information that is used in the next step
  subspace.Sin(k).U  = Uin;
  subspace.Sin(k).S  = Sin;
  subspace.Sin(k).n  = Nin;
  subspace.Sout(k).U = Uout;
  subspace.Sout(k).S = Sout;
  subspace.Sout(k).n = Nout;

  % covariance matrix of unit-norm 'components' -> how does this relate to
  % multivariate decomp? This is I guess equivalent mathematically
  [U,S] = svd(C);
  S     = diag(S);
  Nintersect = getN(Nintersect, S, 'intersection');

  Ae(indx) = U(:, 1:Nintersect)'*Qin;

  % keep the subspace information
  subspace.T(k).U = U;
  subspace.T(k).S = S;
  subspace.T(k).C = C; clear C;
  subspace.T(k).n = Nintersect;
end

function N = getN(N, S, name)

ttext = sprintf('enter the dimension for the %s field: ', name);
if isempty(N)
  N  = input(ttext);
elseif ischar(N) && isequal(N, 'interactive') && ~any(strcmp(name, {'outside' 'intersection'}))
  figure, plot(log10(S),'-o'); drawnow
  N = input(ttext);
elseif ischar(N) && isequal(N, 'interactive') && any(strcmp(name, {'outside' 'intersection'}))
  figure, plot(S, '-o'); drawnow
  N = input(ttext);
elseif ischar(N) && isequal(N, 'all')
  N = find(S./S(1)>1e5*eps, 1, 'last');
elseif isnumeric(N) && N<1
  N = find(S<=N, 1, 'last');
end
fprintf('Using %d dimensions for the %s field\n', N, name);


% Compute hub disruption indices (HDIs) using BCT functions. See Achard et
% al 2012; Mansour et al 2016.
%
% ..
%     Author and copyright information:
%
%     Copyright (C) 2020 Yoni Ashar
%
%     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/>.
% ..
%
% :Inputs:
%
%   **bs:**
%        brainpathway_multisubject object. Will use connectivity.regions.r
%        for computing the HDIs.
%
%   **refgroup:**
%        Two options: 1) a brainpathway_multisubject object to use
%        as a reference group. Will compute BCT measures on refgroup.connectivity.regions.r
%        and take the mean value as a the reference distribution.
%        2) a vector on integers to use for cross-validated estimation of
%        HDIs, where each integer represents one fold. The matlab functions
%        crossvalind and cvpartition can be helpful in generating these
%        indices. The hold-out set is used as the reference distribution
%
%   **thresh:**
%        Followed by a link density, ranging from .01 - .99. Will use .1 by default.
%
% :Optional Inputs:
%    **'doweighted'**
%       Compute weighted metrics
%
% :Outputs:
%
%   **bs:**
%        Will save the HDI values in bs.graph_prop, overwriting any
%        existing data in that field
%
%   **refgroup:**
%        With the BCT measures saved in the graph_properties.region field
%
% :Examples:
% ::
%
%    load('brainpathway_data_gsr_censoring_pipeline_nosmoothing.mat');
%    load('paingen_reference_group.mat');
%
%    [bs, refgroup] = compute_HDIs(bs, bs_paingen)
%
% :References:
%   Achard et al 2012, Mansour et al 2016
%
%
% ..
%    Programmers' notes:
%    Initial commit -- Yoni Ashar, April 2020
% ..
function [bs, refgroup] = compute_HDIs(bs, refgroup, thresh, varargin)

doweighted = 0;
if any(strcmp(varargin, 'doweighted')), doweighted = true; end

%% compute BCT measures on my subjects
print_header(sprintf('Computing BCT measures on %d subjects in test group', size(bs.connectivity.regions.r,3)));
bs = bct_toolbox_undirected_graph_metrics(bs, thresh, varargin{:});

%% Compute BCT measures on ref group and regress

% what kind of reference group are we dealing with?

% CV
if isnumeric(refgroup)
    error('CV not implemented yet')

% External reference group
elseif isa(refgroup, 'brainpathway_multisubject')

    print_header(sprintf('Computing BCT measures on %d subjects in external reference group', size(refgroup.connectivity.regions.r,3)));

    %% compute BCT measures on ref group
    refgroup = bct_toolbox_undirected_graph_metrics(refgroup, thresh, varargin{:});

    %% for each subject, regress on mean referece group to get an HDI (i.e., the slope)

    for i=1:size(bs.connectivity.regions.r,3)

        % dont compute HDI on global metrics, only nodal
        % TODO: put in subfunction
        mymetrics = bs.graph_properties.regions.Properties.VariableNames;
        metrics = {};
        for m=1:length(mymetrics)
            if numel(bs.graph_properties.regions.(mymetrics{m})(1,:)) > 1
                metrics{end+1} = mymetrics{m};
            end
        end

        % compute the HDIs
        for m=1:length(metrics) % for each metric

            betas = regress(mean(refgroup.graph_properties.regions.(metrics{m}))', [ones(489, 1) bs.graph_properties.regions.(metrics{m})(i,:)' - mean(refgroup.graph_properties.regions.(metrics{m}))']);

            %figure; scatter(mean(graph_prop_reference_group.(metrics{m})), graph_prop.(metrics{m})(i,:) - mean(graph_prop_reference_group.(metrics{m}))), lsline, title(metrics{m})
            bs.HDIs.regions.(metrics{m})(i) = betas(2);
        end
    end

% Unknown
else
    error('Unknown type of reference group')
end


% THIS SOFTWARE AND ANY ACCOMPANYING DOCUMENTATION IS RELEASED "AS IS."  THE U.S. GOVERNMENT MAKES NO WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, CONCERNING THIS SOFTWARE AND ANY ACCOMPANYING DOCUMENTATION, INCLUDING, WITHOUT LIMITATION, ANY WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE.  IN NO EVENT WILL THE U.S. GOVERNMENT BE LIABLE FOR ANY DAMAGES, INCLUDING ANY LOST PROFITS, LOST SAVINGS OR OTHER INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE, OR INABILITY TO USE, THIS SOFTWARE OR ANY ACCOMPANYING DOCUMENTATION, EVEN IF INFORMED IN ADVANCE OF THE POSSIBILITY OF SUCH DAMAGES.
%
% file: get_ci_ml.m
% computes confidence interval
%  - assumes x is the number of successes in n Bernoulli trials
%  - finds confidence interval around the estimate for p
%  - with confidence level 1 - alpha
%  - method used is based on integrating f(p|x)
%  - finds an interval of minimal length
%
% 010312 tdr created from get_ci_nibp

function ci = get_ci_ml(x,n,alpha)

if nargin < 3, error('Requires three input arguments (x,n,alpha)'); end;

p_hat=x/n;

if ( p_hat == 0 | p_hat == 1)
    ci = interval_est_p(x,n,alpha,'cs1'); % min length is same a "symmetric" when it bumps up against zero or one.
    return;
end;

% the following assumes that 0 < p_hat < 1

y_too_small = 0;
y_too_big = binopdf_01ok(x,n,p_hat);
y_guess = (y_too_small + y_too_big)/2;
continue_search = 1;

%close_enough = alpha/1000;
close_enough = 0.00001;
convergence_limit = 100;
convergence_count = 0;
hit_limit = 0;

p_ll = p_hat/2;
p_ul = (1 + p_hat)/2;

% start search
while (continue_search)
    convergence_count = convergence_count + 1;

    % find p lower limit: p_ll
        continue_p_search = 1;
        p_convergence_count = 0;
        hit_p_limit = 0;
        p_close_enough = 0;
        p_limit_too_small = 0; % 0 for ll, p_hat for ul
        p_limit_too_big = p_hat; % p_hat for ll, 1 for ul
        p_limit = (p_limit_too_big + p_limit_too_small) / 2;
        while (continue_p_search)
            p_convergence_count = p_convergence_count + 1;
            p_limit_y_guess = binopdf_01ok(x,n,p_limit);

            if (p_limit_y_guess > y_guess)
                p_limit_too_big = p_limit; % too_big for ll, too_small for ul
            else
                p_limit_too_small = p_limit; % too_small for ll, too_big for ul
            end;
            % stop or adjust p_ll_guess
            if (convergence_limit == p_convergence_count) hit_p_limit = 1; end;
            if ((p_limit_too_big - p_limit_too_small) < (close_enough / (10*n))) p_close_enough = 1; end; %see DNp2454.
            if (p_close_enough | hit_p_limit)
                continue_p_search = 0;
            else
                p_limit = (p_limit_too_big + p_limit_too_small) / 2;
            end;
        end;
        p_ll = p_limit; % ll or ul
        if hit_p_limit warning('method ''ml'' convergence limit reached for lower interval value.'); end;

    % find p upper limit: p_ul
        continue_p_search = 1;
        p_convergence_count = 0;
        hit_p_limit = 0;
        p_close_enough = 0;
        p_limit_too_small = p_hat; % 0 for ll, p_hat for ul
        p_limit_too_big = 1; % p_hat for ll, 1 for ul
        p_limit = (p_limit_too_big + p_limit_too_small) / 2;
        while (continue_p_search)
            p_convergence_count = p_convergence_count + 1;
            p_limit_y_guess = binopdf_01ok(x,n,p_limit);

            if (p_limit_y_guess > y_guess)
                p_limit_too_small = p_limit; % too_big for ll, too_small for ul
            else
                p_limit_too_big = p_limit; % too_small for ll, too_big for ul
            end;
            % stop or adjust p_ul_guess
            if (convergence_limit == p_convergence_count) hit_p_limit = 1; end;
            if ((p_limit_too_big - p_limit_too_small) < (close_enough / (10*n))) p_close_enough = 1; end;
            if (p_close_enough | hit_p_limit)
                continue_p_search = 0;
            else
                p_limit = (p_limit_too_big + p_limit_too_small) / 2;
            end;
        end;
        p_ul = p_limit; % ll or ul
        if hit_p_limit warning('method ''ml'' convergence limit reached for upper interval value.'); end;

    alpha_guess = (n+1)*(integrate_binopdf(x,n,0,p_ll) + integrate_binopdf(x,n,p_ul,1));

    % adjust y_too_small, y_too_big
    if (alpha_guess > alpha)
        y_too_big = y_guess;
    else
        y_too_small = y_guess;
    end;

    % stop or compute new y_guess
    delta_alpha = abs(alpha - alpha_guess);
    if (convergence_count == convergence_limit) hit_limit = 1; end;
    if ((delta_alpha <= close_enough) | hit_limit)
        continue_search = 0;
    else
        y_guess = (y_too_big + y_too_small)/2;
    end;
end;

if hit_limit warning('method ''ml'' convergence limit reached - results may not be accurate.'); end;

ci=[p_hat p_ll p_ul];

return;


function T = slfld(X, nums, varargin)
%SLFLD Performs Fisher Linear Discriminant Analysis
%
% $ Syntax $
%   - T = slfld(X, nums)
%   - T = slfld(X, nums, ...)
%
% $ Arguments $
%   - X:        the training sample matrix
%   - nums:     the numbers of samples in all classes
%   - T:        the solved transform matrix
%
% $ Description $
%   - T = slfld(X, nums) performs Fisher linear discriminant analysis
%     on the samples X in default way.
%
%   - T = slfld(X, nums, ...) performs Fisher linear discriminant analysis
%     on the samples X according to the specified properties.
%     \*
%     \t   Table 1.  The properties of Fisher Discriminant Analysis   \\
%     \h     name     &     description                                \\
%           'prepca'  &  Whether to perform a preamble PCA to first
%                        reduce the dimensions to the samples' rank.
%                        default = false.                              \\
%           'whiten' &  The cell containing the arguments for computing
%                       the whitening transform.  default = {}.       \\
%           'dimset'  &  The cell containing the arguments for determining
%                        the output feature dimension. default = {}.
%                        (refer to sldim_by_eigval).                   \\
%           'Sb'      &  The pre-computed between-class scattering matrix
%                        or the cell containing the arguments for
%                        computing the scatter matrix in the form
%                        {type, ...}, which is input to slscatter.     \\
%           'Sw'      &  The pre-computed within-class scattering matrix
%                        or the cell containing the arguments for
%                        computing the scatter matrix in the form
%                        {type, ...}, which is input to slscatter.     \\
%         'weights'   &  The sample weights. default = [].             \\
%     \*
%
% $ Remarks $
%   -# The function solves the transform in mainly two stages:
%      First, whiten the samples so that the between-class scattering
%      become the identity matrix; then solves the projection to maximize
%      the whitened between-class scattering. If prepca is set to true
%      a preamble step for reducing the dimensions to rank by PCA is taken.
%
%   -# The function is very flexible. By tuning the arguments for
%      pre-pca, whitening, and the computation of Sb and Sw, it turns to
%      be various LDA-based algorithms, including fisher LDA,
%      regularized LDA, weighted pairwise LDA, dual-space LDA etc.
%
%   -# If Sw or its computing rule is given, the whiten transform is
%      directly solved from Sw,  otherwise the whitening transform is
%      solved from samples. If Sb is given, the whitened between-class
%      scattering is computed by directly applying the whiten transform
%      to Sb, otherwise, Sb is computed from whitenned samples. If both
%      Sb and Sw are given, then the samples are not used in the function.
%      In this cases, you can simply input an empty X.
%
%   -# If both Sb and Sw are given, the pre-pca step will not be conducted.
%      no matter whether prepca is true or false.
%
% $ History $
%   - Created by Dahua Lin on Apr 30th, 2006
%   - Modified by Dahua Lin on Sep 10th, 2006
%       - replace sladd by sladdvec to increase efficiency.
%

%% parse and verify input arguments

if nargin < 2
    raise_lackinput('slfld', 2);
end

% check size

if ~isempty(X)
    if ndims(X) ~= 2
        error('sltoolbox:invaliddims', ...
            'The sample matrix X should be a 2D matrix');
    end
    [d, n] = size(X);

    k = length(nums);
    if ~isequal(size(nums), [1, k]);
        error('sltoolbox:invaliddims', ...
            'The nums vector should be a row vector');
    end
    if sum(nums) ~= n
        error('sltoolbox:sizmismatch', ...
            'The total number in nums is not consistent with that in X');
    end
end

% check options

opts.prepca = false;
opts.whiten = {};
opts.dimset = {};
opts.Sb = {'Sb'};
opts.Sw = {'Sw'};
opts.weights = [];
opts = slparseprops(opts, varargin{:});

has_Sb = ~isempty(opts.Sb) && isnumeric(opts.Sb);
has_Sw = ~isempty(opts.Sw) && isnumeric(opts.Sw);
if has_Sb && has_Sw
    use_samples = false;
    d = size(opts.Sw, 1);

    if ~isequal(size(opts.Sb), [d, d]) || ~isequal(size(opts.Sw), [d, d])
        error('sltoolbox:sizmismatch', ...
            'Size consistency in Sb and Sw');
    end

else
    if isempty(X)
        error('sltoolbox:invalidargs', ...
            'The samples cannot be empty when Sb or Sw is not pre-computed');
    end
    use_samples = true;
    if (has_Sb && ~isequal(size(opts.Sb), [d, d])) || (has_Sw && ~isequal(size(opts.Sw), [d, d]))
        error('sltoolbox:sizmismatch', ...
            'Size consistency in Sb and Sw');
    end

end
w = opts.weights;

%% Compute

%% Step 0: Pre-PCA
pca_computed = false;
if use_samples && opts.prepca
    SPCA = slpca(X, 'weights', w);
    X = SPCA.P' * sladdvec(X, -SPCA.vmean, 1);
    pca_computed = true;
end

%% Step 1: Compute whiten transform

if has_Sw
    TW = slwhiten_from_cov(opts.Sw, opts.whiten{:});
elseif ~isempty(opts.Sw) && ~isequal(opts.Sw, {'Sw'})
    Sw = slscatter(X, opts.Sw{:}, 'sweights', w, 'nums', nums);
    TW = slwhiten_from_cov(Sw);
    clear Sw;
else
    TW = slwhiten_from_samples(make_withinclass_diffvecs(X, w, nums), ...
        'weights', w, opts.whiten{:});
end

if pca_computed
    T1 = SPCA.P * TW;
    clear SPCA TW;
else
    T1 = TW;
    clear TW;
end

%% Step 2: Compute the second-stage transform

if has_Sb
    WSb = T1' * opts.Sb * T1;
else
    X = T1' * X;
    WSb = slscatter(X, opts.Sb{:}, 'sweights', w, 'nums', nums);
end
[evs, T2] = slsymeig(WSb);
rk2 = sldim_by_eigval(evs, opts.dimset{:});
T2 = T2(:, 1:rk2);

%% Integrate the transforms

T = T1 * T2;

%% The function for making the difference vectors
function Y = make_withinclass_diffvecs(X, w, nums)

mvs = slmeans(X, w, nums);
Y = X;
[sp, ep] = slnums2bounds(nums);
k = length(nums);
for i = 1 : k
    Y(:, sp(i):ep(i)) = sladdvec(X(:, sp(i):ep(i)), -mvs(:,i), 1);
end


function varargout = subsref(varargin)
%SUBSREF   CHEBFUN2 subsref.
% ( )
%   F(X, Y) returns the values of the CHEBFUN2 F evaluated at (X,Y). See
%   CHEBFUN/FEVAL for further details. F(:, Y) returns a chebfun
%   representing the function F along that column slice, and F(X, :)
%   returns a chebfun representing F along that row slice. F(:, :) returns
%   F.
%
%   F(G) computes the composition of F and G where G is a CHEBFUN with two
%   columns, a CHEBFUN2V, a CHEBFUN3V with two components, or a DISKFUNV.
%   If G is a CHEBFUN with one column, a CHEBFUN2, a CHEBFUN3, a DISKFUN or
%   a SPHEREFUN, F(G) is interpreted as F(real(G), imag(G)), regardless of
%   whether G is real or complex.
%
%   F(X, Y) with CHEBFUNs X and Y returns the CHEBFUN G(t) = F(X(t), Y(t)).
%   If X and Y are CHEBFUN2 objects, then F(X, Y) is a CHEBFUN2.
%   If X and Y are CHEBFUN3 objects, then F(X, Y) is a CHEBFUN3.
%
% .
%   F.PROP returns the property PROP of F as defined by GET(F, 'PROP').
%
% {}
%   F{S1, S2, S3, S4} restricts F to the domain [S1, S2, S3, S4]. See
%   CHEBFUN2/RESTRICT for further details. Note that F{[S1,S2, S3, S4]} is
%   not supported due to the behaviour of the MATLAB subsref() command.
%
% See also FEVAL, GET, RESTRICT, SUBSREF.

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

[varargout{1:nargout}] = subsref@separableApprox(varargin{:});

end


function M = globMatrixIFE3DFace(fun1,fun2,fem1,fem2,femIF,d1,j1,d2,j2)

%% USAGE: generate global matrix on Interface Faces
%             e.g.  \int_{Omega^I} [coef1 Du*n] {coef2 v} dx
%
% INPUTS:
% fun1 --- coefficient function in front of test function
% fun2 --- coefficient function in front of trial function
% fem1 --- global DoF for test function space
% fem2 --- global DoF for trial function space
% femIF --- quadrature info on interface faces
% d1 --- derivative info for test function
% d2 --- derivative info for trial function
%            d = 0: function value
%            d = 1: normal gradient value Du*n
% j1 --- jump info for test function
% j2 --- jump info for trial function
%            j = 0: average e.g. {u} = 1/2*(uL+uR)
%            j = 1: average e.g. [u] = uL-uR
%            Note: on Dirichlet boundary: [u] = {u} = u
% OUTPUTS:
% [I J X] --- triplets of the sparse matrix.

% Last Modified: 08/07/2020 by Xu Zhang

%% 0. Initialization
dof1 = fem1.ldof; dof2 = fem2.ldof;
if strcmp(fem1.type,'P1')||strcmp(fem1.type,'DGP1')||strcmp(fem1.type,'CR')
    feEvalBas1 = @evalP1Bas3D;
elseif strcmp(fem1.type,'P2')||strcmp(fem1.type,'DGP2')
    feEvalBas1 = @evalP2Bas3D;
end

if strcmp(fem2.type,'P1')||strcmp(fem2.type,'DGP1')||strcmp(fem2.type,'CR')
    feEvalBas2 = @evalP1Bas3D;
elseif strcmp(fem2.type,'P2')||strcmp(fem2.type,'DGP2')
    feEvalBas2 = @evalP2Bas3D;
end

%% 1. Matrix on Internal Interface Faces
A = femIF.area; nm = femIF.normal;
gw = femIF.gw; gx = femIF.gx; gy = femIF.gy; gz = femIF.gz;
nt = size(femIF.tL,1); % NOT # of interface elements, but quadrature element
ntot = 4*nt*dof1*dof2;
I = zeros(ntot,1); J = zeros(ntot,1); X = zeros(ntot,1);

coef1 = feval(fun1,gx,gy,gz);
coef2 = feval(fun2,gx,gy,gz);

IbasL = cell(dof1,1); IbasR = cell(dof1,1);
if d1 == 0
    for i = 1:dof1
        IbasL{i} = feEvalBas1(femIF.basL(:,:,i), gx, gy, gz, [0,0,0]);
        IbasR{i} = feEvalBas1(femIF.basR(:,:,i), gx, gy, gz, [0,0,0]);
    end
elseif d1 == 1
    for i = 1:dof1
        IbasLx = feEvalBas1(femIF.basL(:,:,i), gx, gy, gz, [1,0,0]);
        IbasLy = feEvalBas1(femIF.basL(:,:,i), gx, gy, gz, [0,1,0]);
        IbasLz = feEvalBas1(femIF.basL(:,:,i), gx, gy, gz, [0,0,1]);

        IbasRx = feEvalBas1(femIF.basR(:,:,i), gx, gy, gz, [1,0,0]);
        IbasRy = feEvalBas1(femIF.basR(:,:,i), gx, gy, gz, [0,1,0]);
        IbasRz = feEvalBas1(femIF.basR(:,:,i), gx, gy, gz, [0,0,1]);

        IbasL{i} = IbasLx.*nm(:,1) + IbasLy.*nm(:,2) + IbasLz.*nm(:,3);
        IbasR{i} = IbasRx.*nm(:,1) + IbasRy.*nm(:,2) + IbasRz.*nm(:,3);
    end
end

JbasL = cell(dof2,1);  JbasR = cell(dof2,1);
if d2 == 0
    for j = 1:dof2
        JbasL{j} = feEvalBas2(femIF.basL(:,:,j), gx, gy, gz, [0,0,0]);
        JbasR{j} = feEvalBas2(femIF.basR(:,:,j), gx, gy, gz, [0,0,0]);
    end
elseif d2 == 1
    for j = 1:dof2
        JbasLx = feEvalBas2(femIF.basL(:,:,j), gx, gy, gz, [1,0,0]);
        JbasLy = feEvalBas2(femIF.basL(:,:,j), gx, gy, gz, [0,1,0]);
        JbasLz = feEvalBas2(femIF.basL(:,:,j), gx, gy, gz, [0,0,1]);
        JbasRx = feEvalBas2(femIF.basR(:,:,j), gx, gy, gz, [1,0,0]);
        JbasRy = feEvalBas2(femIF.basR(:,:,j), gx, gy, gz, [0,1,0]);
        JbasRz = feEvalBas2(femIF.basR(:,:,j), gx, gy, gz, [0,0,1]);
        JbasL{j} = JbasLx.*nm(:,1) + JbasLy.*nm(:,2) + JbasLz.*nm(:,3);
        JbasR{j} = JbasRx.*nm(:,1) + JbasRy.*nm(:,2) + JbasRz.*nm(:,3);
    end
end

% Jump and Average
if j1 == 1
    for i = 1:dof1
        IbasL{i} = IbasL{i}; IbasR{i} = -1*IbasR{i};
    end
elseif j1 == 0
    for i = 1:dof1
        IbasL{i} = 1/2*IbasL{i}; IbasR{i} = 1/2*IbasR{i};
    end
end
if j2 == 1
    for j = 1:dof2
        JbasL{j} = JbasL{j};  JbasR{j} = -1*JbasR{j};
    end
elseif j2 == 0
    for j = 1:dof2
        JbasL{j} = 1/2*JbasL{j}; JbasR{j} = 1/2*JbasR{j};
    end
end

ind = 0;
for i = 1:dof1
    for j = 1:dof2
        I(ind+1:ind+4*nt) = [femIF.tL(:,i);femIF.tL(:,i);femIF.tR(:,i);femIF.tR(:,i)];
        J(ind+1:ind+4*nt) = [femIF.tL(:,j);femIF.tR(:,j);femIF.tL(:,j);femIF.tR(:,j)];
        X(ind+1:ind+4*nt) = [...
            A.*sum((((coef1.*IbasL{i}).*(coef2.*JbasL{j})).*gw'),2); ...
            A.*sum((((coef1.*IbasL{i}).*(coef2.*JbasR{j})).*gw'),2); ...
            A.*sum((((coef1.*IbasR{i}).*(coef2.*JbasL{j})).*gw'),2); ...
            A.*sum((((coef1.*IbasR{i}).*(coef2.*JbasR{j})).*gw'),2)];
        ind = ind + 4*nt;
    end
end
M = sparse(I,J,X,size(fem1.p,1),size(fem2.p,1));

%% 2. Matrix on Boundary Interface Faces
if size(femIF.tB,1) ~= 0
    nmB = femIF.normalB;
    AB = femIF.areaB; gxB = femIF.gxB; gyB = femIF.gyB; gzB = femIF.gzB;
    ntB = size(femIF.tB,1); % not number of interface element, but quadrature element
    ntotB = ntB*dof1*dof2;
    IB = zeros(ntotB,1); JB = zeros(ntotB,1); XB = zeros(ntotB,1);

    coef1B = feval(fun1,gxB,gyB,gzB);
    coef2B = feval(fun2,gxB,gyB,gzB);
    IbasB = cell(dof1,1);
    if d1 == 0
        for i = 1:dof1
            IbasB{i} = feEvalBas1(femIF.basB(:,:,i), gxB, gyB, gzB, [0,0,0]);
        end
    elseif d1 == 1
        for i = 1:dof1
            IbasBx = feEvalBas1(femIF.basB(:,:,i), gxB, gyB, gzB, [1,0,0]);
            IbasBy = feEvalBas1(femIF.basB(:,:,i), gxB, gyB, gzB, [0,1,0]);
            IbasBz = feEvalBas1(femIF.basB(:,:,i), gxB, gyB, gzB, [0,0,1]);
            IbasB{i} = IbasBx.*nmB(:,1) + IbasBy.*nmB(:,2) + IbasBz.*nmB(:,3);
        end
    end

    JbasB = cell(dof2,1);
    if d2 == 0
        for j = 1:dof2
            JbasB{j} = feEvalBas2(femIF.basB(:,:,j), gxB, gyB, gzB, [0,0,0]);
        end
    elseif d2 == 1
        for j = 1:dof2
            JbasBx = feEvalBas2(femIF.basB(:,:,j), gxB, gyB, gzB, [1,0,0]);
            JbasBy = feEvalBas2(femIF.basB(:,:,j), gxB, gyB, gzB, [0,1,0]);
            JbasBz = feEvalBas2(femIF.basB(:,:,j), gxB, gyB, gzB, [0,0,1]);
            JbasB{j} = JbasBx.*nmB(:,1) + JbasBy.*nmB(:,2) + JbasBz.*nmB(:,3);
        end
    end

    ind = 0;
    for i = 1:dof1
        for j = 1:dof2
            IB(ind+1:ind+ntB) = femIF.tB(:,i);
            JB(ind+1:ind+ntB) = femIF.tB(:,j);
            XB(ind+1:ind+ntB) = AB.*sum((((coef1B.*IbasB{i}).*(coef2B.*JbasB{j})).*gw'),2);
            ind = ind + ntB;
        end
    end
    MB = sparse(IB,JB,XB,size(fem1.p,1),size(fem2.p,1));
    M = M + MB;
end


function [w, infos] = slbfgs(problem, in_options)
% Stochastic limited-memory quasi-newton methods (Stochastic L-BFGS) algorithms.
%
% Inputs:
%       problem     function (cost/grad/hess)
%       in_options  options
% Output:
%       w           solution of w
%       infos       information
%
% References:
%       sub_mode:   SQN:
%                   Byrd, R. H., Hansen, S. L., Nocedal, J., & Singer, Y.
%                   "A stochastic quasi-Newton method for large-scale optimization,"
%                   SIAM Journal on Optimization, 26(2), 1008-1031, 2016.
%
%       sub_mode:   SVRG-SQN:
%                   Philipp Moritz, Robert Nishihara, Michael I. Jordan,
%                   "A Linearly-Convergent Stochastic L-BFGS Algorithm,"
%                   Artificial Intelligence and Statistics (AISTATS), 2016.
%
%       sub_mode:   SVRG LBFGS:
%                   R. Kolte, M. Erdogdu and A. Ozgur,
%                   "Accelerating SVRG via second-order information,"
%                   OPT2015, 2015.
%
%
% Created by H.Kasai on Oct. 15, 2016
% Modified by H.Kasai on Mar. 25, 2018

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

    % set local options
    local_options.sub_mode = 'SQN';  % SQN or SVRG-SQN or SVRG-LBFGS
    local_options.mem_size = 20;

    % merge options
    options = mergeOptions(get_default_options(d), local_options);
    options = mergeOptions(options, in_options);

    % set paramters
    if options.batch_size > n
        options.batch_size = n;
    end

    if ~isfield(in_options, 'batch_hess_size')
        options.batch_hess_size = 20 * options.batch_size;
    end

    if options.batch_hess_size > n
        options.batch_hess_size = n;
    end

    if strcmp(options.sub_mode, 'SQN') || strcmp(options.sub_mode, 'SVRG-SQN')
        if ~isfield(options, 'L')
            options.L = 20;
        else
            options.L = in_options.L;
        end
    elseif strcmp(options.sub_mode, 'SVRG-LBFGS')
        options.L = Inf;
    end

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

    s_array = [];
    y_array = [];
    u_old = w;
    u_new = zeros(d,1);

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

    % set start time
    start_time = tic();

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

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

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

        if strcmp(options.sub_mode, 'SVRG-SQN') || strcmp(options.sub_mode, 'SVRG-LBFGS')
            % compute full gradient
            %full_grad_new = problem.grad(w,1:n);
            full_grad_new = problem.full_grad(w);
            % count gradient evaluations
            grad_calc_count = grad_calc_count + n;
        end

        if strcmp(options.sub_mode, 'SVRG-LBFGS')
            if epoch > 0
                % store cavature pair
                s_array = [s_array w - w0];
                y_array = [y_array full_grad_new - full_grad];

                % remove overflowed pair
                if(size(s_array,2)>options.mem_size)
                    s_array(:,1) = [];
                    y_array(:,1) = [];
                end
            end
        end

        if strcmp(options.sub_mode, 'SVRG-SQN') || strcmp(options.sub_mode, 'SVRG-LBFGS')
            % store w for SVRG
            w0 = w;
            full_grad = full_grad_new;
        end

        for j = 1 : num_of_bachces

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

            % calculate gradient
            start_index = (j-1) * options.batch_size + 1;
            indice_j = perm_idx(start_index:start_index+options.batch_size-1);
            grad = problem.grad(w, indice_j);

            % calculate variance reduced gradient
            if strcmp(options.sub_mode, 'SVRG-SQN') || strcmp(options.sub_mode, 'SVRG-LBFGS')
                grad_w0 = problem.grad(w0,indice_j);
                grad = full_grad + grad - grad_w0;
            end

            if epoch > 0
                % perform LBFGS two loop recursion
                Hg = lbfgs_two_loop_recursion(grad, s_array, y_array);
                % update w
                w = w + (step*Hg);
            else
                w = w - (step*grad);
            end

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

            % calculate averaged w
            u_new = u_new + w/options.L;

            % update LBFGS vectors Hessian at every L iteration for 'SQN' or 'SVRG-SQN'
            % 'SVRG-LBFGS' does nothing because of L = Inf
            if(mod(total_iter,options.L)==0 && total_iter)

                % calcluate Hessian-vector product using subsamples
                %sub_indices = datasample((1:n), options.batch_hess_size);
                % "datasample" is supported only in statistics package in Octave.
                % To avoid the packege, the following is an alternative. Modified by H.K. on Mar. 27, 2018.
                perm_sub_idx_hessian = randperm(n);
                sub_indices = perm_sub_idx_hessian(1:options.batch_hess_size);

                % calculate hessian
                %H = problem.hess(w, sub_indices);
                %Hv = H*(u_new - u_old);
                % calculate hessian-vector product
                Hv = problem.hess_vec(w, u_new-u_old, sub_indices);

                % store cavature pair
                % 'y' curvature pair is calculated from a Hessian-vector product.
                s_array = [s_array u_new - u_old];
                y_array = [y_array Hv];

                % remove overflowed pair
                if(size(s_array,2)>options.mem_size)
                    s_array(:,1) = [];
                    y_array(:,1) = [];
                end

                u_old = u_new;
                u_new = zeros(d,1);

                % count gradient evaluations
                grad_calc_count = grad_calc_count + options.batch_hess_size;
            end

            total_iter = total_iter + 1;
        end

        % measure elapsed time
        elapsed_time = toc(start_time);

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

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

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

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


% ------------------------------------------------------
% SwarmOps - Heuristic optimization for Matlab
% Copyright (C) 2003-2010 Magnus Erik Hvass Pedersen.
% Please see the file license.txt for license details.
% SwarmOps on the internet: http://www.Hvass-Labs.org/
% ------------------------------------------------------

% Many Optimizing Liaisons (MOL) optimization method is a
% simplification of PSO originally by Eberhart et al. (1, 2).
% MOL does not have any attraction to the particle's own best
% known position. It is similar to the "Social Only" PSO
% suggested by Kennedy (3) and was studied more thoroguhly
% by Pedersen et al. (4) who found it to sometimes outperform
% PSO and have behavioural parameters that were easier to tune.
% This is a parallelized version.
% Literature references:
% (1) J. Kennedy and R. Eberhart. Particle swarm optimization.
%     In Proceedings of IEEE International Conference on Neural
%     Networks, volume IV, pages 1942-1948, Perth, Australia, 1995
% (2) Y. Shi and R.C. Eberhart. A modified particle swarm optimizer.
%     In Proceedings of the IEEE International Conference on
%     Evolutionary Computation, pages 69-73, Anchorage, AK, USA, 1998.
% (3) J. Kennedy. The particle swarm: social adaptation of knowledge,
%     In: Proceedings of the IEEE International Conference on
%     Evolutionary Computation, Indianapolis, USA, 1997.
% (4) M.E.H. Pedersen and A.J. Chipperfield. Simplifying particle swarm
%     optimization. Applied Soft Computing, volume 10, pages 618-628, 2010.
% Parameters:
%     problem; name or handle of optimization problem, e.g. @myproblem.
%     data; data-struct, see e.g. the file myproblemdata.m
%     parameters; behavioural parameters for optimizer,
%                 see file molparameters.m
% Returns:
%     bestX; best found position in the search-space.
%     bestFitness; fitness of bestX.
%     evaluations; number of fitness evaluations performed.
function [bestX, bestFitness, evaluations] = molparallel(problem, data, parameters)

    % Copy data contents to local variables for convenience.
    n = data.Dim;
    acceptableFitness = data.AcceptableFitness;
    maxEvaluations = data.MaxEvaluations;
    lowerBound = data.LowerBound;
    upperBound = data.UpperBound;

    % Behavioural parameters for this optimizer.
    s = parameters(1);        % Swarm-size
    omega = parameters(2);    % Inertia weight.
    phiG = parameters(3);     % Swarm's best weight.

    % Initialize the velocity boundaries.
    range = upperBound-lowerBound;
    lowerVelocity = -range;
    upperVelocity = range;

    % Initialize swarm.
    x = initpopulation(s, n, data.LowerInit, data.UpperInit);    % Particle positions.
    v = initpopulation(s, n, lowerVelocity, upperVelocity);      % Velocities.

    % Compute fitness of initial particle positions. (Parallel)
    fitness = zeros(1, s); % Preallocate array for efficiency.
    parfor i=1:s
        fitness(i) = feval(problem, x(i,:), data);
    end

    % Determine best particle.
    [bestFitness, bestIndex] = min(fitness);
    bestX = x(bestIndex,:);

    % Perform optimization iterations until acceptable fitness
    % is achieved or the maximum number of fitness evaluations
    % has been performed.
    evaluations = s; % Fitness evaluations above count as iterations.
    while (evaluations < maxEvaluations) && (bestFitness > acceptableFitness)

        % Update particle velocities and positions. (Non-parallel)
	for i=1:s
            % Pick random weight.
            rG = rand(1, 1);

            % Update velocity for i'th particle.
            v(i,:) = omega * v(i,:) + rG * phiG * (bestX - x(i,:));

            % Bound velocity.
            v(i,:) = bound(v(i,:), lowerVelocity, upperVelocity);

            % Update position for i'th particle.
            x(i,:) = x(i,:) + v(i,:);

            % Bound position to search-space.
            x(i,:) = bound(x(i,:), lowerBound, upperBound);
        end

        % Compute fitness. (Parallel)
        % Only this fitness evaluation is parallelized
        % which makes synchronization easier.
        parfor i=1:s
            fitness(i) = feval(problem, x(i,:), data);
        end

        % Update swarm's best-known position. (Non-parallel)
        [newBestFitness, newBestIndex] = min(fitness);
        if newBestFitness < bestFitness
            % Update swarm's best-known fitness.
            bestFitness = newBestFitness;

            % Update swarm's best-known position.
            % This must be copied because the particles
            % will continue to move in the search-space.
            bestX = x(newBestIndex,:);
        end

        % Increment counter.
        evaluations = evaluations + s;
    end
end

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


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% runmcs.m %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% test driver for running MCS
% with advice on how to choose the tuning parameters
%
% applied to the Jones test functions with default boxes
%
% To solve your own problem, copy this file (to ownmcs.m, say)
% and adapt the first part labelled `problem definition'.
% Then run the driver by typing `ownmcs' at the Matlab prompt.
%
% If you are not satisfied with the results, or if you want to play
% with the tuning parameters, you also need to adapt the second part
% labelled `change MCS settings'. In particular, for wide bounds,
% you'll probably get better results if you supply your own
% initialization list.
%
% On typing `runmcs' at the Matlab prompt,
% the unmodified file produces test results for the six-hump camel
% function; by only changing the data assignment you can also get
% results for the other test functions from Jones et al.
% You may also play with the bounds by modifying the default bounds.
%

clear; clear mex; clear global;
format compact;format long

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%% problem definition %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% define objective function
%
% Each function value f=f(x) is obtained by MCS using a call
% f=feval(fcn,data,x)
% where data is an arbitrary data object passed to mcs. If you have
% several data items, put them into a cell array or structure array
% called data.
% If there are no data, use fcn='feval' and the function name as data.
%
fcn = 'feval';
data = 'cam';	% select test function from Jones et al. test set
		% bra  n=2   Branin
		% cam  n=2   six-hump camel
		% gpr  n=2   Goldstein-Price
		% shu  n=2   Shubert
		% hm3  n=3   Hartman 3
		% s10  n=4   Shekel 10
		% sh5  n=4   Shekel 5
		% sh7  n=4   Shekel 7
		% hm6  n=6   Hartman 6
path(path,'jones');	% add path to directory with function

% define bounds on variables (+-inf allowed)
%
% u: column vector of lower bounds
% v: column vector of upper bounds
% u(k)<v(k) is required
%
[u,v,fglob] = defaults(data); 	% returns bounds used by Jones et al.
				% and known global optimum
dimension=length(u)		% show dimension
known_global_opt_value=fglob	% show known global minimum value
% u=[-5,0]';v=[10,15]';    		% bra default bounds
% u=[-3,-2]';v=[3,2]';     		% cam default bounds
% u=[-2,-2]';v=[2,2]';     		% gpr default bounds
% u=[-10,-10]';v=[10,10]'; 		% shu default bounds
% u=[0,0,0]';v=[1,1,1]';   		% hm3 default bounds
% u=[0,0,0,0]';v=[10,10,10,10]';   	% sh5,sh7,s10 default bounds
% u=[0,0,0,0,0,0]';v=[1,1,1,1,1,1]';   	% hm6 default bounds
% modify the problem to be unconstrained by activating the next line
% u = -Inf*ones(size(u));v=-u;

use_defaults=1
% *** If you just want to use the default settings,
% *** you don't need to edit the rest of the file,
% *** If you are not satisfied with the results
% *** (this may happen especially when your box bounds are very wide),
% *** or if you want to play with the tuning parameters,
% *** set use_defaults=0, and modify the rest of the file
% *** according to your curiosity or ingenuity

if use_defaults,
  % easy to use version - all parameters preset
  % defaults are being used, it suffices to call
  %%%%%%%%%%%%%%%%%% simple MCS call %%%%%%%%%%%%%%%%%%
  [xbest,fbest,xmin,fmi,ncall,ncloc]=mcs(fcn,data,u,v);
  % or, with less output,
  % [xbest,fbest]=mcs(fcn,data,u,v);

  xbest	  		% best point found
  fbest     		% best function value
  fglob			% global minimum (known for test functions)
  ncall	  		% number of function values used
  ncloc	  		% number of function values in local searches

  % xmin	  	% columns are points in 'shopping basket'
			% may be good alternative local minima
  % fmi	  		% function values in 'shopping basket'
  nbasket = length(fmi) % number of points in 'shopping basket'

  return;
end;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%% change MCS settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% flexible use version - all parameters may be modified

%
% define amount of output printed
prt = 1;	% print level
		% prt = 0: no output
		% prt = 1: # sweep, minimal nonempty level, # f-calls,
		%          best point and function value
		% prt > 1: in addition levels and function values of
		%          boxes containing the known global minimizers
		%          of a test function

% define global strategy
%
% smax governs the relative amount of global versus local search. By
% increasing smax, more weight is given to global search.
%
% Increasing or decreasing stop(1) increases or decreases the amount
% of work allowed before concluding that nothing more is gained;
% the default choice is quite conservative, and may try to locate
% a better point long after the global minimum has been found.
% stop(1)=5 works for many easier problems, too, with much fewer
% wasted function values.
%
% Increasing nf allows to spend more time on boxes that have a chance
% on their level to contain better points. This may be important for
% hard problems, or for problems with very wide bounds.
%
% But in each case, it is unclear in advance what change would be most
% beneficial to a particular problem.
% We had very mixed experience; if you have many similar problems to
% solve, the best thing to do is to experiment with a few problems to
% find the best values, and to use these on the others.
%
n = length(u);		% problem dimension
smax = 5*n+10;		% number of levels used
nf = 50*n^2; 		% limit on number of f-calls
stop(1) = 3*n;		% = m, integer defining stopping test
stop(2) = -inf;		% = freach, function value to reach
			% if m>0, run until m sweeps without progress
			% if m=0, run until fbest<=freach
			% (or about nf function calls were used)

if 0, 	% known global optimum, for tests only
	% then the entries of stop have a different meaning
  stop(1) = 1.e-4;	% run until this relative error is achieved
  stop(2) = fglob;	% known global optimum value
  stop(3) = 1.e-10;	% stopping tolerance for tiny fglob
end;

% define initialization strategy
%
% for wide boxes, it is advisable (and for unbounded search regions
% strongly advisable) to define a customized initialization list
% that contains for each coordinate at least three reasonable values.
% Without such an initialization list, mcs makes default assumptions
% that roughly amount to estimating reasonable magnitudes from the
% bounds and in case iinit=1 from assuming order unity if this is
% within the bounds.
%
% for a self-defined initialization list, the user should
% write an m-script file init0.m containing a matrix x0 with n
% rows and at least 3 columns and two n-vectors l and L
% the ith column of x0 contains the initialization list
% values for the ith coordinate, their number is L(i), and
% x0(i,l(i)) is the ith coordinate of the initial point

iinit = 0;	% 0: simple initialization list
		%    (default for finite bounds;
		%     do not use this for very wide bounds)
		% 1: safeguarded initialization list
		%    (default for unbounded search regions)
		% 2: (5*u+v)/6, (u+v)/2, (u+5*v)/6
		% 3: initialization list with line searches
		% else: self-defined initialization list
		%       stored in file init0.m

% parameters for local search
%
% A tiny gamma (default) gives a quite accurate but in higher
% dimensions slow local search. Increasing gamma leads to less work
% per local search but a less accurately localized minimizer
%
% If it is known that the Hessian is sparse, providing the sparsity
% pattern saves many function evaluations since the corresponding
% entries in the Hessian need not be estimated. The default pattern
% is a full matrix.
%
local = 50;		% local = 0: no local search
			% else: maximal number of steps in local search
gamma = eps;		% acceptable relative accuracy for local search
hess = ones(n,n);	% sparsity pattern of Hessian

% defaults are not being used, use the full calling sequence
% (including at least the modified arguments)
%%%%%%%%%%%%%%%%%%%%%%% full MCS call %%%%%%%%%%%%%%%%%%%%%%
[xbest,fbest,xmin,fmi,ncall,ncloc]=...
  mcs(fcn,data,u,v,prt,smax,nf,stop,iinit,local,gamma,hess);

xbest	  		% best point found
fbest     		% best function value
fglob			% global minimum (known for test functions)
ncall	  		% number of function values used
ncloc	  		% number of function values in local searches

% xmin	  		% columns are points in 'shopping basket'
			% may be good alternative local minima
% fmi	  		% function values in 'shopping basket'
nbasket = length(fmi) 	% number of points in 'shopping basket'


function hf_out = lhs_operation_joint(hf, samplesf, reg_filter, init_samplef, XH, init_hf, proj_reg)

% This is the left-hand-side operation in Conjugate Gradient

hf_out = cell(size(hf));

% Extract projection matrix and filter separately
P = cellfun(@real, hf(2,1,:), 'uniformoutput',false);
hf = hf(1,1,:);

% Get sizes
num_features = length(hf);
filter_sz = zeros(num_features,2);
for k = 1:num_features
    filter_sz(k,:) = [size(hf{k},1), size(hf{k},2)];
end
[~, k1] = max(filter_sz(:,1));  % Index for the feature block with the largest spatial size
block_inds = 1:num_features;
block_inds(k1) = [];
output_sz = [size(hf{k1},1), 2*size(hf{k1},2)-1];

% Compute the operation corresponding to the data term in the optimization
% (blockwise matrix multiplications)
%implements: A' diag(sample_weights) A f

% sum over all features and feature blocks
sh = sum(bsxfun(@times, samplesf{k1}, hf{k1}), 3);    % assumes the feature with the highest resolution is first
pad_sz = cell(1,1,num_features);
for k = block_inds
    pad_sz{k} = (output_sz - [size(hf{k},1), 2*size(hf{k},2)-1]) / 2;

    sh(1+pad_sz{k}(1):end-pad_sz{k}(1), 1+pad_sz{k}(2):end,1,:) = ...
        sh(1+pad_sz{k}(1):end-pad_sz{k}(1), 1+pad_sz{k}(2):end,1,:) + sum(bsxfun(@times, samplesf{k}, hf{k}), 3);
end

% weight all the samples and take conjugate
% sh = bsxfun(@times,sample_weights,sh);
sh = conj(sh);

% multiply with the transpose
hf_out1 = cell(1,1,num_features);
hf_out1{k1} = conj(sum(bsxfun(@times, sh, samplesf{k1}), 4));
for k = block_inds
    hf_out1{k} = conj(sum(bsxfun(@times, sh(1+pad_sz{k}(1):end-pad_sz{k}(1), 1+pad_sz{k}(2):end,1,1,:), samplesf{k}), 4));
end

% compute the operation corresponding to the regularization term (convolve
% each feature dimension with the DFT of w, and the tramsposed operation)
% add the regularization part
% hf_conv = cell(1,1,num_features);
for k = 1:num_features
    reg_pad = min(size(reg_filter{k},2)-1, size(hf{k},2)-1);

    % add part needed for convolution
    hf_conv = cat(2, hf{k}, conj(rot90(hf{k}(:, end-reg_pad:end-1, :), 2)));

    % do first convolution
    hf_conv = convn(hf_conv, reg_filter{k});

    % do final convolution and put toghether result
    hf_out1{k} = hf_out1{k} + convn(hf_conv(:,1:end-reg_pad,:), reg_filter{k}, 'valid');
end

% Stuff related to the projection matrix

% B * P
BP_cell = cell(1,1,num_features);
for k = 1:num_features
    BP_cell{k} = sum(bsxfun(@times, reshape(reshape(init_samplef{k}, [], size(init_samplef{k},3)) * P{k}, size(init_samplef{k},1), size(init_samplef{k},2), []), init_hf{k}), 3);
end

BP = BP_cell{k1};
for k = block_inds
    BP(1+pad_sz{k}(1):end-pad_sz{k}(1), 1+pad_sz{k}(2):end) = ...
        BP(1+pad_sz{k}(1):end-pad_sz{k}(1), 1+pad_sz{k}(2):end) + BP_cell{k};
end

% multiply with the transpose: A^H * BP
hf_out{1,1,k1} = hf_out1{k1} +  bsxfun(@times, BP, conj(samplesf{k1}));

% B^H * BP
fBP = cell(1,1,num_features);
fBP{k1} = reshape(bsxfun(@times, conj(init_hf{k1}), BP), [], size(init_hf{k1},3));

% Compute proj matrix part: B^H * A_m * f
shBP = cell(1,1,num_features);
shBP{k1} = reshape(bsxfun(@times, conj(init_hf{k1}), sh), [], size(init_hf{k1},3));

for k = block_inds
    % multiply with the transpose: A^H * BP
    hf_out{1,1,k} = hf_out1{k} +  bsxfun(@times, BP(1+pad_sz{k}(1):end-pad_sz{k}(1), 1+pad_sz{k}(2):end), conj(samplesf{k}));

    % B^H * BP
    fBP{k} = reshape(bsxfun(@times, conj(init_hf{k}), BP(1+pad_sz{k}(1):end-pad_sz{k}(1), 1+pad_sz{k}(2):end)), [], size(init_hf{k},3));

    % Compute proj matrix part: B^H * A_m * f
    shBP{k} = reshape(bsxfun(@times, conj(init_hf{k}), sh(1+pad_sz{k}(1):end-pad_sz{k}(1), 1+pad_sz{k}(2):end)), [], size(init_hf{k},3));
end

% hf_out2 = cell(1,1,num_features);
for k = 1:num_features
    fi = size(hf{k},1) * (size(hf{k},2)-1) + 1;  % index where the last frequency column starts

    % B^H * BP
    hf_out2 = 2*real(XH{k} * fBP{k} - XH{k}(:,fi:end) * fBP{k}(fi:end,:)) + proj_reg * P{k};

    % Compute proj matrix part: B^H * A_m * f
    hf_out{2,1,k} = hf_out2 + (2*real(XH{k} * shBP{k} - XH{k}(:,fi:end) * shBP{k}(fi:end,:)));
end
end


function [wnew,freqnew]=updateGlyph(P,X,C,w,a,s,freq)
%Syntax: [wnew,freqnew]=updateGlyph(P,X,C,w,a,s,freq)
%____________________________________________________
%
% Weight matrix update of a Spherical Self-Organized Feature Map.
%
% wnew is the updated weights' matrix.
% freqnew is the updated count-dependent parameter.
% P is the A-by-B matrix with the patterns to be classified.
% X is the N-by-3 matrix with the cartesian coordinates of the points on
%   the sphere.
% C is a cell array with the neighbors. The {i,j} cell represents the
%   neighbors of radius i of the j-th sphere point.
% w is the weights matrix to be updated.
% a is the training parameter.
% s is the neighborhood size parameter.
% freq is the count-dependent parameter.
%
%
% References:
%
% Sangole A., Knopf G. K. (2002): Representing high-dimensional data sets
% as close surfaces. Journal of Information Visualization 1: 111-119
%
% Sangole A., Knopf G. K. (2003): Geometric representations for
% high-dimensional data using a spherical SOFM. International Journal of
% Smart Engineering System Design 5: 11-20
%
% Sangole A., Knopf G. K. (2003): Visualization of random ordered numeric
% data sets using self-organized feature maps. Computers and Graphics 27:
% 963-976
%
% Sangole A. P. (2003): Data-driven Modeling using Spherical
% Self-organizing Feature Maps. Doctor of Philosophy (Ph.D.) Thesis.
% Department of Mechanical and Materials Engineering. Faculty of
% Engineering. The University of Western Ontario, London, Ontario, Canada.
%
%
% Archana P. Sangole, PhD., P.E. (TX chapter)
% School of Physical & Occupational Therapy
% McGill University
% 3654 Promenade Sir-William-Osler
% Montreal, PQ, H3G 1Y5
% e-mail: archana.sangole@mail.mcgill.ca
%
% CRIR, Rehabilitation Institute of Montreal
% 6300 Ave Darlington
% Montreal, PQ, H3S 2J5
% Tel: 514.340.2111 x2188
% Fax: 514.340.2154
%
%
% Alexandros Leontitsis, PhD
% Department of Education
% University of Ioannina
% 45110- Dourouti
% Ioannina
% Greece
%
% University e-mail: me00743@cc.uoi.gr
% Lifetime e-mail: leoaleq@yahoo.com
% Homepage: http://www.geocities.com/CapeCanaveral/Lab/1421
%
% 23-Mar-2006

if nargin<2 | isempty(P)==1 | isempty(X)==1
    error('More input arguments needed. Both P and X are rquired.');
else
    % P must be an A-by-B matrix
    if ndims(P)~=2
        error('P must be an A-by-B matrix.');
    end
    % X must be an M-by-3 matrix
    if size(X,2)~=3 | ndims(X)~=2
        error('X must be an N-by-3 matrix.');
    end
end

if isempty(C)==0
    % C and X must be of the same length
    if length(C)~=length(X)
        error('C and X must be of the same length.');
    end
    % Set the neighborhood radious
    r=size(C,2);
    % Set the h function
    h=exp(-(1:r).^2/(s*r));
end

if nargin<4 | isempty(w)==1
    w=randn(size(X,1),size(P,2));
    for i=1:size(X,2)
        w(:,i)=w(:,i)*std(P(:,i));
    end
else
    % w must be an N-by-B matrix
    if size(w,1)~=size(X,1) | size(w,2)~=size(P,2) | ndims(X)~=2
        error('w must be an N-by-B matrix.');
    end
end

if nargin<5 | isempty(a)==1
    a=0.1;
else
    % a must be a scalar
    if sum(size(a))>2
        error('a must be a scalar.');
    end
end

if nargin<6 | isempty(s)==1
    s=2;
else
    % s must be a scalar
    if sum(size(s))>2
        error('s must be a scalar.');
    end
    % s must be positive
    if s<=0
        error('s must be positive.');
    end
end

if nargin<7 | isempty(freq)==1
    freq=zeros(length(X),1);
else
    % freq must be a vector
    if min(size(freq))>1
        error('freq must be a vector.');
    end
    % freq must have the same length with X
    if length(freq)~=length(X)
        error('freq must have the same length with X.');
    end
end

% Initialize the weights
wnew=w;

% Initialize freqnew
freqnew=freq;

% Randomize the order of the patterns
q=randperm(size(P,1));

% For each uniformly random pattern
for j=1:size(P,1)
    % Measure the difference between the current P and all the weights
    A=q(j)*ones(size(wnew,1),1);
    d=P(A,:)-wnew;
    % Calculate the norm and apply the count-dependent parameter
    d=sqrt(sum((d.^2)'))'.*(freqnew+1);
    % Obtain the minimum distance and the corresponding index
    [D,I]=min(d);
    % Calculate the new weights
    wnew(I,:)=wnew(I,:)+a*(P(q(j),:)-wnew(I,:));
    % If the neighborhood matrix is given
    if isempty(C)==0
        % For each neghborhood radius
        for k=1:r
            A=q(j)*ones(size(C{I,k}));
            % Calculate the new weights of the neighbors
            wnew(C{I,k},:)=wnew(C{I,k},:)+a*h(k)*(P(A,:)-wnew(C{I,k},:));
            % Update the count-dependent parameter for the neighbors
            freqnew(C{I,k})=freqnew(C{I,k})+h(k);
        end
    end
    % Update the count-dependent parameter
    freqnew(I)=freqnew(I)+1;
end


%% A Simple and Robust Vertical Handoff Algorithm for HeterogeneousWireless Mobile Networks
%   Function Parameters:
%   C=cost of Ith network      S=Security     P=Power
%   D = Network Conditions     F= Network Performance
%   V = Velocity of MN         T= Estimated Time MN will stay in the
%                                   network coverage
%   NS = Network Set of eligible candidates for VHO
%   N = Number of Networks Presently available for evaluation.
%   lambda = mean number of request arrivals per unit time
%   mu = mean number of calls serviced per unit time
%   wx = weight function of the 'x' parameter for evaluating EVHDF
%
% Take as input from user the various network parameters for each network
% under consideration.
%
%   Reference: A Simple and Robust Vertical Handoff Algorithm for
%   Heterogenous Wireless Mobile Networks
%   Paper Authors: Daojing He, Caixia Chi, Sammy Chan, Chun Chen, Jiajun Bu,
%   Mingjian Yin.
%   Wireless Pers Commun DOI 10.1007/s11277-010-9922-x
%
%   Code Author: Anshul Thakur
%   Contact: anshulthakurjourneyendless@gmail.com
%   Created: 4/4/2011
%   Copyright Owner: Anshul Thakur

%% Note that the code that has been commented out are of the various
% parameters that the author did not consider for his evaluation. User may
% uncomment the necessary part to include more parameters in their
% evaluation
function code_modified()
%This section of code takes in various parameters of the different networks
%between which decision of a handoff is to be made.
fprintf('\nThe number of networks under evaluation N: \n')
N=input('=');
b=zeros(1,N);
RSS=zeros(1,N);
%V=zeros(1,N);
T=zeros(1,N);
%P=zeros(1,N);
%C=zeros(1,N);
%fj=zeros(1,N);
%wc=zeros(1,N);
%ws=zeros(1,N);
%wf=zeros(1,N);
wp=zeros(1,N);
wd=zeros(1,N);
M=zeros(1,N);
lambda=zeros(1,N);
mu=zeros(1,N);
pj=zeros(1,N);
NL=0;
fprintf('\nEnter the threshold values of various parameters when prompted.\n');
    bi=input('Threshold Current Available Bandwidth: ');
    RSSi=input('Threshold Received Signal Strength:  ');
    %Vi=input('Threshold Velocity of Mobile Station:  ');
    Ti=input('Threshold Estimated Time MS will be in present network:  ');
    %Pi=input('Threshold Battery Power of MS  :');
    %Ci=input('Threshold Cost of Network to MS  :');
for i=1:N
    fprintf('Enter the values for the network number %d when prompted.\n', i);
    if(i==1)
        fprintf('Enter the values of the currently connected network\n');
    end
    b(1,i)=input('Current Available Bandwidth: ');
    RSS(1,i)=input('Received Signal Strength:  ');
    %V(1,i)=input('Velocity of Mobile Station:  ');
    T(1,i)=input('Estimated Time MS will be in present network:  ');
    %P(1,i)=input('Battery Power of MS  :');
    %C(1,i)=input('Cost of Network to MS  :');
    %s(1,i)=input('Security level in Network    :');
    pj(1,i)=input('Power Dissipation in Network  :');
    %fj(1,i)=input('Network Performance Parameter of Network    :');
    lambda(1,i)=input('Mean number of request arrivals per unit time    :');
    mu(1,i)=input('Mean number of calls serviced per unit time  :');
    fprintf('\n \t Enter the Network Dependent Weights to the following parameters\n');
    %wc(1,i)=input('Cost of Service:');
    %ws(1,i)=input('Security Parameter:');
    wp(1,i)=input('Power Consumption: ');
    wd(1,i)=input('Network Conditions:  ');
    %wf(1,i)=input('Network Performance     :');

    %% This part evaluates the first step decision function to find the eligible
    % set of networks out of the available networks with values greater
    % than threshold
    %M(1,i)=((b(1,i)-bi)>0)&&((RSS(1,i)-RSSi)>0)&&((V(1,i)-Vi)>0)&&((T(1,i)-Ti)>0)&&((P(1,i)-Pi)>0)&&((C(1,i)-Ci)>0);
    M(1,i)=((b(1,i)-bi)>0)&&((RSS(1,i)-RSSi)>0)&&((T(1,i)-Ti)>0);
    if M(1,i)==1
        NL=NL+1;
    end
end
if(NL==0)
    fprintf('\nNo eligible networks found. Remain Connected to existing network itself\n');
else
%% Decision making when Network list has some networks in it
    NS=zeros(1,NL);
    hi=zeros(1,NL);
    EQ=zeros(1,NL);
    Dj=zeros(1,NL);
   x=1;
   for i=1:N
        if M(1,i)~=0
            NS(1,x)=i;
            x=x+1;
        end
   end
   if ((NL==1)&&(NS(1,1)==1))
       fprintf('\nNo other network is having minimum service quality better than threshold. Stay in same network\n');
   elseif((NL==1)&&(NS(1,1)~=1))
       fprintf('\n Handoff to new network with Network ID %d',NS(1,1));
   else
       fprintf('\nIs the Mobile Node Reource rich or Resource Poor?\n');
       res=input('r for Rich/ p for Poor      ','s');
       if(strcmp(res,'p'))
           if(NS(1,1)==1)
               frpintf('\nNo handoff required for Resource Poor Mobile Node at present\n');
           else
               fprintf('\nPerform Handoff with any of the %d Networks in the pool',NL);
           end
       elseif(strcmp(res,'r'))
           for i=1:NL
               index=NS(1,i);
               hi(1,i)=dyn_cal_prob(lambda(1,index),mu(1,index),b(1,index));
               Dj(1,i)=(b(1,index))/(hi(1,i));
           end
           %% This section evaluates the Extended Vertical Handoff Decision
           %  Function for the network pool previously selected as eligible
           for i=1:NL
               %index=NS(i,i);
               %Ct(1,i)=C(1,index);
               %EQ(1,i)=((wc(1,index)*(1/Ct(1,i)))/min(Ct,[],2))+(ws(1,index)*s(1,index)/max(s,[],2))+(wp(1,index)*pj(1,index)/max(pj,[],2))+(wd(1,index)*Dj(1,i)/max(Dj,[],2))+(wf(1,index)*fj(1,index)/max(fj,[],2));
               %EQ(1,i)=Dj(1,i)/max(Dj,[],2);
               EQ(1,i)=(wp(1,index)*pj(1,index)/max(pj,[],2))+(wd(1,index)*Dj(1,i)/max(Dj,[],2));
           end
           [para,netwrk]=max(EQ,[],2);
           if netwrk==1
               fprintf('\nCurrently selected Network is the best network, no Handoff required\n');
           else
               fprintf('\nPerform handoff with Network %d for best performance\n',NS(netwrk));
           end
       end
   end
    figure;
    subplot(2,1,1);
    stem(NS(1,:),hi(1,:));
    ylabel('New Dynamic Call Blocking Probability');
    xlabel('Network Number');
    title('a.   NDCBP vs Network ');
    subplot(2,1,2);
    stem(NS(1,:),EQ(1,:));
    title('   b.  EVHDF vs Network');
    xlabel('Netowrk');
    ylabel('Extended Vertical Handoff Decision Function');
end
end

%% Function to compute the New Dynamic Call blocking Probability
function hi=dyn_cal_prob(lambda, mu, bi)
    b=(lambda/mu);
    a=(b.^(bi))/factorial(bi);
    sumi=0;
    for n=0:bi
        sumi=sumi+((b.^exp(n))/factorial(n));
    end
    hi=a/sumi;
end


From fessler@eecs.umich.edu Thu Jun  1 16:56 EDT 2000
To: millsk@engin.umich.edu
Subject: ml-em
Cc: fessler@eecs.umich.edu
Content-Type: text
Content-Length: 539

function f = mlem(f, g, h)

if nargin < 3
	ftrue = zeros(11); ftrue(3:5,4:7) = 1; ftrue(4,5) = 0;
	h = ones(3,3);
	g = conv2(ftrue, h, 'same');
	g = g + 0.1 * randn(size(g));
	g = max(g, 0);

	f = g;
	imagesc(f), drawnow
	for ii=1:100
		f = mlem(f, g, h);
		imagesc(f), drawnow
	end
return
end

if any(g < 0), error 'need nonnegative data', end
gp = conv2(f, h, 'same');
if any(g > 0 & ~gp), error 'model mismatch', end
ratio = g ./ (gp + eps * (gp==0));

sens = conv2(ones(size(f)), h, 'same');
f = f .* conv2(ratio, h, 'same') ./ sens;


%
% output = bilateralFilter( data, edge, ...
%                          edgeMin, edgeMax, ...
%                          sigmaSpatial, sigmaRange, ...
%                          samplingSpatial, samplingRange )
%
% Bilateral and Cross-Bilateral Filter using the Bilateral Grid.
%
% Bilaterally filters the image 'data' using the edges in the image 'edge'.
% If 'data' == 'edge', then it the standard bilateral filter.
% Otherwise, it is the 'cross' or 'joint' bilateral filter.
% For convenience, you can also pass in [] for 'edge' for the normal
% bilateral filter.
%
% Note that for the cross bilateral filter, data does not need to be
% defined everywhere.  Undefined values can be set to 'NaN'.  However, edge
% *does* need to be defined everywhere.
%
% data and edge should be of the greyscale, double-precision floating point
% matrices of the same size (i.e. they should be [ height x width ])
%
% data is the only required argument
%
% edgeMin and edgeMax specifies the min and max values of 'edge' (or 'data'
% for the normal bilateral filter) and is useful when the input is in a
% range that's not between 0 and 1.  For instance, if you are filtering the
% L channel of an image that ranges between 0 and 100, set edgeMin to 0 and
% edgeMax to 100.
%
% edgeMin defaults to min( edge( : ) ) and edgeMax defaults to max( edge( : ) ).
% This is probably *not* what you want, since the input may not span the
% entire range.
%
% sigmaSpatial and sigmaRange specifies the standard deviation of the space
% and range gaussians, respectively.
% sigmaSpatial defaults to min( width, height ) / 16
% sigmaRange defaults to ( edgeMax - edgeMin ) / 10.
%
% samplingSpatial and samplingRange specifies the amount of downsampling
% used for the approximation.  Higher values use less memory but are also
% less accurate.  The default and recommended values are:
%
% samplingSpatial = sigmaSpatial
% samplingRange = sigmaRange
%

% Copyright (c) 2007 Jiawen Chen, Sylvain Paris, and Fredo Durand
%
% Permission is hereby granted, free of charge, to any person obtaining a copy
% of this software and associated documentation files (the "Software"), to deal
% in the Software without restriction, including without limitation the rights
% to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
% copies of the Software, and to permit persons to whom the Software is
% furnished to do so, subject to the following conditions:
%
% The above copyright notice and this permission notice shall be included in
% all copies or substantial portions of the Software.
%
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
% FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
% AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
% LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
% OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
% THE SOFTWARE.

function output = bilateralFilter( data, edge, edgeMin, edgeMax, sigmaSpatial, sigmaRange, ...
    samplingSpatial, samplingRange )

if( ndims( data ) > 2 ),
    error( 'data must be a greyscale image with size [ height, width ]' );
end

if( ~isa( data, 'double' ) ),
    error( 'data must be of class "double"' );
end

if ~exist( 'edge', 'var' ),
    edge = data;
elseif isempty( edge ),
    edge = data;
end

if( ndims( edge ) > 2 ),
    error( 'edge must be a greyscale image with size [ height, width ]' );
end

if( ~isa( edge, 'double' ) ),
    error( 'edge must be of class "double"' );
end

inputHeight = size( data, 1 );
inputWidth = size( data, 2 );

if ~exist( 'edgeMin', 'var' ),
    edgeMin = min( edge( : ) );
    warning( 'edgeMin not set!  Defaulting to: %f\n', edgeMin );
end

if ~exist( 'edgeMax', 'var' ),
    edgeMax = max( edge( : ) );
    warning( 'edgeMax not set!  Defaulting to: %f\n', edgeMax );
end

edgeDelta = edgeMax - edgeMin;

if ~exist( 'sigmaSpatial', 'var' ),
    sigmaSpatial = min( inputWidth, inputHeight ) / 16;
    fprintf( 'Using default sigmaSpatial of: %f\n', sigmaSpatial );
end

if ~exist( 'sigmaRange', 'var' ),
    sigmaRange = 0.1 * edgeDelta;
    fprintf( 'Using default sigmaRange of: %f\n', sigmaRange );
end

if ~exist( 'samplingSpatial', 'var' ),
    samplingSpatial = sigmaSpatial;
end

if ~exist( 'samplingRange', 'var' ),
    samplingRange = sigmaRange;
end

if size( data ) ~= size( edge ),
    error( 'data and edge must be of the same size' );
end

% parameters
derivedSigmaSpatial = sigmaSpatial / samplingSpatial;
derivedSigmaRange = sigmaRange / samplingRange;

paddingXY = floor( 2 * derivedSigmaSpatial ) + 1;
paddingZ = floor( 2 * derivedSigmaRange ) + 1;

% allocate 3D grid
downsampledWidth = floor( ( inputWidth - 1 ) / samplingSpatial ) + 1 + 2 * paddingXY;
downsampledHeight = floor( ( inputHeight - 1 ) / samplingSpatial ) + 1 + 2 * paddingXY;
downsampledDepth = floor( edgeDelta / samplingRange ) + 1 + 2 * paddingZ;

gridData = zeros( downsampledHeight, downsampledWidth, downsampledDepth );
gridWeights = zeros( downsampledHeight, downsampledWidth, downsampledDepth );

% compute downsampled indices
[ jj, ii ] = meshgrid( 0 : inputWidth - 1, 0 : inputHeight - 1 );

% ii =
% 0 0 0 0 0
% 1 1 1 1 1
% 2 2 2 2 2

% jj =
% 0 1 2 3 4
% 0 1 2 3 4
% 0 1 2 3 4

% so when iterating over ii( k ), jj( k )
% get: ( 0, 0 ), ( 1, 0 ), ( 2, 0 ), ... (down columns first)

di = round( ii / samplingSpatial ) + paddingXY + 1;
dj = round( jj / samplingSpatial ) + paddingXY + 1;
dz = round( ( edge - edgeMin ) / samplingRange ) + paddingZ + 1;

% perform scatter (there's probably a faster way than this)
% normally would do downsampledWeights( di, dj, dk ) = 1, but we have to
% perform a summation to do box downsampling
for k = 1 : numel( dz ),

    dataZ = data( k ); % traverses the image column wise, same as di( k )
    if ~isnan( dataZ  ),

        dik = di( k );
        djk = dj( k );
        dzk = dz( k );

        gridData( dik, djk, dzk ) = gridData( dik, djk, dzk ) + dataZ;
        gridWeights( dik, djk, dzk ) = gridWeights( dik, djk, dzk ) + 1;

    end
end

% make gaussian kernel
kernelWidth = 2 * derivedSigmaSpatial + 1;
kernelHeight = kernelWidth;
kernelDepth = 2 * derivedSigmaRange + 1;

halfKernelWidth = floor( kernelWidth / 2 );
halfKernelHeight = floor( kernelHeight / 2 );
halfKernelDepth = floor( kernelDepth / 2 );

[gridX, gridY, gridZ] = meshgrid( 0 : kernelWidth - 1, 0 : kernelHeight - 1, 0 : kernelDepth - 1 );
gridX = gridX - halfKernelWidth;
gridY = gridY - halfKernelHeight;
gridZ = gridZ - halfKernelDepth;
gridRSquared = ( gridX .* gridX + gridY .* gridY ) / ( derivedSigmaSpatial * derivedSigmaSpatial ) + ( gridZ .* gridZ ) / ( derivedSigmaRange * derivedSigmaRange );
kernel = exp( -0.5 * gridRSquared );

% convolve
blurredGridData = convn( gridData, kernel, 'same' );
blurredGridWeights = convn( gridWeights, kernel, 'same' );

% divide
blurredGridWeights( blurredGridWeights == 0 ) = -2; % avoid divide by 0, won't read there anyway
normalizedBlurredGrid = blurredGridData ./ blurredGridWeights;
normalizedBlurredGrid( blurredGridWeights < -1 ) = 0; % put 0s where it's undefined

% for debugging
% blurredGridWeights( blurredGridWeights < -1 ) = 0; % put zeros back

% upsample
[ jj, ii ] = meshgrid( 0 : inputWidth - 1, 0 : inputHeight - 1 ); % meshgrid does x, then y, so output arguments need to be reversed
% no rounding
di = ( ii / samplingSpatial ) + paddingXY + 1;
dj = ( jj / samplingSpatial ) + paddingXY + 1;
dz = ( edge - edgeMin ) / samplingRange + paddingZ + 1;

% interpn takes rows, then cols, etc
% i.e. size(v,1), then size(v,2), ...
output = interpn( normalizedBlurredGrid, di, dj, dz );


function d2Vlims = opf_vlim_hess(x, lambda, mpc, idx, mpopt)
%OPF_VLIM_HESS  Evaluates Hessian of voltage magnitudes.
%   D2VLIMS = OPF_VLIM_HESS(X, LAMBDA, MPC, IDX, MPOPT)
%
%   Hessian evaluation function for voltage magnitudes.
%
%   Inputs:
%     X : optimization vector
%     LAMBDA : column vector of Lagrange multipliers on active and reactive
%              power balance constraints
%     MPC : MATPOWER case struct
%     IDX : index of buses whose voltage magnitudes should be fixed
%     MPOPT : MATPOWER options struct
%
%   Outputs:
%     D2VLIMS : Hessian of voltage magnitudes.
%
%   Example:
%       d2Vlims = opf_vlim_hess(x, lambda, mpc, idx, mpopt);
%
%   See also OPF_VLIM_FCN.

%   MATPOWER
%   Copyright (c) 2018, Power Systems Engineering Research Center (PSERC)
%   by Baljinnyam Sereeter, Delft University of Technology
%   and Ray Zimmerman, PSERC Cornell
%
%   This file is part of MATPOWER.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://matpower.org for more info.

%% unpack data
[Vr, Vi] = deal(x{:});

%% problem dimensions
nb = length(Vi);            %% number of buses
n = length(idx);            %% number of buses with voltage limits

%%----- evaluate Hessian of voltage limit constraints -----
nlam = length(lambda) / 2;
if nlam
    lam = lambda(nlam+1:2*nlam) - lambda(1:nlam);   %% lamVmax - lamVmin
else    %% keep dimensions of empty matrices/vectors compatible
    lam = zeros(0,1);
end

dlam = sparse(idx, idx, 2*lam, nb, nb);
zz = sparse(nb, nb);

%% construct Hessian
d2Vlims = [dlam zz; zz dlam];


function [X, fX, i] = fmincg(f, X, options, P1, P2, P3, P4, P5)
% Minimize a continuous differentialble multivariate function. Starting point
% is given by "X" (D by 1), and the function named in the string "f", must
% return a function value and a vector of partial derivatives. The Polack-
% Ribiere flavour of conjugate gradients is used to compute search directions,
% and a line search using quadratic and cubic polynomial approximations and the
% Wolfe-Powell stopping criteria is used together with the slope ratio method
% for guessing initial step sizes. Additionally a bunch of checks are made to
% make sure that exploration is taking place and that extrapolation will not
% be unboundedly large. The "length" gives the length of the run: if it is
% positive, it gives the maximum number of line searches, if negative its
% absolute gives the maximum allowed number of function evaluations. You can
% (optionally) give "length" a second component, which will indicate the
% reduction in function value to be expected in the first line-search (defaults
% to 1.0). The function returns when either its length is up, or if no further
% progress can be made (ie, we are at a minimum, or so close that due to
% numerical problems, we cannot get any closer). If the function terminates
% within a few iterations, it could be an indication that the function value
% and derivatives are not consistent (ie, there may be a bug in the
% implementation of your "f" function). The function returns the found
% solution "X", a vector of function values "fX" indicating the progress made
% and "i" the number of iterations (line searches or function evaluations,
% depending on the sign of "length") used.
%
% Usage: [X, fX, i] = fmincg(f, X, options, P1, P2, P3, P4, P5)
%
% See also: checkgrad
%
% Copyright (C) 2001 and 2002 by Carl Edward Rasmussen. Date 2002-02-13
%
%
% (C) Copyright 1999, 2000 & 2001, Carl Edward Rasmussen
%
% Permission is granted for anyone to copy, use, or modify these
% programs and accompanying documents for purposes of research or
% education, provided this copyright notice is retained, and note is
% made of any changes that have been made.
%
% These programs and documents are distributed without any warranty,
% express or implied.  As the programs were written for research
% purposes only, they have not been tested to the degree that would be
% advisable in any important application.  All use of these programs is
% entirely at the user's own risk.
%
% [ml-class] Changes Made:
% 1) Function name and argument specifications
% 2) Output display
%

% Read options
if exist('options', 'var') && ~isempty(options) && isfield(options, 'MaxIter')
    length = options.MaxIter;
else
    length = 100;
end

RHO = 0.01;                            % a bunch of constants for line searches
SIG = 0.5;       % RHO and SIG are the constants in the Wolfe-Powell conditions
INT = 0.1;    % don't reevaluate within 0.1 of the limit of the current bracket
EXT = 3.0;                    % extrapolate maximum 3 times the current bracket
MAX = 20;                         % max 20 function evaluations per line search
RATIO = 100;                                      % maximum allowed slope ratio

argstr = ['feval(f, X'];                      % compose string used to call function
for i = 1:(nargin - 3)
  argstr = [argstr, ',P', int2str(i)];
end
argstr = [argstr, ')'];

if max(size(length)) == 2, red=length(2); length=length(1); else red=1; end
S=['Iteration '];

i = 0;                                            % zero the run length counter
ls_failed = 0;                             % no previous line search has failed
fX = [];
[f1 df1] = eval(argstr);                      % get function value and gradient
i = i + (length<0);                                            % count epochs?!
s = -df1;                                        % search direction is steepest
d1 = -s'*s;                                                 % this is the slope
z1 = red/(1-d1);                                  % initial step is red/(|s|+1)

while i < abs(length)                                      % while not finished
  i = i + (length>0);                                      % count iterations?!

  X0 = X; f0 = f1; df0 = df1;                   % make a copy of current values
  X = X + z1*s;                                             % begin line search
  [f2 df2] = eval(argstr);
  i = i + (length<0);                                          % count epochs?!
  d2 = df2'*s;
  f3 = f1; d3 = d1; z3 = -z1;             % initialize point 3 equal to point 1
  if length>0, M = MAX; else M = min(MAX, -length-i); end
  success = 0; limit = -1;                     % initialize quanteties
  while 1
    while ((f2 > f1+z1*RHO*d1) || (d2 > -SIG*d1)) && (M > 0)
      limit = z1;                                         % tighten the bracket
      if f2 > f1
        z2 = z3 - (0.5*d3*z3*z3)/(d3*z3+f2-f3);                 % quadratic fit
      else
        A = 6*(f2-f3)/z3+3*(d2+d3);                                 % cubic fit
        B = 3*(f3-f2)-z3*(d3+2*d2);
        z2 = (sqrt(B*B-A*d2*z3*z3)-B)/A;       % numerical error possible - ok!
      end
      if isnan(z2) || isinf(z2)
        z2 = z3/2;                  % if we had a numerical problem then bisect
      end
      z2 = max(min(z2, INT*z3),(1-INT)*z3);  % don't accept too close to limits
      z1 = z1 + z2;                                           % update the step
      X = X + z2*s;
      [f2 df2] = eval(argstr);
      M = M - 1; i = i + (length<0);                           % count epochs?!
      d2 = df2'*s;
      z3 = z3-z2;                    % z3 is now relative to the location of z2
    end
    if f2 > f1+z1*RHO*d1 || d2 > -SIG*d1
      break;                                                % this is a failure
    elseif d2 > SIG*d1
      success = 1; break;                                             % success
    elseif M == 0
      break;                                                          % failure
    end
    A = 6*(f2-f3)/z3+3*(d2+d3);                      % make cubic extrapolation
    B = 3*(f3-f2)-z3*(d3+2*d2);
    z2 = -d2*z3*z3/(B+sqrt(B*B-A*d2*z3*z3));        % num. error possible - ok!
    if ~isreal(z2) || isnan(z2) || isinf(z2) || z2 < 0 % num prob or wrong sign?
      if limit < -0.5                               % if we have no upper limit
        z2 = z1 * (EXT-1);                 % the extrapolate the maximum amount
      else
        z2 = (limit-z1)/2;                                   % otherwise bisect
      end
    elseif (limit > -0.5) && (z2+z1 > limit)         % extraplation beyond max?
      z2 = (limit-z1)/2;                                               % bisect
    elseif (limit < -0.5) && (z2+z1 > z1*EXT)       % extrapolation beyond limit
      z2 = z1*(EXT-1.0);                           % set to extrapolation limit
    elseif z2 < -z3*INT
      z2 = -z3*INT;
    elseif (limit > -0.5) && (z2 < (limit-z1)*(1.0-INT))  % too close to limit?
      z2 = (limit-z1)*(1.0-INT);
    end
    f3 = f2; d3 = d2; z3 = -z2;                  % set point 3 equal to point 2
    z1 = z1 + z2; X = X + z2*s;                      % update current estimates
    [f2 df2] = eval(argstr);
    M = M - 1; i = i + (length<0);                             % count epochs?!
    d2 = df2'*s;
  end                                                      % end of line search

  if success                                         % if line search succeeded
    f1 = f2; fX = [fX' f1]';
    fprintf('%s %4i | Cost: %4.6e\r', S, i, f1);
    s = (df2'*df2-df1'*df2)/(df1'*df1)*s - df2;      % Polack-Ribiere direction
    tmp = df1; df1 = df2; df2 = tmp;                         % swap derivatives
    d2 = df1'*s;
    if d2 > 0                                      % new slope must be negative
      s = -df1;                              % otherwise use steepest direction
      d2 = -s'*s;
    end
    z1 = z1 * min(RATIO, d1/(d2-realmin));          % slope ratio but max RATIO
    d1 = d2;
    ls_failed = 0;                              % this line search did not fail
  else
    X = X0; f1 = f0; df1 = df0;  % restore point from before failed line search
    if ls_failed || i > abs(length)          % line search failed twice in a row
      break;                             % or we ran out of time, so we give up
    end
    tmp = df1; df1 = df2; df2 = tmp;                         % swap derivatives
    s = -df1;                                                    % try steepest
    d1 = -s'*s;
    z1 = 1/(1-d1);
    ls_failed = 1;                                    % this line search failed
  end
  if exist('OCTAVE_VERSION')
    fflush(stdout);
  end
end
fprintf('\n');