function [p,f] = spm_powell(p,xi,tolsc,func,varargin)
% Powell optimisation method
% FORMAT [p,f] = spm_powell(p,xi,tolsc,func,varargin)
%   p        - Starting parameter values
%   xi       - columns containing directions in which to begin searching
%   tolsc    - stopping criteria, optimisation stops when
%                sqrt(sum(((p-p_prev)./tolsc).^2))<1
%   func     - name of evaluated function
%   varargin - remaining arguments to func (after p)
%
%   p        - final parameter estimates
%   f        - function value at minimum
%__________________________________________________________________________
%
% Method is based on Powell's optimisation method described in
% Numerical Recipes (Press, Flannery, Teukolsky & Vetterling).
%__________________________________________________________________________
% Copyright (C) 2001-2011 Wellcome Trust Centre for Neuroimaging

% John Ashburner
% $Id: spm_powell.m 4423 2011-08-04 16:28:51Z guillaume $

p = p(:);
f = feval(func,p,varargin{:});
for iter=1:512
    %if numel(p)>1, fprintf('iteration %d...\n', iter); end;            %-#
    ibig = numel(p);
    pp   = p;
    fp   = f;
    del  = 0;
    for i=1:length(p)
        ft = f;
        [p,junk,f] = min1d(p,xi(:,i),func,f,tolsc,varargin{:});
        if abs(ft-f) > del,
            del  = abs(ft-f);
            ibig = i;
        end
    end
    if numel(p)==1 || sqrt(sum(((p(:)-pp(:))./tolsc(:)).^2))<1, return; end
    ft = feval(func,2.0*p-pp,varargin{:});
    if ft < f
        [p,xi(:,ibig),f] = min1d(p,p-pp,func,f,tolsc,varargin{:});
    end
end
warning('Too many optimisation iterations');

%==========================================================================
% function [p,pi,f] = min1d(p,pi,func,f,tolsc,varargin)
%==========================================================================
function [p,pi,f] = min1d(p,pi,func,f,tolsc,varargin)
% Line search for minimum.

global lnm % used in funeval
lnm      = struct('p',p,'pi',pi,'func',func,'args',[]);
lnm.args = varargin;

min1d_plot('Init', 'Line Minimisation','Function','Parameter Value');
min1d_plot('Set', 0, f);

tol      = 1/sqrt(sum((pi(:)./tolsc(:)).^2));
t        = bracket(f);
[f,pmin] = search(t,tol);
pi       = pi*pmin;
p        = p + pi;

%if length(p)<12,
%    for i=1:length(p), fprintf('%-8.4g ', p(i)); end;                  %-#
%    fprintf('| %.5g\n', f);                                            %-#
%else
%    fprintf('%.5g\n', f);                                              %-#
%end
min1d_plot('Clear');

%==========================================================================
% function f = funeval(p)
%==========================================================================
function f = funeval(p)
% Reconstruct parameters and evaluate.

global lnm % defined in min1d
pt = lnm.p+p.*lnm.pi;
f  = feval(lnm.func,pt,lnm.args{:});
min1d_plot('Set',p,f);

%==========================================================================
% function t = bracket(f)
%==========================================================================
function t = bracket(f)
% Bracket the minimum (t(2)) between t(1) and t(3)

gold   = (1+sqrt(5))/2; % Golden ratio

t(1)   = struct('p',0,'f',f);
t(2).p = 1;
t(2).f = funeval(t(2).p);

% if t(2) not better than t(1) then swap
if t(2).f > t(1).f
    t(3) = t(1);
    t(1) = t(2);
    t(2) = t(3);
end

t(3).p = t(2).p + gold*(t(2).p-t(1).p);
t(3).f = funeval(t(3).p);

while t(2).f > t(3).f

    % fit a polynomial to t
    tmp = cat(1,t.p)-t(2).p;
    pol = pinv([ones(3,1) tmp tmp.^2])*cat(1,t.f);

    % minimum is when gradient of polynomial is zero
    % sign of pol(3) (the 2nd deriv) should be +ve
    if pol(3)>0
        % minimum is when gradient of polynomial is zero
        d    = -pol(2)/(2*pol(3)+eps);

        % A very conservative constraint on the displacement
        if d > (1+gold)*(t(3).p-t(2).p),
            d = (1+gold)*(t(3).p-t(2).p);
        end
        u.p  = t(2).p+d;
    else
        % sign of pol(3) (the 2nd deriv) is not +ve
        % so extend out by golden ratio instead
        u.p  = t(3).p+gold*(t(3).p-t(2).p);
    end

    % FUNCTION EVALUATION
    u.f  = funeval(u.p);

    if (t(2).p < u.p) == (u.p < t(3).p)

        % u is between t(2) and t(3)
        if u.f < t(3).f
            % minimum between t(2) and t(3) - done
            t(1) = t(2);
            t(2) = u;
            return
        elseif u.f > t(2).f
            % minimum between t(1) and u - done
            t(3) = u;
            return;
        end
    end

    % Move all 3 points along
    t(1) = t(2);
    t(2) = t(3);
    t(3) = u;
end

%==========================================================================
% function [f,p] = search(t, tol)
%==========================================================================
function [f,p] = search(t, tol)
% Brent's method for line searching - given that minimum is bracketed

gold1 = 1-(sqrt(5)-1)/2;

% Current and previous displacements
d     = Inf;
pd    = Inf;

% sort t into best first order
[junk,ind] = sort(cat(1,t.f));
t   = t(ind);
brk = [min(cat(1,t.p)) max(cat(1,t.p))];

for iter=1:128
    % check stopping criterion
    if abs(t(1).p - 0.5*(brk(1)+brk(2)))+0.5*(brk(2)-brk(1)) <= 2*tol
        p = t(1).p;
        f = t(1).f;
        return;
    end

    % keep last two displacents
    ppd = pd;
    pd  = d;

    % fit a polynomial to t
    tmp = cat(1,t.p)-t(1).p;
    pol = pinv([ones(3,1) tmp tmp.^2])*cat(1,t.f);

    % minimum is when gradient of polynomial is zero
    d   = -pol(2)/(2*pol(3)+eps);
    u.p = t(1).p+d;

    % check so that displacement is less than the last but two,
    % that the displaced point is between the brackets
    % and that the solution is a minimum rather than a maximum
    eps2 = 2*eps*abs(t(1).p)+eps;
    if abs(d) > abs(ppd)/2 || u.p < brk(1)+eps2 || u.p > brk(2)-eps2 || pol(3)<=0
        % if criteria are not met, then golden search into the larger part
        if t(1).p >= 0.5*(brk(1)+brk(2)),
            d = gold1*(brk(1)-t(1).p);
        else
            d = gold1*(brk(2)-t(1).p);
        end
        u.p = t(1).p+d;
    end

    % FUNCTION EVALUATION
    u.f = funeval(u.p);

    % Insert the new point into the appropriate position and update
    % the brackets if necessary
    if u.f <= t(1).f
        if u.p >= t(1).p, brk(1)=t(1).p; else brk(2)=t(1).p; end
        t(3) = t(2);
        t(2) = t(1);
        t(1) = u;
    else
        if u.p < t(1).p, brk(1)=u.p; else brk(2)=u.p; end
        if u.f <= t(2).f
            t(3) = t(2);
            t(2) = u;
        elseif u.f <= t(3).f
            t(3) = u;
        end
    end
end

%==========================================================================
% function min1d_plot(action,arg1,arg2,arg3)
%==========================================================================
function min1d_plot(action,arg1,arg2,arg3)
% Visual output for line minimisation
persistent min1dplot

if ~nargin, action = 'Init'; end

% Find the Interactive window and exit if not
%--------------------------------------------------------------------------
fg = spm_figure('FindWin','Interactive');
if isempty(fg), return; end

%-Initialize
%--------------------------------------------------------------------------
if strcmpi(action,'init')
    if nargin<4, arg3 = 'Function';          end
    if nargin<3, arg2 = 'Value';             end
    if nargin<2, arg1 = 'Line minimisation'; end

    min1dplot = struct('pointer',get(fg,'Pointer'),...
                       'name',   get(fg,'Name'),...
                       'ax',     [],...
                       'buffer', get(fg,'DoubleBuffer'));
    min1d_plot('Clear');
    set(fg,'Pointer','Watch');
    set(fg,'DoubleBuffer','on');
    min1dplot.ax = axes('Position', [0.15 0.1 0.8 0.75],...
                        'Box',      'on',...
                        'Parent',   fg);
    lab = get(min1dplot.ax,'Xlabel');
    set(lab,'string',arg3,'FontSize',10);
    lab = get(min1dplot.ax,'Ylabel');
    set(lab,'string',arg2,'FontSize',10);
    lab = get(min1dplot.ax,'Title');
    set(lab,'string',arg1);
    line('Xdata',[], 'Ydata',[],...
        'LineWidth',2,'Tag','LinMinPlot',...
        'LineStyle','-','Marker','o',...
        'Parent',min1dplot.ax);
    drawnow;

%-Reset
%--------------------------------------------------------------------------
elseif strcmpi(action,'set')
    br = findobj(fg,'Tag','LinMinPlot');
    if ~isempty(br)
        [xd,indx] = sort([get(br,'Xdata') arg1]);
        yd = [get(br,'Ydata') arg2];
        yd = yd(indx);
        set(br,'Ydata',yd,'Xdata',xd);
        drawnow;
    end

%-Clear
%--------------------------------------------------------------------------
elseif strcmpi(action,'clear')
    fg = spm_figure('FindWin','Interactive');
    if isstruct(min1dplot)
        if ishandle(min1dplot.ax), delete(min1dplot.ax); end
        set(fg,'Pointer',min1dplot.pointer);
        set(fg,'Name',min1dplot.name);
        set(fg,'DoubleBuffer',min1dplot.buffer);
    end
    spm_figure('Clear',fg);
    drawnow;
end


function [models] = featureSelectionTime_HN(X,Y,maxOrder,nBoot,Info,censoring,seed)
% -------------------------------------------------------------------------
% function [models] = featureSelection(X,Y,maxOrder,nBoot,Info,imbalance,batchNum)
% -------------------------------------------------------------------------
% DESCRIPTION:
% This function computes feature set selection according to the 0.632+
% bootstrap methodology for an input matrix of features and and input
% outcome vector, and for multiple model orders as defined by the user.
% See ref. [1] for more details. This function uses logistic regression
% utilities from DREES <http://www.cerr.info/drees>, and a fast
% implementation of AUC calculation by Enric Junqué de Fortuny that is
% available at: <http://www.mathworks.com/matlabcentral/fileexchange/41258-faster-roc-auc>
%
% NOTE: This function now maximizes 0.5*AUC + 0.5*(1-abs(SENSITIVITY-SPECIFICITY))
% -------------------------------------------------------------------------
% REFERENCE:
% [1] Vallieres, M. et al. (2015). A radiomics model from joint FDG-PET and
%     MRI texture features for the prediction of lung metastases in soft-tissue
%     sarcomas of the extremities. Physics in Medicine and Biology, 60(14),
%     5471-5496. doi:10.1088/0031-9155/60/14/5471
% -------------------------------------------------------------------------
% INPUTS:
% - X: Matrix of size [nInst X nFeat], specifying the numerical data of the
%      features of the input features, where 'nInst' refers to the number
%      of instances in X, and 'nFeat' to the number of features in X.
%      Each column is a different feature.
% - Y: Column vector of size [nInst X 1] specifying the outcome status
%      (1 or 0) for all instances.
% - maxOrder: Integer specifying the maximal model order to construct.
% - nBoot: Number of bootstrap samples to use.
% - Info: Cell of size [nFeat X 1] of strings specifying the name of each
%         feature in 'X'.
% - imbalance: String specifying the type of imbalance-adjustement strategy
%              employed. Either 'IABR' for imbalance-adjusted bootstrap
%              resampling (see ref.[1]), or 'IALR' for imbalance-adjusted
%              logistic regression.
% - batchNum: (optional input). If present, integer that specifies the
%             batch number for parallelization purposes.
% -------------------------------------------------------------------------
% OUTPUTS:
% - models: Structure specifying the resulting multivariable models for the
%           input feature set in 'data', for each model order. Example for
%           order 4:
%        --> models.Order4.Data: Matrix of size [nInst X order], specifying
%                                the selected features, in order of
%                                selection.
%        --> models.Order4.Name: Cell specifying the names of the selected
%                                features, in order of selection.
% -------------------------------------------------------------------------
% AUTHOR(S):
% - Martin Vallieres <mart.vallieres@gmail.com>
% - DREES development team <http://www.cerr.info/drees> (logistic regression)
% - Enric Junqué de Fortuny (fastAUC.cpp)
% -------------------------------------------------------------------------
% HISTORY:
% - Creation - May 2015
% - Revision I - July 2015: including imbalance-adjusted logistic regression
% - Revision II - July 2015: maximizing 0.5*AUC + 0.5*(1-abs(SENSITIVITY-SPECIFICITY))
%--------------------------------------------------------------------------
% STATEMENT:
% This file is part of <https://github.com/mvallieres/radiomics/>,
% a package providing MATLAB programming tools for radiomics analysis.
% --> Copyright (C) 2015  Martin Vallieres
% --> Copyright 2010, Joseph O. Deasy, on behalf of the DREES development team.
%
%    This package 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 package 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 package.  If not, see <http://www.gnu.org/licenses/>.
%
%    _______________________________________________________________
%
% --> Copyright (c) 2013, Enric Junqué de Fortuny
%     All rights reserved.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are
% met:
%
%     * Redistributions of source code must retain the above copyright
%       notice, this list of conditions and the following disclaimer.
%     * Redistributions in binary form must reproduce the above copyright
%       notice, this list of conditions and the following disclaimer in
%       the documentation and/or other materials provided with the distribution
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
% POSSIBILITY OF SUCH DAMAGE.
% -------------------------------------------------------------------------

