function [model, varargout] = gmlvq_core(trainSet, trainLab, varargin)
%GMLVQ_core.m - trains the Generalized Matrix LVQ algorithm
%NOTE: minimal requirement version 7.4.0.336 (R2007a)
%
%  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  %% Use the wrapper GMLVQ.M to access the functionality in style of %%
%  %% the SOM Toolbox (i.e. with data structs).                       %%
%  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  example for usage:
%  trainSet = [1,2,3;4,5,6;7,8,9];
%  trainLab = [1;1;2];
%  GMLVQ_model=GMLVQ_train(trainSet,trainLab); % minimal parameters required
%  estimatedTrainLabels = GMLVQ_classify(trainSet, GMLVQ_model);
%  trainError = mean( trainLab ~= estimatedTrainLabels );
%
% input:
%  trainSet : matrix with training samples in its rows
%  trainLab : vector with the labels of the training set
% optional parameters:
%  PrototypesPerClass: (default=1) the number of prototypes per class used. This could
%  be a number or a vector with the number for each class
%  initialPrototypes : (default=[]) a set of prototypes to start with. If not given initialization near the class means
%  initialMatrix     : the matrix omega to start with. If not given random
%  initialization for rectangular matrices and Unity for squared omega
%  dim               : (default=nb of features for training) the maximum rank or projection dimension
%  regularization    : (default=0) values usually between 0 and 1 treat with care.
%  Regularizes the eigenvalue spectrum of omega'*omega to be more homogeneous
%  testSet           : (default=[]) an optional test set used to compute
%  the test error. The last column is expected to be a label vector
%  comparable        : (default=0) a flag which resets the random generator
%  to produce comparable results if set to 1
%  optimization      : (default=fminlbfgs) indicates which optimization is used: sgd or fminlbfgs
% parameter for the stochastic gradient descent sgd
%  nb_epochs             : (default=100) the number of epochs for sgd
%  learningRatePrototypes: (default=[]) the learning rate for the prototypes.
%  Could be the start and end value used for a sigmoidal spectrum or a vector of length nb_epochs
%  learningRateMatrix    : (default=[]) the learning rate for the matrix.
%  Could be the start and end value used for a sigmoidal spectrum or a vector of length nb_epochs
%  MatrixStart           : (default=1) the epoch to start the matrix training
% parameter for the build-in function fminlbfgs
%  threshstop       : (default=0) the training error for early stopping
%  nb_reiterations  : (default=100) the number of optimization reiterations performed
%  useEarlyStopping : (default=1) use early stopping based on threshstop
%  Display          : (default=iter) the optimization output 'iter' or 'off'
%  GradObj          : (default=on) use the gradient information or not
%  HessUpdate       : (default=lbfgs) the update can be 'lbfgs', 'bfgs' or 'steepdesc'
%  TolFun           : (default=1e-6) the tolerance
%  MaxIter          : (default=2500) the maximal number of iterations
%  MaxFunEvals      : (default=1000000) the maximal number of function evaluations
%  TolX             : (default=1e-10) tolerance
%  DiffMinChange    : (default=1e-10) minimal change
%
% output: the GMLVQ model with prototypes w their labels c_w and the matrix omega
%  optional output:
%  initialization : a struct containing the settings
%  trainError     : error in the training set
%  testError      : error in the training set (only computed if 'testSet' is given)
%  costs          : the output of the cost function
%
% Citation information:
% Petra Schneider, Michael Biehl, Barbara Hammer:
% Adaptive Relevance Matrices in Learning Vector Quantization. Neural Computation 21(12): 3532-3561 (2009)
%
% K. Bunte, P. Schneider, B. Hammer, F.-M. Schleif, T. Villmann and M. Biehl,
% Limited Rank Matrix Learning - Discriminative Dimension Reduction and Visualization,
% Neural Networks, vol. 26, nb. 4, pp. 159-173, 2012.
%
% P. Schneider, K. Bunte, B. Hammer and M. Biehl, Regularization in Matrix Relevance Learning,
% IEEE Transactions on Neural Networks, vol. 21, nb. 5, pp. 831-840, 2010.
%
% Kerstin Bunte (modified based on the code of Marc Strickert http://www.mloss.org/software/view/323/ and Petra Schneider)
% uses the Fast Limited Memory Optimizer fminlbfgs.m written by Dirk-Jan Kroon available at the MATLAB central
% kerstin.bunte@googlemail.com
% Fri Nov 09 14:13:52 CEST 2012
%
% Conditions of GNU General Public License, version 2 and BSD License apply.
% See file 'license-gpl2.txt' and 'BSD_license.txt' enclosed in this package.
% Programs are not for use in critical applications!
%

% Contributed to SOM Toolbox vs2, December 3rd, 2012 by Alexander Schulz
% Copyright (c) Kerstin Bunte
% http://www.cis.hut.fi/projects/somtoolbox/

nout = max(nargout,1)-1;
p = inputParser;   % Create an instance of the class.
p.addRequired('trainSet', @isfloat);
p.addRequired('trainLab', @(x) length(x)==size(trainSet,1) & isnumeric(x));

p.addParamValue('PrototypesPerClass', ones(1,length(unique(trainLab))), @(x)(sum(~(x-floor(x)))/length(x)==1 && (length(x)==length(unique(trainLab)) || length(x)==1)));
p.addParamValue('initialPrototypes',[], @(x)(size(x,2)-1==size(trainSet,2) && isfloat(x)));
p.addParamValue('initialMatrix',[], @(x)(size(x,2)==size(trainSet,2) && isfloat(x)));
p.addParamValue('dim',size(trainSet,2), @(x)(~(x-floor(x)) && x<=size(trainSet,2) && x>0));
p.addParamValue('regularization',0, @(x)(isfloat(x) && x>=0));
p.addOptional('testSet', [], @(x)(size(x,2)-1)==size(trainSet,2) & isfloat(x));
p.addOptional('comparable', 0, @(x)(~(x-floor(x))));
p.addOptional('optimization', 'fminlbfgs', @(x)any(strcmpi(x,{'sgd','fminlbfgs'})));
% parameter for the stochastic gradient descent
p.addOptional('nb_epochs', 100, @(x)(~(x-floor(x))));
p.addParamValue('learningRatePrototypes', [], @(x)(isfloat(x) || isa(x,'function_handle'))); % && (length(x)==2 || length(x)==p.Results.epochs)
p.addParamValue('learningRateMatrix', [], @(x)(isfloat(x)  || isa(x,'function_handle')));
p.addOptional('MatrixStart', 1, @(x)(~(x-floor(x))));
% parameter for the build-in function
p.addOptional('threshstop',0,@(x) isfloat(x));
p.addOptional('nb_reiterations',100,@(x)(~(x-floor(x))));
p.addOptional('useEarlyStopping',1,@(x)(~(x-floor(x))));
p.addOptional('Display', 'off', @(x)any(strcmpi(x,{'iter','off'})));
p.addOptional('GradObj', 'on', @(x)any(strcmpi(x,{'on','off'})));
p.addOptional('HessUpdate', 'lbfgs', @(x)any(strcmpi(x,{'lbfgs', 'bfgs', 'steepdesc'})));
p.addOptional('TolFun',1e-6,@(x) isfloat(x));
p.addOptional('MaxIter', 2500, @(x)(~(x-floor(x))));
p.addOptional('MaxFunEvals', 1000000, @(x)(~(x-floor(x))));
p.addOptional('TolX',1e-10,@(x) isfloat(x));
p.addOptional('DiffMinChange',1e-10,@(x)isfloat(x));
p.CaseSensitive = true;
p.FunctionName = 'GMLVQ';
% Parse and validate all input arguments.
p.parse(trainSet, trainLab, varargin{:});

%%% check if results should be comparable
if p.Results.comparable,
    rng('default');
end
%%% set useful variables
nb_samples = size(trainSet,1);
nb_features = size(trainSet,2);
% labels should be a row vector
if size(trainLab,1)~=nb_samples, trainLab = trainLab';end

classes = unique(trainLab);
nb_classes = length(classes);
dim = p.Results.dim;
MatrixStart = p.Results.MatrixStart;
testSet = p.Results.testSet;
% global regularization;
regularization = p.Results.regularization;
if regularization, disp(['Regularize the eigenvalue spectrum of omega''*omega with ',num2str(regularization)]);end

initialization = rmfield(p.Results, 'trainSet');
initialization.trainSet = [num2str(nb_samples),'x',num2str(nb_features),' matrix'];
initialization = rmfield(initialization, 'trainLab');
initialization.trainLab = ['vector of length ',num2str(length(trainLab))];
if ~isempty(testSet)
    initialization = rmfield(initialization, 'testSet');
    initialization.testSet = [num2str(size(testSet,1)),'x',num2str(size(testSet,2)),' matrix'];
end
switch(p.Results.optimization)
case{'sgd'}
    initialization = rmfield(initialization, 'useEarlyStopping');
    initialization = rmfield(initialization, 'Display');
    initialization = rmfield(initialization, 'GradObj');
    initialization = rmfield(initialization, 'HessUpdate');
    initialization = rmfield(initialization, 'TolFun');
    initialization = rmfield(initialization, 'MaxIter');
    initialization = rmfield(initialization, 'MaxFunEvals');
    initialization = rmfield(initialization, 'TolX');
    initialization = rmfield(initialization, 'DiffMinChange');
    initialization = rmfield(initialization, 'nb_reiterations');
case{'fminlbfgs'}
    disp('The fminlbfgs optimization uses some global variables:');
    disp('threshstop earlystopped useEarlyStopping');
    initialization = rmfield(initialization, 'nb_epochs');
    initialization = rmfield(initialization, 'learningRatePrototypes');
    initialization = rmfield(initialization, 'learningRateMatrix');
    initialization = rmfield(initialization, 'MatrixStart');
    nb_reiterations = p.Results.nb_reiterations;
end
% Display all arguments.
%disp 'Settings for GMLVQ:'
%disp(initialization);

%%% check the number of prototypes per class if one integer is given and turn
%%% it into a vector
nb_ppc = p.Results.PrototypesPerClass;
if length(nb_ppc)~=nb_classes,
    nb_ppc = ones(1,nb_classes)*nb_ppc;
end

%%% initialize the prototypes
if isempty(p.Results.initialPrototypes)
    % initialize near the class centers
    w = zeros(sum(nb_ppc),nb_features);
    c_w = zeros(sum(nb_ppc),1);
    actPos = 1;
    for actClass=1:nb_classes
        nb_prot_c = nb_ppc(actClass);
        classMean = mean(trainSet(trainLab==classes(actClass),:));
        % set the prototypes to the class mean and add a random variation between -0.1 and 0.1
        w(actPos:actPos+nb_prot_c-1,:) = classMean(ones(nb_prot_c,1),:)+(rand(nb_prot_c,nb_features)*2-ones(nb_prot_c,nb_features))/10;
        c_w(actPos:actPos+nb_prot_c-1) = classes(actClass);
        actPos = actPos+nb_prot_c;
    end
else
    % initialize with given w
    w = p.Results.initialPrototypes(:,1:end-1);
    c_w = p.Results.initialPrototypes(:,end);
end
%%% initialize the matrix
if isempty(p.Results.initialMatrix)
    if(p.Results.dim==nb_features)
        omega = eye(nb_features);
    else % initialize with random numbers between -1 and 1
        omega = rand(dim,nb_features)*2-ones(dim,nb_features);
    end
else
    omega = p.Results.initialMatrix;
end
% normalize the matrix
omega = omega / sqrt(sum(diag(omega'*omega)));
model = struct('w',w,'c_w',c_w,'omega',omega);
clear w c_w omega;

% GMLVQ_algorithm = struct('update',@GMLVQ_update,'classify',@GMLVQ_classify,'costfun',@GLVQ_costfun);
switch(p.Results.optimization)
case{'sgd'}
    %%% gradient descent variables
    nb_epochs = p.Results.nb_epochs;
    % compute the vector of nb_epochs learning rates alpha for the prototype learning
    if isa(p.Results.learningRatePrototypes,'function_handle')
        % with a given function specified from the user
        alphas = arrayfun(p.Results.learningRatePrototypes, 1:nb_epochs);
    elseif length(p.Results.learningRatePrototypes)>2
        if length(p.Results.learningRatePrototypes)==nb_epochs
            alphas = p.Results.learningRatePrototypes;
        else
            disp('The learning rate vector for the prototypes does not fit the nb of epochs');
            return;
        end
    else
        % or use an decay with a start and a decay value
        if isempty(p.Results.learningRatePrototypes)
            initialization.learningRatePrototypes = [nb_features/100, nb_features/10000];
        end
        alpha_start = initialization.learningRatePrototypes(1);
        alpha_end = initialization.learningRatePrototypes(2);
        alphas = arrayfun(@(x) alpha_start * (alpha_end/alpha_start)^(x/nb_epochs), 1:nb_epochs);
    %     alphas = arrayfun(@(x) alpha_start / (1+(x-1)*alpha_end), 1:nb_epochs);
    end
    % compute the vector of nb_epochs learning rates epsilon for the Matrix learning
    epsilons = zeros(1,nb_epochs);
    if isa(p.Results.learningRateMatrix,'function_handle')
        % with a given function specified from the user
    % 	epsilons = arrayfun(p.Results.learningRateMatrix, 1:nb_epochs);
        epsilons(MatrixStart:nb_epochs) = arrayfun(p.Results.learningRateMatrix, MatrixStart:nb_epochs);
    elseif length(p.Results.learningRateMatrix)>2
        if length(p.Results.learningRateMatrix)==nb_epochs
            epsilons = p.Results.learningRateMatrix;
        else
            disp('The learning rate vector for the Matrix does not fit the nb of epochs');
            return;
        end
    else
        % or use an decay with a start and a decay value
        if isempty(p.Results.learningRateMatrix)
            initialization.learningRateMatrix = [nb_features/1000, nb_features/100000];
        end
        eps_start = initialization.learningRateMatrix(1);
        eps_end = initialization.learningRateMatrix(2);
    %     epsilons = arrayfun(@(x) eps_start * (eps_end/eps_start)^(x/nb_epochs), 1:nb_epochs);
        epsilons(MatrixStart:nb_epochs) = arrayfun(@(x) eps_start * (eps_end/eps_start)^((x-MatrixStart)/(nb_epochs-MatrixStart)), MatrixStart:nb_epochs);
    end

    %%% initialize requested outputs
    trainError = [];
    costs = [];
    testError = [];
    if nout>=2,
        % train error requested
        trainError = ones(1,nb_epochs+1);
        estimatedLabels = GMLVQ_classify(trainSet, model); % error after initialization
        trainError(1) = sum( trainLab ~= estimatedLabels )/nb_samples;
        if nout>=3,
            % test error requested
            if isempty(testSet)
                testError = [];
                disp('The test error is requested, but no labeled test set given. Omitting the computation.');
            else
                testError = ones(1,nb_epochs+1);
                estimatedLabels = GMLVQ_classify(testSet(:,1:end-1), model); % error after initialization
                testError(1) = sum( testSet(:,end) ~= estimatedLabels )/length(estimatedLabels);
            end
            if nout>=4,
                % costs requested
%                 LabelEqPrototype = trainLab*ones(1,numel(model.c_w)) == (model.c_w*ones(1,nb_samples))';
                disp('The computation of the costs is an expensive operation, do it only if you really need it!');
                costs = ones(1,nb_epochs+1);
                costs(1) = GMLVQ_costfun(trainSet, trainLab, model, regularization);
%                 costs(1) = sum(arrayfun(@(idx) GLVQ_costfun(min(dist(idx,model.c_w == trainLab(idx))),...
%                                                             min(dist(idx,model.c_w ~= trainLab(idx))))-regTerm, 1:size(dist,1)));
            end
        end
    end

    %%% optimize with stochastic gradient descent
    for epoch=1:nb_epochs
        if mod(epoch,100)==0, disp(epoch); end
        % generate order to sweep through the trainingset
        order = randperm(nb_samples);
        % perform one sweep through trainingset
        for i=1:nb_samples
            % select one training sample randomly
            xi = trainSet(order(i),:);
            c_xi = trainLab(order(i));

%             dist = ((xi(ones(size(model.w,1),1),:))-model.w)*model.omega'*(model.omega*((xi(ones(size(model.w,1),1),:))-model.w)');
%             dist = diag(dist);
            dist = sum((bsxfun(@minus, xi, model.w)*model.omega').^2, 2);
            % determine the two winning prototypes
            % nearest prototype with the same class
            [sortDist,sortIdx] = sort(dist);
            count = 1;
            J = sortIdx(count);
            while model.c_w(sortIdx(count)) ~= c_xi,
                count = count+1;
                J = sortIdx(count);
            end
            dJ = sortDist(count);
            count = 1;
            K = sortIdx(count);
            while model.c_w(sortIdx(count)) == c_xi,
                count = count+1;
                K = sortIdx(count);
            end
            dK = sortDist(count);
    %         disp([J,K,dJ,dK]);
            wJ = model.w(J,:);
            wK = model.w(K,:);
            % prototype update
            norm_factor = (dJ + dK)^2;
            DJ = (xi-wJ);
            DK = (xi-wK);

            oo = model.omega'*model.omega;

            dwJ = (2*dK/norm_factor)*2*oo*DJ';
            dwK = (2*dJ/norm_factor)*2*oo*DK';
    %         dwJ = (2*dK/norm_factor)*2*model.omega'*model.omega*DJ';
    %         dwK = (2*dJ/norm_factor)*2*model.omega'*model.omega*DK';
            model.w(J,:) = wJ + alphas(epoch) * dwJ';
            model.w(K,:) = wK - alphas(epoch) * dwK';
            % update matrices
            if epsilons(epoch)>0, % epoch >= MatrixStart
                f1 = (2*dK/norm_factor)*2*(model.omega*DJ')*DJ;
                f2 = (2*dJ/norm_factor)*2*(model.omega*DK')*DK;
                % update omega
                if regularization,
                    f3 = (pinv(model.omega))';
                else
                    f3 = 0;
                end
                model.omega = model.omega-epsilons(epoch) * (f1-f2  - regularization * f3);
                % normalization
                model.omega = model.omega / sqrt(sum(diag(oo)));
            end
        end
        if nout>=2,
            % train error requested
            estimatedLabels = GMLVQ_classify(trainSet, model); % error after epoch
            trainError(epoch+1) = sum( trainLab ~= estimatedLabels )/nb_samples;
            if nout>=3,
                % test error requested
                if ~isempty(testSet)
                    estimatedLabels = GMLVQ_classify(testSet(:,1:end-1), model); % error after initialization
                    testError(epoch+1) = sum( testSet(:,end) ~= estimatedLabels )/length(estimatedLabels);
                end
                if nout>=4,
                    % costs requested
                    costs(epoch+1) = GMLVQ_costfun(trainSet, trainLab, model, regularization);
%                     costs(epoch+1) = sum(arrayfun(@(idx) GLVQ_costfun(min(dist(idx,model.c_w == trainLab(idx))),...
%                                                                       min(dist(idx,model.c_w ~= trainLab(idx))))-regTerm, 1:size(dist,1)));
                end
            end
        end
    end
case{'fminlbfgs'}
    %%% optimization options
    options = struct( ...
      'Display',p.Results.Display, ...
      'GradObj',p.Results.GradObj, ...
      'GradConstr',false, ...
      'GoalsExactAchieve',0, ...
      'TolFun',p.Results.TolFun, ...
      'MaxIter',p.Results.MaxIter, ...
      'MaxFunEvals', p.Results.MaxFunEvals, ...
      'TolX',p.Results.TolX, ...
      'DiffMinChange',p.Results.DiffMinChange, ...
      'OutputFcn','LVQ_progresser', ...
      'HessUpdate',p.Results.HessUpdate ...
    );
    clear('progresser'); % memory therein might need reset
    global threshstop earlystopped useEarlyStopping % for LVQ_progresser.m datval labval n_vec
    useEarlyStopping = p.Results.useEarlyStopping; % use early stopping
    earlystopped = false;
    threshstop = p.Results.threshstop; % stop if classification below this threshold for early stopping
%     prototypeLabel = model.c_w;
    nb_prototypes = numel(model.c_w);
%     LabelEqualsPrototype = trainLab*ones(1,nb_prototypes) == (model.c_w*ones(1,nb_samples))';
    LabelEqualsPrototype = bsxfun(@eq,trainLab,model.c_w');
    earlystopped = false; % don't change, assigned in progresser.m
    clear('progresser'); % memory therein might need reset
    newfval = realmax('single');
    % fminlbfgs optimizer courtesy of Dirk-Jan Kroon:
    % http://www.mathworks.de/matlabcentral/fileexchange/23245
    % early stopping to be implemented in progresser function
    variables = zeros(dim+nb_prototypes,size(trainSet,2));
    variables(1:nb_prototypes,:) = model.w;
    variables(nb_prototypes+1:end,:) = model.omega;
    LRprototypes = 1; % learn prototype locations
    LRrelevances = 0; % don't learn metric
%     [variables,fval] = fminlbfgs(@GMLVQ_optfun,variables,options);
    [variables,fval] = fminlbfgs(@(variables) GMLVQ_optfun(variables,trainSet,LabelEqualsPrototype,LRrelevances,LRprototypes,model.c_w,regularization),variables,options);
    if not(isempty(fval))
        newfval = fval;
    end
    LRprototypes = 0; % don't learn prototype locations
    LRrelevances = 1; % learn metric
%     [variables,fval] = fminlbfgs(@GMLVQ_optfun,variables,options);
    [variables,fval] = fminlbfgs(@(variables) GMLVQ_optfun(variables,trainSet,LabelEqualsPrototype,LRrelevances,LRprototypes,model.c_w,regularization),variables,options);

    if not(isempty(fval))
        newfval = fval;
    end
    if not(isempty(fval))
      clear('progresser'); % memory therein might need reset
      LRprototypes = 1;
      for i = 1:nb_reiterations  % depending on data, re-iterations might further improve
%         [variables,fval] = fminlbfgs(@GMLVQ_optfun,variables,options);
        [variables,fval] = fminlbfgs(@(variables) GMLVQ_optfun(variables,trainSet,LabelEqualsPrototype,LRrelevances,LRprototypes,model.c_w,regularization),variables,options);
        if not(isempty(fval))
          if abs(fval - newfval) < 1e-3
            newfval = fval;
            break
          end
          newfval = fval;
        end
        if isempty(fval) || earlystopped
          break
        end
      end
    end
    model.w = variables(1:nb_prototypes,:);
    model.omega = variables(nb_prototypes+1:end,:);
    model.omega = model.omega / sqrt(sum(diag(model.omega'*model.omega)));
    if nout>=2,
        % train error requested
        estimatedLabels = GMLVQ_classify(trainSet, model); % error after initialization
        trainError = mean( trainLab ~= estimatedLabels );
        if nout>=3,
            % test error requested
            if isempty(testSet)
                testError = [];
                disp('The test error is requested, but no labeled test set given. Omitting the computation.');
            else
                estimatedLabels = GMLVQ_classify(testSet(:,1:end-1), model); % error after initialization
                testError = mean( testSet(:,end) ~= estimatedLabels );
            end
            if nout>=4,
                % costs requested
                costs = GMLVQ_costfun(trainSet, trainLab, model, regularization);
%                 dist = computeDistance(trainSet, model.w, model);
%                 if regularization,
%                     regTerm = regularization * log(det(model.omega*model.omega'));
%                 else
%                     regTerm = 0;
%                 end
%                 costs = sum(arrayfun(@(idx) GLVQ_costfun(min(dist(idx,model.c_w == trainLab(idx))),...
%                                                          min(dist(idx,model.c_w ~= trainLab(idx)))), 1:size(dist,1)))-regTerm;
            end
        end
    end
end
%%% output of the training
varargout = cell(nout);
for k=1:nout
	switch(k)
		case(1)
			varargout(k) = {initialization};
		case(2)
			varargout(k) = {trainError};
		case(3)
			varargout(k) = {testError};
		case(4)
            varargout(k) = {costs};
	end
end

function cost = GMLVQ_costfun(trainSet, trainLab, model, regularization)
%GMLVQ_costfun.m - computes the costs for a given training set and GMLVQ
%model with or without regularization
%  example for usage:
%  trainSet = [1,2,3;4,5,6;7,8,9];
%  trainLab = [1;1;2];
%  GMLVQ_model=GMLVQ_train(trainSet,trainLab); % minimal parameters required
%  costs = GMLVQ_costfun(trainSet, trainLab, GMLVQ_model, 0);
%
% input:
%  trainSet : matrix with training samples in its rows
%  trainLab : a vector of training labels
%  model    : GMLVQ model with prototypes w their labels c_w and the matrix omega
%  regularization: the factor>=0 for the regularization
%
% output    : cost function value
%
% Kerstin Bunte (based on the code from Marc Strickert)
% kerstin.bunte@googlemail.com
% Mon Nov 05 09:05:52 CEST 2012
%
% Conditions of GNU General Public License, version 2 apply.
% See file 'license-gpl2.txt' enclosed in this package.
% Programs are not for use in critical applications!
%
nb_samples = length(trainLab);
% labels should be a row vector
if size(trainLab,1)~=nb_samples, trainLab = trainLab';end

% LabelEqPrototype = trainLab*ones(1,numel(model.c_w)) == (model.c_w*ones(1,nb_samples))';
LabelEqPrototype = bsxfun(@eq,trainLab,model.c_w');
dists = computeDistance(trainSet, model.w, model);
if regularization,
    regTerm = regularization * log(det(model.omega*model.omega'));
% if strcmp(p.Results.optimization,'sgd')
else
    regTerm = 0;
end
Dwrong = dists;
Dwrong(LabelEqPrototype) = realmax(class(Dwrong));   % set correct labels impossible
distwrong = min(Dwrong.'); % closest wrong
clear Dwrong;

Dcorrect = dists;
Dcorrect(~LabelEqPrototype) = realmax(class(Dcorrect)); % set wrong labels impossible
distcorrect = min(Dcorrect.'); % closest correct
clear Dcorrect;
clear dists;
distcorrectpluswrong = distcorrect + distwrong;
distcorrectminuswrong = distcorrect - distwrong;
mu = distcorrectminuswrong ./ distcorrectpluswrong;
if regularization,
    regTerm = regularization * log(det(model.omega*model.omega'));
else
    regTerm = 0;
end
cost = sum(mu)-regTerm;

function [f G]  = GMLVQ_optfun(variables,training_data,LabelEqualsPrototype,LRrelevances,LRprototypes,prototypeLabel,regularization)
% [f G] = GMLVQ_optfun(variables)
% function to be optimzed by matrix relevance learning vector quantization
% variables = [prototype matrix;omega matrix]
% global variables are
%   training_data        : data vectors as row vectors, i.e. attributes in columns
%   LabelEqualsPrototype : binary matrix indicating coocurrences of
%   training labels and prototype labels
%   prototypeLabel       : label vector for the prototypes
%   regularization       : the regularization parameter
%   LRrelevances         : learning rate for the relevance matrix
%   LRprototypes         : learning rate for the prototypes
%
% Kerstin Bunte (modified based on the code of Marc Strickert http://www.mloss.org/software/view/323/)
% kerstin.bunte@googlemail.com
% Fri Nov 09 14:13:52 CEST 2012
%
% Conditions of GNU General Public License, version 2 apply.
% See file 'license-gpl2.txt' enclosed in this package.
% Programs are not for use in critical applications!
%
if isempty(LRprototypes) % values between 1e-2,1e-3,... 1e-8 seem pragmatic
    LRprototypes = 1; % no relevance learning by default
end
if isempty(LRrelevances) % values between 1e-2,1e-3,... 1e-8 seem pragmatic
    LRrelevances = 0; % no relevance learning by default
end
[n_data, n_dim] = size(training_data);
nb_prototypes =  numel(prototypeLabel);
omegaT = variables(nb_prototypes+1:end,:)';
n_vec = size(variables,1) - nb_prototypes;

dists = squaredEuclidean(training_data*omegaT, variables(1:nb_prototypes,:)*omegaT);

Dwrong = dists;
Dwrong(LabelEqualsPrototype) = realmax(class(Dwrong));   % set correct labels impossible
[distwrong pidxwrong] = min(Dwrong.'); % closest wrong
clear Dwrong;

Dcorrect = dists;
Dcorrect(~LabelEqualsPrototype) = realmax(class(Dcorrect)); % set wrong labels impossible
[distcorrect pidxcorrect] = min(Dcorrect.'); % closest correct
clear Dcorrect;

distcorrectpluswrong = distcorrect + distwrong;
distcorrectminuswrong = distcorrect - distwrong;
mu = distcorrectminuswrong ./ distcorrectpluswrong;
% callitq = 1./(1 + exp(-squashsigmoid * mu)); % apply sigmoidal

if regularization,
    regTerm = regularization * log(det(omegaT'*omegaT));
else
    regTerm = 0;
end
f = sum(mu)-regTerm;
% f = mean(callitq);

if nargout > 1  % gradient needed not just function eval
    G = zeros(size(variables)); % initially no gradient
    %       callitq = squashsigmoid * callitq .* (1-callitq); % derivative of sigmoid
    %       distcorrectpluswrong = 2 * callitq ./ distcorrectpluswrong.^2; % degeneration?
    distcorrectpluswrong = 4 ./ distcorrectpluswrong.^2; % norm_factor for derivative for every data sample
    if LRrelevances > 0
        Gw = zeros(n_vec,n_dim);
    end
    for k=1:nb_prototypes%(n_vec+1):size(lambda,1) % update all prototypes
        idxc = (k == pidxcorrect);  % Js: idxs where actual prototype is nearest correct
        idxw = (k == pidxwrong);    % Ks: idxs where actual prototype is nearest wrong

        dcd =  distcorrect(idxw) .* distcorrectpluswrong(idxw);
        dwd =    distwrong(idxc) .* distcorrectpluswrong(idxc);
        if LRrelevances > 0
            % part of derivative of distance
            difc = bsxfun(@minus,training_data(idxc,:),variables(k,:)); % DJs
            difw = bsxfun(@minus,training_data(idxw,:),variables(k,:)); % DKs
            % update omega
            Gw = Gw - (bsxfun(@times,difw,dcd.') * omegaT).' * difw + ...
                      (bsxfun(@times,difc,dwd.') * omegaT).' * difc;
            if LRprototypes > 0
                G(k,:) = dcd * difw - dwd * difc;
            end
        else
            if LRprototypes > 0
                G(k,:) = dcd * training_data(idxw,:) - dwd * training_data(idxc,:) + (sum(dwd)-sum(dcd)) * variables(k,:);
            end
        end
    end
if regularization,
    f3 = (pinv(omegaT'))';
else
    f3 = 0;
end
    % some rescalings needed
    if LRrelevances > 0
        G(nb_prototypes+1:nb_prototypes+n_vec,:) = 2/n_data * LRrelevances * Gw - regularization*f3;
    end
    if LRprototypes > 0
        G(1:nb_prototypes,:) = 1./n_data * LRprototypes * G(1:nb_prototypes,:) * omegaT * omegaT.';
    end
    G = G .* (1 + .0001 * (rand(size(G))-.5)); % help break symmetries
end
% if 0,
% w = variables(1:nb_prototypes,:);
% dJs = zeros(1,nb_samples);
% dKs = zeros(1,nb_samples);
% Js = zeros(1,nb_samples);
% Ks = zeros(1,nb_samples);
% norm_factors = zeros(1,nb_samples);
% DJs = zeros(nb_samples,size(training_data,2));
% DKs = zeros(nb_samples,size(training_data,2));
% for i=1:nb_samples
%     % select one training sample randomly
%     xi = training_data(i,:);
%     c_xi = trainLab(i);
%
%     dist = ((xi(ones(size(w,1),1),:))-w)*omegaT*(omegaT'*((xi(ones(size(w,1),1),:))-w)');
%     dist = diag(dist);
%     % determine the two winning prototypes
%     % nearest prototype with the same class
%     [sortDist,sortIdx] = sort(dist);
%     count = 1;
%     J = sortIdx(count);
%     while prototypeLabel(sortIdx(count)) ~= c_xi,
%         count = count+1;
%         J = sortIdx(count);
%     end
%     dJ = sortDist(count);
%     dJs(i) = dJ;
%     Js(i) = J;
%     count = 1;
%     K = sortIdx(count);
%     while prototypeLabel(sortIdx(count)) == c_xi,
%         count = count+1;
%         K = sortIdx(count);
%     end
%     dK = sortDist(count);
%     dKs(i) = dK;
%     Ks(i) = K;
%
%     wJ = w(J,:);
%     wK = w(K,:);
%     % prototype update
%     norm_factors(i) = 4/((dJ + dK)^2);
%     DJ = (xi-wJ);
%     DK = (xi-wK);
%     DJs(i,:) = DJ;
%     DKs(i,:) = DK;
% end
% end

function D = squaredEuclidean(A, B)
% computes the sqared Euclidean distance
%
%   D = squaredEuclidean(X) returns the squared Euclidean distance matrix of data in rows of X
%   D = squaredEuclidean(X, Y) returns the distance matrix with all distances between the points in X and Y.
%
if nargin == 1 % means that one matrix
    D = bsxfun(@plus, sumsquared(A,2), bsxfun(@minus, sumsquared(A,2).', 2*A*A.')); % 2*(Y*Y.')
else
    D = bsxfun(@plus, sumsquared(A,2), bsxfun(@minus, sumsquared(B,2).', 2*A*B.'));
end
D = max(D,0);

function sq  = sumsquared(x,dim)
% sq  = sumsquared(x,dim)
% sum of all squared element of matrix x along dimension dim

persistent isoctave

if isempty(isoctave)
  isoctave = exist('OCTAVE_VERSION','builtin');
end

if nargin == 1
  dim = 1;
end

if isoctave
  sq = sumsq(x,dim);
else
  sq = sum(x.^2, dim);
end


function [ChipsOut, Scrambler] = PacketBuilder(DataBits, G, Gs);
%
% PACKET BUILDER 	This function build the IS-95 forward Channel Data Packets.
% 						Block Diagram:
%						InData -> [Viterbi Encoder] -> [Interleaver] -> [Scrambler]
%
% 						9.6 KBps for 20 msec packet --> 192 bits (including 8 zeros tail) 184 data bits netto
% 						9.6*2 = 19.2 KBps (384 bits) after Viterbi incoding
% 						19.2 KBps * 64 = 1.2288 Mbps after Walsh and Spreading
%
% 						Inputs: DataBits - data to transmit (binary form)
%                         G - Viterbi Encoder generation polynom
%                         Gs - Long sequence generation polynom (scrambler)
%
% 						Outputs: ChipsOut - The result chip sequnece entered to modulator (binary form)
%                          Scrambler - Scrambler sequence%

global Zs

K = size(G, 2); 		% memory of Viterbi Polynoms
L = size(G, 1);      % number of chips per each data bit

N = 64*L*(length(DataBits)+K-1);		% Number of chips (9.6 Kbps -> 1.288 Mbps)

%====================== PACKET BUILDER =================
%-------- Create a Data Block (184 data bits) concatenated by 8 zeros
chips = VitEnc(G, [DataBits; zeros(K-1,1)]);	% Viterbi Encoder

%-------- Interleaver ---------
INTERL = reshape(chips, 24, 16);			% IN-> columns, OUT-> rows
chips = reshape(INTERL', length(chips), 1);  % Rate = 19.2 KBps

%----------- Scrambler---------
% Rate = 19.2 KBps
[LongSeq Zs] = PNGen(Gs, Zs, N);  % Long sequence Generation (at rate 1.2288 Mbps)
Scrambler = LongSeq(1:64:end); 	 % Decimation of Long Sequence

ChipsOut = xor(chips, Scrambler);


function b = isodd(x)
%ISODD  True for odd numbers.
%
%   ISODD(X) returns 1's where the elements of X are odd numbers and 0's where
%   they are not.
%
%   See also ISINT, ISEVEN.

%   Author:      Peter J. Acklam
%   Time-stamp:  2003-04-12 14:28:40 +0200
%   E-mail:      pjacklam@online.no
%   URL:         http://home.online.no/~pjacklam

   error(nargchk(1, 1, nargin));
   if ~isnumeric(x)
      error('Argument must be a numeric array.');
   end

   cls = class(x);                      % class of input argument
   if isempty(x)
      b = feval(cls, x);                % return empty array of same class
   else
      switch cls
         case 'double'
            b = mod(x, 2) == 1;
         case 'single'
            % "mod" is not defined for class "single"; so convert input to
            % double, compare, and convert back
            b = single(mod(double(x), 2) == 1);
         case {'uint8', 'uint16', 'uint32', 'uint64'}
            b = bitand(x, 1);
         case {'int8', 'int16', 'int32', 'int64'}
            error('Not implemented for classes int8, int16, int32, and int64.');
         otherwise
            error('Argument is of unrecognized class.');
      end
   end


function y = sum_square_abs( varargin )

%SUM_SQUARE   Sum of squares.
%   For vectors, SUM_SQUARE_ABS(X) is the sum of the squares of the
%   absolute values of the elements of the vector; i.e., SUM(ABS(X).^2).
%
%   For matrices, SUM_SQUARE_ABS(X) is a row vector containing the
%   application of SUM_SQUARE_ABS to each column. For N-D arrays, the
%   SUM_SQUARE_ABS operation is applied to the first non-singleton
%   dimension of X.
%
%   SUM_SQUARE(X,DIM) takes the sum along the dimension DIM of X.
%
%   Disciplined convex programming information:
%       If X is real, then SUM_SQUARE(X,...) is convex and nonmonotonic in
%       X. If X is complex, then SUM_SQUARE(X,...) is neither convex nor
%       concave. Thus, when used in CVX expressions, X must be affine. DIM
%       must be constant.

try
    varargin{end+1:2} = [];
    y = sum_square( varargin{:}, true );
catch exc
	cvx_throw( exc );
end

% Copyright 2005-2014 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function f = cos(f)
%COS   Cosine of a CHEBFUN3T object.
%
%   COS(F) returns the cosine of F.

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

% Check for empty:
if ( isempty(f) )
    return
end

op = @(x,y,z) cos(feval(f, x, y, z));  % Resample.
f = chebfun3t(op, f.domain);           % Call constructor.

end


im = imgRead(101085,'gray');
fb = fbCreate(6,1,1,3);
ntex = 32;
[tmap,tex] = computeTextons(fbRun(fb,im),ntex);
[tim,tperm] = visTextons(tex,fb);
wt = zeros(ntex,1);
for i = 1:ntex,
  wt(i) = sum(abs(tim{i}(:))); % L1 norm of texton
end
wt = wt / max(wt(:));
tsim = zeros(ntex);
for i = 1:ntex,
  for j = 1:ntex,
    tsim(i,j) = sum(sum(abs(tim{i}-tim{j})));
  end
end
r = 10;
norient = 6;
tic; [tg,theta] = tgmo(tmap,ntex,r,norient,tsim); toc;
aa = cell(size(tg));
bb = cell(size(tg));
cc = cell(size(tg));
for i = 1:numel(tg),
  tic; [c,b,a] = fitparab(tg{i},r,theta(i)); toc;
  aa{i}=a; bb{i}=b; cc{i}=c;
end
tgs = cell(size(tg));
pb = zeros(size(tmap));
for i = 1:numel(tgs),
  tgs{i} = max(0,cc{i}) .* (aa{i}<0) .* exp(-abs(bb{i})/0.1);
  pb = max(pb,tgs{i});
end
pb2 = zeros(size(tmap));
for i = 1:numel(tgs),
  pb2 = max(pb2,(tgs{i}==pb).*nonmax(tgs{i},theta(i)));
end

figure(1); clf;
imshow(im);

figure(2); clf;
imagesc(mymontage({tim{tperm}}));
axis image; colorbar;

figure(3); clf;
imagesc(tmap);
truesize;

figure(4); clf;
imagesc(pb);
truesize;

figure(5); clf;
imagesc(pb2)
truesize;


function mae = CalMAE(smap, gtImg)
% Code Author: Wangjiang Zhu
% Email: wangjiang88119@gmail.com
% Date: 3/24/2014
if size(smap, 1) ~= size(gtImg, 1) || size(smap, 2) ~= size(gtImg, 2)
    error('Saliency map and gt Image have different sizes!\n');
end

if ~islogical(gtImg)
    gtImg = gtImg(:,:,1) > 128;
end

smap = im2double(smap(:,:,1));
fgPixels = smap(gtImg);
fgErrSum = length(fgPixels) - sum(fgPixels);
bgErrSum = sum(smap(~gtImg));
mae = (fgErrSum + bgErrSum) / numel(gtImg);


classdef nnsigmoid < nntest
  methods (Test)
   function basic(test)
      x = test.randn(5,5,1,1)/test.range ;
      y = vl_nnsigmoid(x) ;
      dzdy = test.randn(size(y)) ;
      dzdx = vl_nnsigmoid(x,dzdy) ;
      test.der(@(x) vl_nnsigmoid(x), x, dzdy, dzdx, 1e-3) ;
    end
  end
end


%--- help for ts/pdecomp ---
%
%  Parametric decomposition into trend, seasonal and irregular components
%
%  ::
%
%    out=pdecomp(y)
%    out=pdecomp(y,doLog)
%    out=pdecomp(y,doLog,dorder)
%
%  Args:
%
%     y (ts): time series to decompose
%     doLog (true | {false}): if log, do a multiplicative decomposition
%       otherwise the decomposition is additive
%     dorder (integer | {2}): detrending order
%
%  Returns:
%     :
%
%     - **out** [struct] :
%
%       - **trend** [ts] : estimated trend
%       - **sc**    [ts] : estimated seasonal component
%       - **sa**    [ts] : seasonally adjusted data
%       - **ic**    [ts] : estimated irregular component
%
%  Note:
%
%     If there are many variables and the variables are named, the first level
%     of the structure will be the names of the different variables.
%
%  See also:
%     - :func:`npdecomp <ts.npdecomp>`
%
%


function pass = test_cdr( )
% Test diskfun cdr() command

tol = 1000*chebfunpref().cheb2Prefs.chebfun2eps;

% Check it works:
f = diskfun( @(x,y,z) exp(-((x-.4).^2+(y-.9).^2)));
[C, D, R] = cdr( f );
pass(1) = norm( chebfun2(@(t,r) feval(f,t,r, 'polar'),[-pi,pi,-1,1])...
- C * D * R' ) < tol;

end


function L=wfbtlength(Ls,wt,varargin);
%WFBTLENGTH  WFBT length from signal
%   Usage: L=wfbtlength(Ls,wt);
%
%   `wfbtlength(Ls,wt)` returns the length of a Wavelet system that is long
%   enough to expand a signal of length *Ls*. Please see the help on
%   |wfbt| for an explanation of the parameter *wt*.
%
%   If the returned length is longer than the signal length, the signal
%   will be zero-padded by |wfbt| to length *L*.
%
%   In addition, the function accepts flags defining boundary extension
%   technique as in |wfbt|. The returned length can be longer than the
%   signal length only in case of `'per'` (periodic extension).
%
%   See also: wfbt, fwt

% AUTHOR: Zdenek Prusa

complainif_notposint(Ls,'Ls','WFBTLENGTH');

definput.import = {'fwt'};
[flags,kv]=ltfatarghelper({},definput,varargin);

% Initialize the wavelet filters structure
if ~isstruct(wt)
   wt = wfbtinit(wt);
end

if(flags.do_per)
   a = treeSub(wt);
   L = filterbanklength(Ls,a);
else
   L = Ls;
end


function obj = Mrate_method(obj)
% traditional SVD algorithm for rate maximization

global  H Ns V_ropt W_ropt;
t1 = clock;
[U,~,V] = svd(H);
V_ropt = V(:,1:Ns);
%power constraint
V_ropt = V_ropt / norm(V_ropt,'fro');
W_ropt = U(:,1:Ns);

t2 = clock;
runtime = etime(t2,t1);
obj.runtime = obj.runtime + runtime;
obj.V_B = V_ropt;
obj.W_B = W_ropt;
obj = get_metric(obj);


function in = iresponse_proxy(in)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 'iresponse_proxy' computes the impulse response functions to shock
% identified via instrumental variables
% Codes are written based on the Mertens Ravn (2013, AER) source codes,
% modified to
%1) handle different length of VAR and factors
%2) Multiple instruments to explain the same shock

% Filippo Ferroni, 6/1/2015
% Revised, 2/15/2017
% Revised, 3/21/2018
% Revised, 9/11/2019
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Y         = in.vars(in.p+1:end,:);
[T,n]     = size(Y);
[T_m,n_m] = size(in.proxies);

% number of proxies
k  = 1;
%Assuming proxies start at least p periods later
instrument  = in.proxies(1:end,:);

% Identification
%%%%%%%%%%%%%%%%%

% covariance of the reduced form shocks
in.Sigma_m = in.Sigma;

% Instrument on VAR residuals
Phib = [ones(T_m,1) instrument]\in.res(T-T_m-in.T_m_end+1:T-in.T_m_end,:);

% Fitted values of the identified shocks
% here is where thes prior on beta should enter , instead of Phib(:,1)
% m = beta e1 + v
uhat1           =   [ones(T_m,1) instrument]*Phib(:,1);

% regress the fitted values on the other reduced form shocks
b21ib11_TSLS    =   [ones(T_m,1) uhat1]\in.res(T-T_m-in.T_m_end+1:T-in.T_m_end,2:end);
b21ib11_TSLS    =   b21ib11_TSLS(2:end,:)';
b21ib11         =   b21ib11_TSLS;

% Identification of b11 and b12 from the covariance matrix of the VAR
Sig11   = in.Sigma_m(1:k,1:k);
Sig21   = in.Sigma_m(k+1:n,1:k);
Sig22   = in.Sigma_m(k+1:n,k+1:n);
ZZp     = b21ib11*Sig11*b21ib11'-(Sig21*b21ib11'+b21ib11*Sig21')+Sig22;
b12b12p = (Sig21- b21ib11*Sig11)'*(ZZp\(Sig21- b21ib11*Sig11));
b11b11p = Sig11-b12b12p;
b11     = sqrt(b11b11p);
in.b1   = [b11; b21ib11*b11];
in.Phib = Phib;

% Impulse Responses
%%%%%%%%%%%%%%%%%%%%
% initial shock: eps(1,1)=1
irs(in.p+1,:) = in.b1(:,1);

for jj = 2:in.irhor%+max(max(VAR.term_spreads_matur),max(VAR.real_rates_init+VAR.real_rates_matur-1))
    lvars = (irs(in.p+jj-1:-1:jj,:))';
    irs(in.p+jj,:) = lvars(:)'*in.Phi(1:in.p * n,:);
end

in.irs   = irs(in.p+1:in.p+in.irhor,:);
in.uhat1 = uhat1;

if in.compute_F_stat == 1
    % F-test
    %%%%%%%%%%%%%%%%%%%%%%%%%
    XX_m      = [ones(T_m,1) instrument];
    Res_m     = in.res( T-T_m-in.T_m_end+1 : T-in.T_m_end , :)- XX_m * Phib;
    Res_const = in.res( T-T_m-in.T_m_end+1 : T-in.T_m_end,1) - ...
        ones(T_m,1)*(ones(T_m,1)\in.res(T-T_m-in.T_m_end+1 : T-in.T_m_end,1));

    SST_m      = Res_const'*Res_const;
    SSE_m      = Res_m(:,1)'*Res_m(:,1);
    in.F_m     = ((SST_m-SSE_m)/n_m)/ ...
        (SSE_m/(length(in.res(T-T_m-in.T_m_end+1 : T-in.T_m_end,1))-(n_m+1)));
    in.R2_m    = (1-SSE_m/(SST_m));
    in.R2adj_m = in.R2_m-...
        (n_m / ((length(in.res(T-T_m-in.T_m_end+1 : T-in.T_m_end,1)) -(n_m+1))))*(1-in.R2_m);

    % Calculate robust standard errors
    SS_m = zeros(n_m+1,n_m+1);
    for ii = 1 : T_m
        SS_m = SS_m+1/T_m * XX_m(ii,:)'*XX_m(ii,:)*Res_m(ii,1)^2;
    end
    Avarb_m    = inv(1/T_m * XX_m'*XX_m)*SS_m *inv(1/T_m * XX_m'*XX_m);
    RR_m       = [zeros(n_m,1) eye(n_m)];
    WW_m       = T_m*(RR_m*Phib(:,1))' * inv(RR_m*Avarb_m*RR_m') * (RR_m*Phib(:,1));
    in.F_m_rob = WW_m/n_m;
end


classdef CEC2010_F10 < PROBLEM
% <single> <real> <constrained>
% CEC'2010 constrained optimization benchmark problem

%------------------------------- Reference --------------------------------
% R. Mallipeddi and P. N. Suganthan, Problem definitions and evaluation
% criteria for the CEC 2010 competition on constrained real-parameter
% optimization, Nanyang Technological University, Singapore, 2010.
%------------------------------- 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),'CEC2010.mat'),'Data');
            obj.O = Data{10}.O;
            obj.M = 1;
            if isempty(obj.D) || obj.D < 30
                obj.D   = 10;
                obj.Mat = Data{10}.M_10;
            else
                obj.D   = 30;
                obj.Mat = Data{10}.M_30;
            end
            if isempty(obj.D); obj.D = 10; end
            obj.D = min(obj.D,length(obj.O));
            obj.lower    = zeros(1,obj.D) - 500;
            obj.upper    = zeros(1,obj.D) + 500;
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            Z = 1 + PopDec - repmat(obj.O(1:size(PopDec,2)),size(PopDec,1),1);
            PopObj = sum(100*(Z(:,1:end-1).^2-Z(:,2:end)).^2+(Z(:,1:end-1)-1).^2,2);
        end
        %% Calculate constraint violations
        function PopCon = CalCon(obj,PopDec)
            Z = PopDec - repmat(obj.O(1:size(PopDec,2)),size(PopDec,1),1);
            Y = Z*obj.Mat;
            PopCon = abs(sum((Y.*sin(sqrt(abs(Y)))),2)) - 1e-4;
        end
    end
end


function [resCe, rhsCe] = electrolyteDiffusion(ce,dCe,jflux,T,param)
% electrolyteDiffusion evaluates the residual for the electrolyte
% concentration of Li-ions in the electrolyte solution.

%   This file is part of the LIONSIMBA Toolbox
%
%	Official web-site: 	http://sisdin.unipv.it/labsisdin/lionsimba.php
% 	Official GitHUB: 	https://github.com/lionsimbatoolbox/LIONSIMBA
%
%   LIONSIMBA: A Matlab framework based on a finite volume model suitable for Li-ion battery design, simulation, and control
%   Copyright (C) 2016-2018 :Marcello Torchio, Lalo Magni, Davide Raimondo,
%                            University of Pavia, 27100, Pavia, Italy
%                            Bhushan Gopaluni, Univ. of British Columbia,
%                            Vancouver, BC V6T 1Z3, Canada
%                            Richard D. Braatz,
%                            Massachusetts Institute of Technology,
%                            Cambridge, Massachusetts 02142, USA
%
%   Main code contributors to LIONSIMBA 2.0:
%                           Ian Campbell, Krishnakumar Gopalakrishnan,
%                           Imperial college London, London, UK
%
%   LIONSIMBA is a free Matlab-based software distributed with an MIT
%   license.

% Diffusion coefficients
% Comment this for benchmark purposes
Deff_p = param.ElectrolyteDiffusionFunction(ce(1:param.Np),T(param.Nal+1:param.Nal+param.Np),param,'p');
Deff_s = param.ElectrolyteDiffusionFunction(ce(param.Np+1:param.Np+param.Ns),T(param.Nal+param.Np+1:param.Nal+param.Np+param.Ns),param,'s');
Deff_n = param.ElectrolyteDiffusionFunction(ce(param.Np+param.Ns+1:end),T(param.Nal+param.Np+param.Ns+1:end-param.Ncu),param,'n');

% Uncomment this for benchmark purposes
% Deff_p = repmat(param.Dp*param.eps_p^param.brugg_p,param.Np,1);
% Deff_s = repmat(param.Ds*param.eps_s^param.brugg_s,param.Ns,1);
% Deff_n = repmat(param.Dn*param.eps_n^param.brugg_n,param.Nn,1);

% Interpolation of the diffusion coefficients
[Deff_p, Deff_s, Deff_n] = interpolateDiffusionCoefficients(Deff_p,Deff_s,Deff_n,param);

%% Positive A Matrix
A_p = - diag(Deff_p);

A_p(2:end,2:end)    = A_p(2:end,2:end) - diag(Deff_p(1:end-1));
A_p(1:end-1,2:end)  = A_p(1:end-1,2:end) + diag(Deff_p(1:end-1));
A_p(2:end,1:end-1)  = A_p(2:end,1:end-1) + diag(Deff_p(1:end-1));
%% Separator A Matrix
A_s = - diag(Deff_s);

A_s(2:end,2:end)    = A_s(2:end,2:end) - diag(Deff_s(1:end-1));
A_s(1:end-1,2:end)  = A_s(1:end-1,2:end) + diag(Deff_s(1:end-1));
A_s(2:end,1:end-1)  = A_s(2:end,1:end-1) + diag(Deff_s(1:end-1));
%% Negative A Matrix
A_n = - diag(Deff_n);

A_n(2:end,2:end)    = A_n(2:end,2:end) - diag(Deff_n(1:end-1));
A_n(1:end-1,2:end)  = A_n(1:end-1,2:end) + diag(Deff_n(1:end-1));
A_n(2:end,1:end-1)  = A_n(2:end,1:end-1) + diag(Deff_n(1:end-1));
% Fix the last elements of the A_n
A_n(end,end-1:end)  = [Deff_n(end-1) -Deff_n(end-1)];
%% A_tot matrix
A_tot = blockDiagonalMatrix(param,A_p,A_s,A_n);

% Divide by the deltax and the length of the positive electrode
A_tot(1:param.Np,1:param.Np)    = A_tot(1:param.Np,1:param.Np)/(param.deltax_p^2*param.len_p^2);
% Reset values on the lines for the interfaces conditions
A_tot(param.Np,:)           = 0;
A_tot(param.Np+1,:)         = 0;

% Divide by the deltax and the length of the separator
A_tot(param.Np+1:param.Np+param.Ns,param.Np+1:param.Np+param.Ns)    = A_tot(param.Np+1:param.Np+param.Ns,param.Np+1:param.Np+param.Ns)/(param.deltax_s^2 * param.len_s^2);
% Reset values on the lines for the interfaces conditions
A_tot(param.Np+param.Ns,:)      = 0;
A_tot(param.Np+param.Ns+1,:)    = 0;

% Divide by the deltax and the length of the negative electrode
A_tot(param.Np+param.Ns+1:end,param.Np+param.Ns+1:end)      = A_tot(param.Np+param.Ns+1:end,param.Np+param.Ns+1:end)/(param.deltax_n^2 * param.len_n^2);

%% Interface between separator and positive electrode (last volume in the positive electrode)

% Compute the common denominator at the interface
den_s   = (param.deltax_p*param.len_p/2 + param.deltax_s*param.len_s/2);
% Last diffusion coefficient of the positive electrode
last_p  = Deff_p(end-1)/(param.deltax_p*param.len_p);
% Diffusion coefficient on the interface
first_s = Deff_p(end)/den_s;
% Fix the values at the boundaries
A_tot(param.Np,param.Np-1:param.Np+1) = [last_p -(last_p+ first_s) first_s]/(param.deltax_p*param.len_p*param.eps_p);

%% Interface between separator and positive electrode (first volume in the separator)

% Compute the common denominator at the interface
den_s       = (param.deltax_p*param.len_p/2 + param.deltax_s*param.len_s/2);
% First diffusion coefficient in the separator
second_s    = Deff_s(1)/(param.deltax_s*param.len_s);
% Diffusion coefficient on the interface
first_s     = Deff_p(end)/den_s;

A_tot(param.Np+1,param.Np:param.Np+2) = [first_s -(first_s+second_s) second_s]/(param.deltax_s*param.len_s*param.eps_s);

%% Interface between separator and negative electrode (last volume in the separator)

% Compute the common denominator at the interface
den_s   = (param.deltax_s*param.len_s/2 + param.deltax_n*param.len_n/2);
% Last diffusion coefficient in the separator
last_s  = Deff_s(end-1)/(param.deltax_s*param.len_s);
% Diffusion coefficient on the interface
first_n = Deff_s(end)/den_s;

A_tot(param.Np+param.Ns,param.Np+param.Ns-1:param.Np+param.Ns+1) = [last_s -(last_s+first_n) first_n]/(param.deltax_s*param.len_s*param.eps_s);

%% Interface between separator and negative electrode (first volume in the negative electrode)

% Compute the common denominator at the interface
den_n       = (param.deltax_s*param.len_s/2 + param.deltax_n*param.len_n/2);
% First diffusion coefficient in the negative electrode
second_n    = Deff_n(1)/(param.deltax_n*param.len_n);
% Diffusion coefficient on the interface
first_n     = Deff_s(end)/den_n;

A_tot(param.Np+param.Ns+1,param.Np+param.Ns:param.Np+param.Ns+2) = [first_n -(first_n+second_n) second_n]/(param.deltax_n*param.len_n*param.eps_n);

%% Useful stuff
a_tot       = [
    repmat(param.a_i(1),param.Np,1);...
    zeros(param.Ns,1);...
    repmat(param.a_i(3),param.Nn,1)...
    ];

jflux_tot   = [
    jflux(1:param.Np);...
    zeros(param.Ns,1);...
    jflux(param.Np+1:end)...
    ];

eps_tot     = [
    repmat(param.eps_p,param.Np,1);...
    repmat(param.eps_s,param.Ns,1);...
    repmat(param.eps_n,param.Nn,1)
    ];

K = 1./eps_tot;
A_eps = diag(K);

% Build porosities matrix
if(~isa(eps_tot,'casadi.MX') && ~isa(eps_tot,'casadi.SX'))
    A_eps = A_eps + diag(K(1:end-1),1);
    A_eps = A_eps + diag(K(1:end-1),-1);
    A_eps = sparse(A_eps);
else
    A_u_l                   = diag(K(1:end-1));
    A_eps(1:end-1,2:end)    = A_eps(1:end-1,2:end)+A_u_l;
    A_eps(2:end,1:end-1)    = A_eps(2:end,1:end-1)+A_u_l;
end

A_eps(param.Np,param.Np-1:param.Np+1) = 1;
A_eps(param.Np+1,param.Np:param.Np+2) = 1;

A_eps(param.Np+param.Ns,param.Np+param.Ns-1:param.Np+param.Ns+1) = 1;
A_eps(param.Np+param.Ns+1,param.Np+param.Ns:param.Np+param.Ns+2) = 1;

G = A_eps.*A_tot;

% Write the RHS of the equation
rhsCe = (G*ce + K.*(1-param.tplus).*a_tot.*jflux_tot);

% Write the residual of the equation
resCe = dCe - rhsCe;
end


function fiber_curvature_vals=dtiFiberCurvature(fiber)

%For 3xn fiber computes curvature at each point as defined here
%http://en.wikipedia.org/wiki/Curvature (I know it is a shame to use Wiki
%for research -- but oh well
%
%ER 12/2007
xprime=gradient(fiber(1, :));
yprime=gradient(fiber(2, :));
zprime=gradient(fiber(3, :));

fiber_curvature_vals=sqrt((gradient(zprime).*yprime-gradient(yprime).*zprime).^2+(gradient(xprime).*zprime-gradient(zprime).*xprime).^2+(gradient(yprime).*xprime-gradient(xprime).*yprime).^2)./((xprime.^2 + yprime.^2 + zprime.^2).^(3/2));
%fiber_curvature_vals=fiber_curvature_vals./max(fiber_curvature_vals(:));

% These values are sometimes huge. what is the distribution? The original
% data are not unitless.


function [H, WtW, WtV, infos] = nnls_solver(V, W, in_options)
%
% This file is part of NMFLibrary.
%
% Created by H.Kasai on June 20, 2022
%
% Change log:
%

    % set dimensions and samples
    [m, n] = size(V);

    % set local options
    local_options = [];
    local_options.alg_name = 'no_name';
    local_options.algo = 'hals';
    local_options.delta = 1e-6;

    % merge options
    options = mergeOptions(get_nmf_default_options(), local_options);
    options = mergeOptions(options, in_options);

    % initialize
    W = full(W);
    WtW = W' * W;
    WtV = W' * V;
    infos = [];

    % If no initial matrices are provided, H is initialized as follows:
    if ~isfield(options,'init') || isempty(options.init)
        H = nnls_init_nmflibrary(V, W, WtW, WtV);
    else
        H = options.init;
    end

    % switch solvers

    switch options.algo
        case 'hals'

            % perform main routine
            options.V = V;
            options.W = W;
            options.main_routine_mode = true;
            %options.max_epoch = 500;
            options.eps0 = 0;
            options.eps = 1;
            eit1 = cputime;
            alpha = 2;

            % call HALSupdt
            [H, infos] = HALSupdt(H, WtW, WtV, eit1, alpha, options.delta, options);

            if options.verbose > 2
                fprintf('nnls_solver [%d]: H = %.16e, WtW = %.16e, WtV = %.16e\n', length(infos.cost), norm(H), norm(WtW), norm(WtV));
            end

        case 'fpgm'

            options_fpgm.init = H;
            options_fpgm.inner_max_epoch = options.inner_max_epoch;

            % call HALSupdt
            [H, WtW, WtV] = nnls_fpgm(V, W, options_fpgm);

        case 'anls_bpp'

            % call HALSupdt
            H = nnlsm_blockpivot(WtW, WtV, 1, H);

        case 'anls_asgroup'

            % call HALSupdt
            [H, ~, ~] = nnlsm_activeset(WtW, WtV, 0, 1, H);

        otherwise
    end

end


function [] = profiler(data,lonlat,point1,point2,bins,R,units)

% By David Bekaert - December 2010

% give as input the data, the lonlat (2 colums), the profile points [lon lat], the number of
% bins and the radius [m] of the tube used to project points
% by default a 15 bins and a radius of 100m is used.
% for debug figures set debugflag to 1

debugflag=0;

if nargin<7
    units = 'rad';
end
if nargin<6
    R = 100;            % default value of R in m
end
if nargin<5
    bins = 15;          % default number of bins
end
if nargin<4
    error('Specify more input arguments \n');
end
fontsize = 15;          % label fontsize

%%% PLOTTING
profile_fig = figure('name','Cross-sectional plot');
axes('OuterPosition',[0  0  0.7  1])
scatter(lonlat(:,1),lonlat(:,2),3,data,'filled')
hold on
plot([point1(1,1) point2(1,1)]',[point1(1,2) point2(1,2)]','k-*','LineWidth',1.5)
cc =colorbar;
xlabel(cc,units,'fontsize',fontsize)
set(gca,'fontsize',fontsize)
xlabel('Longitude','fontsize',fontsize)
ylabel('Latitude','fontsize',fontsize)
%%% PLOTTING

% crossection computation
% transform to a local reference frame, unit becomes [km]
lonlatXY = llh2local(lonlat',point1);	% transformation of the PS pixels
P1 = llh2local(point1',point1);                 % transformation of the line points
P2 = llh2local(point2',point1);

% rotate the local from such that line becomes horizontal
alpha = atan((P2(2,1)-P1(2,1))/(P2(1,1)-P1(1,1)));		% angle [rad]
rot = [cos(alpha)    -sin(alpha)
       sin(alpha)    cos(alpha)];
XY_new = rot\lonlatXY;                  % X is the first row, Y the second row
P1_new = rot\P1;
P2_new = rot\P2;
clear rot alpha  P1 P2 lonlatXY

if debugflag==1
    figure('name','Debug_plot [1]');
    scatter(XY_new(1,:),XY_new(2,:),3,data,'filled')
    hold on
    scatter([P1_new(1) P2_new(1)],[P1_new(2) P2_new(2)],'ro')
end

% Search for all the PS within distance R
ix_data = find((abs(XY_new(2,:))<=R/1000));

% updating vectors
XY_new = XY_new(:,ix_data);
data_new = data(ix_data);

if debugflag==1
    figure('name','Debug_plot [2]');
    scatter(XY_new(1,:),XY_new(2,:),3,data_new,'filled')
    hold on
    scatter([P1_new(1) P2_new(1)],[P1_new(2) P2_new(2)],'ro')
end

% output to the screen
PS_used = size(ix_data,2);
if (isempty(PS_used)==1 || PS_used==0)
    error('No PS are found within the tube crossection. \n')
else
    fprintf([num2str(PS_used),' PS used to compute projection on crossection \n'])
end

% threshold for minimum PS inside a bin
ps_min = floor(0.5*(PS_used/bins));

% binning of the results
binsize = (max(XY_new(1,:))-min(XY_new(1,:)))/bins;
data_cross_binned = zeros([1 bins]);
bins_xy = zeros([1 bins]);
clear ix

for k=1:bins
    bl = (k-1)*binsize+min(XY_new(1,:));                        % lower bound
    bu = (k)*binsize+min(XY_new(1,:));                          % upper bound
    ix = find(bl<=XY_new(1,:)  & XY_new(1,:)<bu);
    % Only showing bins with a minimum of PS contained
    if size(ix,2)>=ps_min
        bins_xy(1,k) = bl+binsize/2;                            % position center of bin
        data_binned_mean(1,k) = mean(data_new(ix,1));     % take mean value

    else
        bins_xy(1,k) = NaN;
        data_binned_mean(1,k) = NaN;

    end
    clear ix bl bu
end
ix = find(isnan(bins_xy)==1);
if isempty(ix)==0
    bins_xy(ix)=[];
    data_binned_mean(ix)=[];
end

%%% PLOTTING
figure(profile_fig)
axes('OuterPosition',[0.65  0  0.3  1])
plot(data_new,XY_new(1,:),'.','color',[0.9 0.9 0.9])
ylabel('Distance along crossection [km]','fontsize',fontsize)
hold on
plot(data_binned_mean,bins_xy,'k.')
xlabel(units,'fontsize',fontsize)
set(gca,'fontsize',fontsize)
%%% PLOTTING

clear all


function [ a, b ] = p13_lim ( dim_num )

%*****************************************************************************80
%
%% P13_LIM returns the integration limits for problem 13.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 March 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the dimension of the argument.
%
%    Output, real A(DIM_NUM), B(DIM_NUM), the lower and upper
%    limits of integration.
%
  a(1:dim_num) = 0.0;
  b(1:dim_num) = 1.0;

  return
end


function test_failed=test_filterbankscale()
test_failed = 0;

disp('----------FILTERBANKSCALE-----------');

 gr{1}=randn(20,1);
 gr{2}=randn(21,1);
 gr{3}=firfilter('hanning',19);
 gr{4}=firfilter('hanning',20);
 gr{5}=randn(4,1);
 gr{6}=firfilter('hanning',20,'causal');
 gr{7}=firfilter('hanning',20,'delay',13);
 gr{8}=firfilter('hanning',20,'delay',-13);
 gr{7}=firfilter('hanning',20,'delay',14);
 gr{8}=firfilter('hanning',20,'delay',-14);
 gr{9}=firfilter('hamming',19,.3);
 gr{10}=firfilter('hamming',19,.3,'real');
 gr{11}=blfilter('hanning',.19);
 gr{12}=blfilter('hanning',.2);
 gr{13}=blfilter('hanning',.132304);
 gr{14}=blfilter('hanning',.23,'delay',13);
 gr{15}=blfilter('hamming',.23,.3);
 gr{16}=blfilter('hamming',.23,.3,'real');
 % (almost) Allpass filter
 gr{17}=blfilter('hanning',2);
 gr{18}=blfilter('hanning',1);
 gr{19}=blfilter('hanning',2,1);
 gr{20}=blfilter('hanning',1,-1);
 gr{21}=blfilter('hamming',.1,-.3);
 gr{22}=blfilter('hanning',1.7);
 gr{23}=struct('H',randn(10,1),'L',100);
 gr{24}=crand(40,1);
 gr{25}=struct('h',crand(40,1));
 gr{26}=struct('h',crand(40,1),'realonly',1);

 Larr = [100, 211];
 norms = {'1','2','inf'};

 for L = Larr
     gr{23}=struct('H',randn(10,1),'L',L);
     for ii = 0:1
         if ii==0, freqstr = ''; freqflag = 'nofreq';
         else freqstr = 'FREQ'; freqflag = 'freq'; end

         for nId = 1:numel(norms)
             g = filterbankscale(gr,L,norms{nId},freqflag);
             if ii == 0
                 [~,n] = setnorm(ifft(filterbankfreqz(g,1,L)),norms{nId});
             else
                [~,n] = setnorm(filterbankfreqz(g,1,L),norms{nId});
             end

             res = sum(abs(n-1));
             [test_failed,fail]=ltfatdiditfail(res,test_failed);
             fprintf('NORM: %s %s L=%d %s\n',upper(norms{nId}),freqstr,L,fail);
         end
     end
 end

 scal = 0.1;
  for L = Larr
     gr{23}=struct('H',randn(10,1),'L',L);

         for nId = 1:numel(norms)
             grfreqz = filterbankfreqz(gr,1,L);
             g = filterbankscale(gr,scal);

             gfreqz = filterbankfreqz(g,1,L);

             res = sum(sum(scal*grfreqz-gfreqz));
             [test_failed,fail]=ltfatdiditfail(res,test_failed);
             fprintf('SCALE %f L=%d %s\n',scal,L,fail);

         end
 end


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

function rep=DeleteFromRep(rep,EXTRA,gamma)

    if nargin<3
        gamma=1;
    end

    for k=1:EXTRA
        [occ_cell_index occ_cell_member_count]=GetOccupiedCells(rep);

        p=occ_cell_member_count.^gamma;
        p=p/sum(p);

        selected_cell_index=occ_cell_index(RouletteWheelSelection(p));

        GridIndices=[rep.GridIndex];

        selected_cell_members=find(GridIndices==selected_cell_index);

        n=numel(selected_cell_members);

        selected_memebr_index=randi([1 n]);

        j=selected_cell_members(selected_memebr_index);

        rep=[rep(1:j-1); rep(j+1:end)];
    end

end


function [Di_new,delet] = FDDL_UpdateDi (A,X,index,classn,Fish_ipts,Fish_par)
% ========================================================================
% Dictionary updating of FDDL, Version 1.0
% Copyright(c) 2011  Meng YANG, Lei Zhang, Xiangchu Feng and David Zhang
% All Rights Reserved.
%
% -----------------------------------------------------------------------
% Permission to use, copy, or modify this software and its documentation
% for educational and research purposes only and without fee is here
% granted, provided that this copyright notice and the original authors'
% names appear on all copies and supporting documentation. This program
% shall not be used, rewritten, or adapted as the basis of a commercial
% software or hardware product without first obtaining permission of the
% authors. The authors make no representations about the suitability of
% this software for any purpose. It is provided "as is" without express
% or implied warranty.
%----------------------------------------------------------------------
%
% This is an implementation of the algorithm for updating the
% dictionary of FDDL (fix the coefficient)
%
% Please refer to the following paper
%
% Meng Yang, Lei Zhang, Xiangchu Feng, and David Zhang,"Fisher Discrimination
% Dictionary Learning for Sparse Representation", In IEEE Int. Conf. on
% Computer Vision, 2011.
% Meng Yang, Lei Zhang, Jian yang and David Zhang, "Metaface learning for
% sparse representation based face recognition", In ICIP, 2010.
%----------------------------------------------------------------------
%
%  Inputs :   (1) A :    the training data
%             (2) X :    the coefficient matrix of the training data
%             (3) index:   the label of the class being processed
%             (4) classn:  the total number of classes
%             (5) Fish_ipts
%                        . D  the dictionary in the last interation
%                        . trls  the labels of training data
%             (6) Fish_par
%                        .dls   the labels of the dicitonary atoms
%
% Outputs:    (1) Di_new :  the updated dictionary of the index-th class
%             (2) delet:    the indication of deleted atoms
%
%--------------------------------------------------------------------

D    =  Fish_ipts.D;
tau3 =  1;
tau2 =  1;
trls =  Fish_ipts.trls;
dls =  Fish_par.dls;
delet = [];

Do = D(:,dls~=index);
Di = D(:,dls==index);
Ai = A(:,trls==index);
Ao = A(:,trls~=index);
Xi = X(:,trls==index);
Xo = X(:,trls~=index);

Xi_i = Xi(dls==index,:);
Xi_o = Xi(dls~=index,:);
Xo_i = Xo(dls==index,:);
Xo_o = Xo(dls~=index,:);
% X_i  = X(dls==index,:);

Zi = Ai-Do*Xi_o;
Zo = Ao-Do*Xo_o;

for i = 1: size(Di,2)
    Yi = Zi - Di*Xi_i + Di(:,i)*Xi_i(i,:);
    Ui = Ai - Di*Xi_i + Di(:,i)*Xi_i(i,:);
%     Ua = A  - Di*X_i+Di(:,i)*X_i(i,:);
    Vo = Zo - Di*Xo_i + Di(:,i)*Xo_i(i,:);

    UaXti = zeros(size(Yi*(Xi_i(i,:))'));
    for t_i = 1:classn
        if t_i~=index
        Xt_i = X(dls==index,trls==t_i);
        UaXti = UaXti + (0  - Di*Xt_i + Di(:,i)*Xt_i(i,:))*(Xt_i(i,:))';
        end
    end

    tem1 = -Yi*(Xi_i(i,:))' - (tau2)*Ui*(Xi_i(i,:))' - Vo*(Xo_i(i,:))' - tau3*UaXti;
%     tem1 = - (tau2)*Ui*(Xi_i(i,:))';
%     tem1 = -Yi*(Xi_i(i,:))' - (tau2)*Ui*(Xi_i(i,:))'  - tau3*Ua*(Xo_i(i,:))';
    tem  = -tem1;
    if norm(tem,2)<1e-6
        Di(:,i) = zeros(size(tem));
        delet = [delet i];
    else
        Di(:,i) = tem./norm(tem,2);
    end
end

Di_new = Di;


function [v,psnrall] = gapdenoise( y , opt )
%GAPDENOISE Generalized alternating projection (GAP)-based denoising
%framework for compressive sensing reconstruction.
%   v=GAPDENOISE(y,opt) returns the reconstruction result v of the
%   measurements with CASSI or CACTI coding, where y is the measurement
%   matrix, opt is the parameters for the GAP-Denoise algorithm, typically
%   the denoiser applied in the framework.
%   Reference
%   [1] Y. Liu, X. Yuan, J. Suo, D.J. Brady, and Q. Dai, Rank Minimization
%       for Snapshot Compressive Imaging, IEEE Trans. Pattern Anal. Mach.
%       Intell. (TPAMI), DOI:10.1109/TPAMI.2018.2873587, 2018.
%   [2] X. Yuan, Generalized alternating projection based total variation
%       minimization for compressive sensing, in Proc. IEEE Int. Conf.
%       Image Process. (ICIP), pp. 2539-2543, 2016.
%   Code credit
%     Xin Yuan, Bell Labs, xyuan@bell-labs.com, initial version Jul 2,
%       2015.
%     Yang Liu, Tsinghua University, y-liu16@mails.tsinghua.edu.cn, last
%       update Jul 13, 2018.
%
%   See also TEST_GAPDENOISE.
if nargin<2
    opt = [];
end
% [0] default parameter configuration, to be specified
A  = @(x) M_func(x);
At = @(z) Mt_func(z);
if isfield(opt,'Mfunc'),  A  = @(x) opt.Mfunc(x);  end
if isfield(opt,'Mtfunc'), At = @(z) opt.Mtfunc(z); end
% Phisum   = ;
denoiser = 'vbm4d'; % video denoiser
% v0       = At(y); % start point (initialization of iteration)
lambda   = 0.2;     % correction coefficiency
maxiter  = 300;     % maximum number of iteration
acc      = 1;       % enable acceleration
tvweight = 0.07;    % weight for TV denoising
tviter   = 5;       % number of iteration for TV denoising
nosestim = true;    % enable noise estimation (if possible)
sigma    = 10/255;  % noise deviation
eta      = 0.5;     % coefficiency for noise estimation
flag_iqa = true;    % flag of showing image quality assessments (be sure
                    %  to turn it off for benchmark)
save_iter_image = false; % flag of saving all the images of the iteration
                         %  process (true only necessary)
                         %  require assigned directory opt.iter_image_dir

if isfield(opt,'Phisum'),     Phisum = opt.Phisum;   end
if isfield(opt,'denoiser'), denoiser = opt.denoiser; end
if isfield(opt,'v0'),             v0 = opt.v0;       end
if isfield(opt,'lambda'),   lambda   = opt.lambda;   end
if isfield(opt,'maxiter'),   maxiter = opt.maxiter;  end
if isfield(opt,'acc'),           acc = opt.acc;      end
if isfield(opt,'tvweight'), tvweight = opt.tvweight; end
if isfield(opt,'tviter'),     tviter = opt.tviter;   end
if isfield(opt,'nosestim'), nosestim = opt.nosestim; end
if isfield(opt,'sigma'),       sigma = opt.sigma;    end
if isfield(opt,'eta'),           eta = opt.eta;      end
if isfield(opt,'flag_iqa'), flag_iqa = opt.flag_iqa; end
if isfield(opt,'save_iter_image'), save_iter_image = opt.save_iter_image; end

if ~exist('v0','var') || isempty(v0)
    v0 = At(y); % start point (initialization of iteration)
end
y1 = zeros(size(y),'like',y);
psnrall = []; % return empty with no ground truth
% ssimall = []; % return empty with no ground truth
% [1] start iteration
v = v0; % initialization

k = 1; % current number of iteration
for isig = 1:length(maxiter) % extension for a series of noise levels
    nsigma = sigma(isig);
    opt.sigma = nsigma;
    for iter = 1:maxiter(isig)
        for ii = 1:1
            % [1.1] Euclidean projection
            yb = A(v);
            if acc % enable acceleration
                y1 = y1+(y-yb);
                v = v+lambda*(At((y1-yb)./Phisum)); % v=v+lambda*(At*A)^-1*At*dy
            else
                v = v+lambda*(At((y-yb)./Phisum));
            end
        end
        % [1.2] Denoising to match the video prior
        switch lower(denoiser)
            case 'tv' % TV denoising
                v = TV_denoising(v,tvweight,tviter);
            case 'vbm3d' % VBM3D denoising
                [~,v] = VBM3D(v,nsigma,0,0); % noise sigma scaled to [0,255]
            case 'vbm4d' % VBM4D denoising
                if nosestim % noise estimation enabled
                    v = vbm4d(v,-1,'lc',1,1,1,0); % -1 to enable noise estimation
                else % noise estimation disabled
                    v = vbm4d(v,nsigma,'lc',1,1,1,0); % -1 to enable noise estimation
                end
            case 'bm4d' % BM4D denoising
                if nosestim % noise estimation enabled
                    v = bm4d(v,'Gauss',0,'lc',1,0); % 0 to enable noise estimation
                else % noise estimation disabled
                    v = bm4d(v,'Gauss',nsigma,'lc',1,0); % 0 to enable noise estimation
                end
            case 'wnnm_c' % WNNM video denoising (earlier C-style strucuture version)
                v = wnnmvdenoiser(v,[],[],opt); % opt.sigma
            case 'wnnm' % WNNM video denoising (MATLAB-style matrix version)
                v = wnnm_vdenoise(v,[],opt); % opt.sigma
            otherwise
                error('Unsupported denoiser %s!',denoiser);
        end
        % % [1.3] update noise standard deviation
        % nsigma = eta*sqrt(abs(sigma^2-var(v(:)-v0(:))));
        % opt.sigma = nsigma;
        % [1.4] save and show intermediate results of psnr and ssim
        if flag_iqa && isfield(opt,'orig') && (~isempty(opt.orig))
            psnrall(k) = psnr(double(v),double(opt.orig)); % record all psnr
            % ssimall(k) = ssim(double(v),double(opt.orig)); % record all ssim
            if (mod(k,5)==0)
                fprintf('  GAP-%s iteration % 4d, sigma %.1f, PSNR %2.2f dB.\n',...
                    upper(opt.denoiser),k,nsigma*255,psnrall(k));
            end
            if save_iter_image % save all the images of the iteration process
                MAXB = opt.MAXB;
                % save each frame in the assigned directory
                nim = size(v,ndims(v)); % number of frames in the video
                for iim = 1:nim
                    im = v(:,:,iim);
                    impsnr = psnr(im,opt.orig(:,:,iim),1);
                    imssim = ssim(im,opt.orig(:,:,iim));
                    if k == 1 % mkdir for the first iteration
                        subdir = sprintf('%s/frame%02d',opt.iter_image_dir,iim); % frame-wise
                        if ~exist(subdir,'dir')
                            mkdir(subdir);
                        end
                    end
                    imwrite(uint8(im*MAXB),sprintf('%s/frame%02d/frame%02d_iter%03d_sigma%.1f_psnr%2.2f_ssim%.4f.png',...
                        opt.iter_image_dir,iim,iim,k,nsigma*MAXB,impsnr,imssim));
                end
            end
        end
        k = k+1;
    end % GAP loop [maxiter]
end % sigma loop [length(sigma)]

end


function LR = computeLR(S, dt, L, M)
%COMPUTELR   Create a contour around each eigenvalue of the linear part of a
%SPINOPERATOR.
%   LR = COMPUTELR(S, DT, L, M) outputs a matrix to be used for the complex
%   means. DT is the time-step, L is the linear part of the SPINOPERATOR S, and
%   M is the number of points to discretize the contour.

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

% Set-up:
dim = getDimension(S); % spatial dimension (1, 2 or 3)
nVars = S.numVars;     % number of unknown functions
N = size(L, 1)/nVars;  % grid points

% Roots of unity:
if ( isreal(L) == 1 )
    % Use only upper-half circle when eigenvalues are real:
    r = exp(1i*pi*((1:M) - .5)/M);
else
    r = exp(2i*pi*((1:M) - .5)/M);
end

% Move each root of unity around each entry of DT*L:
LR = dt*repmat(L(:), 1, M) + repmat(r, nVars*N^dim, 1);

end


close all
tstart=tic;
addpath '~/project/MATLAB/PatternAnalysis/'
addpath '~/project/MATLAB/qdots/'
addpath '~/project/MATLAB/qdots/FastICA_25/'
% Nt = [100, 500, 1000, 5000];
Nt = 1000;
rs = 0.25; %resizing faction
sizevec = [0 32, 0 32];
nx = sizevec(2)-sizevec(1);
ny = sizevec(4)-sizevec(3);

center = ceil([nx, ny]/2);
d = [center; center + [1 0]];

lambda = 400; %nm
pixelsize = 106*rs; %in nm after resizing...
lambdapix = lambda/pixelsize; %pixels
NA = 1.0;
s = 1.4/(2*pi)*(lambdapix/NA); %gaussian approx of airy
% s = 1/rs*s; %for before resizing...

[X, Y] = meshgrid(-20:20);
psf = exp( -(X.^2+Y.^2)/(2*s^2) );
psf = psf / sum(psf(:));

maxphot = 100; % maximal expexted number of photons in one pixel
%  photcount = 100; %mabe made it such that sum(psf(:))=photcount...
offset = 0.01; % general offset as a fraction of maximum
meth = {'ica', 'nmf'};
path = [];
for ii = 1:4
    [dpixc, dveccr, N] = generatedata(d, sizevec, psf, maxphot, offset, Nt(ii), rs);
    n = num2str(ii);
    imstiled(dpixc(:,:,1:12));
    SaveImageFULL(['dpixc_' n], 'p')
    numOfIC = N;
    for jj = 1:2
        if jj==1
            [icasig, A, W] = fastica (dveccr, 'numOfIC', numOfIC, 'g', 'tanh');
            sica = size(A,2);
            icapix = reshape(A,nx*rs, ny*rs, sica);
        else
            [w,h]=nmf(dveccr',numOfIC,1);
            icapix=shiftdim(reshape(h,numOfIC,nx*rs,ny*rs), 1);
        end
        name = [n '_' meth{jj}];
        comp_images(icapix, dpixc, d, rs, saveon, name, path)
    end

end


% Usage: compute_rmse( Yrec, dataset, remove_val )
%        compute_rmse( name_of_file_with_reconstructions )
function [ out, lat, lon, time ] = metoffice_compute_rmse( Yrec, dataset, remove_val )

if isstr(Yrec)
    setup = get_setup( Yrec )

    % Load reconstructions
    datapath = metoffice_get_path();
    load( [ datapath, Yrec ], 'Yrec' );
    %load( [ datapath '/RecTest/RESULTS/' Yrec ], 'Yrec' );
else
    setup.dataset = dataset;
    setup.remove_val = remove_val;
    setup
end

% Remove values below -1.8
Yrec( Yrec < -1.8 ) = -1.8;

[ true_sst, Itest, Itrain, time, train_sst ] = metoffice_get_testdata( setup.dataset );
%[ true_sst, Itest, Itrain, time, train_sst ] = load_testdata( setup.dataset );

if setup.remove_val
    Ival = get_valset( setup.remove_val, time, Itrain );
    Itrain = Itrain & ~Ival;
end

% Load lat and lon to compute the area weights
datapath = metoffice_get_path();
load( [ datapath, dataset ], 'lat', 'lon' )
%load( [ datapath '/RecTest/DATASETS/' dataset ], 'lat', 'lon' )
lts = repmat( lat, 1, length(lon) );
weights = cosd( lts(:) );
clear lts lat lon

err = true_sst - Yrec;
out.train = get_rmse( err, Itrain, weights );
out.test = get_rmse( err, Itest, weights );
if setup.remove_val
    out.val = get_rmse( err, Ival, weights );
end

%out.Yrec = Yrec;

out.trdata = get_rmse( train_sst-Yrec, Itrain, weights );

%out.test.err = err;
%out.test.err(~Itest) = NaN;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function out = get_rmse( err, I, weights )

err2 = err.^2;
err2 = bsxfun( @times, weights, err2 );
Iw = bsxfun( @times, weights, I );

err2(~I) = 0;
out.rmse_fld = sqrt( sum(err2,2)./sum(I,2) );

out.rmse_ts = sqrt( sum(err2,1)./sum(I,1) );

out.rmse = sqrt( sum( err2(:) )/sum(I(:)) );

if 1
    out.rmse2 = sum(err2,1)./sum(Iw,1);
    out.rmse2( isnan(out.rmse2) ) = [];
    out.rmse2 = sqrt( mean( out.rmse2 ) );
end

out.err_fld = sqrt(sum(err2,2));
out.err_ts = sqrt(sum(err2,1));

% Smoothed time series: take 5 year moving average
err2_ts = sum(err2,1);
nobs_ts = sum(I,1);

nfilt = 5*12;
T = size(err2,2);
err2_sm = conv( ones(1,nfilt), err2_ts );
err2_sm = err2_sm( nfilt/2+(1:T) );
nobs_sm = conv( ones(1,nfilt), nobs_ts );
nobs_sm = nobs_sm( nfilt/2+(1:T) );

out.rmse_smts = sqrt( err2_sm./nobs_sm );

out.err_smts = sqrt(err2_sm);

% Median
out.medwe = median( err(I) );
out.medwe_fld = NaN*zeros(size(err,1),1);
for i = 1:size(err,1)
    out.medwe_fld(i) = median( err(i, I(i,:) ) );
end

out.medwe_ts = NaN*zeros(1,size(err,2));
for i = 1:size(err,2)
    out.medwe_ts(i) = median( err( I(:,i), i ) );
end


%compute speed in the direction perpendicular to tail-central direction
function [data,units]=compute_velpertc(trx,n)

larvae=trx.exp2flies{n};
numlarvae=numel(larvae);
velpertc=cell(1,numlarvae);
%tailcentralangperp=cell(1,numlarvae);
for i=1:numlarvae
    larva=larvae(i);
    %tailcentralangperp{1,i}=trx(larva).tailcentralang-pi/2;
    velpertc{1,i}=trx(larva).velmag_ctr.*(cos(trx(larva).velang).*cos(trx(larva).tailcentralang(1,1:end-1)+pi/2)+sin(trx(larva).velang).*sin(trx(larva).tailcentralang(1,1:end-1)+pi/2));
end

units=parseunits('mm/s');
data=velpertc;


function [A,B] = hmxQRSVD(A,B,tol)
%+========================================================================+
%|                                                                        |
%|         OPENHMX - LIBRARY FOR H-MATRIX COMPRESSION AND ALGEBRA         |
%|           openHmx is part of the GYPSILAB toolbox for Matlab           |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2018.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : hmxQRSVD.m                                    |
%|    #    |   VERSION    : 0.52                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 01.01.2019                                    |
%| ( === ) |   SYNOPSIS   : QR factorization and SVD recompression for    |
%|  `---'  |                low-rank matrices                             |
%+========================================================================+

if ~isempty(A)
    % QR Factorisation A = QA * RA;
    [QA,RA] = qr(A,0);

    % QR Factorisation Bt = QB * RB
    [QB,RB] = qr(B.',0);

    % Singular value decomposition U*S*V = RA * RB.'
    try
        [U,S,V] = svd(RA * RB.','econ');
    catch
        return
    end

    % Compression rank
    s = diag(S);
    n = sum( s./s(1) >= tol );

    % Recompression A = QA * U * S
    A = QA * (U(:,1:n) * S(1:n,1:n));

    % Recompression B = V' * QB^t
    B = V(:,1:n)' * QB.';
end
end


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

function [BB1, shift] = bb_predict(BB0,pt0,pt1)

of  = pt1 - pt0;
dx  = median(of(1,:));
dy  = median(of(2,:));

d1  = pdist(pt0','euclidean');
d2  = pdist(pt1','euclidean');
s   = median(d2./d1);

s1  = 0.5*(s-1)*bb_width(BB0);
s2  = 0.5*(s-1)*bb_height(BB0);

BB1  = [BB0(1)-s1; BB0(2)-s2; BB0(3)+s1; BB0(4)+s2] + [dx; dy; dx; dy];
shift = [s1; s2];


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

%source: http://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=05291740
% A Triaxial Accelerometer Calibration Method Using a Mathematical Model
% by S.P. Won, F. Golnaraghi

function [B, G] = IMUcalibIteration(S_x, S_y, S_z, B, G)
    % gravitation at Helsinki
    g0 = 9.8189;

    % current estimate of accelerations
    A_x = (S_x - B(1)) / G(1);
    A_y = (S_y - B(2)) / G(2);
    A_z = (S_z - B(3)) / G(3);

    % squares
    A_x2 = (A_x.*A_x);
    A_y2 = (A_y.*A_y);
    A_z2 = (A_z.*A_z);

    % error estimate
    E = A_x2 + A_y2 + A_z2 - (g0*g0);

    ACCEL = [A_x2 A_y2 A_z2 A_x A_y A_z];

    CAL = ACCEL \ E;

    % gain change
    G_change2 = 1 ./ (1 - CAL(1:3));
    G_change = sqrt(abs(G_change2));

    % bias change
    B_change = CAL(4:6) .* G .* G_change2 .* 0.5;

    % update estimates
    G = G .* G_change;
    B = B + B_change;
end


function UNew = fluidNeumann3D(varargin);
% fluidNeumann3D: solve fluid registraion in 3D with Neumann
%        boundary conditions
%
%
% author: Nathan D. Cahill
% email: nathan.cahill@rit.edu
% affiliation: Rochester Institute of Technology
% date: January 2014
% licence: GNU GPL v3
%
% Copyright Nathan D. Cahill
% Code available from https://github.com/tomdoel/npReg
%
%

% parse input arguments
[DU,F,mu,lambda,PixSize,M,N,P,RegularizerFactor,HX,HY,HZ] = parse_inputs(varargin{:});

% multiply F by adjoint of Navier-Lame equations
FNew = adjointNL(F/RegularizerFactor,mu,lambda,0,M,N,P);

% compute sine transform of new force field
FS = discreteCosineTransform(FNew,M,N,P);

% construct images of coordinates scaled by pi/(N or M or P)
[a,b,c] = ndgrid(pi*(0:(M-1))/(M-1),pi*(0:(N-1))/(N-1),pi*(0:(P-1))/(P-1));

% construct LHS factor
LHSfactor = mu.*(lambda+2*mu).*(2*cos(a) + 2*cos(b) + 2*cos(c) - 6).^2;

% if gamma is zero, set origin term to 1, as DC term does not matter
LHSfactor(1,1,1) = 1;

% solve for FFT of U
VS = cat(4,FS(:,:,:,1)./LHSfactor,FS(:,:,:,2)./LHSfactor,FS(:,:,:,3)./LHSfactor);

% perform inverse DST
V = discreteCosineTransform(VS,M,N,P);

% now perform Euler integration to construct new displacements
UNew = zeros(M,N,P,3);
UNew(:,:,:,1) = (1 - imfilter(V(:,:,:,1),HX,'replicate','same')).*V(:,:,:,1) - ...
    imfilter(V(:,:,:,2),HY,'replicate','same').*V(:,:,:,2) - ...
    imfilter(V(:,:,:,3),HZ,'replicate','same').*V(:,:,:,3);
UNew(:,:,:,2) = -imfilter(V(:,:,:,1),HY,'replicate','same').*V(:,:,:,1) + ...
    (1 - imfilter(V(:,:,:,2),HY,'replicate','same')).*V(:,:,:,2) - ...
    imfilter(V(:,:,:,3),HY,'replicate','same').*V(:,:,:,3);
UNew(:,:,:,3) = -imfilter(V(:,:,:,1),HZ,'replicate','same').*V(:,:,:,1) - ...
    imfilter(V(:,:,:,2),HZ,'replicate','same').*V(:,:,:,2) + ...
    (1 - imfilter(V(:,:,:,3),HZ,'replicate','same')).*V(:,:,:,3);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function FS = discreteCosineTransform(F,M,N,P);
% compute discrete cosine transform of 3-D vector field

% initialize resulting array
FS = F;

% first perform cosine transform down columns
len = 2*M-2; ind = 1:M;
for p=1:P
    for n=1:N
        s = fft(FS(:,n,p,:),len,1);
        FS(:,n,p,:) = real(s(ind,:,:,:));
    end
end
FS = sqrt(2/(M-1))*FS;

% next perform cosine transform across rows
len = 2*N-2; ind = 1:N;
for p=1:P
    for m=1:M
        s = fft(FS(m,:,p,:),len,2);
        FS(m,:,p,:) = real(s(:,ind,:,:));
    end
end
FS = sqrt(2/(N-1))*FS;

% finally perform cosine transform across pages
len = 2*P-2; ind = 1:P;
for n=1:N
    for m=1:M
        s = fft(FS(m,n,:,:),len,3);
        FS(m,n,:,:) = real(s(:,:,ind,:));
    end
end
FS = sqrt(2/(P-1))*FS;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function FNew = adjointNL(F,mu,lambda,gamma,M,N,P);
% multiply vector field F by adjoint Navier-Lame equations

% initialize FNew
FNew = zeros(M,N,P,3);

% construct filter that implements 3-D Laplacian
L = (lambda+2*mu)*cat(3,[0 0 0;0 1 0;0 0 0],[0 1 0;1 -6 1;0 1 0],[0 0 0;0 1 0;0 0 0]);

% we will need to use L to form two different filters
% L1 = -(lambda+2*mu)*L; L1(2,2,2) = gamma + L1(2,2,2);
% L2 = -mu*L; L2(2,2,2) = gamma + L2(2,2,2);

% construct grad div filters
GD11 = (lambda+mu)*cat(3,zeros(3,3),[0 1 0;0 -2 0;0 1 0],zeros(3,3));
GD22 = ipermute(GD11,[2 1 3]);
GD33 = ipermute(GD11,[3 2 1]);
GD23 = zeros(3,3,3);
GD23(2,1,1) = 1; GD23(2,3,3) = 1; GD23(2,1,3) = -1; GD23(2,3,1) = -1;
GD23 = GD23*(lambda+mu)/4;
GD12 = ipermute(GD23,[3 1 2]);
GD13 = ipermute(GD23,[2 3 1]);

% perform filtering
FNew(:,:,:,1) = imfilter(F(:,:,:,1),L-GD11,'replicate') + ...
    imfilter(F(:,:,:,2),-GD12,'replicate') + ...
    imfilter(F(:,:,:,3),-GD13,'replicate');
FNew(:,:,:,2) = imfilter(F(:,:,:,1),-GD12,'replicate') + ...
    imfilter(F(:,:,:,2),L-GD22,'replicate') + ...
    imfilter(F(:,:,:,3),-GD23,'replicate');
FNew(:,:,:,3) = imfilter(F(:,:,:,1),-GD13,'replicate') + ...
    imfilter(F(:,:,:,2),-GD23,'replicate') + ...
    imfilter(F(:,:,:,3),L-GD33,'replicate');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [DU,F,mu,lambda,PixSize,M,N,P,RegularizerFactor,HX,HY,HZ] = parse_inputs(varargin);

% get displacement field and check size
F = varargin{2};
PixSize = varargin{4}(1:3);
M = varargin{5};
N = varargin{6};
P = varargin{7};
mu = varargin{8};
lambda = varargin{9};
RegularizerFactor = varargin{10};
DU = varargin{11};
HX = varargin{12};
HY = varargin{13};
HZ = varargin{14};
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


function PARA = setMIpara(PARA,varargin)

% default parameter:
kernel  = 'cos4';
sigma   = 32;
minR    = -50;  maxR    = 300; ngvR = 8;
minT    = -50;  maxT    = 300; ngvT = 8;
entropyTol = 1e-8;

% overwrite PARA
for k=1:length(varargin)/2,
  str = sprintf('PARA=setfield(PARA,''%s'',varargin{%d});',varargin{2*k-1},2*k);
  disp(str);
  eval(str);
end;

% enpack PARA
fn = fieldnames(PARA);
for j=1:length(fn),
  str = sprintf('%s=getfield(PARA,fn{%d});',fn{j},j);
%   disp(str);
  eval(str);
end;

% check integration
tol     = 1e-2;
maxngv  = 512;

while 1,
  gvR  = linspace(minR,maxR,ngvR);
  yR   = feval(kernel,gvR,sigma);
  intR = sum(yR)*(gvR(2)-gvR(1));
  if abs(intR-1) <= tol, break; end;
  ngvR = 2*ngvR;
  if ngvR>maxngv, error('to many bins for grayvalues of R'); end;
end;
while 1,
  gvT  = linspace(minT,maxT,ngvT);
  yT   = feval(kernel,gvT,sigma);
  intT = sum(yT)*(gvT(2)-gvT(1));
  if abs(intT-1) <= tol, break; end;
  ngvT = 2*ngvT;
  if ngvT>maxngv, error('to many bins for grayvalues of T'); end;
end;

PARA.kernel = kernel;
PARA.sigma  = sigma;
PARA.minR   = minR;
PARA.maxR   = maxR;
PARA.ngvR   = ngvR;
PARA.gvR    = gvR;
PARA.minT   = minT;
PARA.maxT   = maxT;
PARA.ngvT   = ngvT;
PARA.gvT    = gvT;
PARA.entropyTol = entropyTol;

return;
% ------------------------------------------------------------------------------


function scatterplot(iter,ubest,fval,action,gpstruct,optimState,options)
%SCATTERPLOT Scatter plot of optimization iteration

if size(gpstruct.x, 1) == 0; return; end

figure(100);

nvars = size(gpstruct.x,2);
MeshSize = optimState.meshsize;

if options.Debug
    display(['GP hyper-parameters at iteration ' num2str(iter) ':']);
    gpstruct.hyp.mean
    (gpstruct.hyp.cov')
    (gpstruct.hyp.lik)
end

nrows = 2;
ncols = ceil((ceil(nvars/2) + 1)/2);

for iDim = 1:ceil(nvars/2)
    subplot(nrows,ncols,iDim);
    d1 = iDim*2 - 1;
    d2 = min(nvars,iDim*2);

    index = 1:optimState.Xmax;
    hold off;

    % Plot mesh size box
    s = MeshSize*ones(1,nvars);
    sstar = ubest;
    plot([sstar(d1) + s(d1),sstar(d1) + s(d1),sstar(d1) - s(d1),sstar(d1) - s(d1),sstar(d1) + s(d1)], ...
        [sstar(d2) - s(d2),sstar(d2) + s(d2),sstar(d2) + s(d2),sstar(d2) - s(d2),sstar(d2) - s(d2)], ...
        '-','LineWidth',1,'Color',[0.25 0.5 0]); hold on;

    % Plot all points
    idx = isfinite(optimState.Y(index(1:end-1)));
    plot(optimState.U(idx,d1),optimState.U(idx,d2),'.','MarkerSize',5,'MarkerFaceColor',[0 0 0]);
    idx = ~isfinite(optimState.Y(index(1:end-1)));
    plot(optimState.U(idx,d1),optimState.U(idx,d2),'*','MarkerSize',5,'MarkerFaceColor',[0 0 0]);

    % Plot training set
    if ~isfield(gpstruct,'erry') || isempty(gpstruct.erry); erry = false(size(gpstruct.y)); else erry = gpstruct.erry; end
    idx = ~erry;
    plot(gpstruct.x(idx,d1),gpstruct.x(idx,d2),'o','MarkerSize',5,'MarkerFaceColor',[0.5 0.5 0.5],'MarkerEdgeColor','none');
    idx = erry;
    plot(gpstruct.x(idx,d1),gpstruct.x(idx,d2),'*','MarkerSize',5,'MarkerFaceColor',[0.5 0.5 0.5],'MarkerEdgeColor',[0.5 0.5 0.5]);

    miny = min(optimState.Y(index));
    maxy = max(optimState.Y(index));
    deg = (optimState.Y(index) - miny)./(maxy - miny);
    %for iX = 1:optimState.Xmax
    %    col = bsxfun(@plus, deg*0.8, 0.1*[1 1 1]);
    %    plot(optimState.X(iX,d1),optimState.X(iX,d2),'o','MarkerFaceColor',col(iX,:)); hold on;
    %end

    % Plot true minimum
    if ~isempty(options.TrueMinX)
        plot(options.TrueMinX(d1),options.TrueMinX(d2),'o','MarkerFaceColor',[0.25 0 1], 'MarkerEdgeColor', 'none');
    end

    % Plot last evaluated point
    plot(optimState.U(index(end),d1),optimState.U(index(end),d2),'o','MarkerFaceColor',[1 0.4 0.4]);

    % Plot incumbent
    plot(sstar(d1),sstar(d2),'d','MarkerFaceColor',[0.25 1 0], 'MarkerEdgeColor', 'none');

    box off;
    set(gca,'TickDir','out');

    % Zoom level
    s = 4*MeshSize*ones(1,nvars);
    abox = [sstar(d1),sstar(d1),sstar(d2),sstar(d2)] + [-s(d1),s(d1),-s(d2),s(d2)];
    axis(abox);
end

subplot(nrows,ncols,nrows*ncols);
cla(gca);
box off;
axis off;
text(0.2,0.9,['Iteration: ' num2str(iter)]);
text(0.2,0.8,['f-count: ' num2str(optimState.funccount)]);
text(0.2,0.7,['f(x): ' num2str(fval,'%.6g')]);
text(0.2,0.6,['MeshScale: ' num2str(MeshSize,'%.6g')]);
text(0.2,0.5,['Method: ']);
text(0.2,0.4,['Action: ' action]);

set(gcf,'Color','w');
drawnow;

end


function p = cs_nd (A)
%CS_ND generalized nested dissection ordering.
%   p = cs_nd(A) computes the nested dissection ordering of a matrix.  Small
%   submatrices (order 500 or less) are ordered via cs_amd.  A must be sparse
%   and symmetric (use p = cs_nd(A|A') if it is not symmetric).
%
%   Example:
%       A = delsq (numgrid ('L', 300)) ;    % matrix used in 'bench'
%       p = cs_nd (A) ;
%       cspy (A (p,p)) ;
%
%   See also CS_AMD, CS_SEP, CS_ESEP, CS_NSEP, AMD.

%   Copyright 2006-2007, Timothy A. Davis.
%   http://www.cise.ufl.edu/research/sparse

n = size (A,1) ;
if (n == 1)
    p = 1 ;
elseif (n < 500)
    p = cs_amd (A) ;                % use cs_amd on small graphs
else
    [s a b] = cs_nsep (A) ;         % find a node separator
    a = a (cs_nd (A (a,a))) ;       % order A(a,a) recursively
    b = b (cs_nd (A (b,b))) ;       % order A(b,b) recursively
    p = [a b s] ;                   % concatenate to obtain the final ordering
end


function net = gtm(dim_latent, nlatent, dim_data, ncentres, rbfunc, ...
   prior)
%GTM	Create a Generative Topographic Map.
%
%	Description
%
%	NET = GTM(DIMLATENT, NLATENT, DIMDATA, NCENTRES, RBFUNC), takes the
%	dimension of the latent space DIMLATENT, the number of data points
%	sampled in the latent space NLATENT, the dimension of the data space
%	DIMDATA, the number of centres in the RBF model NCENTRES, the
%	activation function for the RBF RBFUNC and returns a data structure
%	NET. The parameters in the RBF and GMM sub-models are set by calls to
%	the corresponding creation routines RBF and GMM.
%
%	The fields in NET are
%	  type = 'gtm'
%	  nin = dimension of data space
%	  dimlatent = dimension of latent space
%	  rbfnet = RBF network data structure
%	  gmmnet = GMM data structure
%	  X = sample of latent points
%
%	NET = GTM(DIMLATENT, NLATENT, DIMDATA, NCENTRES, RBFUNC, PRIOR),
%	sets a Gaussian zero mean prior on the parameters of the RBF model.
%	PRIOR must be a scalar and represents the inverse variance of the
%	prior distribution.  This gives rise to a weight decay term in the
%	error function.
%
%	See also
%	GTMFWD, GTMPOST, RBF, GMM
%

%	Copyright (c) Ian T Nabney (1996-2001)

net.type = 'gtm';
% Input to functions is data
net.nin = dim_data;
net.dim_latent = dim_latent;

% Default is no regularisation
if nargin == 5
   prior = 0.0;
end

% Only allow scalar prior
if isstruct(prior) | size(prior) ~= [1 1]
   error('Prior must be a scalar');
end

% Create RBF network
net.rbfnet = rbf(dim_latent, ncentres, dim_data, rbfunc, ...
   'linear', prior);

% Mask all but output weights
net.rbfnet.mask = rbfprior(rbfunc, dim_latent, ncentres, dim_data);

% Create field for GMM output model
net.gmmnet = gmm(dim_data, nlatent, 'spherical');

% Create empty latent data sample
net.X = [];


function value = mean(sF, varargin)
% calculates the mean value for an univariate S2Fun or calculates the mean along a specified dimension fo a multimodal S2Fun
%
% Syntax
%   value = mean(sF)
%   sF = mean(sF, d)
%
% Input
%  sF - @S2FunHarmonic
%  d - dimension to take the mean value over
%
% Output
%  sF - S2FunHarmonic
%  value - double
%
% Description
%
% If sF is a 3x3 S2Fun then
% mean(sF) returns a 3x3 matrix with the mean values of each function
% mean(sF, 1) returns a 1x3 S2Fun wich contains the pointwise means values along the first dimension
%

s = size(sF);
if nargin == 1
  sF = sF.subSet(':');
  value = real(sF.fhat(1, :))/sqrt(4*pi);
  value = reshape(value, s);
else
  s = size(sF);
  value = sum(sF, varargin{1})./s(varargin{1});
end

end


function Y = abs(X)
    % Absolute value.
    %   ABS(X) is the absolute value of the elements of X. When
    %   X is complex, ABS(X) is the complex modulus (magnitude) of
    %   the elements of X.

    % Convert inputs to SymExpression
    % X = SymExpression(X);

    % construct the operation string
    sstr = ['Abs[' X.s ']'];

    % create a new object with the evaluated string
    Y = SymExpression(sstr);
end