classdef CEC2020_F9 < PROBLEM
% <single> <real>
% Composition function 2

%------------------------------- Reference --------------------------------
% C .T. Yue, K. V. Price, P. N. Suganthan, J. J. Liang, M. Z. Ali, B. Y.
% Qu, N. H. Awad, and P. P Biswas, Problem definitions and evaluation
% criteria for the CEC 2020 special session and competition on single
% objective bound constrained numerical optimization, Zhengzhou University,
% China and Nanyang Technological University, Singapore, 2019.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    properties
        O;      % Optimal decision vector
        Mat;	% Rotation matrix
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            CallStack = dbstack('-completenames');
            load(fullfile(fileparts(CallStack(1).file),'CEC2020.mat'),'Data');
            obj.O = Data{9}.o;
            obj.M = 1;
            if isempty(obj.D) || obj.D < 10
                obj.D   = 5;
                obj.Mat = Data{9}.M_5;
            elseif obj.D < 15
                obj.D   = 10;
                obj.Mat = Data{9}.M_10;
            elseif obj.D < 20
                obj.D   = 15;
                obj.Mat = Data{9}.M_15;
            else
                obj.D   = 20;
                obj.Mat = Data{9}.M_20;
            end
            obj.lower    = zeros(1,obj.D) - 100;
            obj.upper    = zeros(1,obj.D) + 100;
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            lambda = [10 1e-6 10 1];
            delta  = [10 20 30 40];
            bias   = [0 100 200 300];
            func   = {@Ackley,@Elliptic,@Griewank,@Rastrigin};
            W      = zeros(size(PopDec,1),4);
            F      = zeros(size(W));
            for i = 1 : size(W,2)
                tmp    = sum((PopDec-repmat(obj.O(i,1:size(PopDec,2)),size(PopDec,1),1)).^2,2);
                W(:,i) = 1./(sqrt(tmp)+1e-10).*exp(-tmp/2/obj.D/delta(i)^2);
                F(:,i) = func{i}((PopDec-repmat(obj.O(i,1:size(PopDec,2)),size(PopDec,1),1))*obj.Mat((i-1)*obj.D+1:i*obj.D,:)');
            end
            W = W./repmat(sum(W,2),1,size(W,2));
            PopObj = 2400 + sum(W.*(repmat(lambda,size(F,1),1).*F+repmat(bias,size(F,1),1)),2);
        end
    end
end

function F = Ackley(X)
    F = -20*exp(-0.2*sqrt(mean(X.^2,2))) - exp(mean(cos(2*pi*X),2)) + 20 + exp(1);
end

function F = Elliptic(X)
    F = sum((1e6).^(repmat(0:size(X,2)-1,size(X,1),1)/(size(X,2)-1+1e-6)).*X.^2,2);
end

function F = Griewank(X)
    X = 6*X;
    F = sum(X.^2,2)/4000 - prod(cos(X)./repmat(sqrt(1:size(X,2)),size(X,1),1),2) + 1;
end

function F = Rastrigin(X)
    X = 0.0512*X;
    F = sum(X.^2-10*cos(2*pi*X)+10,2);
end


function [xUpdate,SUpdate,innov,Szz,W]=sqrtCubKalUpdate(xPred,SPred,z,SR,h,xi,w,innovTrans,measAvgFun,stateDiffTrans,stateTrans)
%SQRTCUBKALUPDATE Perform the measurement update step in the square root
%                 cubature Kalman filter with additive measurement noise.
%                 Unlike the non-square root version, the covariance update
%                 is such that finite precision problems cannot lead to a
%                 non-positive (semi-)definite covariance. However, unlike
%                 the non-square root version, all of the cubature weights
%                 must be positive.
%
%INPUTS: xPred The xDim X 1 predicted target state.
%        SPred The xDim X xDim lower-triangular square root predicted state
%              covariance matrix.
%            z The zDim X 1  measurement vector.
%           SR The zDim X zDim lower-triangular square root of the
%              measurement covariance matrix in the native coordinate
%              system of the measurement.
%            h A function handle for the measurement function that the
%              state as its argument.
%           xi An xDim X numCubPoints matrix of cubature points. If this
%              and the next parameter are omitted or empty matrices are
%              passed, then fifthOrderCubPoints(xDim). It is suggested that
%              xi and w be provided to avoid needless recomputation of the
%              cubature points.
%            w A numCubPoints X 1 vector of the weights associated with the
%              cubature points.
%   innovTrans An optional function handle that computes and optionally
%              transforms the value of the difference between the
%              observation and any predicted points. This is called as
%              innovTrans(a,b) and the default if omitted or an empty
%              matrix is passed is @(a,b)bsxfun(@minus,a,b). This must be
%              able to handle sets of values. For a zDimX1 measurement,
%              either of the inputs could be zDimXN in size while one of
%              the inputs could be zDimX1 in size.  This only needs to be
%              supplied when a measurement difference must be restricted
%              to a certain range. For example, the innovation between two
%              angles will be 2*pi if one angle is zero and the other
%              2*pi, even though they are the same direction. In such an
%              instance, a function handle to the
%              wrapRange(bsxfun(@minus,a,b),-pi,pi) function with the
%              appropriate parameters should be passed for innovTrans.
%   measAvgFun An optional function handle that, when given N measurement
%              values with weights, produces the weighted average. This
%              function only has to be provided if the domain of the
%              measurement is not linear. For example, when averaging
%              angular values, then the function meanAng should be used.
% stateDiffTrans An optional function handle that takes an xDimXN matrix of
%              N differences between states and transforms them however
%              might be necessary. If not transformation is necessary, this
%              parameter can be omitted or an empty matrix passed.
%   stateTrans An optional function that takes a state estimate and
%              transforms it. This is useful if one wishes the elements of
%              the state to be bound to a certain domain. For example, if
%              an element of the state is an angle, one might generally
%              want to bind it to the region +/-pi.
%
%OUTPUTS: xUpdate The xDim X 1 updated state vector.
%         SUpdate The updated xDim X xDim lower-triangular square root
%                 state covariance matrix.
%      innov, Szz The zDimX1 innovation and the zDimXzDim square root
%                 innovation covariance matrix are returned in case one
%                 wishes to analyze the consistency of the estimator or use
%                 those values in gating or likelihood evaluation.
%               W The xDimXzDim gain used in the update. This can be
%                 useful when gating and using the function
%                 calcMissedGateCov.
%
%If the function h needs additional parameters beyond the state, then the
%parameters can be passed by using an anonymous function as the function
%handle. For example, suppose that the measurement function is measFunc and
%it needs the additional parameters param1 and param2. In this instance,
%rather than using
%h=@measFunc
%one should use
%h=@(x)measFunc(x,param1,param2)
%This way, every time sqrtCubKalUpdate calls measFunc (via h) with a
%different x, those two parameters are always passed.
%
%The mathematics behind the function sqrtCubKalUpdate are described in more
%detail in Section IX of [1] and in [2]. Note that this is essentially one
%type of square root "unscented Kalman filter" with additive noise. One
%simply has to provide the filter with the appropriate cubature points and
%weights.
%
%The optional parameters innovTrans and measAvgFun are not described in
%references [1] and [2], but allow for possible modifications to the filter
%as described in [3] The parameters have been added to allow the filter to
%be used with angular quantities. For example, if the measurement consisted
%of range and angle, z=[r;theta], then
%innovTrans=@(a,b)[bsxfun(@minus,a(1,:),b(1,:));
%                  wrapRange(bsxfun(@minus,a(2,:),b(2,:)),-pi,pi)];
%measAvgFun=@(z,w)[calcMixtureMoments(z(1,:),w);
%                  meanAng(z(2,:),w')];
%should be used to approximately deal with the circular nature of the
%measurements.
%
%REFERENCES:
%[1] D. F. Crouse , "Basic tracking using nonlinear 3D monostatic and
%    bistatic measurements," IEEE Aerospace and Electronic Systems Magazine,
%    vol. 29, no. 8, Part II, pp. 4-53, Aug. 2014.
%[2] I. Arasaratnam and S. Haykin, "Cubature Kalman filters," IEEE
%    Transactions on Automatic Control, vol. 54, no. 6, pp. 1254-1269,
%    Jun. 2009.
%[3] D. F. Crouse, "Cubature/ unscented/ sigma point Kalman filtering with
%    angular measurement models," in Proceedings of the 18th International
%    Conference on Information Fusion, Washington, D.C., 6-9 Jul. 2015.
%
%July 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    xDim=size(xPred,1);

    if(nargin<6||isempty(xi))
        [xi,w]=fifthOrderCubPoints(xDim);
    end

    if(nargin<8||isempty(innovTrans))
        %The function just returns the input.
        innovTrans=@(a,b)bsxfun(@minus,a,b);
    end

    if(nargin<9||isempty(measAvgFun))
        measAvgFun=@(zPoints,w)calcMixtureMoments(zPoints,w);
    end

    if(nargin<10||isempty(stateDiffTrans))
        stateDiffTrans=@(x)x;
    end

    if(nargin<11||isempty(stateTrans))
        stateTrans=@(x)x;
    end

    zDim=size(z,1);
    numCubPoints=size(xi,2);
    sqrtW=sqrt(w);
    %Calculate the updated estimates

    %Predicted cubature state points
    xPredPoints=stateTrans(transformCubPoints(xi,xPred,SPred));

    %Predicted, centered cubature state points
    xPredCenPoints=bsxfun(@times,stateDiffTrans(bsxfun(@minus,xPredPoints,xPred)),sqrtW');

    %Predicted cubature measurement points
    zPredPoints=zeros(zDim,numCubPoints);
    for curP=1:numCubPoints
        zPredPoints(:,curP)=h(xPredPoints(:,curP));
    end

    %Measurement prediction.
    zPred=measAvgFun(zPredPoints,w);

    %Centered, predicted cubature measurement points, transformed as
    %necessary to keep the values within a desired range.
    zPredCenPoints=bsxfun(@times,innovTrans(zPredPoints,zPred),sqrtW');

    %Root innovation covariance
    Szz=tria([zPredCenPoints,SR]);

    %The cross covariance.
    Pxz=xPredCenPoints*zPredCenPoints';

    %The filter gain
    W=(Pxz/Szz')/Szz;

    %The innovation, transformed as necessary to keep values in a desired
    %range.
    innov=innovTrans(z,zPred);

    %Updated state estimate
    xUpdate=stateTrans(xPred+W*innov);

    %Updated state root covariance
    SUpdate=tria([stateDiffTrans(xPredCenPoints-W*zPredCenPoints),W*SR]);
end

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


function A11 = synA11min(A10, A01, cmax)
%-----------------------------------------------------------------------------
%
% For each point of colour 11 this function assigns the minimum value at the
% neighbouring gridpoints of colours 10 and 01.
%
% Design and implementation by:
% Dr. Paul M. de Zeeuw <Paul.de.Zeeuw@cwi.nl>  http://homepages.cwi.nl/~pauldz/
% Last Revision: December 7, 2001.
% (c) 1998-2002 Stichting CWI, Amsterdam
%-----------------------------------------------------------------------------
[n10, m10]=size(A10);
[n01, m01]=size(A01);
n11=n10;
m11=m01;
%[n11, m11]=size(A11);
if     m11 == m10
  S=min(stripL(extR(A10, cmax)), A10);
elseif m11 == m10-1
  S=min(stripL(A10), stripR(A10));
else
  disp([' size A10 = ' int2str(size(A10)) ' size A01 = ' int2str(size(A01))]);
  error(' synA11min - A10 and A01 do not match ');
end
if     n11 == n01
  T=min(A01, stripU(extD(A01, cmax)));
elseif n11 == n01-1
  T=min(stripD(A01), stripU(A01));
else
  disp([' size A10 = ' int2str(size(A10)) ' size A01 = ' int2str(size(A01))]);
  error(' synA11min - A10 and A01 do not match ');
end
%Note: all(size(S) == size(T)) & all(size(S) == [n11 m11]) always holds.
A11=min(S, T);
%-----------------------------------------------------------------------------


function [Y,W,SetupStruc] = Process_ILRMA_PF(s,Transfer,SetupStruc)
K = SetupStruc.ILRMA_PF.K;
hop = SetupStruc.ILRMA_PF.hop;
win = hanning(K,'periodic');
win = win/sqrt(sum(win(1:hop:K).^2));
SetupStruc.ILRMA_PF.win = win;  % Preserve 'win' in 'SetupStruc'
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
N = size(s,2);
for i = 1:N
    X(:,:,i) = fft(enframe(s(:,i),win,hop)');
end
frame_N = size(X,2);
K_m = K/2+1;
Num = size(Transfer,3);
Y = zeros((frame_N-1)*hop+K,Num);
Y_f = zeros(Num,frame_N,K);
%%%%%%%%%%%%%%%%%%%%%%%%%% Obtain processing matrix 'W'
X_sp = zeros(Num,frame_N,K_m);
W_ILRMA = zeros(Num,Num,K_m);
V_sp = zeros(Num,N,K_m);
theta = 10^-6;
for i = 1:K_m
    X_f = permute(X(i,:,:),[3 2 1]);
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%% Initialize W by PCA
    [E,D] = PCA(X_f,1,Num);
    V = sqrt(D)\E';
    V_sp(:,:,i) = V;
    X_sp(:,:,i) = V*X_f;
    %%%%%%%%%%%% Adjust amplitude of 'w'
    W_o = eye(Num);
    y_f = W_o*V*X_f;
    W_ILRMA(:,:,i) = W_o;
    Y_f(:,:,i) = y_f;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%% ILRMA iterations
epsi = 1e-6;
max_iteration = 300;
L = 2*Num;  %%%%% the number of NMF basis
Z = max(rand(Num,L),epsi);
Z = Z./(ones(Num)*Z);
T = max(rand(K_m,L),epsi);
V = max(rand(L,frame_N),epsi);
R = zeros(K_m,frame_N,Num);
for i = 1:Num
    R(:,:,i) = (ones(K_m,1)*Z(i,:)).*T*V;
end
P = permute(Y_f(:,:,1:K_m),[3,2,1]);
P = abs(P).^2;
% K_sp = sqrt(K_m);
pObj = inf;
bZ = zeros(size(Z));
bT = zeros(size(T));
bV = zeros(size(V));
lamd = zeros(Num,1);
A = zeros(1001,2)-1; %%%% Show the decrease of the value of cost funtion, ILRMA max iterations 1000
for iteration = 1:max_iteration
    %%%%%% NMF with partioning function Z
    for i = 1:Num
        bZ(i,:) = sqrt((T'*(P(:,:,i).*(R(:,:,i).^(-2)))).*V*ones(frame_N,1)./((T'*R(:,:,i).^(-1)).*V*ones(frame_N,1)))';
    end
    Z = max(Z./(ones(Num)*Z),epsi);
    for i = 1:Num
        R(:,:,i) = (ones(K_m,1)*Z(i,:)).*T*V;
    end
    for i = 1:K_m
        P_temp = permute(P(i,:,:),[2 3 1]);
        R_temp = permute(R(i,:,:),[2 3 1]);
        bT(i,:) = sqrt((V*(P_temp.*(R_temp.^(-2)))).*Z'*ones(Num,1)./((V*R_temp.^(-1)).*Z'*ones(Num,1)))';
    end
    T = max(T.*bT,epsi);
    for i = 1:Num
        R(:,:,i) = (ones(K_m,1)*Z(i,:)).*T*V;
    end
    for i = 1:frame_N
        P_temp = permute(P(:,i,:),[1 3 2]);
        R_temp = permute(R(:,i,:),[1 3 2]);
        bV(:,i) = sqrt((T'*(P_temp.*(R_temp.^(-2)))).*Z'*ones(Num,1)./((T'*R_temp.^(-1)).*Z'*ones(Num,1)));
    end
    V = max(V.*bV,epsi);
    for i = 1:Num
        R(:,:,i) = (ones(K_m,1)*Z(i,:)).*T*V;
    end
    %%%%% AuxIVA
    dlw = 0;
    for i = 1:K_m
        W = W_ILRMA(:,:,i);
        X_f = X_sp(:,:,i);
       dlw = dlw +log(abs(det(W))+epsi);
        for i_n = 1:Num
            G_ = permute(R(i,:,i_n),[3 2 1]);
            G_ = repmat(G_+epsi,Num,1);
            Vk = (X_f./G_)*X_f'/frame_N;
            if rcond(Vk)<theta
                Vk = Vk+eye(Num)*min(eig(Vk))*theta;
            end
            wk = inv(W*Vk);
            wk = wk(:,i_n);
            wk = wk/(sqrt(wk'*Vk*wk)+epsi);
            W(i_n,:) = wk';
        end
        W_ILRMA(:,:,i) = W;
        Y_f(:,:,i) = W*X_f;
    end
    P = permute(Y_f(:,:,1:K_m),[3,2,1]);
    P = abs(P).^2;
    Obj = ((sum(sum(sum(log(R+epsi))))+sum(sum(sum(P./R))))/frame_N-2*dlw)/(Num*K_m);
    dObj = pObj-Obj;
    pObj = Obj;
    A(iteration,:) = [Obj,abs(dObj)/abs(Obj)];
    if(abs(dObj)/abs(Obj)<theta)
       break;
    end
    for i = 1:Num
        lamda = sqrt(sum(sum(P(:,:,i))/(frame_N*K_m)));
        lamd(i) = lamda;
        for i_k = 1:K_m
            W_ILRMA(i,:,i_k) = W_ILRMA(i,:,i_k)/lamda;
            for l = 1:L
                T(i_k,l) = T(i_k,l)*sum(Z(:,l))/lamda^2;
            end
        end
        P(:,:,i) = P(:,:,i)/lamda^2;
        R(:,:,i) = R(:,:,i)/lamda^2;
    end
    for l = 1:L
        Z_temp = sum(Z(:,l)./lamd.^(2));
        for i = 1:Num
            Z(i,l) = Z(i,l)/lamd(i)^2/Z_temp;
        end
    end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%% Post processing
W = zeros(Num,N,K_m);
Y_f(:,:,1) = zeros(Num,frame_N);
for i = 2:K_m
    W_inv = pinv(W_ILRMA(:,:,i)*V_sp(:,:,i));
    for ii = 1:Num
        Y_f(ii,:,i) = Y_f(ii,:,i)*W_inv(1,ii);
        W_ILRMA(ii,:,i) = W_ILRMA(ii,:,i)*W_inv(1,ii);
    end
    W(:,:,i) = W_ILRMA(:,:,i)*V_sp(:,:,i);
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    if(i~=K_m)
        Y_f(:,:,K+2-i) = conj(Y_f(:,:,i));
    end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Recover signals
if(K/hop==2)
    win = ones(K,1);
end
for i = 1:Num
    y_temp = permute(Y_f(i,:,:),[3 2 1]);
    Y(:,i) = overlapadd(real(ifft(y_temp))',win,hop);
end
return;


function [P,Uinit,output] = cp_als(X,R,varargin)
%CP_ALS Compute a CP decomposition of any type of tensor.
%
%   P = CP_ALS(X,R) computes an estimate of the best rank-R
%   CP model of a tensor X using an alternating least-squares
%   algorithm.  The input X can be a tensor, sptensor, ktensor, or
%   ttensor. The result P is a ktensor.
%
%   P = CP_ALS(X,R,'param',value,...) specifies optional parameters and
%   values. Valid parameters and their default values are:
%      'tol' - Tolerance on difference in fit {1.0e-4}
%      'maxiters' - Maximum number of iterations {50}
%      'dimorder' - Order to loop through dimensions {1:ndims(A)}
%      'init' - Initial guess [{'random'}|'nvecs'|cell array]
%      'printitn' - Print fit every n iterations; 0 for no printing {1}
%
%   [P,U0] = CP_ALS(...) also returns the initial guess.
%
%   [P,U0,out] = CP_ALS(...) also returns additional output that contains
%   the input parameters.
%
%   Note: The "fit" is defined as 1 - norm(X-full(P))/norm(X) and is
%   loosely the proportion of the data described by the CP model, i.e., a
%   fit of 1 is perfect.
%
%   NOTE: Updated in various minor ways per work of Phan Anh Huy. See Anh
%   Huy Phan, Petr Tichavsk? Andrzej Cichocki, On Fast Computation of
%   Gradients for CANDECOMP/PARAFAC Algorithms, arXiv:1204.1586, 2012.
%
%   Examples:
%   X = sptenrand([5 4 3], 10);
%   P = cp_als(X,2);
%   P = cp_als(X,2,'dimorder',[3 2 1]);
%   P = cp_als(X,2,'dimorder',[3 2 1],'init','nvecs');
%   U0 = {rand(5,2),rand(4,2),[]}; %<-- Initial guess for factors of P
%   [P,U0,out] = cp_als(X,2,'dimorder',[3 2 1],'init',U0);
%   P = cp_als(X,2,out.params); %<-- Same params as previous run
%
%   See also KTENSOR, TENSOR, SPTENSOR, TTENSOR.
%
%MATLAB Tensor Toolbox.
%Copyright 2015, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2015) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

%% Extract number of dimensions and norm of X.
N = ndims(X);
normX = norm(X);

%% Set algorithm parameters from input or by using defaults
params = inputParser;
params.addParamValue('tol',1e-4,@isscalar);
params.addParamValue('maxiters',50,@(x) isscalar(x) & x > 0);
params.addParamValue('dimorder',1:N,@(x) isequal(sort(x),1:N));
params.addParamValue('init', 'random', @(x) (iscell(x) || ismember(x,{'random','nvecs'})));
params.addParamValue('printitn',1,@isscalar);
params.parse(varargin{:});

%% Copy from params object
fitchangetol = params.Results.tol;
maxiters = params.Results.maxiters;
dimorder = params.Results.dimorder;
init = params.Results.init;
printitn = params.Results.printitn;

%% Error checking

%% Set up and error checking on initial guess for U.
if iscell(init)
    Uinit = init;
    if numel(Uinit) ~= N
        error('OPTS.init does not have %d cells',N);
    end
    for n = dimorder(2:end);
        if ~isequal(size(Uinit{n}),[size(X,n) R])
            error('OPTS.init{%d} is the wrong size',n);
        end
    end
else
    % Observe that we don't need to calculate an initial guess for the
    % first index in dimorder because that will be solved for in the first
    % inner iteration.
    if strcmp(init,'random')
        Uinit = cell(N,1);
        for n = dimorder(2:end)
            Uinit{n} = rand(size(X,n),R);
        end
    elseif strcmp(init,'nvecs') || strcmp(init,'eigs')
        Uinit = cell(N,1);
        for n = dimorder(2:end)
            Uinit{n} = nvecs(X,n,R);
        end
    else
        error('The selected initialization method is not supported');
    end
end

%% Set up for iterations - initializing U and the fit.
U = Uinit;
fit = 0;

if printitn>0
  fprintf('\nCP_ALS:\n');
end

%% Main Loop: Iterate until convergence

if (isa(X,'sptensor') || isa(X,'tensor')) && (exist('cpals_core','file') == 3)

    %fprintf('Using C++ code\n');
    [lambda,U] = cpals_core(X, Uinit, fitchangetol, maxiters, dimorder);
    P = ktensor(lambda,U);

else

    UtU = zeros(R,R,N);
    for n = 1:N
        if ~isempty(U{n})
            UtU(:,:,n) = U{n}'*U{n};
        end
    end

    for iter = 1:maxiters

        fitold = fit;

        % Iterate over all N modes of the tensor
        for n = dimorder(1:end)

            % Calculate Unew = X_(n) * khatrirao(all U except n, 'r').
            Unew = mttkrp(X,U,n);

            % Compute the matrix of coefficients for linear system
            Y = prod(UtU(:,:,[1:n-1 n+1:N]),3);
            Unew = Unew / Y;
            if issparse(Unew)
                Unew = full(Unew);   % for the case R=1
            end

            % Normalize each vector to prevent singularities in coefmatrix
            if iter == 1
                lambda = sqrt(sum(Unew.^2,1))'; %2-norm
            else
                lambda = max( max(abs(Unew),[],1), 1 )'; %max-norm
            end

            Unew = bsxfun(@rdivide, Unew, lambda');

            U{n} = Unew;
            UtU(:,:,n) = U{n}'*U{n};
        end

        P = ktensor(lambda,U);
        if normX == 0
            fit = norm(P)^2 - 2 * innerprod(X,P);
        else
            normresidual = sqrt( normX^2 + norm(P)^2 - 2 * innerprod(X,P) );
            fit = 1 - (normresidual / normX); %fraction explained by model
        end
        fitchange = abs(fitold - fit);

        % Check for convergence
        if (iter > 1) && (fitchange < fitchangetol)
            flag = 0;
        else
            flag = 1;
        end

        if (mod(iter,printitn)==0) || ((printitn>0) && (flag==0))
            fprintf(' Iter %2d: f = %e f-delta = %7.1e\n', iter, fit, fitchange);
        end

        % Check for convergence
        if (flag == 0)
            break;
        end
    end
end

%% Clean up final result
% Arrange the final tensor so that the columns are normalized.
P = arrange(P);
% Fix the signs
P = fixsigns(P);

if printitn>0
    if normX == 0
        fit = norm(P)^2 - 2 * innerprod(X,P);
    else
        normresidual = sqrt( normX^2 + norm(P)^2 - 2 * innerprod(X,P) );
        fit = 1 - (normresidual / normX); %fraction explained by model
    end
  fprintf(' Final f = %e \n', fit);
end

output = struct;
output.params = params.Results;
output.iters = iter;


function sF = approximation(nodes, y, varargin)
% computes a least square problem to get an approximation
% Syntax
%   sF = S2FunHarmonic.approximation(S2Grid, f)
%   sF = S2FunHarmonic.approximation(S2Grid, f, 'bandwidth', bandwidth, 'tol', TOL, 'maxit', MAXIT, 'weights', W)
%
% Input
%  S2Grid - grid on the sphere
%  f      - function values on the grid (may be multidimensional)
%
% Options
%  bandwidth  - maximum degree of the spherical harmonics used to approximate the function
%  to         - tolerance for lsqm
%  maxIt      - maximum number of iterations for lsqm
%  W          - weight w_n for the node nodes (default: voronoi weights)
%

% make points unique
s = size(y);
y = reshape(y, length(nodes), []);
[nodes,~,ind] = unique(nodes(:));

% take the mean over duplicated nodes
for k = 1:size(y,2)
  yy(:,k) = accumarray(ind,y(:,k),[],@nanmean); %#ok<AGROW>
end

% consider the case of symmetry
if isa(nodes,'Miller')

  nodes = nodes.symmetrise('noAntipodal').';
  nodes.antipodal = nodes.CS.isLaue;
  yy = repmat(yy,size(nodes,2),1);
  nodes = nodes(:);

end

y = reshape(yy, [length(nodes) s(2:end)]);

tol = get_option(varargin, 'tol', 1e-6);
maxit = get_option(varargin, 'maxit', 40);

if check_option(varargin, 'antipodal') || nodes.antipodal
  nodes.antipodal = true;
  bw = get_option(varargin, 'bandwidth', ceil(sqrt(length(nodes))));
  bw = floor(bw/2)*2; % make bandwidth even
  mask = sparse((bw+1)^2); % only use even polynomial degree
  for m = 0:2:bw
    mask((m^2+1):(m^2+2*m+1), (m^2+1):(m^2+2*m+1)) = speye(2*m+1);
  end
else
  bw = get_option(varargin, 'bandwidth', ceil(sqrt(length(nodes)/2)));
  mask = speye((bw+1)^2);
end

W = get_option(varargin, 'weights');
if isempty(W)
  W = nodes.calcVoronoiArea;
else
  W = W(ind);
end
W = sqrt(W(:));

% initialize nfsft
nfsftmex('precompute', bw, 1000, 1, 0);
plan = nfsftmex('init_advanced', bw, length(nodes), 1);
nfsftmex('set_x', plan, [nodes.rho'; nodes.theta']); % set vertices
nfsftmex('precompute_x', plan);

b = W.*y;

s = size(b);
b = reshape(b, s(1), []);
num = size(b, 2);

fhat = zeros((bw+1)^2, num);
for index = 1:num
  [fhat(:, index), flag] = lsqr(...
    @(x, transp_flag) afun(transp_flag, x, plan, W, mask), ...
    b(:, index), tol, maxit);
  fhat(:, index) = mask*fhat(:, index);
end
fhat = reshape(fhat, [(bw+1)^2 s(2:end)]);

% finalize nfsft
nfsftmex('finalize', plan);

sF = S2FunHarmonic(fhat);

% ensure symmetry if required
if isa(nodes,'Miller'), sF = sF.symmetrise(nodes.CS); end

end

function y = afun(transp_flag, x, plan, W, mask)

if strcmp(transp_flag, 'transp')

  x = x.*W;

  nfsftmex('set_f', plan, x);
  nfsftmex('adjoint', plan);
  y = nfsftmex('get_f_hat_linear', plan);

  y = mask*y;

elseif strcmp(transp_flag, 'notransp')

  x = mask*x;

  nfsftmex('set_f_hat_linear', plan, x);
  nfsftmex('trafo', plan);
  y = nfsftmex('get_f', plan);

  y = y.*W;

end

end


%SVC Trainable classifier: Support Vector Machine
%
% 	[W,J] = SVC(A,KERNEL,C)
%   [W,J] = A*SVC([],KERNEL,C)
%   [W,J] = A*SVC(KERNEL,C)
%
% INPUT
%   A	      Dataset
%   KERNEL  - Untrained mapping to compute kernel by A*(A*KERNEL) during
%             training, or B*(A*KERNEL) during testing with dataset B.
%           - String to compute kernel matrices by FEVAL(KERNEL,B,A)
%           Default: linear kernel (PROXM('p',1))
%   C       Regularization parameter (optional; default: 1)
%
% OUTPUT
%   W       Mapping: Support Vector Classifier
%   J       Object indices of support objects
%
% DESCRIPTION
% Optimizes a support vector classifier for the dataset A by quadratic
% programming. The non-linearity is determined by the kernel.
% If KERNEL = 0 it is assumed that A is already the kernelmatrix (square).
% In this case also a kernel matrix B should be supplied at evaluation by
% B*W or PRMAP(B,W).
%
% There are several ways to define KERNEL, e.g. PROXM('r',1) for a
% radial basis kernel or by USERKERNEL for a user defined kernel.
%
% If C is NaN this regularisation parameter is optimised by REGOPTC.
%
% See for more possibilties SVCINFO
%
% EXAMPLE
% a = gendatb;                     % generate banana classes
% [w,J] = a*svc(proxm('p',3));  % compute svm with 3rd order polynomial
% a*w*testc                        % show error on train set
% scatterd(a)                      % show scatterplot
% plotc(w)                         % plot classifier
% hold on;
% scatterd(a(J,:),'o')             % show support objcts
%
% SEE ALSO (PRTools Guide)
% MAPPINGS, DATASETS, PROXM, USERKERNEL, NUSVC, RBSVC, LIBSVC, REGOPTC

% Copyright: D. de Ridder, D. Tax, S. Verzakov, R.P.W. Duin, r.p.w.duin@37steps.com
% Faculty EWI, Delft University of Technology
% P.O. Box 5031, 2600 GA Delft, The Netherlands

function [W,J,C,nu,alginf] = svc(varargin)

  mapname = 'SVC';
  argin = shiftargin(varargin,{'prmapping','char','cell'});
  argin = setdefaults(argin,[],proxm([],'p',1),1,1);

  if mapping_task(argin,'definition')

    W = define_mapping(argin,'untrained',mapname);

	elseif mapping_task(argin,'training')			% Train a mapping.

    [a,kernel,C,Options] = check_for_old_call(argin);
    if isequal(Options,1), Options = []; end

    DefOptions.mean_centering   = 1;
    DefOptions.pd_check         = 1;
    DefOptions.bias_in_admreg   = 1;
    DefOptions.pf_on_failure    = 1;
    DefOptions.multiclass_mode  = 'single';

    Options = updstruct(DefOptions,Options,1);

    islabtype(a,'crisp');
    isvaldfile(a,1,2); % at least 1 object per class, 2 classes
    a = testdatasize(a,'objects');
    [m,k,c] = getsize(a);
    nlab = getnlab(a);

    % The SVC is basically a 2-class classifier. More classes are
    % handled by mclassc.

    if c == 2   % two-class classifier

      if (isnan(C))|length(C)>1     % optimize trade-off parameter
        defs = {proxm([],'p',1),1,[]};
        if isnan(C) % use a default range of C values
          parmin_max = [0,0;1e-2,1e2;0,0];   % kernel and Options can not be optimised
        else % use the range that is supplied by the user
          parmin_max = [0 0; min(C) max(C); 0 0];
          C = NaN;
        end
        [W,J,C,nu,alginf] = regoptc(a,mfilename,{kernel,C,Options},defs,[2],parmin_max,testc([],'soft'));
      else
        % Compute the parameters for the optimization:
        y = 3 - 2*nlab;
        if isequal(kernel,0)
          s = [];
          u = [];
          in_size = 0; % to allow old and new style calls
        else
          if Options.mean_centering
            u = mean(a);         % shift origin for better accuracy
            a = a - repmat(u,[m,1]);
          else
            u = [];
          end
          in_size = k;
        end
        K = compute_kernel(a,a,kernel);
        K = min(K,K');   % make sure kernel is symmetric
        [v,J,C,nu] = svo(+K,y,C,Options);

        % Store the results:
        if ~isequal(kernel,0)
          s = a(J,:);
        end
        W = prmapping(mfilename,'trained',{u,s,v,kernel,J},getlablist(a),in_size,2);
        % Note: even in case kernel is a mapping, we store the untrained
        % mapping, as training is just administration
        W = cnormc(W,a);
        W = setcost(W,a);

        alginf.svc_type = 'C-SVM';
        alginf.kernel = kernel;
        alginf.C   = C;
        alginf.nu  = nu;
        alginf.nSV = length(J);
        alginf.classsizes = [nnz(y==1), nnz(y==-1)];
        alginf.pf = isnan(nu);
        W = setname(W,'SVM');

      end

    else   % multi-class classifier:

      [W,J,C,nu,alginf] = mclassc(a,prmapping(mfilename,{kernel,C,Options}),Options.multiclass_mode);
      W = W*classc;

    end
    W = setname(W,mapname);

  else % Evaluation

    [a,v] = deal(argin{1:2});
    w = +v;
    m = size(a,1);

    if isempty(w{2})
      K = a; % user supplied testkernel
      J = w{5};
      if size(K,2) > length(J) & size(K,2) >= max(J)
        K = K(:,J); % probably full test kernel
      elseif size(K,2) ~= length(J)
        error('Wrong test kernel supplied')
      end
    else
      % The first parameter w{1} stores the mean of the dataset. When it
      % is supplied, remove it from the dataset to improve the numerical
      % precision. Then compute the kernel matrix using proxm:
      if isempty(w{1})
        K = a*(w{2}*w{4});
      else
        K = (a-ones(m,1)*w{1})*(w{2}*w{4});
      end
    end
    % Data is mapped by the kernel, now we just have a linear
    % classifier  w*x+b:
    d = [K ones(m,1)] * w{3};
    W = setdat(a,[d -d],v);

  end

return

function K = compute_kernel(a,s,kernel)

	% compute a kernel matrix for the objects a w.r.t. the support objects s
	% given a kernel description

	if  ischar(kernel) % routine supplied to compute kernel
		K = feval(kernel,a,s);
	elseif iscell(kernel)
		K = feval(kernel{1},a,s,kernel{2:end});
	elseif ismapping(kernel)
		K = a*prmap(s,kernel);
	elseif kernel == 0 % we have already a kernel
		K = a;
	else
		error('Do not know how to compute kernel matrix')
	end

	K = +K;

return

function [a,kernel,C,par] = check_for_old_call(argin)

  [a,kernel,C,par] = deal(argin{:});
  if ischar(kernel) && exist(kernel,'file') ~= 2
    kernel = proxm(kernel,C);
    C = par;
    par = [];
  end

return


function [x,w,P,Z] = fnnlsb(XtX,Xty,P_old,Z_old,tol)
%FNNLSb	Non-negative least-squares.
%
%     	%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%	%							 %
%	% Please note this version of NNLS is for advanced users %
%	% Please refer to the m-file FNNLS for a simpler fast    %
%	% version of NNLS				         %
%	%						         %
%     	%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
% 	Adapted from NNLS of Mathworks, Inc.
%
%	x = fnnlsb(A,b) returns the vector X that solves x = pinv(A)*b
%	in a least squares sense, subject to x >= 0.
%
%	A default tolerance of TOL = MAX(SIZE(A)) * NORM(A,1) * EPS
%	is used for deciding when elements of x are less than zero.
%	This can be overridden with x = fnnlsb(A,b,P_old,Z_old,TOL).
%
%	[x,w] = fnnlsb(A,b) also returns dual vector w where
%	w(i) < 0 where x(i) = 0 and w(i) = 0 where x(i) > 0.
%       [X,W,P,Z] = fnnlsb(A,b) also returns the index set P and Z
%	that explicitly tells which constraints (variables) are
% 	active, which are not. These can be used as subsequent
%	input to fnnlsb if only minor changes are expected in a new
%	problem. P designates the passive (unconstrained) set, while
%	designates the active constraints. For initialisation use
% 	P=zeros(size(Xty));
%	Z=[1:length(Xty)]';
%
%	See also FNNLS and NNLS

%	L. Shure 5-8-87
%	Revised, 12-15-88,8-31-89 LS.
%	(Partly) Copyright (c) 1984-94 by The MathWorks, Inc.

%	Modified by R. Bro 5-7-96 according to
%       Bro R., de Jong S., Journal of Chemometrics, 1997, 11, 393-401
% 	Corresponds to the FNNLSb algorithm in the paper
%
%
%	Rasmus bro
%	Chemometrics Group, Food Technology
%	Dept. Dairy and Food Science
%	Royal Vet. & Agricultural
%	DK-1958 Frederiksberg C
%	Denmark
%	rb@kvl.dk
%	http://newton.foodsci.kvl.dk/rasmus.html

%  Reference:
%  Lawson and Hanson, "Solving Least Squares Problems", Prentice-Hall, 1974.

% initialize variables
if nargin < 5
    tol = 10*eps*norm(XtX,1)*max(size(XtX));
end
[m,n] = size(XtX);
P = P_old(:)';
PP = find(P);
Z = Z_old(:)';
ZZ=find(Z);
iter = 0;
itmax = 30*n;
z=zeros(n,1);
x=z;
z(PP)=(Xty(PP)'/XtX(PP,PP)');

    while any((z(PP) <= tol)) & iter < itmax
        iter = iter + 1;
        QQ = find((z <= tol) & P');
        alpha = min(x(QQ)./(x(QQ) - z(QQ)));
        x = x + alpha*(z - x);
        ij = find(abs(x) < tol & P' ~= 0);
        Z(ij)=ij';
        P(ij)=zeros(1,max(size(ij)));
        PP = find(P);
        ZZ = find(Z);
        nzz = size(ZZ);
        z(PP)=(Xty(PP)'/XtX(PP,PP)');
        z(ZZ) = zeros(nzz(2),nzz(1));
        z=z(:);
    end
    x = z;
    w = Xty-XtX*x;

% set up iteration criterion
iter = 0;

% outer loop to put variables into set to hold positive coefficients
while any(Z) & any(w(ZZ) > tol)
    [wt,t] = max(w(ZZ));
    t = ZZ(t);
    P(1,t) = t;
    Z(t) = 0;
    PP = find(P);
    ZZ = find(Z);
    nzz = size(ZZ);
    z(PP)=(Xty(PP)'/XtX(PP,PP)');
    z(ZZ) = zeros(nzz(2),nzz(1))';
    z=z(:);
    while any((z(PP) <= tol)) & iter < itmax
        iter = iter + 1;
        QQ = find((z <= tol) & P');
        alpha = min(x(QQ)./(x(QQ) - z(QQ)));
        x = x + alpha*(z - x);
        ij = find(abs(x) < tol & P' ~= 0);
        Z(ij)=ij';
        P(ij)=zeros(1,max(size(ij)));
        PP = find(P);
        ZZ = find(Z);
        nzz = size(ZZ);
        z(PP)=(Xty(PP)'/XtX(PP,PP)');
        z(ZZ) = zeros(nzz(2),nzz(1));
        z=z(:);
    end
    x = z;
    w = Xty-XtX*x;
end

x=x(:);


function Retinex = retinex_mccann99(L, nIterations)

global OPE RRE Maximum
[nrows ncols] = size(L) ;                              % get size of the input image
nLayers = ComputeLayers(nrows, ncols) ;                % compute the number of pyramid layers
nrows = nrows/( 2 ^nLayers) ;                            % size of image to process for layer 0
ncols = ncols/( 2 ^nLayers) ;
if (nrows*ncols > 25 )                                % not processing images of area > 25
  error( ' invalid image size. ' )                       % at first layer
end
Maximum = max(L(:)) ;                                  % maximum color value in the image
OP = Maximum*ones([nrows ncols]) ;                     % initialize Old Product
for layer = 0 :nLayers
   RR = ImageDownResolution(L, 2 ^(nLayers-layer)) ;    % reduce input to required layer size

   OPE = [zeros(nrows, 1 ) OP zeros(nrows, 1 )] ;          % pad OP with additional columns
   OPE = [zeros( 1 ,ncols+ 2 ) ; OPE; zeros(1,ncols+2)];  % and rows
   RRE = [RR(:, 1 ) RR RR(:,end)] ;                      % pad RR with additional columns
   RRE = [RRE( 1 ,:) ; RRE; RRE(end,:)];                % and rows

   for iter = 1 :nIterations
     CompareWithNeighbor(- 1 , 0 ) ;                      % North
     CompareWithNeighbor(- 1 , 1 ) ;                      % North-East
     CompareWithNeighbor( 0 , 1 ) ;                       % East
     CompareWithNeighbor( 1 , 1 ) ;                       % South-East
     CompareWithNeighbor( 1 , 0 ) ;                       % South
     CompareWithNeighbor( 1 , - 1 ) ;                      % South-West
     CompareWithNeighbor( 0 , - 1 ) ;                      % West
     CompareWithNeighbor(- 1 , - 1 ) ;                     % North-West
   end
   NP = OPE( 2 :(end- 1 ), 2 :(end- 1 )) ;
   OP = NP(:, [fix( 1 : 0 . 5 :ncols) ncols]) ;              %%% these two lines are equivalent with
   OP = OP([fix( 1 : 0 . 5 :nrows) nrows], :) ;              %%% OP = imresize(NP, 2) if using Image
   nrows = 2 *nrows ; ncols = 2*ncols;                 % Processing Toolbox in MATLAB
end
Retinex = NP ;

function CompareWithNeighbor(dif_row, dif_col)
global OPE RRE Maximum
% Ratio-Product operation
IP = OPE( 2 + dif_row: (end- 1 +dif_row), 2 + dif_col: (end- 1 +dif_col)) + ...
     RRE( 2 :(end- 1 ), 2 :(end- 1 )) - RRE( 2 + dif_row: (end- 1 +dif_row), 2 + dif_col: (end- 1 +dif_col)) ;

IP(IP > Maximum) = Maximum ;                           % The Reset step

% ignore the results obtained in the rows or columns for which the neighbors are undefined
if (dif_col == - 1 ) IP(:, 1 ) = OPE( 2 :(end- 1 ), 2 ) ; end
if (dif_col == + 1 ) IP(:,end) = OPE( 2 :(end- 1 ),end- 1 ) ; end
if (dif_row == - 1 ) IP( 1 ,:) = OPE( 2 , 2 :(end- 1 )) ; end
if (dif_row == + 1 ) IP(end,:) = OPE(end- 1 , 2 :(end- 1 )) ; end
NP = (OPE( 2 :(end- 1 ), 2 :(end- 1 )) + IP)/ 2 ;               % The Averaging operation
OPE( 2 :(end- 1 ), 2 :(end- 1 )) = NP ;

function Layers = ComputeLayers(nrows, ncols)
power = 2 ^fix(log2(gcd(nrows, ncols))) ;               % start from the Greatest Common Divisor
while(power > 1 & ((rem(nrows, power) ~= 0 ) | (rem(ncols, power) ~= 0 )))
   power = power/ 2 ;                                   % and find the greatest common divisor
end                                                  % that is a power of 2
Layers = log2(power) ;

function Result = ImageDownResolution(A, blocksize)
[rows, cols] = size(A) ;                               % the input matrix A is viewed as
result_rows = rows/blocksize ;                         % a series of square blocks
result_cols = cols/blocksize ;                         % of size = blocksize
Result = zeros([result_rows result_cols]) ;
for crt_row = 1 :result_rows                          % then each pixel is computed as
   for crt_col = 1 :result_cols                       % the average of each such block
      Result(crt_row, crt_col) = mean2(A( 1 +(crt_row- 1 )* blocksize: crt_row*blocksize, ...
                                       1 +(crt_col- 1 )* blocksize: crt_col*blocksize)) ;
   end
en


function u = minL2Potts2DADMM(f, gamma, varargin)
%minL2Potts2DADMM A ADMM splitting strategy for the two-dimensional vector valued L^2-Potts
%problem
%
%Description:
% Minimizes the (2D) Potts problem
%
%  \gamma \| u \|_0 + \| A u - f \|_p^p -> min
%
% using ADMM splitting an dynamic programming
%
%Reference:
% M. Storath, A. Weinmann
% Fast partitioning of vector-valued images",
% SIAM Journal on Imaging Sciences, 2014

% written by M. Storath
% $Date: 2014-06-30 11:26:34 +0200 (Mo, 30. Jun 2014) $	$Revision: 99 $

[m,n,~] = size(f);

% parse options
ip = inputParser;
addParamValue(ip, 'muInit', gamma*1e-2);
addParamValue(ip, 'muStep', 2);
addParamValue(ip, 'tol', 1e-10);
addParamValue(ip, 'isotropic', true);
addParamValue(ip, 'verbose', false);
addParamValue(ip, 'weights', ones(m,n));
addParamValue(ip, 'multiThreading', true);
addParamValue(ip, 'quantization', true);
addParamValue(ip, 'useADMM', true);
parse(ip, varargin{:});
par = ip.Results;

% check args
assert(par.muStep > 1, 'Variable muStep must be > 1.');
assert(all(par.weights(:) >= 0), 'Weights must be >= 0.');
assert(par.tol > 0, 'Stopping tolerance must be > 0.');
assert(par.muInit > 0, 'muInit must be > 0.');

% cast data to PLImage
plf = pottslab.PLImage(f);

% main program (calls Java routines)
if par.isotropic
    % near-isotropic discretization
    omega(1) = sqrt(2.0) - 1.0;
    omega(2) = 1.0 - sqrt(2.0)/2.0;
    % alternative neighborhood weights
    %omega(1) = (2 * sqrt(2.0) - 1.0)/3;
    %omega(2) = (2 - sqrt(2.0))/3;
    plu = pottslab.JavaTools.minL2PottsADMM8(plf, gamma, par.weights, par.muInit, par.muStep, par.tol, par.verbose, par.multiThreading, par.useADMM, omega);

else
    % anisotropic discretization
    plu = pottslab.JavaTools.minL2PottsADMM4(plf, gamma, par.weights, par.muInit, par.muStep, par.tol, par.verbose, par.multiThreading, par.useADMM);
end

% reshape the 1D array given by .toDouble()
u = reshape( plu.toDouble(), size(f) );

% to remove small remaining variations in result (algorithm works with floats)
if par.quantization
    u = round(u * 255)/255;
end

end


function traj = dirTrans_Euler(config)

n = config.grid.nTrajPts;

% Create the initial guess:
guess.time = linspace(config.guess.time(1),config.guess.time(end),n);
guess.state = interp1(...
    config.guess.time', config.guess.state', guess.time')';
guess.control = interp1(...
    config.guess.time', config.guess.control', guess.time')';

% Create a list of all linear constraints, then add function handles:
[problem, pack] = buildConstraints(guess,config);
problem.objective = @(z)( costFunctionWrapper(z,pack) );
problem.nonlcon = @(z)( nonLinCon(z,pack, config) );

% Solve using fmincon:
[zSoln,fSoln,exitFlag] = fmincon(problem);

% Post-processing:
[t,x,u] = unPackDecVar(zSoln,pack);

traj.time = linspace(t(1),t(2),n);
traj.state = x;
traj.control = u;
traj.objVal = fSoln;
traj.exitFlag = exitFlag;
traj.interp.state = @(tt)( interp1(traj.time',traj.state',tt')' );
traj.interp.control = @(tt)( interp1(traj.time',traj.control',tt')' );

end

function [C, Ceq] = nonLinCon(z,pack,config)
%
% This function enforces the dynamics of the cart-pole system
%

% Based on the Trapazoid Method for discretization, as defined in Bett's book,
% chapter 4.

n = pack.nState(2);
[t,x,u] = unPackDecVar(z,pack);
dt = (t(2)-t(1))/(n-1);

% Evaluate the dynamics at each collocation point
dx = cartPoleDynamics(x,[u; zeros(size(u))],config.dyn);

% Trapazoid rule:
idxLow = 1:(n-1);
idxUpp = 2:n;
intStateTrap = 0.5*dt*(dx(:,idxLow) + dx(:,idxUpp));
intStateCol = x(:,idxUpp)-x(:,idxLow);

% Defect constraint:
defect = intStateTrap - intStateCol;
%
% % user-defined boundary constraints:
% [bndIneq, bndEq] = boundaryConstraint(t,x(:,1),x(:,end),config.userData);

C = [];%bndIneq;
Ceq = [reshape(defect,numel(defect),1)];% bndEq];

end

function cost = costFunctionWrapper(z,pack)

[t,x,u] = unPackDecVar(z,pack);

% Trapazoid rule to integrate cost function:
tt = linspace(t(1),t(end),pack.nState(2));  %Time vector
dc = costFunction(tt,x,u);
nTime = size(x,2);
dt = (t(2)-t(1))/(nTime-1);
w = ones(nTime,1); w([1,end]) = 0.5;
cost = dt*dc*w;

end


function [min_v,min_i,min_diff] = local_min(F,S,ep)
  % LOCAL_MIN  find values and indices of local minima of a scalar field
  % defined on a mesh
  %
  % [min_v,min_i,min_diff] = local_min(F,S,ep)
  %
  % Inputs:
  %   F #faces by 3 list of triangle indices
  %   S #vertices by 1 list of scalar values
  % Outputs:
  %   min_v  #minima list of values at local minima
  %   min_i  #minima list of indices into S of local minima
  %   min_diff #minima list of min difference between each minima and its
  %     neighbors
  %

  if ~exist('ep','var')
    ep = 0;
  end

  % number of vertices
  n = numel(S);
  if size(F,2) == 2
    E = F;
  else
    E = edges(F);
  end
  E = [E;fliplr(E)];
  A = sparse( ...
    E(:,1), ...
    E(:,2), ...
    S(E(:,1))-S(E(:,2)), ...
    n,n);
  minA = minnz(A)';
  [min_i,~] = find(minA>-ep);
  min_diff = minA(min_i);
  min_v = S(min_i);

end


function [assignment, cost] = assignmentsuboptimal1(distMatrix)
%ASSIGNMENTSUBOPTIMAL1    Compute suboptimal assignment
%   ASSIGNMENTSUBOPTIMAL1(DISTMATRIX) computes a suboptimal assignment
%   (minimum overall costs) for the given rectangular distance or cost
%   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
%   rows. 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 element is chosen and the assignment is made. If
%   there are still assignments open, the minimum element in the distance
%   matrix is searched and the corresponding assignment is made.
%
%   In scenarios without any forbidden assignments, the algorithm reduces
%   to the last step, which will provide 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.
%
%   assignment.html  File Exchange  Donate via PayPal
%
%   Markus Buehren
%   Last modified 05.07.2011

% 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
  % tracks, 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


%DEMO_MODELASSESMENT2  Demonstration for model assessment when the observation
%                      model is non-Gaussian
%
%  Description
%    We will consider the classification problem in demo_classific.
%    The analysis is conducted with full Gaussian process using
%    both probit and logit likelihood. The performance of these two
%    models are compared by evaluating the ten-fold
%    cross-validation, leave-one-out cross-validation, WAIC, DIC
%    and the effective number of parameters The inference will be
%    conducted using maximum a posterior (MAP) estimate for the
%    parameters using EP and Laplace approximation, via full Markov
%    chain Monte Carlo (MCMC) and with an integration approximation
%    (IA) for the parameters.
%
%    This demo is organised in two parts:
%     1) data analysis with with probit likelihood
%     2) data analysis with with logit likelihood
%
%  See also
%    DEMO_CLASSIFIC1, DEMO_MODELASSESMENT1
%
% Copyright (c) 2009-2010 Jarno Vanhatalo
% Copyright (c) 2010 Aki Vehtari

% 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.

% =====================================
% 1) data analysis with probit likelihood
% =====================================
disp('Data analysis with probit likelihood')
S = which('demo_classific');
L = strrep(S,'demo_classific.m','demodata/synth.tr');
x=load(L);
y=x(:,end);
y = 2.*y-1;
x(:,end)=[];
[n, nin] = size(x);

DIC=repmat(NaN,1,8);DIC2=repmat(NaN,1,8);DIC_latent=repmat(NaN,1,8);
p_eff=repmat(NaN,1,8);p_eff2=repmat(NaN,1,8);p_eff_latent=repmat(NaN,1,8);p_eff_latent2=repmat(NaN,1,8);

% Create covariance functions
gpcf = gpcf_sexp('lengthScale', [0.9 0.9], 'magnSigma2', 2);

% Set the prior for the parameters of covariance functions
pl = prior_logunif();
gpcf = gpcf_sexp(gpcf, 'lengthScale_prior', pl,'magnSigma2_prior', pl); %

% Create the GP structure
gp = gp_set('lik', lik_probit, 'cf', gpcf, 'jitterSigma2', 1e-4);

% ------- Laplace approximation --------
disp(['Probit with Laplace integration over the latent values '; ...
      'and MAP estimate for the parameters                    '])

% Set the approximate inference method
gp = gp_set(gp, 'latent_method', 'Laplace');

n=length(y);

% Set the options for the optimization
opt=optimset('TolFun',1e-3,'TolX',1e-3);
% Optimize with the quasi-Newton method
gp=gp_optim(gp,x,y,'opt',opt,'optimf',@fminlbfgs);

% Evaluate the effective number of parameters and DIC with focus on
% latent variables.
models{1} = 'pr_Laplace';
p_eff_latent(1) = gp_peff(gp, x, y);
[DIC_latent(1), p_eff_latent2(1)] = gp_dic(gp, x, y);
WAIC(1) = gp_waic(gp,x,y);

% Evaluate the 10-fold cross-validation results.
cvres = gp_kfcv(gp, x, y, 'display', 'fold');
mlpd_cv(1) = cvres.mlpd_cv;

% Evaluate the leave-one-out cross-validation results.
[Ef,Varf,lpy] =  gp_loopred(gp, x, y);
mlpd_loo(1) = mean(lpy);

% ------- Expectation propagation --------
disp(['Probit with EP integration over the latent values and MAP '; ...
      'estimate for the parameters                               '])

% Set the approximate inference method
gp = gp_set(gp, 'latent_method', 'EP');

% Set the options for the optimization
opt=optimset('TolFun',1e-3,'TolX',1e-3);
% Optimize with the BFGS quasi-Newton method
gp=gp_optim(gp,x,y,'opt',opt,'optimf',@fminlbfgs);

% Evaluate the effective number of parameters and DIC with focus on
% latent variables.
models{2} = 'pr_EP';
p_eff_latent(2) = gp_peff(gp, x, y) ;
[DIC_latent(2), p_eff_latent2(2)] = gp_dic(gp, x, y);
WAIC(2) = gp_waic(gp,x,y);

% Evaluate the 10-fold cross-validation results.
cvres = gp_kfcv(gp, x, y, 'display', 'fold');
mlpd_cv(2) = cvres.mlpd_cv;

% Evaluate the leave-one-out cross-validation results.
[Ef,Varf,lpy] =  gp_loopred(gp, x, y);
mlpd_loo(2) = mean(lpy);

% ------- MCMC ---------------
disp(['Probit with MCMC integration over the latent values and '; ...
      'the parameters                                          '])

% Set the approximate inference method
gp = gp_set(gp, 'latent_method', 'MCMC');

% Sample
mcopt.nsamples=220;mcopt.display=20;
[rgp,gp]=gp_mc(gp, x, y, mcopt);
rgp=thin(rgp, 21, 2);

% Evaluate the effective number of parameters and DIC with focus on
% latent variables.
models{3} = 'pr_MCMC';
[DIC(3), p_eff(3)] =  gp_dic(rgp, x, y, 'focus', 'hyper');
[DIC2(3), p_eff2(3)] =  gp_dic(rgp, x, y);
WAIC(3) = gp_waic(rgp,x,y);

% Evaluate the 10-fold cross-validation results.
mcopt.nsamples=50;mcopt.display=20;
cvres = gp_kfcv(gp, x, y, 'inf_method', 'MCMC', 'opt', mcopt, 'display', 'fold');
mlpd_cv(3) = cvres.mlpd_cv;

% Evaluate the leave-one-out cross-validation results.
[Ef,Varf,lpy] =  gp_loopred(rgp, x, y);
mlpd_loo(3) = mean(lpy);

% --- Integration approximation approach ---
disp(['Probit with EP integration over the latent values and '; ...
      'grid integration over the parameters                  '])

% Use EP
gp = gp_set(gp, 'latent_method', 'EP');

% Set the options for the optimization
opt=optimset('TolFun',1e-3,'TolX',1e-3);
% Optimize with the BFGS quasi-Newton method
gp=gp_optim(gp,x,y,'opt',opt,'optimf',@fminlbfgs);

% now perform the integration
clear opt
opt.int_method = 'grid';
opt.step_size = 2;
gp_array = gp_ia(gp, x, y, opt);

models{4} = 'pr_IA';
[DIC(4), p_eff(4)] =  gp_dic(gp_array, x, y, 'focus', 'hyper');
[DIC2(4), p_eff2(4)] =  gp_dic(gp_array, x, y);
WAIC(4) = gp_waic(gp_array,x,y);

% Then the 10 fold cross-validation.
cvres = gp_kfcv(gp, x, y, 'inf_method', 'IA', 'opt', opt, 'display', 'fold');
mlpd_cv(4) = cvres.mlpd_cv;

% Evaluate the leave-one-out cross-validation results.
[Ef,Varf,lpy] =  gp_loopred(gp_array, x, y);
mlpd_loo(4) = mean(lpy);

% =====================================
% 2) data analysis with logit likelihood
% =====================================
disp('Data analysis with logit likelihood')

S = which('demo_classific');
L = strrep(S,'demo_classific.m','demodata/synth.tr');
x=load(L);
y=x(:,end);
y = 2.*y-1;
x(:,end)=[];
[n, nin] = size(x);

% Create covariance functions
gpcf = gpcf_sexp('lengthScale', [0.9 0.9], 'magnSigma2', 2);

% Set the prior for the parameters of covariance functions
pl = prior_logunif();
gpcf = gpcf_sexp(gpcf, 'lengthScale_prior', pl,'magnSigma2_prior', pl); %

% Create the likelihood structure
lik = ('init');

% Create the GP structure
gp = gp_set('lik', lik_logit, 'cf', gpcf, 'jitterSigma2', 1e-4);

% ------- Laplace approximation --------
disp(['Logit with Laplace integration over the latent values and '; ...
      'MAP estimate for the parameters                           '])

% Set the approximate inference method
gp = gp_set(gp, 'latent_method', 'Laplace');

% Set the options for the optimization
opt=optimset('TolFun',1e-3,'TolX',1e-3);
% Optimize with the BFGS quasi-Newton method
gp=gp_optim(gp,x,y,'opt',opt,'optimf',@fminlbfgs);

% Evaluate the effective number of parameters and DIC with focus on
% latent variables.
models{5} = 'lo_Laplace';
p_eff_latent(5) = gp_peff(gp, x, y);
[DIC_latent(5), p_eff_latent2(5)] = gp_dic(gp, x, y);
WAIC(5) = gp_waic(gp,x,y);

% Evaluate the 10-fold cross-validation results.
cvres = gp_kfcv(gp, x, y, 'display', 'fold');
mlpd_cv(5) = cvres.mlpd_cv;

% Evaluate the leave-one-out cross-validation results.
[Ef,Varf,lpy] =  gp_loopred(gp, x, y);
mlpd_loo(5) = mean(lpy);

% ------- Expectation propagation --------
disp(['Logit with EP integration over the latent values and MAP'; ...
      'estimate for the parameters                             '])

% Set the approximate inference method
gp = gp_set(gp, 'latent_method', 'EP');

% Set the options for the optimization
opt=optimset('TolFun',1e-3,'TolX',1e-3);
% Optimize with the BFGS quasi-Newton method
gp=gp_optim(gp,x,y,'opt',opt,'optimf',@fminlbfgs);

% Evaluate the effective number of parameters and DIC with focus on
% latent variables.
models{6} = 'lo_EP';
p_eff_latent(6) = gp_peff(gp, x, y) ;
[DIC_latent(6), p_eff_latent2(6)] = gp_dic(gp, x, y);
WAIC(6) = gp_waic(gp,x,y);

% Evaluate the 10-fold cross-validation results.
cvres = gp_kfcv(gp, x, y, 'display', 'fold');
mlpd_cv(6) = cvres.mlpd_cv;

% Evaluate the leave-one-out cross-validation results.
[Ef,Varf,lpy] =  gp_loopred(gp, x, y);
mlpd_loo(6) = mean(lpy);

% ------- MCMC ---------------
disp(['Logit with MCMC integration over the latent values and '; ...
      'the parameters                                         '])

% Set the approximate inference method
gp = gp_set(gp, 'latent_method', 'MCMC');

% Sample
mcopt.nsamples=200;mcopt.display=20;
[rgp,gp] = gp_mc(gp, x, y, mcopt);
rgp=thin(rgp, 21, 2);

% Evaluate the effective number of parameters and DIC with focus on latent variables.
models{7} = 'lo_MCMC';
[DIC(7), p_eff(7)] =  gp_dic(rgp, x, y, 'focus', 'hyper');
[DIC2(7), p_eff2(7)] =  gp_dic(rgp, x, y);
WAIC(7) = gp_waic(rgp,x,y);

% Evaluate the 10-fold cross-validation results.
mcopt.nsamples=50;mcopt.display=20;
cvres = gp_kfcv(gp, x, y, 'inf_method', 'MCMC', 'opt', mcopt, 'display', 'fold');
mlpd_cv(7) = cvres.mlpd_cv;

% Evaluate the leave-one-out cross-validation results.
[Ef,Varf,lpy] =  gp_loopred(rgp, x, y);
mlpd_loo(7) = mean(lpy);

% --- Integration approximation approach ---
disp(['Logit with EP integration over the latent values and grid '; ...
      'integration over the parameters                           '])

% Use EP
gp = gp_set(gp, 'latent_method', 'EP');

% Set the options for the optimization
opt=optimset('TolFun',1e-3,'TolX',1e-3);
% Optimize with the BFGS quasi-Newton method
gp=gp_optim(gp,x,y,'opt',opt,'optimf',@fminlbfgs);

% now perform the integration
clear opt
opt.int_method = 'grid';
opt.step_size = 2;
gp_array = gp_ia(gp, x, y, opt);

models{8} = 'lo_IA';
[DIC(8), p_eff(8)] =  gp_dic(gp_array, x, y, 'focus', 'hyper');
[DIC2(8), p_eff2(8)] =  gp_dic(gp_array, x, y);
WAIC(8) = gp_waic(gp_array,x,y);

% Then the 10 fold cross-validation.
cvres = gp_kfcv(gp, x, y, 'inf_method', 'IA', 'opt', opt, 'display', 'fold');
mlpd_cv(8) = cvres.mlpd_cv;

% Evaluate the leave-one-out cross-validation results.
[Ef,Varf,lpy] =  gp_loopred(gp_array, x, y);
mlpd_loo(8) = mean(lpy);

%========================================================
% PART 4 Print the results
%========================================================
disp('Summary of the results')

S = '       ';
for i = 1:length(models)
    S = [S '  ' models{i}];
end

S = sprintf([S '\n CV-mlpd  %6.2f    %6.2f  %6.2f  %6.2f   %6.2f    %6.2f  %6.2f  %6.2f'], mlpd_cv);
S = sprintf([S '\n LOO-mlpd %6.2f    %6.2f  %6.2f  %6.2f   %6.2f    %6.2f  %6.2f  %6.2f'], mlpd_loo);
S = sprintf([S '\n ']);
S = sprintf([S '\n WAIC     %6.2f    %6.2f  %6.2f  %6.2f   %6.2f    %6.2f  %6.2f  %6.2f'], WAIC);
S = sprintf([S '\n ']);
S = sprintf([S '\n DIC_h    %6.2f    %6.2f  %6.2f  %6.2f   %6.2f    %6.2f  %6.2f  %6.2f'], DIC);
S = sprintf([S '\n DIC_a    %6.2f    %6.2f  %6.2f  %6.2f   %6.2f    %6.2f  %6.2f  %6.2f'], DIC2);
S = sprintf([S '\n DIC_l    %6.2f    %6.2f  %6.2f  %6.2f   %6.2f    %6.2f  %6.2f  %6.2f'], DIC_latent);
S = sprintf([S '\n peff_h   %6.2f    %6.2f  %6.2f  %6.2f   %6.2f    %6.2f  %6.2f  %6.2f'], p_eff);
S = sprintf([S '\n peff_a   %6.2f    %6.2f  %6.2f  %6.2f   %6.2f    %6.2f  %6.2f  %6.2f'], p_eff2);
S = sprintf([S '\n peff_l   %6.2f    %6.2f  %6.2f  %6.2f   %6.2f    %6.2f  %6.2f  %6.2f'], p_eff_latent);
S = sprintf([S '\n peff_l2  %6.2f    %6.2f  %6.2f  %6.2f   %6.2f    %6.2f  %6.2f  %6.2f'], p_eff_latent2);
S = sprintf([S '\n ']);
S = sprintf([S '\n The notation is as follows:']);
S = sprintf([S '\n pr_*     = probit likelihood and inference method']);
S = sprintf([S '\n lo_*     = logit likelihood and inference method']);
S = sprintf([S '\n CV-mlpd  = mean log predictive density from the 10-fold CV. ']);
S = sprintf([S '\n LOO-mlpd = mean log predictive density from the 10-fold CV. ']);
S = sprintf([S '\n WAIC     = Widely applicable information criterion. ']);
S = sprintf([S '\n DIC_h    = DIC with focus on parameters. ']);
S = sprintf([S '\n DIC_a    = DIC with focus on parameters and laten variables (all). ']);
S = sprintf([S '\n DIC_l    = DIC with focus on latent variables. ']);
S = sprintf([S '\n peff_h   = effective number of parameters (latent variables marginalized). ']);
S = sprintf([S '\n peff_a   = effective number of parameters and latent variables. ']);
S = sprintf([S '\n peff_l   = effective number of latent variables evaluated with gp_peff. ']);
S = sprintf([S '\n peff_l2  = effective number of latent variables evaluated with gp_dic. ']);
S = sprintf([S '\n '])


function [x,y,xy] = q2grid(x,y,xy,mv,bound);
%q2grid   biquadratic element grid generator
%   [x,y,xy] = q2grid(x,y,xy,mv,bound);
%   input
%          x          x coordinate vector
%          y          y coordinate vector
%          xy         vertex coordinate vector
%          mv         Q2 macroelement mapping matrix
%          bound      boundary vertex vector
%
% postpocesses Q2 element partitioning information to
% give standard approximation in the case of stretched grids
%   IFISS function: DJS; 28 February 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage
xx=xy(:,1); yy=xy(:,2); nvtx=length(xx);
%
%% recompute mid-side points in the case of stretched grids
% y-direction
yv=yy; ny=length(y);
for k=2:2:ny;
yold=y(k); ynew=0.5*(y(k+1)+y(k-1));
l=find(yy==yold); yv(l)=ynew; y(k)=ynew;
end
% x-direction
xv=xx; nx=length(x);
for k=2:2:nx;
xold=x(k); xnew=0.5*(x(k+1)+x(k-1));
l=find(xx==xold); xv(l)=xnew; x(k)=xnew;
end
xy=[xv,yv];
%
% plotting of the grid
mel=length(mv(:,1));
%if mel <= 256,
adj=sparse(nvtx,nvtx);
for i=1:mel
adj(mv(i,1),mv(i,2))=1;
adj(mv(i,2),mv(i,3))=1;
adj(mv(i,3),mv(i,4))=1;
adj(mv(i,4),mv(i,1))=1;
end
figure(10)
gplot(adj,xy,'b')
axis('square')
hold on
plot(xy(:,1),xy(:,2),'ro')
xybd=xy(bound,:);
plot(xybd(:,1),xybd(:,2),'ko')
hold off
title('Q2 finite element subdivision')
drawnow
%end
return


function [ ret_box8d ] = getRotateBox8D( box8d, orientation, center_x, center_y)
    boxes = box8d;
    rotate_mat = [cosd(orientation), -sind(orientation); sind(orientation), cosd(orientation)];

    %% move to center
    boxes(:,1) = boxes(:,1) - center_y;
    boxes(:,2) = boxes(:,2) - center_x;
    boxes = boxes(:,[2 1])';
    ret_box8d = rotate_mat * boxes;
    ret_box8d = ret_box8d([2,1],:)';

    %% resotre position
    ret_box8d(:,1) = ret_box8d(:,1) + center_y;
    ret_box8d(:,2) = ret_box8d(:,2) + center_x;

    ret_box8d = round(ret_box8d);
end


function [tt]=tt_qlaplacez_dn(d)

% returns a rank-8 QTT decomposition of
% Delta_{1}^{-1} \otimes \Id_{2} \ otimes \ldots \otimes \Id_{D} + \ldots +
%  + \Id_{1} \ otimes \ldots \otimes \Id_{D-1} \otimes Delta_{D}^{-1},
% Delta_{k} being a discretization of Laplace operator on 2^{d(k)} points
% uniform grid,
% Dirichlet boundary conditions being imposed
%
% D=size(d,2) must be >= 1
%
% September 3, 2010
% Vladimir Kazeev
% vladimir.kazeev@gmail.com
% INM RAS
% Moscow, Russia
%
% Look for details in the Preprint No. 75, 2010 of
% Max-Planck Institute for Mathematics in the Sciences
% Vladimir A. Kazeev and Boris N. Khoromskij
% On explicit QTT representation of Laplace operator and its inverse
% http://www.mis.mpg.de/publications/preprints/2010/prepr2010-75.html

d=fliplr(d);
D=size(d,2);
tt=cell(sum(d),1);
I=eye(2);
J=zeros(2);
J(1,2)=1;
I2=zeros(2);
I2(2,2)=1;
E=ones(2);

if (D == 1)
	for key=1 : d
		tt{key}=eye(2);
	end
else

	key=0;
	for k=1 : D
		for kappa=1 : d(k)
			key=key+1;
			if (kappa == 1)
				if (k == 1)
					tt{key}=zeros(2,2,5);
					tt{key}(:,:,1)=I+I2+J+J';
					tt{key}(:,:,2)=2*E;
					tt{key}(:,:,3)=I2+J'+E;
					tt{key}(:,:,4)=I2+J+E;
					tt{key}(:,:,5)=I;
				elseif (k == D)
					tt{key}=zeros(2,2,2,4);
					tt{key}(:,:,1,1)=I;
					tt{key}(:,:,2,1)=I+I2+J+J';
					tt{key}(:,:,2,2)=2*E;
					tt{key}(:,:,2,3)=I2+J'+E;
					tt{key}(:,:,2,4)=I2+J+E;
				else
					tt{key}=zeros(2,2,2,8);
					tt{key}(:,:,1,1)=I+I2+J+J';
					tt{key}(:,:,1,2)=2*E;
					tt{key}(:,:,1,3)=I2+J'+E;
					tt{key}(:,:,1,4)=I2+J+E;
					tt{key}(:,:,1,5)=I;
					tt{key}(:,:,2,5)=I+I2+J+J';
					tt{key}(:,:,2,6)=2*E;
					tt{key}(:,:,2,7)=I2+J'+E;
					tt{key}(:,:,2,8)=I2+J+E;
				end
			elseif (kappa == d(k))
				if (k == D)
					tt{key}=zeros(2,2,4);
					tt{key}(:,:,1)=I;
					tt{key}(:,:,2)=I2;
					tt{key}(:,:,3)=J;
					tt{key}(:,:,4)=J';
				elseif (k == 1)
					tt{key}=zeros(2,2,5,2);
					tt{key}(:,:,1,1)=I;
					tt{key}(:,:,2,1)=I2;
					tt{key}(:,:,3,1)=J;
					tt{key}(:,:,4,1)=J';
					tt{key}(:,:,5,2)=I;
				else
					tt{key}=zeros(2,2,8,2);
					tt{key}(:,:,1,1)=I;
					tt{key}(:,:,2,1)=I2;
					tt{key}(:,:,3,1)=J;
					tt{key}(:,:,4,1)=J';
					tt{key}(:,:,5,2)=I;
					tt{key}(:,:,6,2)=I2;
					tt{key}(:,:,7,2)=J;
					tt{key}(:,:,8,2)=J';
				end
			else
				if (k == D)
					tt{key}=zeros(2,2,4,4);
					tt{key}(:,:,1,1)=I;
					tt{key}(:,:,2,2)=2*E;
					tt{key}(:,:,3,3)=E;
					tt{key}(:,:,4,4)=E;
					tt{key}(:,:,2,1)=I2;
					tt{key}(:,:,3,1)=J;
					tt{key}(:,:,4,1)=J';
					tt{key}(:,:,2,3)=I2+J';
					tt{key}(:,:,2,4)=I2+J;
				elseif (k == 1)
					tt{key}=zeros(2,2,5,5);
					tt{key}(:,:,1,1)=I;
					tt{key}(:,:,2,2)=2*E;
					tt{key}(:,:,3,3)=E;
					tt{key}(:,:,4,4)=E;
					tt{key}(:,:,2,1)=I2;
					tt{key}(:,:,3,1)=J;
					tt{key}(:,:,4,1)=J';
					tt{key}(:,:,2,3)=I2+J';
					tt{key}(:,:,2,4)=I2+J;
					tt{key}(:,:,5,5)=I;
				else
					tt{key}=zeros(2,2,8,8);
					tt{key}(:,:,1,1)=I;
					tt{key}(:,:,2,2)=2*E;
					tt{key}(:,:,3,3)=E;
					tt{key}(:,:,4,4)=E;
					tt{key}(:,:,2,1)=I2;
					tt{key}(:,:,3,1)=J;
					tt{key}(:,:,4,1)=J';
					tt{key}(:,:,2,3)=I2+J';
					tt{key}(:,:,2,4)=I2+J;
					tt{key}(:,:,5,5)=I;
					tt{key}(:,:,6,6)=2*E;
					tt{key}(:,:,7,7)=E;
					tt{key}(:,:,8,8)=E;
					tt{key}(:,:,6,5)=I2;
					tt{key}(:,:,7,5)=J;
					tt{key}(:,:,8,5)=J';
					tt{key}(:,:,6,7)=I2+J';
					tt{key}(:,:,6,8)=I2+J;
				end
			end
		end
	end
end

tt=tt_matrix(tt); % @Bydlocode
return
end


function y = G2a(X)
% mca for rtd function
% reduced order A matrix - no opamps
% X = [R1 R2 R3 R4 R5 R6 R7 R8 R9 RT E1]
R1=X(1);R2=X(2);R3=X(3);R4=X(4);R5=X(5);R6=X(6);
R7=X(7);R8=X(8);R9=X(9);RT=X(10);E1=X(11);
%
A=[1/R1+1/R4+1/RT -1/RT -1/R4 0 -1/R1;
-1/RT 1/R5+1/R6+1/RT -1/R5 0 0;
-1/R4 0 1/R2+1/R3+1/R4 -1/R3 0;
0 -1/R5 1/R5+1/R7 0 0;
0 0 0 0 -1/R9];
%
B=[0;E1/R6;E1/R2;0;E1/R8];
C=A\B;y=C(4);


function restricedRowBool = getCorrespondingRows(S, rowBool, colBool, mode)
% Returns a boolean vector that is true for a subset of the true rows in
% `rowBool` according to whether the rows 'exclusive', 'inclusive', or
% 'partial' -ly correspond to true entries in `colBool`
%
% USAGE:
%
%    restricedRowBool = getCorrespondingRows(S, rowBool, colBool, mode)
%
% INPUTS:
%    S:                     `m x n` stoichiometric matrix
%    rowBool:               `m x 1` boolean vector
%    colBool:               `n x 1` boolean vector
%    mode:                  'exclusive' , 'inclusive' or 'partial'
%
% OUTPUT:
%    restrictedRowBool:     `m x 1` boolean vector
%
% EXAMPLE:
%
%    S =
%        -1     0     0     0     0
%         2    -3     0     0     0
%         0     4    -5     0     0
%         0     0     6    -7     0
%         0     0     0     0     0
%
%    rowBool = [1; 1; 1; 1; 1];
%    colBool = [1; 1; 1; 0; 0];
%
%    % Therefore, the subset of rows and columns considered for inclusion are
%    %    -1     0     0
%    %     2    -3     0
%    %     0     4    -5
%    %     0     0     6
%    %     0     0     0
%
%    % If mode = 'exclusive' then restrictedRowBool corresponds to this subset
%    %    -1     0     0
%    %     2    -3     0
%    %     0     4    -5
%    % i.e subset of rowBool metabolites exclusively involved in the colBool
%    % reactions
%
%    % If mode = 'inclusive' then restrictedRowBool corresponds to this subset
%    %    -1     0     0
%    %     2    -3     0
%    %     0     4    -5
%    %     0     0     6
%    % i.e subset of rowBool metabolites involved in colBool reactions
%
%    % If mode ='partial' then restrictedRowBool corresponds to the extra rows
%    % with inclusive that are not present with exclusive.
%
% .. Author: - Ronan Fleming, July 2016

if ~islogical(rowBool)
    error('rowBool must be a logical vector')
end
if ~islogical(colBool)
    error('colBool must be a logical vector')
end

[mlt,nlt]=size(S);

if length(rowBool)~=mlt
    error('length of rowBool must equal size(S,1)')
end

if length(colBool)~=nlt
    error('length of rowBool must equal size(S,2)')
end

restricedRowBool=false(mlt,1);
switch mode
    case 'exclusive'
        %metatbolites exclusively involved in certain reactions
        restricedRowBool(rowBool)=     any(S(rowBool, colBool),2)...
                                    & ~any(S(rowBool,~colBool),2);
    case 'inclusive'
        %corresponding reactions involving certain metabolites
        restricedRowBool(rowBool) = any(S(rowBool, colBool),2);
    case 'partial'
        %metatbolites exclusively involved in certain reactions
        restricedRowBool(rowBool)=     any(S(rowBool, colBool),2)...
                                    & ~any(S(rowBool,~colBool),2);

        %corresponding reactions involving certain metabolites
        restricedRowBool2=false(mlt,1);
        restricedRowBool2(rowBool) = any(S(rowBool, colBool),2);
        %difference
        restricedRowBool= restricedRowBool2  & ~restricedRowBool;
   otherwise
        error(['Did not recognise mode: ' mode])
end


function sparse_grid_gl_test01 ( dim_min, dim_max, level_max_min, ...
  level_max_max )

%*****************************************************************************80
%
%% TEST01 tests SPARSE_GRID_GL_SIZE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_MIN, the minimum spatial dimension to consider.
%
%    Input, integer DIM_MAX, the maximum spatial dimension to consider.
%
%    Input, integer LEVEL_MAX_MIN, the minimum value of LEVEL_MAX to consider.
%
%    Input, integer LEVEL_MAX_MAX, the maximum value of LEVEL_MAX to consider.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST01\n' );
  fprintf ( 1, '  SPARSE_GRID_GL_SIZE returns the number of distinct\n' );
  fprintf ( 1, '  points in a Gauss-Legendre sparse grid.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Note that, unlike most sparse grids, a sparse grid\n' );
  fprintf ( 1, '  based on Gauss-Legendre points is almost entirely \n' );
  fprintf ( 1, '  NOT nested.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Hence the point counts should be much higher than for a grid of\n' );
  fprintf ( 1, '  the same level, but using rules such as Fejer1 or Fejer2 or\n' );
  fprintf ( 1, '  Gauss-Patterson or Newton-Cotes-Open or Newton-Cotes-Open-Half.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Each sparse grid is of spatial dimension DIM,\n' );
  fprintf ( 1, '  and is made up of all product grids of levels up to LEVEL_MAX.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   DIM: ' );

  for dim_num = dim_min : dim_max
    fprintf ( 1, '  %8d', dim_num );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   LEVEL_MAX\n' );
  fprintf ( 1, '\n' );

  for level_max = level_max_min : level_max_max
    fprintf ( 1, '    %4d', level_max );
    for dim_num = dim_min : dim_max
      point_num = sparse_grid_gl_size ( dim_num, level_max );
      fprintf ( 1, '  %8d', point_num );
    end
    fprintf ( 1, '\n' );
  end

  return
end


function T=Entropy_Yen(X,Trange);
hg=hist(X,Trange);
pg=hg./sum(hg);
Ti=0;
TrangeSpan=Trange(pg>0);
JT=zeros(length(TrangeSpan),1);
for Tk=TrangeSpan
    Ti=Ti+1;
    PT=sum(pg(Trange<=Tk));
    Cf=-log10(sum((pg(Trange<=Tk)./PT).^2));
    Cb=-log10(sum((pg(Trange>Tk)./(1-PT)).^2));
    JT(Ti)=Cf+Cb;
end
JT(isinf(JT))=0;
JT(isnan(JT))=0;
[~,Tind]=max(JT);
T=TrangeSpan(Tind);
end


function  [gx] = g_Kalman4bandits (x, P, ~, in)
% // VBA toolbox //////////////////////////////////////////////////////////
%
% [gx] = g_Kalman4bandits (x, P, u, in)
% softmax decision rule for N-armed bandit task as in Daw et al. 2006
%
% /////////////////////////////////////////////////////////////////////////

% Get parameter values
% -------------------------------------------------------------------------
% inverse temperature
beta = exp (P(1)); % exp: [-Inf,Inf] -> [0 Inf]
% bonus to exploration
phi = P(2);

% Extract expectation and variance of each bandit
% -------------------------------------------------------------------------
nBandits = in.nBandits;
mu = x(1 : nBandits);
sigma2 = x(nBandits + (1 : nBandits)) .^ 2;

% apply softmax
% -------------------------------------------------------------------------
gx = VBA_softmax(beta * (mu + phi * sigma2));


function [ title, key, totcrd, ptrcrd, indcrd, valcrd, rhscrd, mxtype, ...
  nrow, ncol, nnzero, neltvl, ptrfmt, indfmt, valfmt, rhsfmt, rhstyp, ...
  nrhs, nrhsix, colptr, rowind, values, rhsval, rhsptr, rhsind, guess, ...
  exact ] = hb_file_read ( input_unit )

%*****************************************************************************80
%
%% HB_FILE_READ reads an HB file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 April 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Iain Duff, Roger Grimes, John Lewis,
%    User's Guide for the Harwell-Boeing Sparse Matrix Collection,
%    October 1992.
%
%  Parameters:
%
%    Input, integer INPUT_UNIT, the unit from data is read.
%
%    Output, character ( len = 72 ) TITLE, a title for the matrix.
%
%    Output, character ( len = 8 ) KEY, an identifier for the matrix.
%
%    Output, integer TOTCRD, the total number of lines of data.
%
%    Output, integer PTRCRD, the number of input lines for pointers.
%
%    Output, integer INDCRD, the number of input lines for row indices.
%
%    Output, integer VALCRD, the number of input lines for numerical values.
%
%    Output, integer RHSCRD, the number of input lines for right hand sides.
%
%    Output, character ( len = 3 ) MXTYPE, the matrix type.
%    First character is R for Real, C for complex, P for pattern only.
%    Second character is S for symmetric, U for unsymmetric, H for
%      Hermitian, Z for skew symmetric, R for rectangular.
%    Third character is A for assembled and E for unassembled
%      finite element matrices.
%
%    Output, integer NROW, the number of rows or variables.
%
%    Output, integer NCOL, the number of columns or elements.
%
%    Output, integer NNZERO.  In the case of assembled sparse matrices,
%    this is the number of nonzeroes.  In the case of unassembled finite
%    element matrices, in which the right hand side vectors are also
%    stored as unassembled finite element vectors, this is the total
%    number of entries in a single unassembled right hand side vector.
%
%    Output, integer NELTVL, the number of finite element matrix entries,
%    set to 0 in the case of assembled matrices.
%
%    Output, character ( len = 16 ) PTRFMT, the format for reading pointers.
%
%    Output, character ( len = 16 ) INDFMT, the format for reading indices.
%
%    Output, character ( len = 20 ) VALFMT, the format for reading values.
%
%    Output, character ( len = 20 ) RHSFMT, the format for reading values
%    of the right hand side.
%
%    Output, character ( len = 3 ) RHSTYP, the right hand side type.
%    First character is F for full storage or M for same as matrix.
%    Second character is G if starting "guess" vectors are supplied.
%    Third character is X if exact solution vectors are supplied.
%    Ignored if NRHS = 0.
%
%    Output, integer NRHS, the number of right hand sides.
%
%    Output, integer NRHSIX, the number of row indices (set to 0
%    in the case of unassembled matrices.)  Ignored if NRHS = 0.
%
%    Output, integer COLPTR(NCOL+1), COLPTR(I) points to the location of
%    the first entry of column I in the sparse matrix structure.
%
%    Output, integer ROWIND(NNZERO) or ROWIND(NELTVL), the row index of
%    each item.
%
%    Output, real VALUES(NNZERO) or VALUES(NELTVL), the nonzero values
%    of the matrix.
%
%    If RHSTYP(1:1) == 'F':
%
%      Output, integer RHSPTR(*), is not used.
%
%      Output, integer RHSIND(*), is not used.
%
%      Output, real RHSVAL(NROW,NRHS), contains NRHS dense right hand
%      side vectors.
%
%    If RHSTYP(1:1) = 'M' and MXTYPE(3:3) = 'A':
%
%      Output, integer RHSPTR(NRHS+1), RHSPTR(I) points to the location of
%      the first entry of right hand side I in the sparse right hand
%      side vector.
%
%      Output, integer RHSIND(NRHSIX), indicates, for each entry of
%      RHSVAL, the corresponding row index.
%
%      Output, real RHSVAL(NRHSIX), contains the value of the right hand
%      side entries.
%
%    If RHSTYP(1:1) = 'M' and MXTYPE(3:3) = 'E':
%
%      Output, integer RHSPTR(*), is not used.
%
%      Output, integer RHSIND(*), is not used.
%
%      Output, real RHSVAL(NNZERO,NRHS), contains NRHS unassembled
%      finite element vector right hand sides.
%
%    Output, real GUESS(NROW,NRHS), the starting guess vectors.
%
%    Output, real EXACT(NROW,NRHS), the exact solution vectors.
%

%
%  Read the header block.
%
  [ title, key, totcrd, ptrcrd, indcrd, valcrd, rhscrd, mxtype, ...
    nrow, ncol, nnzero, neltvl, ptrfmt, indfmt, valfmt, rhsfmt, rhstyp, ...
    nrhs, nrhsix ] = hb_header_read ( input_unit );
%
%  Read the matrix structure.
%
  [ colptr, rowind ] = hb_structure_read ( input_unit, ncol, mxtype, ...
    nnzero, neltvl, ptrcrd, ptrfmt, indcrd, indfmt );
%
%  Read the matrix values.
%
  values = hb_values_read ( input_unit, valcrd, mxtype, nnzero, neltvl, ...
    valfmt );
%
%  Read the right hand sides.
%
  [ rhsval, rhsptr, rhsind ] = hb_rhs_read ( input_unit, nrow, nnzero, ...
    nrhs, nrhsix, rhscrd, ptrfmt, indfmt, rhsfmt, mxtype, rhstyp );
%
%  Read the starting guesses.
%
  guess = hb_guess_read ( input_unit, nrow, nrhs, rhscrd, rhsfmt, rhstyp );
%
%  Read the exact solutions.
%
  exact = hb_exact_read ( input_unit, nrow, nrhs, rhscrd, rhsfmt, rhstyp );

  return
end


classdef AbstractFixedStepIntegrator < AbstractIntegrator
    %AbstractFixedStepIntegrator Summary of this class goes here
    %   Detailed explanation goes here

    properties(Constant, Access=private)
        %RK5
%         At = [ 1/5,          0,           0,            0,         0
%                3/40,         9/40,        0,            0,         0
%                44/45        -56/15,       32/9,         0,         0
%                19372/6561,  -25360/2187,  64448/6561,  -212/729,   0
%                9017/3168,   -355/33,      46732/5247,   49/176,   -5103/18656];
%
%         Bt = [35/384, 0, 500/1113, 125/192, -2187/6784, 11/84];
%
%         Ct = [1/5; 3/10; 4/5; 8/9; 1];

        %RK4
%         At = [1/2,  0,  0;
%                0, 1/2,  0;
%                0,   0,  1;]
%
%        Bt = [1/6, 1/3, 1/3, 1/6];
%
%        Ct = [1/2, 1/2, 1];

        %RK2
%         At = [1, 1];
%         Bt = [1/2, 1/2];
%
%         Ct = [1];

        %Ralston's 4th Order Method
%         At = [1/2,   0,   0;
%                 0, 1/2,   0;
%                 0,   0,   1];
%
%        Bt = [1/6, 1/3, 1/3, 1/6];
%
%        Ct = [1/2, 1/2, 1];
    end

    methods(Static)
        function [t,y, te,ye,ie] = integrate(odefun, tspan, y0, options)
            [h,y0,~] = AbstractFixedStepIntegrator.checkInputs(odefun, tspan, y0);

            if(all(h>0))
                propDirPos = true;
            elseif(all(h<0))
                propDirPos = false;
            else
                error('Prop direction might be consistently forward or backwards.');
            end

            if(not(isempty(options.OutputFcn)) && ...
               isa(options.OutputFcn,'function_handle'))
                hasOutput = true;
                outputFcn = options.OutputFcn;
            else
                hasOutput = false;
                outputFcn = [];
            end

            if(not(isempty(options.Events)) && ...
               isa(options.Events,'function_handle'))
                hasEvents = true;
                eventFcn = options.Events;
            else
                hasEvents = false;
                eventFcn = [];
            end

            neq = length(y0);
            N = length(tspan);
            t = NaN(N,1);
            Y = NaN(neq,N);

            Y(:,1) = y0;
            t(1) = tspan(1);

            if(hasOutput)
                outputFcn([tspan(1) tspan(end)],y0,'init');
            end

            ie = [];
            te = [];
            ye = [];
            if(hasEvents)
                [prevEvtValues,~,~] = eventFcn(t(1),Y(:,1)); %#ok<RHSFN>
            else
                prevEvtValues = [];
            end

            evtTerminate = false;
            outputTerminate = false;

            i = 2;
            while(keepLooping(propDirPos, i, tspan))
                ti = tspan(i-1);
                if(breakLoop(propDirPos, ti, tspan(end)))
                    break;
                end

                hi = h(i-1);
                yi = Y(:,i-1);

                t(i) = tspan(i);
                Y(:,i) = AbstractFixedStepIntegrator.stepOnce(odefun, ti, yi, hi, neq);

                if(hasEvents)
                    [evtValues,evtIsTerminal,evtDirection] = eventFcn(t(i),Y(:,i)); %#ok<RHSFN>

                    signChangesBool = (prevEvtValues .* evtValues < 0);
                    evtValZeroBool = evtValues == 0;

                    rootDetect = signChangesBool | evtValZeroBool;
                    if(any(rootDetect))
                        rootFcnFzero = @(subHi) AbstractFixedStepIntegrator.rootFindingFunc(subHi, odefun, ti, yi, neq, eventFcn, true);

                        if(all(rootDetect == evtValZeroBool)) %we nailed it on the money
                            tSubI = t(i);
                            exitflag = 1;

                        else %use root finding to actually find the event root
                            m = (evtValues - prevEvtValues) / (t(i) - t(i-1));
                            b = (evtValues - t(i) .* m);
                            t0 = -b./m;

                            t0(rootDetect == 0) = NaN;
                            [minT0, ~] = min(t0);

%                             [tSubI, ~, exitflag] = fzero(rootFcnFzero, minT0);

                            tSubI = SecantMethod(rootFcnFzero, t(i-1), minT0, 1e-6);

                            if(isnan(tSubI))
                                exitflag = -1;
                            else
                                exitflag = 1;
                            end

                            if(exitflag ~= 1 || tSubI < t(i-1) || tSubI > t(i))
                                rootFcnFminBnd = @(subHi) AbstractFixedStepIntegrator.rootFindingFunc(subHi, odefun, ti, yi, neq, eventFcn, false);
                                [tSubI,~,exitflag,~] = fminbnd(rootFcnFminBnd, t(i-1), t(i), optimset('TolX',1E-5));
                            end
                        end

                        if(exitflag == 1)
                            diffForDeriv = sign(evtValues - prevEvtValues);

                            [~, evtInd] = rootFcnFzero(tSubI);
                            if(evtDirection(evtInd) == 0 || ...
                               diffForDeriv(evtInd) == evtDirection(evtInd))

                                tspan1 = tspan(1:i-1);
                                tspan2 = tspan(i:end);
                                tspan = [tspan1, tSubI, tspan2];

                                h1 = h(1:i-1);
                                h2 = h(i:end);
                                hi = tSubI - ti;
                                h = [h1, hi, h2];

                                t(i) = tspan(i);
                                Y(:,i) = AbstractFixedStepIntegrator.stepOnce(odefun, ti, yi, hi, neq);

                                ie(end+1,1) = evtInd;
                                te(end+1,1) = tSubI;
                                ye(end+1,:) = Y(:,i)';

                                if(evtIsTerminal(evtInd) == 1)
                                    evtTerminate = true;
                                else
                                    evtTerminate = false;
                                end
                            else
                                evtTerminate = false;
                            end
                        end
                    end

                    prevEvtValues = evtValues;
                end

                if(hasOutput)
                    status = outputFcn(t(i),Y(:,i),[]);
                    if(status == 1)
                        outputTerminate = true;
                    else
                        outputTerminate = false;
                    end
                end

                if(evtTerminate || outputTerminate)
                    break;
                end

                i=i+1;
            end

            y = Y.';

            bool = ~isnan(t);
            t = t(bool);
            y = y(bool,:);

            if(hasOutput)
                outputFcn([],[],'done');
            end
        end

    end

    methods(Static, Access=protected)
        [A,B,C] = getButcherTableauData();

        function [h,y0,f0] = checkInputs(odefun, tspan, y0)
            if ~isnumeric(tspan)
                error('TSPAN should be a vector of integration steps.');
            end

            if ~isnumeric(y0)
                error('Y0 should be a vector of initial conditions.');
            end

            h = diff(tspan);
            if any(sign(h(1))*h <= 0)
                error('Entries of TSPAN are not in order.')
            end

            y0 = y0(:);   % Make a column vector.
            try
                f0 = odefun(tspan(1),y0);
            catch ME
                msg = ['Unable to evaluate the ODEFUN at t0,y0: ',ME.message];
                error(msg);
            end

            if ~isequal(size(y0),size(f0))
                error('Inconsistent sizes of Y0 and f(t0,y0).');
            end
        end

        function [yNp1, fEvals] = stepOnce(odefun, ti, yi, hi, neq)
            [A,B,C] = ODE5Integrator.getButcherTableauData(); %needs to be generalized

            nstages = length(B);
            F = NaN(neq,nstages);

            % General explicit Runge-Kutta framework
            F(:,1) = odefun(ti,yi);
            for stage = 2:nstages
                tstage = ti + C(stage-1)*hi;
                ystage = yi + F(:,1:stage-1)*(hi*A(1:stage-1,stage-1));
                F(:,stage) = odefun(tstage,ystage);
            end
            yNp1 = yi + F*(hi*B);
            fEvals = 1 + (nstages - 1);
        end
    end

    methods(Static, Access=private)
        function [rootOutput, I] = rootFindingFunc(subT, odefun, ti, yi, neq, eventFcn, forRootFinder)
            subHi = subT - ti;
            yNp1 = AbstractFixedStepIntegrator.stepOnce(odefun, ti, yi, subHi, neq);

            [evtValues,~,~] = eventFcn(subT,yNp1);

            rootOutput = evtValues;
            [~,I] = min(abs(rootOutput));

            if(forRootFinder)
                rootOutput = rootOutput(I); %for fzero and the like
            else
                rootOutput = abs(rootOutput(I)); %for an optimizer like fminbnd
            end
        end
    end
end

function tf = keepLooping(propDirPos, i, tspan)
    ti = tspan(i-1);
    if(propDirPos)
        tf = ti < tspan(end);
    else
        tf = ti > tspan(end);
    end
end

function tf = breakLoop(propDirPos, ti, tspanEnd)
    if(propDirPos)
        tf = ti >= tspanEnd;
    else
        tf = ti <= tspanEnd;
    end
end


function [policy, EU] = upot_to_opt_policy(pot)
% UPOT_TO_OPT_POLICY Compute an optimal deterministic policy given a utility potential
% [policy, EU] = upot_to_opt_policy(pot)
%
% policy(a,b, ..., z) = P(do z | a, b, ..), which will be a delta function
% EU is the contraction of this potential, i.e., P .* U

sz = pot.sizes; % mysize(pot.p);
if isempty(sz)
  EU = pot.u;
  policy = [];
  return;
end

parent_size = prod(sz(1:end-1));
self_size = sz(end);
C = pot.p .* pot.u; % contraction
C = reshape(C, parent_size, self_size);
policy = zeros(parent_size, self_size);
for i=1:parent_size
  act = argmax(C(i,:));
  policy(i, act) = 1;
end
policy = myreshape(policy, sz);
EU = sum(C(:));


function [fevd_estimates]=fevd(struct_irf_record,gamma_record,It,Bu,n,IRFperiods,FEVDband)

% function [fevd_record]=fevd(struct_irf_record,gamma_record,It,Bu,IRFperiods,n)
% runs the gibbs sampler to obtain draws from the posterior distribution of FEVD
% inputs:  - cell 'struct_irf_record': record of the gibbs sampler draws for the orthogonalised IRFs
%          - matrix 'gamma_record': record of the gibbs sampler draws for the structural disturbances variance-covariance matrix gamma
%          - integer 'It': total number of iterations of the Gibbs sampler (defined p 28 of technical guide)
%          - integer 'Bu': number of burn-in iterations of the Gibbs sampler (defined p 28 of technical guide)
%          - integer 'IRFperiods': number of periods for IRFs
%          - integer 'n': number of endogenous variables in the BVAR model (defined p 7 of technical guide)
% outputs: - cell 'fevd_record': record of the gibbs sampler draws for the FEVD

% this function implements algorithm 3.1.1
% preliminary tasks
% define the time horizon of FEVD as that of IRFs
FEVDperiods=IRFperiods;

% create the first cell
temp=cell(n,n+1);

% now prepare the evaluation of (3.1.13)
% start by filling the first column of every Tij matrix in the cell
% loop over rows of temp
for jj=1:n
   % loop over columns of temp
   for ii=1:n
   % square each IRF element
   temp{jj,ii}(:,1)=struct_irf_record{jj,ii}(:,1).^2;
   end
end

% fill all the other entries of the Tij matrices
% loop over rows of temp
for jj=1:n
   % loop over columns of temp
   for ii=1:n
      % loop over remaining columns
      for kk=2:FEVDperiods
      % define the column as the square of the corresponding column in orthogonalised_irf_record
      % additioned to the value of the preceeding columns, which creates the cumulation
      temp{jj,ii}(:,kk)=struct_irf_record{jj,ii}(:,kk).^2+temp{jj,ii}(:,kk-1);
      end
   end
end

% multiply each matrix in the cell by the variance of the structural shocks
% to do so, loop over simulations (rows of the Tij matrices)
for jj=1:It-Bu
% recover the covariance matrix of structural shocks gamma for this iteration
gamma=reshape(gamma_record(:,jj),n,n);
% loop over rows of temp
   for ii=1:n
   % loop over columns of temp
      for kk=1:n
      % multiply row jj of the matrix by the variance of the structural shock
      temp{ii,kk}(jj,:)=temp{ii,kk}(jj,:)*gamma(kk,kk);
      end
   end
% then go for next iteration
end

% obtain now the values for Ti, the (n+1)th matrix of each row
% loop over rows of temp
for jj=1:n
% start the summation over Tij matrices
temp{jj,n+1}=temp{jj,1};
   % sum over remaining columns
   for ii=2:n
   temp{jj,n+1}=temp{jj,n+1}+temp{jj,ii};
   end
end

% create the output cell fevd_record
fevd_record=cell(n,n);

% fill the cell
% loop over rows of fevd_record
for jj=1:n
   % loop over columns of fevd_record
   for ii=1:n
   % define the matrix Vfij as the division (pairwise entry) of Tfij by Tfj
   fevd_record{jj,ii}=temp{jj,ii}./temp{jj,n+1};
   end
end

%% create the FEVD estimates output
% create first the cell that will contain the estimates
fevd_estimates=cell(n,n);

% for each variable and each variable contribution along with each period, compute the median, lower and upper bound from the Gibbs sampler records
% consider variables in turn
for ii=1:n
   % consider contributions in turn
   for jj=1:n
      % consider periods in turn
      for kk=1:IRFperiods
      % compute first the lower bound
      fevd_estimates{ii,jj}(1,kk)=quantile(fevd_record{ii,jj}(:,kk),(1-FEVDband)/2);
      % then compute the median
      fevd_estimates{ii,jj}(2,kk)=quantile(fevd_record{ii,jj}(:,kk),0.5);
      % finally compute the upper bound
      fevd_estimates{ii,jj}(3,kk)=quantile(fevd_record{ii,jj}(:,kk),1-(1-FEVDband)/2);
      end
   end
end


function [fMc, fMc95, fMc90] = calc_McBest(mag, fBinning);
    %CALC_MCBEST Calculate best Mc from combining methods

    % First estimation of magnitude of completeness (maximum curvature)
    [vEvents, vMag] = hist(mag, -2:0.1:6);
    nSel = max(find(vEvents == max(vEvents)));
    fMcStart = vMag(nSel);

    % Data container
    mData = [];

    % Magnitude increment
    if ~exist('fBinning')
        fBinning = 0.1;
    end

    for nCnt = (fMcStart - 0.9):fBinning:(fMcStart + 1.5)
        vSel = mag > (nCnt - (fBinning/2));
        %vSel = mag >= nCnt - 0.0499;
        nNumberEvents = length(mag(vSel));
        if nNumberEvents >= 25;
            [fDummy fBValue fDummy fDummy] =  Catalog.bvalue_lib.bmemag(mag(vSel));

            fStartMag = nCnt; % Starting magnitude (hypothetical Mc)

            % log10(N)=A-B*M
            vMag = [fStartMag:fBinning:15]; % Ending magnitude must be sufficiently high
            vNumber = 10.^(log10(nNumberEvents)-fBValue*(vMag - fStartMag));
            vNumber = round(vNumber);

            % Find the last bin with an event
            nLastEventBin = min(find(vNumber == 0)) - 1;
            if isempty(nLastEventBin)
                nLastEventBin = length(vNumber);
            end

            %    ctM=vMag(ct1);

            % Determine set of all magnitude bins with number of events > 0
            ct = round((vMag(nLastEventBin)-fStartMag)*(1/fBinning) + 1);
            %     ct=0;
            %     for I=fStartMag:fBinning:ctM;
            %       ct=ct+1;
            %     end;

            PM=vMag(1:ct);
            vNumber = vNumber(1:ct);
            [bval, vDummy] = hist(mag(vSel),PM);
            b3 = fliplr(cumsum(fliplr(bval)));    % N for M >= (counted backwards)
            res2 = sum(abs(b3 - vNumber))/sum(b3)*100;
            mData = [mData; nCnt res2];
        else
            mData = [mData; nCnt NaN];
        end
    end

    % Evaluation of results

    % Is fMc90 available
    nSel = min(find(mData(:,2) < 10));
    if isempty(nSel)
        fMc90 = NaN
    else
        fMc90 = mData(nSel,1)
    end

    % Is fMc95 available
    nSel = min(find(mData(:,2) < 5));
    if isempty(nSel)
        fMc95 = NaN;
    else
        fMc95 = mData(nSel,1);
    end

    % ?????
    j =  min(find(mData(:,2) < 10 ));
    if isempty(j) == 1; j =  min(find(mData(:,2) < 15 )); end
    if isempty(j) == 1; j =  min(find(mData(:,2) < 20 )); end
    if isempty(j) == 1; j =  min(find(mData(:,2) < 25 )); end
    %j2 =  min(find(dat(:,2) == min(dat(:,2)) ));

    fMc = mData(j,1);
    if isempty(fMc)
        fMc = NaN;
    end
end