% RANDOM NUMBER GENERATOR SEED
rng(seed);

% INITIALIZATION
top = 1-1/exp(1);
low = 1/exp(1);
nFeat = size(X,2);
models = struct; % Final model structure

% GETTING BOOTSTRAP SAMPLES FOR ALL EXPERIMENTS
nInst = numel(Y);
trainSets = ceil(nInst .* rand(nInst,nBoot));
testSets = findBootTestSet(trainSets);

% FORWARD FEATURE SELECTION (for all different starters)
modelMat = zeros(nFeat,maxOrder);
metricMat = zeros(nFeat,maxOrder);
for i = 1:nFeat
     indLeft = 1:nFeat;
     % Order 1
     indLeft(i) = [];
     modelMat(i,1) = i;
     Xtrain = X(:,i);
     [Xtrain] = normalizeZeroOne(Xtrain);

     % Cox regression code
     CIdata = applyCoxfit(Xtrain,Y,Xtrain,Y,censoring,censoring);

     CItemp = 0;
     for n = 1:nBoot
         Xtrain = X(trainSets(:,n),i);Xtest = X(testSets{n},i); Ytrain = Y(trainSets(:,n),1); Ytest = Y(testSets{n},1);
         [Xtrain,Xtest] = normalizeZeroOne(Xtrain,Xtest);

         % Cox regression code
         CIboot = applyCoxfit(Xtrain,Ytrain,Xtest,Ytest,censoring(trainSets(:,n)),censoring(testSets{n}));

         % FOR CI (concordance index)
         alpha = top/(1-low*(CIdata-CIboot)/(CIdata-0.5+eps));
         if alpha > 1
             alpha = 1;
         elseif alpha < top
             alpha = top;
         end
         if CIboot < 0.5
             CIboot = 0.5;
         end
         CItemp = CItemp + (1-alpha)*CIdata + alpha*CIboot;

     end
     CItemp = CItemp/nBoot;
     metricMat(i,1) = CItemp;

     % Going for orders 2 to maxOrder
     for j = 2:maxOrder
         maxMetric = 0;
         for k = 1:(nFeat-j+1)
             indexModel = [modelMat(i,1:(j-1)),indLeft(k)];
             Xtrain = X(:,indexModel);
             [Xtrain] = normalizeZeroOne(Xtrain);

             % Cox regression code
             CIdata = applyCoxfit(Xtrain,Y,Xtrain,Y,censoring,censoring);

             CItemp = 0;
             for n = 1:nBoot
                 Xtrain = X(trainSets(:,n),indexModel); Xtest = X(testSets{n},indexModel); Ytrain = Y(trainSets(:,n),1); Ytest = Y(testSets{n},1);
                 [Xtrain,Xtest] = normalizeZeroOne(Xtrain,Xtest);

                 % Cox regression code
                 CIboot = applyCoxfit(Xtrain,Ytrain,Xtest,Ytest,censoring(trainSets(:,n)),censoring(testSets{n}));

                 % FOR CI (concordance index)
                 alpha = top/(1-low*(CIdata-CIboot)/(CIdata-0.5+eps));
                 if alpha > 1
                     alpha = 1;
                 elseif alpha < top
                     alpha = top;
                 end
                 if CIboot < 0.5
                     CIboot = 0.5;
                 end
                 CItemp = CItemp + (1-alpha)*CIdata + alpha*CIboot;

             end
             CItemp = CItemp/nBoot;
             metricTemp = CItemp;
             if metricTemp >= maxMetric
                 maxMetric = metricTemp;
                 index = indLeft(k);
             end
         end
         modelMat(i,j) = index;
         metricMat(i,j) = maxMetric;
         indLeft(find(indLeft==index)) = [];
     end
end

% OBTAINING MAXIMUM RESULTS FOR EVERY MODEL ORDER (maximum from all different starters)
[~,indMax] = max(metricMat);
for i = 1:maxOrder
    nameOrder = ['Order',num2str(i)];
    models.(nameOrder).Data = X(:,modelMat(indMax(i),1:i));
    models.(nameOrder).Name = cell(i,1);
    for j = 1:i
        models.(nameOrder).Name{j} = Info{modelMat(indMax(i),j)};
    end
end

end


function info = muteinf(A, Y, method)
	n = size(A,1);
	Z = [A Y];
	if(n/10 > 20)
		nbins = 20;
	else
		nbins = max(floor(n/10),10);
	end;
	pA = hist(A, nbins);
	pA = pA ./ n;

	i = find(pA == 0); % that's a hack!
	pA(i) = 0.00001;
	if(strcmp(method,'regression'))
		pY = hist(Y, nbins);
		pY = pY ./ n;
		j = find(pY == 0);
		pY(j) = 0.00001;

		rx = abs(max(A) - min(A)) / nbins;
		ry = abs(max(Y) - min(Y)) / nbins;
		yl = min(Y);
		p = zeros(nbins, nbins);
		for i = 1:nbins
			xl = min(A);
			for j = 1:nbins
				%disp(['intervals [' num2str(xl) ',' num2str(xl+rx) '|' num2str(yl) ',' num2str(yl+ry) ']'])
				interval = (xl <= Z(:,1)) & (yl <= Z(:,2));
				if(j < nbins)
					interval = interval & (Z(:,1) < xl + rx);
				end;
				if(i < nbins)
					interval = interval & (Z(:,2) < yl + ry);
				end;
				%find(interval)
				p(i,j) = length(find(interval));

				if p(i,j) == 0 % hack!
					p(i,j) = 0.00001;
				end

				xl = xl + rx;
			end;
			yl = yl + ry;
		end;
		HA = -sum(pA .* log(pA));
		HY = -sum(pY .* log(pY));
		pA = repmat(pA,nbins,1);
		pY = repmat(pY',1,nbins);
	else
		od = size(Y,2);
		cl = od;
		if(od == 1)
			pY = [length(find(Y==+1)) length(find(Y==-1))] / n;
			cl = 2;
		else
			pY = zeros(1,od);
			for i=1:od
				pY(i) = length(find(Y==+1));
			end;
			pY = pY / n;
		end;
		p = zeros(cl,nbins);
		rx = abs(max(A) - min(A)) / nbins;
		for i = 1:cl
			xl = min(A);
			for j = 1:nbins
				if(i == 2) & (od == 1)
					interval = (xl <= Z(:,1)) & (Z(:,2) == -1);
				else
					interval = (xl <= Z(:,1)) & (Z(:,i+1) == +1);
				end;
				if(j < nbins)
					interval = interval & (Z(:,1) < xl + rx);
				end;
				%find(interval)
				p(i,j) = length(find(interval));

				if p(i,j) == 0 % hack!
					p(i,j) = 0.00001;
				end

				xl = xl + rx;
			end;
		end;
		HA = -sum(pA .* log(pA));
		HY = -sum(pY .* log(pY));
		pA = repmat(pA,cl,1);
		pY = repmat(pY',1,nbins);
	end;
	p = p ./ n;

	info = sum(sum(p .* log(p ./ (pA .* pY))));
	info = 2 * info ./ (HA + HY);


function xf = cfft2(x)

% calculate output size
in_sz = size(x);

% if both dimensions are odd
if all(mod(in_sz(1:2), 2) == 1)
    xf = fftshift(fftshift(fft2(x), 1), 2);
else
    out_sz = in_sz;
    out_sz(1:2) = out_sz(1:2) + mod(out_sz(1:2)+1,2);

    % allocate
    xf = complex(zeros(out_sz, 'single'));

    xf(1:in_sz(1),1:in_sz(2),:,:) = fftshift(fftshift(fft2(x), 1), 2);

    if out_sz(1) ~= in_sz(1)
        xf(end,:,:,:) = conj(fliplr(xf(1,:,:,:)));
    end
    if out_sz(2) ~= in_sz(2)
        xf(:,end,:,:) = conj(flipud(xf(:,1,:,:)));
    end
end


function det = dgbdi ( abd, lda, n, ml, mu, ipvt )

%*****************************************************************************80
%
%% DGBDI computes the determinant of a band matrix factored by DGBCO or DGBFA.
%
%  Discussion:
%
%    If the inverse is needed, use DGBSL N times.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 June 2005
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Dongarra, Moler, Bunch and Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%    ISBN 0-89871-172-X
%
%  Parameters:
%
%    Input, real ABD(LDA,N), the output from DGBCO or DGBFA.
%
%    Input, integer LDA, the leading dimension of the array ABD.
%
%    Input, integer N, the order of the matrix.
%
%    Input, integer ML, MU, the number of diagonals below and above the
%    main diagonal.  0 <= ML < N, 0 <= MU < N.
%
%    Input, integer IPVT(N), the pivot vector from DGBCO or DGBFA.
%
%    Output, real DET(2), the determinant of the original matrix.
%      determinant = DET(1) * 10.0**DET(2)
%    with  1.0 <= abs ( DET(1) ) < 10.0 or DET(1) = 0.0.
%
  ten = 10.0;

  m = ml + mu + 1;
  det(1) = 1.0;
  det(2) = 0.0;

  for i = 1 : n

    if ( ipvt(i) ~= i )
      det(1) = -det(1);
    end

    det(1) = abd(m,i) * det(1);

    if ( det(1) == 0.0 )
      return
    end

    while ( abs ( det(1) ) < 1.0 )
      det(1) = ten * det(1);
      det(2) = det(2) - 1.0;
    end

    while ( ten <= abs ( det(1) ) )
      det(1) = det(1) / ten;
      det(2) = det(2) + 1.0;
    end

  end

  return
end


function varargout = createSoccerBall()
%CREATESOCCERBALL Create a 3D mesh representing a soccer ball.
%
%   It is basically a wrapper of the 'bucky' function in matlab.
%   [V, E, F] = createSoccerBall
%   return vertices, edges and faces that constitute a soccerball
%   V is a 60-by-3 array containing vertex coordinates
%   E is a 90-by-2 array containing indices of neighbor vertices
%   F is a 32-by-1 cell array containing vertex indices of each face
%   Example
%   [v, f] = createSoccerBall;
%   drawMesh(v, f);
%
%   See also
%   meshes, drawMesh, bucky

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2006-08-09
% Copyright 2006 INRA - CEPIA Nantes - MIAJ (Jouy-en-Josas).

%   HISTORY
%   2007-01-04 remove unused variables, enhance output processing
%   2010-12-07 clean up edges, uses formatMeshOutput

% get vertices and adjacency matrix of the buckyball
[b, n] = bucky;

% compute edges
[i, j] = find(b);
e = [i j];
e = unique(sort(e, 2), 'rows');

% compute polygons that correspond to each 3D face
f = minConvexHull(n)';

% format output
varargout = formatMeshOutput(nargout, n, e, f);


function [ a1, a2 ] = r8vec2_sort_a ( n, a1, a2 )

%*****************************************************************************80
%
%% R8VEC2_SORT_A ascending sorts an R8VEC2.
%
%  Discussion:
%
%    An R8VEC2 is two R8VEC's.
%
%    An R8VEC is a vector of R8 values.
%
%    Each item to be sorted is a pair (I,J), with the I
%    and J values stored in separate vectors A1 and A2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 November 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of items of data.
%
%    Input, real A1(N), A2(N), the data to be sorted.
%
%    Output, real A1(N), A2(N), the sorted data.
%
  if ( n <= 1 )
    return
  end
%
%  Initialize.
%
  i = 0;
  indx = 0;
  isgn = 0;
  j = 0;
%
%  Call the external heap sorter.
%
  while ( 1 )

    [ indx, i, j ] = sort_heap_external ( n, indx, isgn );
%
%  Interchange the I and J objects.
%
    if ( 0 < indx )

      [ a1(i), a1(j) ] = r8_swap ( a1(i), a1(j) );
      [ a2(i), a2(j) ] = r8_swap ( a2(i), a2(j) );
%
%  Compare the I and J objects.
%
    elseif ( indx < 0 )

      isgn = r8vec2_compare ( n, a1, a2, i, j );

    elseif ( indx == 0 )

      break

    end

  end

  return
end


function [assignment, cost] = assignmentsuboptimal1(distMatrix)

%ASSIGNMENTSUBOPTIMAL1    Compute suboptimal assignment
%		ASSIGNMENTSUBOPTIMAL1(DISTMATRIX) computes a suboptimal assignment for the
%		given rectangular distance (or weight) matrix, for example the assignment
%		of tracks (in rows) to observations (in columns). The result is a column
%		vector containing the assigned column number in each row (or 0 if no
%		assignment could be done).
%
%		[ASSIGNMENT, COST] = ASSIGNMENTSUBOPTIMAL1(DISTMATRIX) returns the
%		assignment vector and the overall cost.
%
%		The algorithm is designed for distance matrices with many forbidden and
%		singly validated assignments, (rows or columns containing only one finite
%		element). The algorithm first searches the matrix for singly validated
%		columns and rejects all assignments with multiply validated row.
%		Afterwards, singly validated rows are searched and assignments to
%		multiply validated columns are rejected. Then, for each row that
%		validates only with singly validated columns (and the other way around),
%		the minimum elements is chosen and the assignment is made. If there are
%		still assignments open, the minimum element in the distMatrix is
%		searched and the corresponding assignment is made.
%
%		In scenarios without any forbidden assignments, the algorithm reduceds
%		to the last step is will give the same result as ASSIGNMENTOPTIMAL2. If
%		there are only some assignments forbidden, the algorithm will perform
%		poorly because singly validated assignments are preferred.
%
%		The last step can still be optimized, see the comments in
%		ASSIGNMENTOPTIMAL2.
%
%		Written by Markus Buehren, www.Lss.uni-stuttgart.de
%		Last modified 14.12.2004

% initialize
[nOfRows, nOfColumns] = size(distMatrix);
nOfValidObservations  = zeros(nOfRows,1);
nOfValidTracks        = zeros(1,nOfColumns);
assignment            = zeros(nOfRows,1);
cost                  = 0;

% compute number of validations for each track
for row=1:nOfRows
	nOfValidObservations(row) = length(find(isfinite(distMatrix(row,:))));
end

if any(nOfValidObservations < nOfColumns)

	if all(nOfValidObservations == 0)
		return
	end

	repeatSteps = 1;
	while repeatSteps

		repeatSteps = 0;

		% step 1: reject assignments of multiply validated tracks to singly validated observations
		for col=1:nOfColumns
			index = isfinite(distMatrix(:,col));
			nOfValidTracks(col) = length(find(index));
			if any(nOfValidObservations(index) == 1)
				index = index & (nOfValidObservations > 1);
				if any(index)
					distMatrix(index, col)      = inf;
					nOfValidObservations(index) = nOfValidObservations(index) - 1;
					nOfValidTracks(col)         = nOfValidTracks(col) - length(find(index));
					repeatSteps = 1;
				end
			end
		end

		% step 2: reject assignments of multiply validated observations to singly validated tracks
		if nOfColumns > 1
			for row=1:nOfRows
				index = isfinite(distMatrix(row,:));
				if any(nOfValidTracks(index) == 1)
					index = index & (nOfValidTracks > 1);
					if any(index)
						distMatrix(row, index)    = inf;
						nOfValidTracks(index)     = nOfValidTracks(index) - 1;
						nOfValidObservations(row) = nOfValidObservations(row) - length(find(index));
						repeatSteps = 1;
					end
				end
			end
		end

	end % while repeatSteps
	%disp(sprintf('xx = %d', xx));

	% for each multiply validated track that validates only with singly validated
	% observations, choose the observation with minimum distance
	for row=1:nOfRows
		if nOfValidObservations(row) > 1
			index = isfinite(distMatrix(row,:));
			if all(nOfValidTracks(index) == 1)
				[minDist, col] = min(distMatrix(row,:));
				assignment(row)    = col;
				cost               = cost + minDist;
				distMatrix(row,:)  = inf;
				distMatrix(:,col)  = inf;
			end
		end
	end

	% for each multiply validated observation that validates only with singly validated
	% track, choose the track with minimum distance
	for col=1:nOfColumns
		if nOfValidTracks(col) > 1
			index = isfinite(distMatrix(:,col));
			if all(nOfValidObservations(index) == 1)
				[minDist, row] = min(distMatrix(:,col));
				assignment(row)    = col;
				cost               = cost + minDist;
				distMatrix(row,:)  = inf;
				distMatrix(:,col)  = inf;
			end
		end
	end

end

% now, recursively search for the minimum element and do the assignment
while 1

	% find minimum distance observation-to-track pair
	[minDist, index1] = min(distMatrix, [], 1);
	[minDist, index2] = min(minDist);
	row = index1(index2);
	col = index2;

	if isfinite(minDist)

		% make the assignment
		assignment(row)    = col;
		cost               = cost + minDist;
		distMatrix(row, :) = inf;
		distMatrix(:, col) = inf;

	else
		break
	end

end


function c = tapas_hgf_binary_config
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Contains the configuration for the Hierarchical Gaussian Filter (HGF)
% for binary inputs in the absence of perceptual uncertainty.
%
% The HGF is the model introduced in
%
% Mathys C, Daunizeau J, Friston, KJ, and Stephan KE. (2011). A Bayesian foundation
% for individual learning under uncertainty. Frontiers in Human Neuroscience, 5:39.
%
% The binary HGF model has since been augmented with a positive factor kappa1 which
% scales the second level with respect to the first, i.e., the relation between the
% first and second level is
%
% p(x1=1|x2) = s(kappa1*x2), where s(.) is the logistic sigmoid.
%
% By default, kappa1 is fixed to 1, leading exactly to the model introduced in
% Mathys et al. (2011).
%
% This file refers to BINARY inputs (Eqs 1-3 in Mathys et al., (2011));
% for continuous inputs, refer to tapas_hgf_config.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% The HGF configuration consists of the priors of parameters and initial values. All priors are
% Gaussian in the space where the quantity they refer to is estimated. They are specified by their
% sufficient statistics: mean and variance (NOT standard deviation).
%
% Quantities are estimated in their native space if they are unbounded (e.g., the omegas). They are
% estimated in log-space if they have a natural lower bound at zero (e.g., the sigmas).
%
% Parameters can be fixed (i.e., set to a fixed value) by setting the variance of their prior to
% zero. Aside from being useful for model comparison, the need for this arises whenever the scale
% and origin at the j-th level are arbitrary. This is the case if the observation model does not
% contain the representations mu_j and sigma_j. A choice of scale and origin is then implied by
% fixing the initial value mu_j_0 of mu_j and either kappa_j-1 or omega_j-1.
%
% Fitted trajectories can be plotted by using the command
%
% >> tapas_hgf_binary_plotTraj(est)
%
% where est is the stucture returned by tapas_fitModel. This structure contains the estimated
% perceptual parameters in est.p_prc and the estimated trajectories of the agent's
% representations (cf. Mathys et al., 2011). Their meanings are:
%
%         est.p_prc.mu_0       row vector of initial values of mu (in ascending order of levels)
%         est.p_prc.sa_0       row vector of initial values of sigma (in ascending order of levels)
%         est.p_prc.rho        row vector of rhos (representing drift; in ascending order of levels)
%         est.p_prc.ka         row vector of kappas (in ascending order of levels)
%         est.p_prc.om         row vector of omegas (in ascending order of levels)
%
% Note that the first entry in all of the row vectors will be NaN because, at the first level,
% these parameters are either determined by the second level (mu_0 and sa_0) or undefined (rho,
% kappa, and omega).
%
%         est.traj.mu          mu (rows: trials, columns: levels)
%         est.traj.sa          sigma (rows: trials, columns: levels)
%         est.traj.muhat       prediction of mu (rows: trials, columns: levels)
%         est.traj.sahat       precisions of predictions (rows: trials, columns: levels)
%         est.traj.v           inferred variance of random walk (rows: trials, columns: levels)
%         est.traj.w           weighting factors (rows: trials, columns: levels)
%         est.traj.da          volatility prediction errors  (rows: trials, columns: levels)
%         est.traj.ud          updates with respect to prediction  (rows: trials, columns: levels)
%         est.traj.psi         precision weights on prediction errors  (rows: trials, columns: levels)
%         est.traj.epsi        precision-weighted prediction errors  (rows: trials, columns: levels)
%         est.traj.wt          full weights on prediction errors (at the first level,
%                                  this is the learning rate) (rows: trials, columns: levels)
%
% Note that in the absence of sensory uncertainty (which is the assumption here), the first
% column of mu, corresponding to the first level, will be equal to the inputs. Likewise, the
% first column of sa will be 0 always.
%
% Tips:
% - When analyzing a new dataset, take your inputs u and use
%
%   >> est = tapas_fitModel([], u, 'tapas_hgf_binary_config', 'tapas_bayes_optimal_binary_config');
%
%   to determine the Bayes optimal perceptual parameters (given your current priors as defined in
%   this file here, so choose them wide and loose to let the inputs influence the result). You can
%   then use the optimal parameters as your new prior means for the perceptual parameters.
%
% - If you get an error saying that the prior means are in a region where model assumptions are
%   violated, lower the prior means of the omegas, starting with the highest level and proceeding
%   downwards.
%
% - Alternatives are lowering the prior means of the kappas, if they are not fixed, or adjusting
%   the values of the kappas or omegas, if any of them are fixed.
%
% - If the log-model evidence cannot be calculated because the Hessian poses problems, look at
%   est.optim.H and fix the parameters that lead to NaNs.
%
% - Your guide to all these adjustments is the log-model evidence (LME). Whenever the LME increases
%   by at least 3 across datasets, the adjustment was a good idea and can be justified by just this:
%   the LME increased, so you had a better model.
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2012-2017 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Config structure
c = struct;

% Model name
c.model = 'hgf_binary';

% Number of levels (minimum: 3)
c.n_levels = 3;

% Input intervals
% If input intervals are irregular, the last column of the input
% matrix u has to contain the interval between inputs k-1 and k
% in the k-th row, and this flag has to be set to true
c.irregular_intervals = false;

% Sufficient statistics of Gaussian parameter priors

% Initial mus and sigmas
% Format: row vectors of length n_levels
% For all but the first two levels, this is usually best
% kept fixed to 1 (determines origin on x_i-scale). The
% first level is NaN because it is determined by the second,
% and the second implies neutrality between outcomes when it
% is centered at 0.
c.mu_0mu = [NaN, 0, 1];
c.mu_0sa = [NaN, 0, 0];

c.logsa_0mu = [NaN,   log(0.1), log(1)];
c.logsa_0sa = [NaN,          0,      0];

% Rhos
% Format: row vector of length n_levels.
% Undefined (therefore NaN) at the first level.
% Fix this to zero to turn off drift.
c.rhomu = [NaN, 0, 0];
c.rhosa = [NaN, 0, 0];

% Kappas
% Format: row vector of length n_levels-1.
% Fixing log(kappa1) to log(1) leads to the original HGF model.
% Higher log(kappas) should be fixed (preferably to log(1)) if the
% observation model does not use mu_i+1 (kappa then determines the
% scaling of x_i+1).
c.logkamu = [log(1), log(1)];
c.logkasa = [     0,      0];

% Omegas
% Format: row vector of length n_levels.
% Undefined (therefore NaN) at the first level.
c.ommu = [NaN,  -3,  -6];
c.omsa = [NaN, 4^2, 4^2];

% Gather prior settings in vectors
c.priormus = [
    c.mu_0mu,...
    c.logsa_0mu,...
    c.rhomu,...
    c.logkamu,...
    c.ommu,...
         ];

c.priorsas = [
    c.mu_0sa,...
    c.logsa_0sa,...
    c.rhosa,...
    c.logkasa,...
    c.omsa,...
         ];

% Check whether we have the right number of priors
expectedLength = 3*c.n_levels+2*(c.n_levels-1)+1;
if length([c.priormus, c.priorsas]) ~= 2*expectedLength;
    error('tapas:hgf:PriorDefNotMatchingLevels', 'Prior definition does not match number of levels.')
end

% Model function handle
c.prc_fun = @tapas_hgf_binary;

% Handle to function that transforms perceptual parameters to their native space
% from the space they are estimated in
c.transp_prc_fun = @tapas_hgf_binary_transp;

return;


% LOTKA-VOLTERRA system
% System identification: DelayDMDc

clear all, close all, clc
figpath = '../FIGURES/';
datapath = '../DATA/';
addpath('../utils');

ModelName = 'SparseModels';
%% Generate Data
InputSignalType = 'sphs';%prbs; chirp; noise; sine2; sphs; mixed
Ndelay = 1;%35;%35;
ONLY_TRAINING_LENGTH = 1;
getTrainingData

DataTrain.x = x;
DataTrain.t = t;
DataTrain.tspan = tspan;
DataTrain.u = u;
DataTrain.xmean = xmean;
xstd = std(DataTrain.x(:,1));

%% Parameters
close all
rng(0,'twister')

eta_vec = [0.01 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5];
N_ETA = length(eta_vec);

lambda_vec = [0,1e-5,1e-4,1e-3,1e-2,1e-1,1e-0,1e+1];
N_LAMBDA = length(lambda_vec);

model_collection = {'sDMDc','sEDMDc2', 'sEDMDc3','SINDYc'};
N_MODEL = length(model_collection);

N_REP = 30;

options_method.sparsify = 'ILSTH'; % 'LASSO'
options_method.usesine  = 0;

nVars = 2;
%% Initialization
TrainingError = inf*ones(N_ETA,N_LAMBDA,N_MODEL,N_REP);
ValidationError = inf*ones(N_ETA,N_LAMBDA,N_MODEL,N_REP);

PredictionResults_Training = zeros(1000,nVars,N_ETA,N_LAMBDA,N_MODEL,N_REP);
PredictionResults_Validation = zeros(1000,nVars,N_ETA,N_LAMBDA,N_MODEL,N_REP);

%% Model Identification

for iR = 1:N_REP
    counter = 0;

    tstart = tic;
    for iEta = 1:N_ETA

        % Add noise to data
        eps     = eta_vec(iEta)*xstd;
        x       = DataTrain.x + eps*randn(size(DataTrain.x));
        u       = DataTrain.u;
        M       = length(x);

        for iLambda = 1:N_LAMBDA
            lambda = lambda_vec(iLambda);

            %% Model Identification
            Hx  = getHankelMatrix_MV(x - repmat(DataTrain.xmean,[M 1]),1); % No time delay (=1)
            Hu  = getHankelMatrix_MV(u',1);

            % sparse DMDc
            options_method.order  = 1;
            yout = poolData(Hx',nVars,options_method.order,0);
            Y = yout(1:end-1,2:end)'; % remove constant
            Yp = yout(2:end,2:end)';
            U = Hu(1:end-1);
            sys_DMDc = SparseRegression(Y,Yp,U,dt, options_method, lambda);

            % sparse EDMDc
            options_method.order  = 2;
            yout = poolData(Hx',nVars,options_method.order,0);
            Y = yout(1:end-1,2:end)'; % remove constant
            Yp = yout(2:end,2:end)';
            U = Hu(1:end-1);
            sys_EDMDc2 = SparseRegression(Y,Yp,U,dt, options_method, lambda);

            % sparse EDMDc
            options_method.order  = 3;
            yout = poolData(Hx',nVars,options_method.order,0);
            Y = yout(1:end-1,2:end)'; % remove constant
            Yp = yout(2:end,2:end)';
            U = Hu(1:end-1);
            sys_EDMDc3 = SparseRegression(Y,Yp,U,dt, options_method, lambda);

            % SINDYc (not xmean-substracted)
            options_method.order  = 3;
            Xi = NonlinearSparseRegression(x,u',dt,options_method,lambda);

            %% Prediction on training data
            Nt = M-1;
            % sparse DMDc
            options_method.order  = 1;
            x0train = poolData(Hx(:,1)',nVars,options_method.order,0)';
            x0train = x0train(2:end);
            [xDMDc,~] = lsim(sys_DMDc,Hu,tspan(1:Nt),x0train);
            xDMDc = xDMDc(:,end-1:end);
            xDMDc = xDMDc + repmat(xmean,[Nt 1]);
            xPredTrain{1}.x = xDMDc;

            % sparse EDMDc
            options_method.order  = 2;
            x0train = poolData(Hx(:,1)',nVars,options_method.order,0)';
            x0train = x0train(2:end);
            [xEMD2,~] = lsim(sys_EDMDc2,Hu,tspan(1:Nt),x0train);
            xEMD2 = xEMD2(:,1:2);
            xEMD2 = xEMD2 + repmat(xmean,[Nt 1]);
            xPredTrain{2}.x = xEMD2;

            % sparse EDMDc
            options_method.order  = 3;
            x0train = poolData(Hx(:,1)',nVars,options_method.order,0)';
            x0train = x0train(2:end);
            [xEMD3,~] = lsim(sys_EDMDc3,Hu,tspan(1:Nt),x0train);
            xEMD3 = xEMD3(:,1:2);
            xEMD3 = xEMD3 + repmat(xmean,[Nt 1]);
            xPredTrain{3}.x = xEMD3;

            % SINDYc
            p.ahat = Xi(:,1:2);
            p.polyorder = options_method.order; p.usesine = options_method.usesine; p.dt = dt;
            [Ns,N] = size(Hx);
            xSINDYc = zeros(Ns,N); xSINDYc(:,1) = x(1,:)';
            for ct=1:N-1
                xSINDYc(:,ct+1) = rk4u(@sparseGalerkinControl_Discrete,xSINDYc(:,ct),DataTrain.u(ct),dt,1,[],p);
            end
            xSINDYc = xSINDYc';
            xPredTrain{4}.x = xSINDYc;

            % Store results
            for iM = 1:N_MODEL
                PredictionResults_Training(:,:,N_ETA,N_LAMBDA,iM,N_REP) = xPredTrain{iM}.x;
            end

            %             clear ph
            %             figure,box on,
            %             ccolors = get(gca,'colororder');
            %             ph(1) = plot(tspan(1:Nt),x(1:Nt,1),'-','Color','k','LineWidth',1); hold on
            %             plot(tspan(1:Nt),x(1:Nt,2),'-','Color','k','LineWidth',1);
            %             for iM = 1:N_MODEL
            %                 for iVar = 1:nVars
            %                     ph(iM+1) = plot(tspan(1:Nt),xPredTrain{iM}.x(:,iVar),'--','Color',ccolors(iM,:),'LineWidth',1); hold on
            %                 end
            %             end
            %             legend(ph,model_collection)

            for iM = 1:N_MODEL
                TrainingError(iEta,iLambda,iM,iR) = norm(x(1:Nt,:)-xPredTrain{iM}.x,'fro');
            end

            %disp(['Training Error: ', num2str(squeeze(TrainingError(iEta,iLambda,:,iR))')])
            %             disp(['                DMDc: ', num2str(DMDc_err)])
            %             disp(['          sparseDMDc: ', num2str(sparseDMDc_err)])
            %% Prediction on validation data
            Nt = M-1;
            % sparse DMDc
            options_method.order  = 1;
            x0train = poolData(Hx(:,1)',nVars,options_method.order,0)';
            x0train = x0train(2:end);
            [xDMDc,~] = lsim(sys_DMDc,Hu,tspan(1:Nt),x0train);
            xDMDc = xDMDc(:,end-1:end);
            xDMDc = xDMDc + repmat(xmean,[Nt 1]);
            xPredTrain{1}.x = xDMDc;

            % sparse EDMDc
            options_method.order  = 2;
            x0train = poolData(Hx(:,1)',nVars,options_method.order,0)';
            x0train = x0train(2:end);
            [xEMD2,~] = lsim(sys_EDMDc2,Hu,tspan(1:Nt),x0train);
            xEMD2 = xEMD2(:,1:2);
            xEMD2 = xEMD2 + repmat(xmean,[Nt 1]);
            xPredTrain{2}.x = xEMD2;

            % sparse EDMDc
            options_method.order  = 3;
            x0train = poolData(Hx(:,1)',nVars,options_method.order,0)';
            x0train = x0train(2:end);
            [xEMD3,~] = lsim(sys_EDMDc3,Hu,tspan(1:Nt),x0train);
            xEMD3 = xEMD3(:,1:2);
            xEMD3 = xEMD3 + repmat(xmean,[Nt 1]);
            xPredTrain{3}.x = xEMD3;

            % SINDYc
            p.ahat = Xi(:,1:2);
            p.polyorder = options_method.order; p.usesine = options_method.usesine; p.dt = dt;
            [Ns,N] = size(Hx);
            xSINDYc = zeros(Ns,N); xSINDYc(:,1) = x(1,:)';
            for ct=1:N-1
                xSINDYc(:,ct+1) = rk4u(@sparseGalerkinControl_Discrete,xSINDYc(:,ct),DataTrain.u(ct),dt,1,[],p);
            end
            xSINDYc = xSINDYc';
            xPredTrain{4}.x = xSINDYc;

            % Store results
            for iM = 1:N_MODEL
                PredictionResults_Validation(:,:,N_ETA,N_LAMBDA,iM,N_REP) = xPredTrain{iM}.x;
            end

            for iM = 1:N_MODEL
                ValidationError(iEta,iLambda,iM,iR) = norm(x(1:Nt,:)-xPredTrain{iM}.x,'fro');
            end

            counter = counter + 1;
        end
    end

    tstop = toc(tstart);
    disp(['STATUS (',num2str(iR),' of ', num2str(N_REP),'): ', num2str(100*counter/(N_ETA*N_LAMBDA)),'%', '[Time=',num2str(tstop),'s]'])
end

%% STATS
idxnan = isnan(TrainingError(:));
maxerr = max(TrainingError(:));
TrainingError(idxnan) = maxerr; % change later
ErrStats.mean = zeros(N_ETA,N_LAMBDA,N_MODEL);
ErrStats.median = zeros(N_ETA,N_LAMBDA,N_MODEL);
ErrStats.std = zeros(N_ETA,N_LAMBDA,N_MODEL);
ErrStats.min = zeros(N_ETA,N_LAMBDA,N_MODEL);
for iEta = 1:N_ETA
    for iLambda = 1:N_LAMBDA
        for iModel = 1:N_MODEL
            ErrStats.mean(iEta,iLambda,iModel) = mean(TrainingError(iEta,iLambda,iModel,:));
            ErrStats.median(iEta,iLambda,iModel) = median(TrainingError(iEta,iLambda,iModel,:));
            ErrStats.std(iEta,iLambda,iModel) = std(TrainingError(iEta,iLambda,iModel,:));
            ErrStats.min(iEta,iLambda,iModel) = min(TrainingError(iEta,iLambda,iModel,:));
        end
    end
end

%%
err_axis = [min(TrainingError(:)), 1e3];%max(TrainingError(:))
figure,
for iModel = 1:N_MODEL
    subplot(4,4,(iModel-1)*4+1)
    surf(lambda_vec,eta_vec,ErrStats.median(:,:,iModel)), shading interp
    xlabel('\lambda'), ylabel('\eta'), zlabel('median')
    %     zlim([0 1e6])
    caxis(err_axis);
    set(gca,'xscale','log')
    view(2)

    subplot(4,4,(iModel-1)*4+2)
    surf(lambda_vec,eta_vec,ErrStats.mean(:,:,iModel)) , shading interp
    xlabel('\lambda'), ylabel('\eta'), zlabel('mean')
    %     zlim([0 1e6])
    caxis(err_axis);
    set(gca,'xscale','log')
    view(2)

    subplot(4,4,(iModel-1)*4+3)
    surf(lambda_vec,eta_vec,ErrStats.std(:,:,iModel)) , shading interp
    xlabel('\lambda'), ylabel('\eta'), zlabel('std')
    %     zlim([0 1e6])
    caxis(err_axis);
    set(gca,'xscale','log')
    view(2)

    subplot(4,4,(iModel-1)*4+4)
    surf(lambda_vec,eta_vec,ErrStats.min(:,:,iModel)) , shading interp
    xlabel('\lambda'), ylabel('\eta'), zlabel('min')
    %     zlim([0 1e6])
    caxis(err_axis);
    set(gca,'xscale','log')
    view(2)
end
% for iEta = 1:N_ETA
%     for iLambda = 1:N_LAMBDA
%         plot3()
%     end
% end

%% TIme series stats
iEta = 1; iLambda = 1; iM = 4;
data = squeeze(PredictionResults_Training(:,:,iEta,iLambda,iM,N_REP));

xBmin = min(data(:,1:nVars,:),[],3);
xBmax = max(data(:,1:nVars,:),[],3);
clear ph
figure('visible','off'),box on, hold on,
ccolors = get(gca,'colororder');
plot([tB(1),tB(1)],[-25 65],':','Color',[0.4,0.4,0.4],'LineWidth',1.5)
plot([t(end),t(end)],[-25 65],':','Color',[0.4,0.4,0.4],'LineWidth',1.5)
ylim([-25 65])
t1 = text(5,55,'Training', 'FontSize',12);
t2 = text(5+tA(1),55,'Validation', 'FontSize',12);

X=[tB(2:end)',fliplr(tB(2:end)')];                %#create continuous x value array for plotting
Y=[xBmin(:,1)',flipud(xBmax(:,1))'];              %#create y values for out and then back
fillh1 = fill(X,Y,ccolors(1,:)-[0 0.2 0.2]);                  %#plot filled area
fillh1.EdgeColor = ccolors(1,:)-[0 0.2 0.2]; fillh1.FaceAlpha = 0.5;

X=[tB(2:end)',fliplr(tB(2:end)')];                %#create continuous x value array for plotting
Y=[xBmin(:,2)',flipud(xBmax(:,2))'];              %#create y values for out and then back
fillh = fill(X,Y,ccolors(2,:)-[0.1 0.2 0.09]);                  %#plot filled area
fillh.EdgeColor = ccolors(2,:)-[0.1 0.2 0.09]; fillh.FaceAlpha = 0.5;

X=[tB(2:end)',fliplr(tB(2:end)')];                %#create continuous x value array for plotting
Y=[xBmin(:,3)',flipud(xBmax(:,3))'];              %#create y values for out and then back
fillh = fill(X,Y,ccolors(3,:)-[0.1 0.2 0.09]);                  %#plot filled area
fillh.EdgeColor = ccolors(3,:)-[0.1 0.2 0.09]; fillh.FaceAlpha = 0.5;

%             plot(tB(2:end),xBmin(:,1),'-k','LineWidth',2)
%             plot(tB(2:end),xBmin(:,1),'--g','LineWidth',2)

if eps~=0
    ph(4) = plot(tspan,x(:,1),'-','Color',0.7*ones(1,3),'LineWidth',1); %ccolors(1,:)+[0.15 0.3 0.25]
    plot(tspan,x(:,2),'-','Color',0.7*ones(1,3),'LineWidth',1); %ccolors(2,:)+[0.15 0.3 0.25]
    plot(tspan,x(:,3),'-','Color',0.7*ones(1,3),'LineWidth',1);
    ph(1) = plot([DataTrain.t;tA],[DataTrain.x(:,1);xA(:,1)],'-','Color',ccolors(1,:),'LineWidth',1);
    ph(2) = plot([DataTrain.t;tA],[DataTrain.x(:,2);xA(:,2)],'-','Color',ccolors(2,:),'LineWidth',1);
    ph(3) = plot([DataTrain.t;tA],[DataTrain.x(:,3);xA(:,3)],'-','Color',ccolors(3,:),'LineWidth',1);
else
    ph(1) = plot([DataTrain.t;tA],[DataTrain.x(:,1);xA(:,1)],'-','Color',ccolors(1,:),'LineWidth',1);
    ph(2) = plot([DataTrain.t;tA],[DataTrain.x(:,2);xA(:,2)],'-','Color',ccolors(2,:),'LineWidth',1);
    ph(3) = plot([DataTrain.t;tA],[DataTrain.x(:,3);xA(:,3)],'-','Color',ccolors(3,:),'LineWidth',1);
    ph(4) = plot(t,x(:,1),'--','Color',[0 1 0],'LineWidth',1); % Training data
    plot(t,x(:,2),'--','Color',[0 1 0],'LineWidth',1);
    plot(t,x(:,3),'--','Color',[0 1 0],'LineWidth',1);
end

%             ph(4) = plot(tB,xB(:,1),'-.','Color',ccolors(1,:)-[0 0.2 0.2],'LineWidth',2);
%             ph(5) = plot(tB,xB(:,2),'-.','Color',ccolors(2,:)-[0.1 0.2 0.09],'LineWidth',2);
grid off
xlim([0 tv(end)])
xlabel('Time')
ylabel('xi')
set(gca,'LineWidth',1, 'FontSize',14)
set(gcf,'Position',[100 100 300 200])
set(gcf,'PaperPositionMode','auto');

if eps~=0
    lh = legend([ph([1,4]),fillh1],'Truth','Training',ModelName,'Location','NorthWest');
    %             lh.Position = [lh.Position(1)+0.13,lh.Position(2)-0.2,lh.Position(3:4)];
    lh.Position = [lh.Position(1)+0.02,lh.Position(2)-0.06,lh.Position(3:4)];
else
    lh = legend(ph([1,4]),'Truth','Training',ModelName,'Location','NorthWest');
    %             lh.Position = [lh.Position(1)+0.13,lh.Position(2)-0.2,lh.Position(3:4)];
    lh.Position = [lh.Position(1)+0.02,lh.Position(2)-0.06,lh.Position(3:4)];
end

print('-depsc2', '-painters','-loose','-cmyk', [figpath,'EX_',SystemModel,'_SI_',ModelName,'_',InputSignalType,'_Validation_N',sprintf('%04g',Ntrain_vec(iN)),'_Eta',sprintf('%03g',100*eta_vec(iNoise)),'_STATS.eps']);

delete(lh), delete(t1), delete(t2)
%             print('-depsc2', '-painters','-loose','-cmyk', [figpath,'EX_',SystemModel,'_SI_',ModelName,'_',InputSignalType,'_Validation_N',sprintf('%04g',Ntrain_vec(iN)),'_Eta',sprintf('%03g',100*eta_vec(iNoise)),'_STATS_noleg.eps']);

return
%% Show validation
clear ph
figure,box on,
ccolors = get(gca,'colororder');
ph(1) = plot(tspan,x(:,1),'-','Color','k','LineWidth',1); hold on
ph(2) = plot(tspan,x(:,2),'-','Color','k','LineWidth',1);
ph(3) = plot(tspan(Ndelay:Nt+Ndelay-1),xDMDc(:,1),'--','Color','r','LineWidth',2);
ph(4) = plot(tspan(Ndelay:Nt+Ndelay-1),xDMDc(:,2),'--','Color','r','LineWidth',2);
ph(5) = plot(tspan(Ndelay:Nt+Ndelay-1),xDMDc_sparse(:,1),':','Color','b','LineWidth',2);
ph(6) = plot(tspan(Ndelay:Nt+Ndelay-1),xDMDc_sparse(:,2),':','Color','b','LineWidth',2);
xlim([0 100]), ylim([0,120]);
xlabel('Time')
ylabel('Population size')
% legend('Prey (True)','Predator (True)', 'Prey (DMDc)','Predator (DMDc)')
legend(ph([1,3,5]),'True','DMDc',ModelName)
set(gca,'LineWidth',1, 'FontSize',14)
set(gcf,'Position',[100 100 300 200])
set(gcf,'PaperPositionMode','auto')
print('-depsc2', '-loose', '-cmyk', [figpath,'EX_LOTKA_SI_',ModelName,'_',InputSignalType,'.eps']);

DMDc_err = norm(x(1:end-1,:)-xDMDc,'fro');
sparseDMDc_err = norm(x(1:end-1,:)-xDMDc_sparse,'fro');
disp(['Training Error: '])
disp(['                DMDc: ', num2str(DMDc_err)])
disp(['          sparseDMDc: ', num2str(sparseDMDc_err)])

%% Prediction
% Reference
tspanV   = [100:dt:200];
xA      = xv;
tA      = tv;

% Model DMDc
x0      = [x(end,1:2)];
Hunew   = [u(end),uv(1:end)];
[xB,tB] = lsim(sysmodel_DMDc,Hunew,tspanV,[x0-[xmean]]');
xB = xB + repmat(xmean,[length(tB) 1]);

% Model sparseDMDc / sparseEDMDc
if Nstates == 2
    [xB_sparse,tB] = lsim(sysmodel_sparseDMDc,Hunew,tspanV,[x0-[xmean]]');
    xB_sparse = xB_sparse(:,end-1:end);
    xB_sparse = xB_sparse + repmat(xmean,[length(tB) 1]);
elseif Nstates > 2
    x0dmdc = [x0(1)-xmean(1); x0(2)-xmean(2); (x0(1)-xmean(1)).^2; (x0(1)-xmean(1)).*(x0(2)-xmean(2)); (x0(2)-xmean(2)).^2];
    [xB_sparse,tB] = lsim(sysmodel_sparseDMDc,Hunew,tspanV,x0dmdc);
    xB_sparse = xB_sparse(:,1:2);
    xB_sparse = xB_sparse + repmat(xmean,[length(tB) 1]);
end

DMDc_err = norm(xA-xB(1:end-1,:),2);
sparseDMDc_err = norm(xA-xB_sparse(1:end-1,:),2);
disp(['Validation Error: '])
disp(['                DMDc: ', num2str(DMDc_err)])
disp(['          sparseDMDc: ', num2str(sparseDMDc_err)])
%% Show training and prediction
VIZ_SI_Validation

%% Save Data
Model.name = 'sparseDMDc';
Model.sys = sysmodel_DMDc;
Model.Ndelay = Ndelay;
Model.dt = dt;
save(fullfile(datapath,['EX_LOTKA_SI_',ModelName,'_',InputSignalType,'.mat']),'Model')


% Attenuation Compensation Using Time Reversal Example
%
% This example demonstrates how the acoustic attenuation present in
% photoacoustic forward problem can be compensated for using time reversal
% image reconstruction. It builds on the 2D Time Reversal Reconstruction
% For A Circular Sensor Example.
%
% For a more detailed discussion of this example and the underlying
% techniques, see B. E. Treeby, E. Z. Zhang, and B. T. Cox, "Photoacoustic
% tomography in absorbing acoustic media using time reversal," Inverse
% Problems, vol. 26, no. 11, p. 115003, 2010.
%
% author: Bradley Treeby
% date: 6th September 2010
% last update: 25th August 2014
%
% This function is part of the k-Wave Toolbox (http://www.k-wave.org)
% Copyright (C) 2009-2014 Bradley Treeby and Ben Cox

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

clear all;

% =========================================================================
% FORWARD SIMULATION
% =========================================================================

% define the size of the simulation grid and the PML
simulation_size = 512;              % [grid points]
PML_size = 20;                      % [grid points]
x = 52e-3;                          % [m]
y = x;                              % [m]

% reduce the number of grid points in Nx and Ny by the size of the PML so
% that using 'PMLInside' set to false will still give the correct
% simulation size
Nx = simulation_size - 2*PML_size;  % [grid points]
Ny = Nx;                            % [grid points]
dx = x/Nx;                          % [m]
dy = dx;                            % [m]

% create the computational grid
kgrid = makeGrid(Nx, dx, Ny, dy);

% define the properties of the propagation medium
medium_non_absorbing.sound_speed = 1510;	% [m/s]

% create a duplicate of the propagation medium structure and append the
% absorption properties
medium = medium_non_absorbing;
medium.alpha_power = 1.5;
medium.alpha_coeff = 3;              % [dB/(MHz^y cm)]

% store maximum supported frequency
f_max = kgrid.k_max*medium.sound_speed/(2*pi);

% load the shepp logan phantom (note if the simulation or PML sizes are
% changed, the loaded data will need to be resized)
load EXAMPLE_shepp_logan

% smooth the phantom and assign it to the initial pressure
shepp_logan = smooth(kgrid, shepp_logan, true);
source.p0 = shepp_logan;

% define a circular Cartesian sensor mask
sensor_radius = 25e-3;              % [m]
sensor_points = 200;
cart_sensor_mask = makeCartCircle(sensor_radius, sensor_points);
sensor.mask = cart_sensor_mask;

% create the time array used for the simulation, with t_max defined using
% Huygens' principle to avoid artifact trapping in the reconstruction
t_max = 2*sensor_radius/medium.sound_speed;
[kgrid.t_array, dt] = makeTime(kgrid, medium.sound_speed, [], t_max);

% set the input options, switching off the smoothing (the input has already
% been smoothed), setting the PML to be outside the defined grid, casting
% to 'single' to speed up the example, and switching off visualisation
input_args = {'Smooth', false, 'PMLInside', false, 'PMLSize', PML_size, 'DataCast', 'single', 'PlotSim', false};

% run the forward simulation
sensor_data = kspaceFirstOrder2D(kgrid, medium, source, sensor, input_args{:});

% add noise to the recorded sensor data
signal_to_noise_ratio = 40;         % [dB]
sensor_data = addNoise(sensor_data, signal_to_noise_ratio, 'peak');

% =========================================================================
% IMAGE RECONSTRUCTION WITHOUT ABSORPTION COMPENSATION
% =========================================================================

% create a second computation grid for the reconstruction to avoid the
% inverse crime
PML_size = 25;                      % [grid points]
Nx = simulation_size - 2*PML_size;  % [grid points]
Ny = Nx;                            % [grid points]
dx = x/Nx;                          % [m]
dy = dx;                            % [m]
kgrid_recon = makeGrid(Nx, dx, Ny, dy);

% attach the original time array
kgrid_recon.t_array = kgrid.t_array;

% remove the initial pressure field from the source structure
source = rmfield(source, 'p0');

% create a continuous binary sensor mask with the same radius as the
% Cartesian sensor mask used in the forward simulation
pixel_radius = round(sensor_radius/kgrid_recon.dx);
binary_sensor_mask = makeCircle(kgrid_recon.Nx, kgrid_recon.Ny, floor(kgrid_recon.Nx/2) + 1, floor(kgrid_recon.Ny/2) + 1, pixel_radius);

% assign the sensor mask to the sensor structure
sensor.mask = binary_sensor_mask;

% interpolate the simulated sensor data onto the continuous binary sensor
% mask to remove any gaps and assign to the time reversal field
sensor.time_reversal_boundary_data = interpCartData(kgrid_recon, sensor_data, cart_sensor_mask, binary_sensor_mask, 'linear');

% re-assign the input options (the PML_size has changed)
input_args = {'Smooth', false, 'PMLInside', false, 'PMLSize', PML_size, 'DataCast', 'single', 'PlotSim', false};

% run the time-reversal reconstruction using the non absorbing medium
p0_recon = kspaceFirstOrder2D(kgrid_recon, medium_non_absorbing, source, sensor, input_args{:});

% =========================================================================
% CHOOSING THE CUTOFF FREQUENCY
% =========================================================================

% get the average frequency spectrum of the simulated sensor data
num_signals = length(sensor_data(:,1));
[as_f, as] = spect(sensor_data(1, :), 1/dt);
for index = 2:num_signals
    [as_f, sp] = spect(sensor_data(index, :), 1/dt);
    as = as + sp;
end
as = as/num_signals;

% compute the relative power spectrum
ps = log10(as.^2);
offset = max(ps(:));
ps = ps - offset;

% get the frequency spectrum of a single measurement
as_sing = spect(sensor_data(1,:), 1/dt);
ps_sing = log10(as_sing.^2) - offset;

% scale the frequency variable
[f_sc, scale, prefix] = scaleSI(as_f(end));

% define the cutoff frequency for the filter
f_cutoff = 3e6;

% =========================================================================
% IMAGE RECONSTRUCTION WITH ABSORPTION COMPENSATION
% =========================================================================

% create the filter to regularise the absorption parameters
medium.alpha_filter = getAlphaFilter(kgrid_recon, medium, f_cutoff);

% reverse the sign of the absorption proportionality coefficient
medium.alpha_sign = [-1, 1];        % [absorption, dispersion];

% run the time-reversal reconstruction
p0_recon_compensated = kspaceFirstOrder2D(kgrid_recon, medium, source, sensor, input_args{:});

% =========================================================================
% VISUALISATION
% =========================================================================

% plot the amplitude spectrum
figure;
plot(as_f*scale, ps_sing, 'r-');
ylabel('Power Spectrum [dB]');
xlabel(['Frequency [' prefix 'Hz]']);
hold on;
plot(as_f*scale, ps, 'k-');
ylim = [-8 0];
plot([f_cutoff*scale f_cutoff*scale], [ylim(1) ylim(2)], 'k--');
plot([f_max*scale f_max*scale], [ylim(1) ylim(2)], 'k--');
set(gca, 'XLim', [0 15], 'Ylim', ylim);

% plot the filter, reducing the number of plotting points by a factor of 10
figure;
mesh(medium.alpha_filter(1:10:end, 1:10:end), 'EdgeColor', 'black');
axis tight;
view([-29, 46]);

% set the pressure outside the sensor mask to be zero
binary_sensor_map = makeDisc(kgrid_recon.Nx, kgrid_recon.Ny, kgrid_recon.Nx/2, kgrid_recon.Ny/2, pixel_radius - 50);
p0_recon(binary_sensor_map ~= 1) = 0;
p0_recon_compensated(binary_sensor_map ~= 1) = 0;

% shrink the data sets
phantom_padding = 50;
shepp_logan = shepp_logan(1+ phantom_padding:end-phantom_padding, 1+ phantom_padding:end-phantom_padding);
p0_recon = p0_recon(1+ phantom_padding:end-phantom_padding, 1+ phantom_padding:end-phantom_padding);
p0_recon_compensated = p0_recon_compensated(1+ phantom_padding:end-phantom_padding, 1+ phantom_padding:end-phantom_padding);

% plot the initial pressure distribution
figure;
subplot(2, 2, 1), imagesc(kgrid_recon.y_vec(1+ phantom_padding:end-phantom_padding)*1e3, kgrid_recon.x_vec(1+ phantom_padding:end-phantom_padding)*1e3, shepp_logan, [0 1]);
ylabel('x-position [mm]');
xlabel('y-position [mm]');
axis image;
colormap(flipud(gray));

% plot the reconstruction without attenuation compensation
subplot(2, 2, 2), imagesc(kgrid_recon.y_vec(1+ phantom_padding:end-phantom_padding)*1e3, kgrid_recon.x_vec(1+ phantom_padding:end-phantom_padding)*1e3, p0_recon, [0 1]);
ylabel('x-position [mm]');
xlabel('y-position [mm]');
axis image;
colormap(flipud(gray));

% plot the reconstruction with attenuation compensation
subplot(2, 2, 3), imagesc(kgrid_recon.y_vec(1+ phantom_padding:end-phantom_padding)*1e3, kgrid_recon.x_vec(1+ phantom_padding:end-phantom_padding)*1e3, p0_recon_compensated, [0 1]);
ylabel('x-position [mm]');
xlabel('y-position [mm]');
axis image;
colormap(flipud(gray));

% plot a reconstruction profile
figure;
plot(kgrid.y_vec(1+ phantom_padding:end-phantom_padding)*1e3, shepp_logan(end/2,:), 'k');
hold on;
plot(kgrid_recon.y_vec(1+ phantom_padding:end-phantom_padding)*1e3, p0_recon(end/2,:), 'r-');
plot(kgrid_recon.y_vec(1+ phantom_padding:end-phantom_padding)*1e3, p0_recon_compensated(end/2, :), 'b-');
set(gca, 'XLim', [-20, 20], 'YLim', [-0.1 1.1]);
ylabel('Pressure Magnitude [au]');
xlabel('y-position [mm]');
legend('Original', 'No Compensation', 'With Attenuation Compensation', 'Location', 'Best');


function prob = problem_type(om, recheck)
%PROBLEM_TYPE  Return a string identifying the type of mathematical program
%   PROB_TYPE = OM.PROBLEM_TYPE()
%   PROB_TYPE = OM.PROBLEM_TYPE(RECHECK)
%
%   Returns a string identifying the type of mathematical program
%   represented by the current model, based on the variables, costs,
%   and constraints that have been added to the model. Used to
%   automatically select an appropriate solver.
%
%   Linear and nonlinear equations are models with no costs, no inequality
%   constraints, and an equal number of continuous variables and equality
%   constraints. If the number of variables in a nonlinear equation model
%   is one more than the number of constraints, it is a parameterized
%   nonlinear equation.
%
%   Outputs:
%       PROB_TYPE : problem type, one of the following strings:
%           'LEQ'   - linear equations
%           'NLEQ'  - nonlinear equations
%           'PNE'   - parameterized nonlinear equations
%           'LP'    - linear program
%           'QP'    - quadratic program
%           'NLP'   - nonlinear program
%           'MILP'  - mixed-integer linear program
%           'MIQP'  - mixed-integer quadratic program
%           'MINLP' - mixed-integer nonlinear program
%
%   The output value is cached for future calls, but calling with a true
%   value for the optional RECHECK argument will force it to recheck in
%   case the problem type has changed due to modifying the variables,
%   constraints or costs in the model.
%
%   See also OPT_MODEL

%   MP-Opt-Model
%   Copyright (c) 2020, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%
%   This file is part of MP-Opt-Model.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://github.com/MATPOWER/mp-opt-model for more info.

if isempty(om.prob_type) || nargin > 1 && recheck
    nleN = om.getN('nle');      %% nonlinear equalities
    nliN = om.getN('nli');      %% nonlinear inequalities
    nlcN = om.getN('nlc');      %% general nonlinear costs
    qdcN = om.getN('qdc');      %% quadratic costs
    linN = om.getN('lin');      %% linear constraints
    varN = om.getN('var');      %% variables
    if varN == 0
        prob = '';
    elseif nlcN || qdcN         %% problem has costs
        if nliN || nleN || nlcN %% nonlinear
            prob = 'NLP';           %% nonlinear program
        else                    %% linear constraints, no general nonlinear costs
            %% get quadratic cost coefficients
            H = om.params_quad_cost();
            if isempty(H) || ~any(any(H))
                prob = 'LP';        %% linear program
            else
                prob = 'QP';        %% quadratic program
            end
        end
    else                    %% problem has no costs
        if nliN
            error('@opt_model/problem_type: invalid problem - nonlinear inequality constraints with no costs');
        end
        if nleN + linN == varN || nleN + linN == varN - 1   %% square (or almost) system
            if linN > 0
                %% get lower & upper bounds
                [A, l, u] = om.params_lin_constraint();
                if any(l ~= u)
                    error('@opt_model/problem_type: invalid problem - linear inequality constraints with no costs');
                end
            end
            if nleN + linN == varN  %% square system
                if nleN
                    prob = 'NLEQ';      %% square nonlinear set of equations
                else
                    prob = 'LEQ';       %% square linear set of equations
                end
            elseif nleN + linN + 1 == varN  %% square + 1 extra (parameterization) variable
                if nleN
                    prob = 'PNE';       %% parameterized nonlinear set of equations
                else
                    prob = 'PLEQ';      %% parameterized linear set of equations
                    error('@opt_model/problem_type: invalid problem - PNE not implemented for for linear constraints only');
                end
            else
                error('@opt_model/problem_type: invalid problem - PNE must have num of vars = num of constraints + 1');
            end
        else
            error('@opt_model/problem_type: invalid problem - non-square system with no costs');
        end
    end
    if om.is_mixed_integer() && ~strcmp(prob, 'NLEQ')
        prob = ['MI' prob];
    end
    om.prob_type = prob;    %% cache it
else
    prob = om.prob_type;    %% return cached type
end


function [M0,q0,X,x,f,M1,L] = spm_fp(M,x,u)
% Fokker-Planck operators and equilibrium density for dynamic systems
% FORMAT [M0,q0,X,x,f,M1,L] = spm_fp(M,x,u)
%--------------------------------------------------------------------------
% M   - model specification structure
% Required fields:
%    M.f   - dx/dt    = f(x,u,P) or f(x,u,a,P)  {function string or m-file}
%    M.g   - y(t)     = g(x,u,P)                {function string or m-file}
%    M.m   - m inputs
%    M.n   - n states
%    M.l   - l outputs
%    M.x   - (n x 1) = x(0) = expansion point
%    M.W   - (n x n) - precision matrix of state noise
% x    - cell array of vectors specifying evaluation grid
% u    - expansion point for inputs or causes;
%
% M0   - 1st order FP operator dq/dt = M0*q + u*M1*q,  q = p(X);
% q0   - stable or equilibrium mode: M0*q0 = 0
% X    - evaluation points of state space
% x    - cell array of vectors specifying evaluation grid
% f    - flow
% M1   - 2nd order FP operator
% L    - output matrix                 <y> = L*q;
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_fp.m 5219 2013-01-29 17:07:07Z spm $

% default: first level of hierarchical model
%--------------------------------------------------------------------------
M   = M(1);

% default expansion point for inputs
%--------------------------------------------------------------------------
if nargin < 3
    u0 = zeros(M.m,1);
else
    u0 = u;
end

% event space: get or create X - coordinates of evaluation grid
%--------------------------------------------------------------------------
n  = length(M.x);
if nargin < 2

    % use M.X
    %----------------------------------------------------------------------
    X     = M.X;
    for i = 1:size(X,2)
        x{i} = unique(X(:,i));
    end
else

    % use x
    %----------------------------------------------------------------------
    [X,x] = spm_ndgrid(x);
end

% f(x,0)
%--------------------------------------------------------------------------
N     = length(X);
f     = sparse(n,N);
try
    for i = 1:N
        f(:,i) = feval(M.f,X(i,:)',u0,M.pE);
    end
catch
    for i = 1:N
        f(:,i) = feval(M.f,X(i,:)',u0,0,M.pE);
    end
end

% Fokker-Planck operator; i.e., Jacobian J - M0
%==========================================================================
for i = 1:n
    d(i)  = length(x{i});
    dx(i) = x{i}(2) - x{i}(1);
    I{i}  = speye(d(i),d(i));
end
J     = sparse(N,N);

% cycle over dimensions of state space
%--------------------------------------------------------------------------
for i = 1:length(x)

    % differential operators (for positive and negative flows)
    %----------------------------------------------------------------------
    j          = find(f(i,:) < 0);
    u          = sparse(j,1,f(i,j),N,1);
    j          = find(f(i,:) > 0);
    v          = sparse(j,1,f(i,j),N,1);

    dp         = spdiags(ones(d(i),1)*[1 -1],[ 0;1],d(i),d(i))/dx(i);
    dp(:,1)    = 0;
    dq         = spdiags(ones(d(i),1)*[1 -1],[-1;0],d(i),d(i))/dx(i);
    dq(:,end)  = 0;

    % Kronecker tensor products
    %----------------------------------------------------------------------
    Dp         = 1;
    Dq         = 1;
    for j = 1:(i - 1)
        Dp = spm_kron(I{j},Dp);
        Dq = spm_kron(I{j},Dq);
    end
    Dp    = spm_kron(dp,Dp);
    Dq    = spm_kron(dq,Dq);
    for j = (i + 1):n
        Dp = spm_kron(I{j},Dp);
        Dq = spm_kron(I{j},Dq);
    end
    DP{i} = Dp;

    % augment Jacobian
    %----------------------------------------------------------------------
    J     = J + Dp*spdiags(u,0,N,N) + Dq*spdiags(v,0,N,N);

end

% dispersion
%--------------------------------------------------------------------------
C     = inv(M.W);
if length(C) ~= n
    C = C(1)*speye(n,n);
end
for i = 1:n
    for j = 1:n
        if C(i,j)
            J  = J - C(i,j)*DP{i}*DP{j}'/2;
        end
    end
end

% return if only M0 is required
%--------------------------------------------------------------------------
M0    = J;
if nargout == 1, return, end

% stable mode - stochastic iteration
%==========================================================================
q     = sparse(N,1) + 1/N;
for i = 1:1024
    dq = M0*q;
    if norm(dq,1) < exp(-4), break, end
    while min(q + dq) < 0
        dq = dq - dq/2;
    end
    q  = q + dq;
end
for i = 1:n
    nx{i} = length(x{i});
end
q0    = reshape(q,nx{:});

if nargout < 6,  return,  end

% input induced changes to Jacobian dJ/du - M1
%==========================================================================
M1    = spm_diff('spm_fp',M,x,u0,3);
if nargout < 6,  return,  end

% Output matrix L - M1
%==========================================================================

% l(x,0)
%--------------------------------------------------------------------------
L     = sparse(M.l,N);
for i = 1:N
    try
        L(:,i) = feval(M.g,X(i,:)',u0,M.pE);
    catch
        L(:,i) = feval(M.g,X(i,:)',u0,[],M.pE);
    end
end

return

% NOTES: analytic solution for q0
%--------------------------------------------------------------------------
q   = length(M0);
J   = [M0; ones(1,q)];
q0  = sum(inv(J'*J),2);


function [LLF, likelihoods, errors] = sarimax_likelihood(parameters,p,q,constant,seasonal,y,x,sigma)
% PURPOSE:
%   Likelihood function for armaxfilter
%
% USAGE:
%   [LLF, LIKELIHOODS, ERRORS] = armaxfilter_likelihood(PARAMETERS,P,Q,CONSTANT,Y,X,M)
%   [LLF, LIKELIHOODS, ERRORS] = armaxfilter_likelihood(PARAMETERS,P,Q,CONSTANT,Y,X,M)
%
% INPUTS:
%   PARAMETERS  - A vector of GARCH process aprams of the form [constant, arch, garch]
%   P           - Vector containing lag indices of the AR component
%   Q           - Vector containing lag indices of the MA component
%   CONSTANT    - Value indicating whether the model contains a constant (1) or not (0)
%   Y           -
%   X           -
%   M           - Index to first element to use in the recursive residual calculation
%   SIGMA       - T by 1 vector
%
% OUTPUTS:
%   LLF         - Minus 1 times the log likelihood
%   LIKELIHOODS - Time series of likelihoods
%   ERRORS      - Time series of model errors
%
% COMMENTS:
%
% See also armaerrors

% Author: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 3    Date: 4/1/2004

m = size(x,2);
[armaParameters, p, q] = sarma2arma(parameters(m+1:length(parameters)), p, q, seasonal);
parameters = [parameters(1:m);armaParameters];

e = armaxerrors(parameters,p,q,constant,y,x,m,ones(size(y)));
T = length(e);

% Do not divide e by sigma since this is done in armaxerrors
likelihoods =  0.5*(2*log(sigma) + (e./sigma).^2 + log(2*pi));

if isempty(p)
    p = 0;
end
if isempty(q)
    q = 0;
end
if max(q)>max(p) % prepend y and x, if needed
    t = (max(q)-max(p))+1:T;
else
    t = 1:T;
end

likelihoods = likelihoods(t);
errors=e(t);
LLF = sum(likelihoods);

if isnan(LLF)
    LLF=1e7;
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  MATLAB Code for                                                  %
%                                                                   %
%  Multi-Objective Particle Swarm Optimization (MOPSO)              %
%  Version 1.0 - Feb. 2011                                          %
%                                                                   %
%  According to:                                                    %
%  Carlos A. Coello Coello et al.,                                  %
%  "Handling Multiple Objectives with Particle Swarm Optimization," %
%  IEEE Transactions on Evolutionary Computation, Vol. 8, No. 3,    %
%  pp. 256-279, June 2004.                                          %
%                                                                   %
%  Developed Using MATLAB R2009b (Version 7.9)                      %
%                                                                   %
%  Programmed By: S. Mostapha Kalami Heris                          %
%                                                                   %
%         e-Mail: sm.kalami@gmail.com                               %
%                 kalami@ee.kntu.ac.ir                              %
%                                                                   %
%       Homepage: http://www.kalami.ir                              %
%                                                                   %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function pop=DetermineDomination(pop)

    npop=numel(pop);

    for i=1:npop
        pop(i).Dominated=false;
        for j=1:i-1
            if ~pop(j).Dominated
                if Dominates(pop(i),pop(j))
                    pop(j).Dominated=true;
                elseif Dominates(pop(j),pop(i))
                    pop(i).Dominated=true;
                    break;
                end
            end
        end
    end

end


function [wedge_data wedge_density] = wedge();

% wedge coordinates
Y = [-7.0 -4.35    0  3.0  0 -4.35 -7.0 -7.0];
Z = [-5.8 -5.8  -2.46    0  0   0    0  -5.8];
figure; line(Y,Z, 'LineStyle', '-', 'Marker', 'o');
disp('This wedge can be modeled as a rectangle and a triangle, as shown below')
Y = [-7.0 -4.35   -4.35 -7.0 -7.0];
Z = [-5.8 -5.8  0    0  -5.8];
figure; line(Y,Z, 'Color', 'b', 'LineStyle', '-', 'Marker', 'o');
Y = [-4.35  3.0  -4.35 -4.35];
Z = [-5.8    0     0   -5.8];
hold on; line(Y,Z, 'Color', 'r', 'LineStyle', '-', 'Marker', 'o'); axis equal;
xlabel('Y/cm'); ylabel('Z/cm'); title('Elekta Universal Wedge');

% From the NIST website, get the attenuation coefficients.
% Components of the wedge:
Pb = 0.96     %Fraction by weigth for Lead
Sb = 0.04     %Fraction by weight for Antimony

% Ref:
%http://physics.nist.gov/PhysRefData/XrayMassCoef/ElemTab/z82.html
%Energy       mu over rho        mu e n over rho
%      (MeV)      (cm2/g)     (cm2/g)
%____________________________________
%
Pb_data =   [1.00000E-03  5.210E+03  5.197E+03
   1.50000E-03  2.356E+03  2.344E+03
   2.00000E-03  1.285E+03  1.274E+03
   2.48400E-03  8.006E+02  7.895E+02
   2.48400E-03  1.397E+03  1.366E+03
   2.53429E-03  1.726E+03  1.682E+03
   2.58560E-03  1.944E+03  1.895E+03
   2.58560E-03  2.458E+03  2.390E+03
   3.00000E-03  1.965E+03  1.913E+03
   3.06640E-03  1.857E+03  1.808E+03
   3.06640E-03  2.146E+03  2.090E+03
   3.30130E-03  1.796E+03  1.748E+03
   3.55420E-03  1.496E+03  1.459E+03
   3.55420E-03  1.585E+03  1.546E+03
   3.69948E-03  1.442E+03  1.405E+03
   3.85070E-03  1.311E+03  1.279E+03
   3.85070E-03  1.368E+03  1.335E+03
   4.00000E-03  1.251E+03  1.221E+03
   5.00000E-03  7.304E+02  7.124E+02
   6.00000E-03  4.672E+02  4.546E+02
   8.00000E-03  2.287E+02  2.207E+02
   1.00000E-02  1.306E+02  1.247E+02
   1.30352E-02  6.701E+01  6.270E+01
   1.30352E-02  1.621E+02  1.291E+02
   1.50000E-02  1.116E+02  9.100E+01
   1.52000E-02  1.078E+02  8.807E+01
   1.52000E-02  1.485E+02  1.131E+02
   1.55269E-02  1.416E+02  1.083E+02
   1.58608E-02  1.344E+02  1.032E+02
   1.58608E-02  1.548E+02  1.180E+02
   2.00000E-02  8.636E+01  6.899E+01
   3.00000E-02  3.032E+01  2.536E+01
   4.00000E-02  1.436E+01  1.211E+01
   5.00000E-02  8.041E+00  6.740E+00
   6.00000E-02  5.021E+00  4.149E+00
   8.00000E-02  2.419E+00  1.916E+00
   8.80045E-02  1.910E+00  1.482E+00
   8.80045E-02  7.683E+00  2.160E+00
   1.00000E-01  5.549E+00  1.976E+00
   1.50000E-01  2.014E+00  1.056E+00
   2.00000E-01  9.985E-01  5.870E-01
   3.00000E-01  4.031E-01  2.455E-01
   4.00000E-01  2.323E-01  1.370E-01
   5.00000E-01  1.614E-01  9.128E-02
   6.00000E-01  1.248E-01  6.819E-02
   8.00000E-01  8.870E-02  4.644E-02
   1.00000E+00  7.102E-02  3.654E-02
   1.25000E+00  5.876E-02  2.988E-02
   1.50000E+00  5.222E-02  2.640E-02
   2.00000E+00  4.606E-02  2.360E-02
   3.00000E+00  4.234E-02  2.322E-02
   4.00000E+00  4.197E-02  2.449E-02
   5.00000E+00  4.272E-02  2.600E-02
   6.00000E+00  4.391E-02  2.744E-02
   8.00000E+00  4.675E-02  2.989E-02
   1.00000E+01  4.972E-02  3.181E-02
   1.50000E+01  5.658E-02  3.478E-02
   2.00000E+01  6.206E-02  3.595E-02];

%Antimony
%Z = 51
%
%ASCII format
%
%____________________________________
%
%     Energy       mu over rho        mu e n over rho
%      (MeV)      (cm2/g)     (cm2/g)
%____________________________________

Sb_data = [1.00000E-03  8.582E+03  8.568E+03
   1.50000E-03  3.491E+03  3.481E+03
   2.00000E-03  1.767E+03  1.759E+03
   3.00000E-03  6.536E+02  6.469E+02
   4.00000E-03  3.169E+02  3.113E+02
   4.13220E-03  2.918E+02  2.863E+02
   4.13220E-03  8.691E+02  8.252E+02
   4.25449E-03  8.308E+02  7.865E+02
   4.38040E-03  7.776E+02  7.392E+02
   4.38040E-03  1.050E+03  9.906E+02
   4.53657E-03  9.743E+02  9.178E+02
   4.69830E-03  8.939E+02  8.457E+02
   4.69830E-03  1.029E+03  9.725E+02
   5.00000E-03  8.846E+02  8.377E+02
   6.00000E-03  5.569E+02  5.305E+02
   8.00000E-03  2.631E+02  2.518E+02
   1.00000E-02  1.459E+02  1.396E+02
   1.50000E-02  4.923E+01  4.657E+01
   2.00000E-02  2.268E+01  2.105E+01
   3.00000E-02  7.631E+00  6.755E+00
   3.04912E-02  7.307E+00  6.452E+00
   3.04912E-02  4.073E+01  1.391E+01
   4.00000E-02  2.027E+01  9.789E+00
   5.00000E-02  1.120E+01  6.400E+00
   6.00000E-02  6.879E+00  4.311E+00
   8.00000E-02  3.176E+00  2.173E+00
   1.00000E-01  1.758E+00  1.237E+00
   1.50000E-01  6.361E-01  4.312E-01
   2.00000E-01  3.381E-01  2.084E-01
   3.00000E-01  1.677E-01  8.504E-02
   4.00000E-01  1.172E-01  5.288E-02
   5.00000E-01  9.453E-02  4.061E-02
   6.00000E-01  8.153E-02  3.465E-02
   8.00000E-01  6.670E-02  2.896E-02
   1.00000E+00  5.797E-02  2.608E-02
   1.25000E+00  5.086E-02  2.378E-02
   1.50000E+00  4.628E-02  2.230E-02
   2.00000E+00  4.105E-02  2.081E-02
   3.00000E+00  3.686E-02  2.043E-02
   4.00000E+00  3.567E-02  2.118E-02
   5.00000E+00  3.559E-02  2.219E-02
   6.00000E+00  3.598E-02  2.321E-02
   8.00000E+00  3.745E-02  2.509E-02
   1.00000E+01  3.921E-02  2.664E-02
   1.50000E+01  4.351E-02  2.920E-02
   2.00000E+01  4.704E-02  3.038E-02];

% put data on the same, unique intervals.
[A, s] = unique(Pb_data(:,1));
[B,I] = unique(Sb_data(:,1));
Pb_data = [B interp1(Pb_data(s,1), Pb_data(s,2), B) interp1(Pb_data(s,1), Pb_data(s,3), B)];
Sb_data = [B interp1(Sb_data(I,1), Sb_data(I,2), B) interp1(Sb_data(I,1), Sb_data(I,3), B)];

wedge_data = Pb*Pb_data + Sb*Sb_data;
figure; loglog(Pb_data(:,1), Pb_data(:,2), Sb_data(:,1), Sb_data(:,2), wedge_data(:,1), wedge_data(:,2));
wedge_density = 11.16;


%% Spherical Functions
%
% The |S2Fun| toolbox is a collection of Matlab classes for numerical computations with functions on the two-dimensional sphere.
% It overloads the default commands for vectors and matrices to compute the analogous operations for functions of the given type.
% The underlying mathematical approach is accomplished via spherical harmonics which form an orthonormal basis of the square-integrable functions on the two-dimensional sphere.
%
%%
%


%DEMO_PASSGP Demonstration of PASS-GP routine for GP classification
%
%  Description
%    Here we demonstrate pass-gp routine for Gaussian Processes
%    classification. Data used is 2-dimensional toy data with Gaussian
%    bumbs defining classes. We demonstrate with both fixed and not fixed
%    sizes of active set for pass-gp.
%
%  See also PASSGP

% Copyright (c) 2013 Ville Tolvanen

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

% Generate toy data
prevstream=setrandstream(0);
[x1,x2]=meshgrid(-5:0.1:5,-5:0.1:5);
x=[x1(:) x2(:)]; x=x(randperm(size(x,1),3000),:);
y=2.*mnorm_pdf(x, [0 0], [0.5 0;0 0.5]) + mnorm_pdf(x, [3 3], [0.5 0;0 0.5]) + mnorm_pdf(x, [-3 -3], [0.5 0;0 0.5]);
y=y+mnorm_pdf(x, [3 -3], [0.5 0;0 0.5])+mnorm_pdf(x, [-3 3], [0.5 0;0 0.5]);
y=y+0.03.*randn(size(y,1),1);
y(y>0.15)=1; y(y<=0.15)=-1;

[xt1, xt2]=meshgrid(-5:0.23:5,-5:0.23:5);
xt=[xt1(:) xt2(:)];
yt=ones(size(xt,1),1);

[n, nin] = size(x);

% Define covariance and likelihood functions and create model
gpcf = gpcf_sexp();
lik=lik_probit();
gp=gp_set('lik', lik, 'cf', gpcf, 'jitterSigma2', 1e-6);

opt=optimset('TolX',1e-3,'TolFun',1e-3,'display','on');
w0=gp_pak(gp);

% fPASS-GP with fixed size of 800 points in active set and data divided to
% 10 subsets with 4 sweeps over data
start=tic;[gp, indA]=passgp(gp, x, y, 'opt', opt, 'npass', 4, 'ninit', 800, 'nsub', 10, 'display', 'on', 'fixed', 'on', 'pexc', 0.1, 'optimn', 2);time=toc(start);
tt=time;
[Eft, Varft, lpyt, Eyt, Varyt]=gp_pred(gp, x(indA,:), y(indA,:), xt, 'yt', yt);
figure, [cc,hh]=contour(reshape(xt(:,1),size(xt1,1), size(xt1,1)), reshape(xt(:,2),size(xt1,1), size(xt1,1)), reshape(exp(lpyt),size(xt1,1), size(xt1,1)), [0.1 0.9]);
clabel(cc,hh); title('Pr(y==1) (fpass-gp)')
param=gp_pak(gp);

% PASS-GP with inclusion threshold 0.65, deletion threshold 0.99, intial
% size of 400 points in active set and 3 sweeps over data.
gp=gp_unpak(gp,w0);
start=tic;[gp, indA2]=passgp(gp, x, y, 'opt', opt, 'pinc', 0.65, 'pdel', 0.99, 'npass', 3, 'ninit', 400, 'nsub', 10, 'display', 'on', 'optimn', 2);time=toc(start);
tt2=time;
[Eft2, Varft2, lpyt2, Eyt2, Varyt2]=gp_pred(gp, x(indA2,:), y(indA2,:), xt, 'yt', yt);
figure, [cc,hh]=contour(reshape(xt(:,1),size(xt1,1), size(xt1,1)), reshape(xt(:,2),size(xt1,1), size(xt1,1)), reshape(exp(lpyt2),size(xt1,1), size(xt1,1)), [0.1 0.9]);
clabel(cc,hh); title('Pr(y==1) (pass-gp)')
param2=gp_pak(gp);

% Full Gaussian Process for comparison
gp=gp_unpak(gp,w0);
opt.Display='iter';
start=tic;gp=gp_optim(gp,x,y,'opt',opt);tt3=toc;
[Eft3, Varft3, lpyt3, Eyt3, Varyt3]=gp_pred(gp, x, y, xt, 'yt', yt);
figure, [cc,hh]=contour(reshape(xt(:,1),size(xt1,1), size(xt1,1)), reshape(xt(:,2),size(xt1,1), size(xt1,1)), reshape(exp(lpyt3),size(xt1,1), size(xt1,1)), [0.1 0.9]);
clabel(cc,hh); title('Pr(y==1) (full gp)')

% Display some statistics

mlpd_fpassgp=mean(mean(lpyt,2))
time_fpassgp=mean(tt)
mlpd_passgp=mean(mean(lpyt2,2))
time_passgp=mean(tt2)
mlpd_full=mean(lpyt3)
time_full=tt3

% Plot data and active sets for both methods
figure(4), subplot(1,2,1),  plot(x(y==1,1),x(y==1,2),'or',x(y==-1,1),x(y==-1,2),'ob');
hold all; plot(x(indA,1), x(indA,2), '.k'); title('Data and active set (fpass-gp)')
legend('y=1', 'y=-1', 'Active set for fpass-gp');
subplot(1,2,2),  plot(x(y==1,1),x(y==1,2),'or',x(y==-1,1),x(y==-1,2),'ob');
hold all; plot(x(indA2,1), x(indA2,2), '.k'); title('Data and active set (pass-gp)')
legend('y=1', 'y=-1', 'Active set for pass-gp');
setrandstream(prevstream);


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

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

l = r.c_prc.n_levels;

pvec(1:l)         = ptrans(1:l);                                  % mux_0
pstruct.mux_0     = pvec(1:l);
pvec(l+1:2*l)     = exp(ptrans(l+1:2*l));                         % sax_0
pstruct.sax_0     = pvec(l+1:2*l);
pvec(2*l+1:3*l)   = ptrans(2*l+1:3*l);                            % mua_0
pstruct.mua_0     = pvec(2*l+1:3*l);
pvec(3*l+1:4*l)   = exp(ptrans(3*l+1:4*l));                       % saa_0
pstruct.saa_0     = pvec(3*l+1:4*l);
pvec(4*l+1)       = exp(ptrans(4*l+1));                           % kau
pstruct.kau       = pvec(4*l+1);
pvec(4*l+2:5*l)   = exp(ptrans(4*l+2:5*l));                       % kax
pstruct.kax       = pvec(4*l+2:5*l);
pvec(5*l+1:6*l-1) = exp(ptrans(5*l+1:6*l-1));                     % kaa
pstruct.kaa       = pvec(5*l+1:6*l-1);
pvec(6*l)         = ptrans(6*l);                                  % omu
pstruct.omu       = pvec(6*l);
pvec(6*l+1:7*l)   = ptrans(6*l+1:7*l);                            % omx
pstruct.omx       = pvec(6*l+1:7*l);
pvec(7*l+1:8*l)   = ptrans(7*l+1:8*l);                            % oma
pstruct.oma       = pvec(7*l+1:8*l);

end


function [stats,st] = SStat_CoxExt(sID_ext,X_ext,d_ext,t_ext,e)
% [stats,st] = SStat_CoxExt(sID_ext,X_ext,d_ext,t_ext,e)
%
% Parameter estimation for the extended Cox model. This function uses as
% input the output of SStat_X_ext but with X_ext having new time-dependent
% columns added by the user (eg. the product of a column of X_ext with
% t_ext. Survival time ties are handled using Efron's method.
%
% Input
% sID_ext: Extended subjects' IDs (nx1).
% X_ext: Extended design matrix (nxp).
% d_ext: Extended censorship status vector (nx1).
% t_ext: Extended survival time vector (nx1).
% e: Convergence epsilon (gradient's norm). Default 10^-3;
%
% Output
% stats.Bhat: Estimated vector of the population regression parameters.
% stats.CovBhat: Estimated covariance matrix of the population regression
% parameters.
% stats.llh: Values of the maximum log-likelihood across the optimization
% process.
% st: Termination state (1 for convergence and 0 otherwise).
%
% $Revision: 1.1 $  $Date: 2015/01/06 17:04:02 $
% Original Author: Jorge Luis Bernal Rusiel
% CVS Revision Info:
%    $Author: mreuter $
%    $Date: 2015/01/06 17:04:02 $
%    $Revision: 1.1 $
% References: Kleinbaum, D.G., Klein, M., 2005. Survival analysis. A self-
% learning approach, second edition. New York: Springer..
%
if nargin < 4
    error('Too few inputs');
elseif nargin < 5
    e = 0.001;
end;
tic;
[n,p] = size(X_ext);
if (length(sID_ext)~=n) || (length(d_ext)~=n) || (length(t_ext)~=n)
    error(['The design matrix X_ext, censorship status vector d_ext, time'...
        ' vector t_ext and subject ID vector sID_ext must all have the same'...
        ' number of rows.']);
end;
%indices of unique failure times in ft_ix (last index when ties happen)
st_ix = find(d_ext==1);
[~,ft_ix] = unique(t_ext(st_ix),'last');
ft_ix = st_ix(ft_ix);
%Starting values
Bhat = zeros(p,1);

%% Iterations
nit = 50;
gnorm = e+1;
it = 1;
display('Starting Newton-Raphson iterations');
while (gnorm>e) && (it<=nit)
    gr = SStat_Gradient(X_ext,t_ext,Bhat,ft_ix);
    He = SStat_Hessian(X_ext,t_ext,Bhat,ft_ix);
    if (cond(He) < 1e+10)
        invHe = He\eye(p);
    else
        [Vtemp,Dtemp] = eig(He);
        invHe = Vtemp*diag(1./max(diag(Dtemp),1e-5))*Vtemp';
    end
    Bhat = Bhat - invHe*gr;
    %log-likelihood
    llh = SStat_Likelihood(X_ext,t_ext,Bhat,ft_ix);
    display(['Likelihood at iteration ' num2str(it) ' : ' num2str(llh)]);
    gnorm = norm(gr);
    display(['Gradient norm: ' num2str(gnorm)]);
    it = it+1;
end;
%% Termination
z_sc = Bhat./sqrt(diag(-invHe));
pval = 2*(1-normcdf(abs(z_sc),0,1));
stats = struct('Bhat',Bhat,'zscore',z_sc,'pval',pval,'CovBhat',-invHe,'llh',llh);
if (gnorm<=e)
    st = 1;
else
    st = 0;
    display(['Algorithm does not converge after ' num2str(nit)...
        ' iterations!!!']);
end;
et = toc;
display(['Total elapsed time is ' num2str(et) ' seconds']);
end

%% Likelihood, Gradient and Hessian

function llk = SStat_Likelihood(X_ext,t_ext,Bhat,ft_ix)
%
% Log-likelihood value.
%
% Input
% X_ext: Extended design matrix.
% t_ext: Extended survival time vector.
% Bhat: Estimated vector of the population regression parameters.
% ft_ix: Failure time indices in X_ext (last index if any tie).
%
% Output
% llk: Log-likelihood value.
%
llk = 0;
nft = length(ft_ix);
for j=1:nft
    term = 0;
    ties = ft_ix(t_ext(ft_ix(j))==t_ext(ft_ix));
    nties = length(ties)-1;
    lpr = X_ext(ties,:)*Bhat;
    aux1 = sum(exp(lpr));
    aux2 = sum(exp(X_ext(t_ext(ft_ix(j))==t_ext,:)*Bhat));
    for l=0:nties
        term = term + log(aux2-l*aux1/(nties+1));
    end;
    llk = llk + sum(lpr) - term;
end;
end

function gr = SStat_Gradient(X_ext,t_ext,Bhat,ft_ix)
%
% Gradient vector for the log-likelihood.
%
% Input
% X_ext: Extended design matrix.
% t_ext: Extended survival time vector.
% Bhat: Estimated vector of the population regression parameters.
% ft_ix: Failure time indices in X_ext (last index if any tie).
%
% Output
% gr: Gradient vector.
%
p = size(X_ext,2);
gr = zeros(p,1);
nft = length(ft_ix);
for j=1:nft
    ties = ft_ix(t_ext(ft_ix(j))==t_ext(ft_ix));
    nties = length(ties)-1;
    riskset = t_ext(ft_ix(j))==t_ext;
    term1 = sum(X_ext(ties,:),1);
    term2 = exp(X_ext(riskset,:)*Bhat);
    term3 = exp(X_ext(ties,:)*Bhat);
    term4 = term2'*X_ext(riskset,:);
    term5 = term3'*X_ext(ties,:);
    term = 0;
    for l=0:nties
        term = term + (term4-l*term5/(nties+1))/(sum(term2)-l*sum(term3)/(nties+1));
    end;
    gr = gr + (term1 - term)';
end;
end

function He = SStat_Hessian(X_ext,t_ext,Bhat,ft_ix)
%
% Hessian matrix for the log-likelihood.
%
% Input
% X_ext: Extended design matrix.
% t_ext: Extended survival time vector.
% Bhat: Estimated vector of the population regression parameters.
% ft_ix: Failure time indices in X_ext (last index if any tie).
%
% Output
% He: Hessian matrix.
%
p = size(X_ext,2);
He = zeros(p,p);
nft = length(ft_ix);
for j=1:nft
    ties = ft_ix(t_ext(ft_ix(j))==t_ext(ft_ix));
    nties = length(ties)-1;
    rsk_ix = find(t_ext(ft_ix(j))==t_ext);
    m = length(rsk_ix);
    term1 = 0;
    for i=1:m
        term1 = term1 + exp(X_ext(rsk_ix(i),:)*Bhat)*X_ext(rsk_ix(i),:)'*X_ext(rsk_ix(i),:);
    end;
    term2 = 0;
    for i=1:nties
        term2 = term2 + exp(X_ext(ties(i),:)*Bhat)*X_ext(ties(i),:)'*X_ext(ties(i),:);
    end;
    term3 = sum(exp(X_ext(rsk_ix,:)*Bhat));
    term4 = sum(exp(X_ext(ties,:)*Bhat));
    term5 = exp(X_ext(rsk_ix,:)*Bhat)'*X_ext(rsk_ix,:);
    term6 = exp(X_ext(ties,:)*Bhat)'*X_ext(ties,:);
    term = 0;
    for l=0:nties
        Z = term5 - l*term6/(nties+1);
        phi = term3 - l*term4/(nties+1);
        term = term + (term1-l*term2/(nties+1))/phi - Z'*Z/(phi*phi);
    end;
    He = He - term;
end;
end