classdef CorrelateCross < dagnn.Filter
  properties
    opts = {'cuDNN'}
    ImageArea = [];
    BatchSize = [];
  end

  methods
    function outputs = forward(obj, inputs, params)
      % Assume the same size
      obj.BatchSize = size(inputs{1});
      obj.ImageArea =  obj.BatchSize(1) * obj.BatchSize(2);
      InputsAll = gpuArray.zeros(obj.ImageArea * obj.BatchSize(end), 1, obj.BatchSize(3), 1, 'single');
      for i = 1:obj.BatchSize(end)
          Start = (i - 1) * obj.ImageArea + 1;
          End =  i * obj.ImageArea;
          InputsAll(Start:End,:,:) = reshape(inputs{1}(:,:,:,i), [obj.ImageArea  1 obj.BatchSize(3)]);
      end
      outputs = obj.CorrelateFoward({InputsAll});
    end

    function outputs = CorrelateFoward(obj, inputs)
        [h,w,d,batchsize]= size(inputs{1});
        inputsfilter = reshape(permute(inputs{1},[3,1,2,4]),1,1,d,h*w,[]);
        outputs{1} = gpuArray.zeros(h,w,h*w,batchsize,'single');
        for i=1:batchsize
            outputs{1}(:,:,:,i) = vl_nnconv(inputs{1}(:,:,:,i), inputsfilter(:,:,:,:,i),[]) ;
        end
    end

    function [derInputs, derParams] = backward(obj, inputs, params, derOutputs)
      InputsAll = gpuArray.zeros(obj.ImageArea * obj.BatchSize(end), 1, obj.BatchSize(3), 1, 'single');
      for i = 1:obj.BatchSize(end)
          Start = (i - 1) * obj.ImageArea + 1;
          End =  i * obj.ImageArea;
          InputsAll(Start:End,:,:) = reshape(inputs{1}(:,:,:,i), [obj.ImageArea  1 obj.BatchSize(3)]);
      end
      derInputsAll = obj.CorrelateBackward({InputsAll}, derOutputs);
      derInputs{1} = gpuArray.zeros(obj.BatchSize, 'single');
      for i = 1:obj.BatchSize(end)
          Start = (i - 1) * obj.ImageArea + 1;
          End =  i * obj.ImageArea;
          derInputs{1}(:,:,:,i) = reshape(derInputsAll(Start:End,:,:), obj.BatchSize(1:3));
      end

      derParams = {} ;
    end

    function derInputs = CorrelateBackward(obj, inputs, derOutputs)
        [h,w,d,batchsize]= size(inputs{1});
        inputsfilter = reshape(permute(inputs{1},[3,1,2,4]),1,1,d,h*w,[]);
        derInputs1 = gpuArray.zeros(h,w,d,batchsize,'single');
        derInputs2 = gpuArray.zeros(h,w,d,batchsize,'single');

        for i=1:batchsize
            [dI1, dI2, ~] = vl_nnconv(...
                inputs{1}(:,:,:,i), inputsfilter(:,:,:,:,i), [], derOutputs{1}(:,:,:,i)) ;
            derInputs1(:,:,:,i) = dI1;
            derInputs2(:,:,:,i) = permute(reshape(dI2,d,h,w,[]),[2 3 1 4]);
        end
        derInputs = derInputs1 + derInputs2;
    end

    function obj = CorrelateCross(varargin)
      obj.load(varargin) ;
    end
  end
end


function [stat, res, stat0] = get_validregions(stat0, res0, clustrules)

ix = 0;

iclust = res0.iclust;
% ipixbad = true(size(res0.M));
for k = 1:length(stat0)

    if ~isempty(stat0(k).region)

%         vs = [stat0(k).region.V];
%         criterion = sum(vs > mean(vs(:)) * clustrules.parent.PixelFractionThreshold);
        if 1 %criterion<clustrules.parent.MaxRegions

            igood = [stat0(k).region.mrs]./[stat0(k).region.mrs0] <clustrules.Compact;
            igood = igood & [stat0(k).region.npix]>clustrules.MinNpix & ...
                [stat0(k).region.npix]<clustrules.MaxNpix;
%             igood = [stat0(k).region.npix]>clustrules.MinNpix;

            igood = find(igood);
            for j = 1:length(igood)
                ix = ix+1;
                stat(ix)             = stat0(k).region(igood(j));

                iclust(stat(ix).ipix) = ix + numel(stat0);

                ismmb = ismember(stat0(k).ipix, stat(ix).ipix);
                stat0(k).ipix(ismmb) = [];
            end
        end
    end
    stat0(k).mrs   = Inf;

end

for k = 1:length(stat0)
    stat0(k).igood = 0;
end
for k = 1:length(stat)
    stat(k).igood = 1;
end

stat(1).region = [];
stat0(1).V = [];

stat0(1).parent = [];

res = res0;
res.iclust = iclust;

% stat(1).ipix = find(res.iclust(:)==1);
% stat(1).mrs = Inf;
% stat(1).mrs0 = Inf;
% stat(1).npix = numel(stat(1).ipix);
% stat(1).lambda = res.lambda(stat(1).ipix);
% stat(1).V = Inf;

stat = [stat0 stat];


function prob = appendMosekSDPdata(F_struc,K,prob)

prob.bardim = K.s;
prob.barc.subj = [];
prob.barc.subk = [];
prob.barc.subl = [];
prob.barc.val = [];
prob.bara.subi = [];
prob.bara.subj = [];
prob.bara.subk = [];
prob.bara.subl = [];
prob.bara.val = [];

C = F_struc(:,1);
A = -F_struc(:,2:end);

% -- Faster fix by Shahar
tops = [1 cumsum(K.s.^2)+1];
top = 1+K.f+K.l+sum(K.q);
[ii,jj,kk] = find(A(top:top + sum(K.s.^2)-1,:));
allcon = floor(interp1(tops,1:length(tops),ii,'linear'));
all_iilocal = ii-tops(allcon)'+1;
a = all_iilocal;
b = K.s(allcon);
allcol = ceil(a(:)./b(:))';
allrow = a(:)' - (allcol-1).*b(:)';
allvar = jj;
allval = kk;
% sort (for backward compatibility?)
[~,ind_sort] = sort(allcon);
allcon = allcon(ind_sort)';
allcol = allcol(ind_sort)';
allrow = allrow(ind_sort)';
allvar = allvar(ind_sort)';
allval = allval(ind_sort)';
% --

keep = find(allrow >= allcol);
allcol = allcol(keep);
allrow = allrow(keep);
allcon = allcon(keep);
allvar = allvar(keep);
allval = allval(keep);
prob.bara.subi = [prob.bara.subi allvar];
prob.bara.subj = [prob.bara.subj allcon];
prob.bara.subk = [prob.bara.subk allrow];
prob.bara.subl = [prob.bara.subl allcol];
prob.bara.val = [prob.bara.val allval];

for j = 1:length(K.s)
    n = K.s(j);
    Ci = C(top:top+n^2-1);
    Ci = tril(reshape(Ci,n,n));
    [k,l,val] = find(Ci);
    prob.barc.subj = [prob.barc.subj j*ones(1,length(k))];
    prob.barc.subk = [prob.barc.subk k(:)'];
    prob.barc.subl = [prob.barc.subl l(:)'];
    prob.barc.val = [prob.barc.val val(:)'];
    top = top + n^2;
end


function [varargout] = plinproj(varargin)

% PLINPROJ projects a point onto a line or linepiece
%
% Use as
%   [proj, dist] = plinproj(l1, l2, r, flag)
% where l1 and l2 are the begin and endpoint of the linepiece, and r is
% the point that is projected onto the line
%
% the optional flag can be:
%   0 (default)  project the point anywhere on the complete line
%   1            project the point within or on the edge of the linepiece

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

% compile the missing mex file on the fly
% remember the original working directory
pwdir = pwd;

% determine the name and full path of this function
funname = mfilename('fullpath');
mexsrc  = [funname '.c'];
[mexdir, mexname] = fileparts(funname);

try
  % try to compile the mex file on the fly
  warning('trying to compile MEX file from %s', mexsrc);
  cd(mexdir);

  if ispc
    mex -I. -c geometry.c
    mex -I. -c plinproj.c ; mex plinproj.c plinproj.obj geometry.obj
  else
    mex -I. -c geometry.c
    mex -I. -c plinproj.c ; mex -o plinproj plinproj.o geometry.o
  end

  cd(pwdir);
  success = true;

catch
  % compilation failed
  disp(lasterr);
  error('could not locate MEX file for %s', mexname);
  cd(pwdir);
  success = false;
end

if success
  % execute the mex file that was just created
  funname   = mfilename;
  funhandle = str2func(funname);
  [varargout{1:nargout}] = funhandle(varargin{:});
end


function [ a, b] = p05_interval ( )

%*****************************************************************************80
%
%% P05_INTERVAL returns a starting interval for optimization for problem 5.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 February 2002
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real A, B, two points defining an interval in which
%    the local minimizer should be sought.
%
  a = 0.0002;
  b = 2.0;

  return
end


 function ob_out = kronI(M, ob_in, varargin)
%|function ob_out = kronI(M, ob_in, varargin)
%|
%| ob_out = kron(eye(M), ob_in)
%|
%| useful for making 3D from 2D objects working slice-by-slice
%|
%| option
%| 'parfor' default 0

arg.parfor = false;
arg = vararg_pair(arg, varargin);

ob_out = fatrix2_kroni(M, ob_in, arg);


clear
path('./icon/',path);
Init;

%Constant value
RAD2DEG = 57.2957795;
DEG2RAD = 0.0174533;
%throttle when UAV is hovering
THR_HOVER = 0.609;
%% Initial condition
ModelInit_PosE = [0, 0, -100];
ModelInit_VelB = [0, 0, 0];
ModelInit_AngEuler = [0, 0, 0];
ModelInit_RateB = [0, 0, 0];
ModelInit_Rads = 557.142;

%max control angle,default 35deg
MAX_CONTROL_ANGLE_ROLL = 35;
MAX_CONTROL_ANGLE_PITCH  = 35;
%max control angle rate,rad/s
MAX_CONTROL_ANGLE_RATE_PITCH = 220;
MAX_CONTROL_ANGLE_RATE_ROLL = 220;
MAX_CONTROL_ANGLE_RATE_Y = 200;
%Maximum control speed, m/s
MAX_CONTROL_VELOCITY_XY = 5;
MAX_CONTROL_VELOCITY_Z = 3;
%Throttle amplitude
MAX_MAN_THR = 0.9;
MIN_MAN_THR = 0.05;
%% run simulink model
PosControl_tune


#!/usr/bin/octave -q
% Copyright 2018 Marc René Schädler
%
% This file is part of the mobile hearing aid prototype project
% The the mobile hearing aid prototype project 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.
%
% The mobile hearing aid prototype project 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 the mobile hearing aid prototype project. If not, see http://www.gnu.org/licenses/.

close all
clear
clc

fs = 48000;

[signal, fs] = audioread('/dev/shm/feedback.wav');
assert(fs==48000);

[feedback, range] = estimate_feedback(signal, fs);

feedback1 = feedback(:,1);
feedback2 = feedback(:,2);
range1 = range(1,:);
range2 = range(2,:);

% Write coefficients and ranges to files
fp = fopen('abhang/src/configuration/feedback1.bin','wb');
fwrite(fp,single(feedback1),'single');
fclose(fp);
fp = fopen('abhang/src/configuration/feedback2.bin','wb');
fwrite(fp,single(feedback2),'single');
fclose(fp);
fp = fopen('abhang/src/configuration/range1.bin','wb');
fwrite(fp,int32(range1),'int32');
fclose(fp);
fp = fopen('abhang/src/configuration/range2.bin','wb');
fwrite(fp,int32(range2),'int32');
fclose(fp);

[~, maxidx1] = max(abs(feedback1));
[~, maxidx2] = max(abs(feedback2));
latency1 = (maxidx1-1).*1000./fs;
latency2 = (maxidx2-1).*1000./fs;

printf("latency1 = %.2fms\nlatency2 = %.2fms\n",latency1,latency2);
printf("range1 = [%i %i]\nrange2 = [%i %i]\n",range1(1),range1(2),range2(1),range2(2));


function [Xpareto,Ypareto,X,Y,XParetoGP,YParetoGP,YParetoGPstd,hypf] = TSEMO_V4(f,X,Y,lb,ub,Opt)
%  Copyright (c) by Eric Bradford, Artur M. Schweidtmann and Alexei Lapkin, 2020-23-05.
%  Version 4 by Eric Bradford (eric.bradford@ntnu.no), Artur M.
%  Schweidtmann (Artur.Schweidtmann@avt.rwth-aachen.de) and Alexei Lapkin
%  (aal35@cam.ac.uk)

% ALGORITHM  Executes the optimization algorithm.

%   OUTPUTS
%       Xpareto      Pareto front approximation from X inputs       [np,D]
%       Ypareto      Pareto front approximation from Y outputs      [np,O]
%       X            Data input vector                              [n+meval,D]
%       Y            Data output vector                             [n+meval,O]
%       XParetoGP    Pareto front approximation of GP model inputs  [npGP,D]
%       YParetoGP    Pareto front approximation of GP model outputs [npGP,D]
%       YParetoGPstd Pareto front standard deviation of GP model    [npGP,D]
%       hypf         Final hyperparameter values []                 [D+2,O]

%   INPUTS
%       f       objective function
%       X       input values of given data set  [n,D]
%       Y       output values of given data set [n,O]
%       lb      lower bounds of input variables [1,D]
%       ub      upper bounds of input variables [1,D]
%       Opt     options struct of algorithm

%   with
%       n       number of given data points
%       meval   maximum number of function evaluations
%       np      number of pareto points of the final dataset
%       npGP    number of pareto points of GP model dataset
%       D       dimension of input space
%       O       dimension of output space

%% Initialize option structure
it = 1;
Opt = set_option_structure(Opt,X,Y);

%% Write initial text file for TSEMO_log
create_log_file(X,Y,Opt,f,lb,ub)

for i = 1:ceil(Opt.maxeval/Opt.NoOfBachSequential)
    tic;
    %% Scale Variables
    [Xnew,Ynew] = ScaleVariables(X,Y,lb,ub,Opt) ;

    %% Training of GP
    for j = 1:Opt.Gen.NoOfGPs
        Opt.GP(j).hyp = TrainingOfGP(Xnew,Ynew(:,j),Opt.GP(j));
    end

    %% Draw samples of GPs
    for j = 1:Opt.Gen.NoOfGPs
        Opt.Sample(j).f = posterior_sample(Xnew,Ynew(:,j),Opt.GP(j));
    end

    %% Determine pareto front of function samples
    [Sample_pareto,Sample_xpareto,Sample_nadir] = Find_sample_pareto(Opt,i);
    Opt.warmstart_pareto = Sample_xpareto;

    %% Sampling point at maximum hypervolume improvement
    [index,hv_imp] = hypervolume_improvement_index(Ynew,Sample_nadir,Sample_pareto,Opt);

    xNew = Sample_xpareto(index,:);
    Xnew = [Xnew;xNew];

    for j = 1:Opt.Gen.NoOfInputDim
        xnewtrue(:,j) = xNew(:,j)*(ub(j)-lb(j)) + lb(j);
    end

    for l = 1 : size(xnewtrue,1)
        ytrue(l,:) = f(xnewtrue(l,:));
    end

    X = [X;xnewtrue];
    Y = [Y;ytrue];
    Ynew = zeros(size(Y,1),Opt.Gen.NoOfGPs);
    for j = 1:Opt.Gen.NoOfGPs
        Ynew(:,j) = (Y(:,j) - mean(Y(:,j)))/std(Y(:,j));
    end

    front = paretofront(Y);
    Xpareto = X(front,:);
    Ypareto = Y(front,:);

    %% Update log each iterations
    update_log_file(it,hv_imp,toc,xnewtrue,ytrue,Opt,Y,ub,lb)

    %% Display
    if it == 1
        fprintf('%10s %10s %10s \n','Iteration', 'HypImp', 'Time(s)');
    end
    fprintf('%10d %10.4g %10.3g \n', it, hv_imp, toc);

    it = it+1;

    %% Determine final hyperparameter values for analysis
    if i == ceil(Opt.maxeval/Opt.NoOfBachSequential)
        for j = 1:Opt.Gen.NoOfGPs
            Opt.GP(j).hyp = TrainingOfGP(Xnew,Ynew(:,j),Opt.GP(j));
        end

        hypf = zeros(Opt.Gen.NoOfInputDim+2,Opt.Gen.NoOfGPs);
        for j = 1:Opt.Gen.NoOfGPs
            covhyp = exp(Opt.GP(j).hyp.cov);
            hypf(:,j) = [covhyp(1:Opt.Gen.NoOfInputDim).*(ub-lb)';covhyp(end)*std(Y(:,j));exp(Opt.GP(j).hyp.lik)*std(Y(:,j))];
        end

        %% Obtain Pareto front from spectral Gaussian process model
        for j = 1:Opt.Gen.NoOfGPs
            [Opt.Mean(j).f,Opt.Mean(j).varf] = mean_sample(Xnew,Ynew(:,j),Opt.GP(j));
        end

        [Mean_pareto,Mean_xpareto] = Find_mean_pareto(Opt);

        for j = 1:Opt.Gen.NoOfInputDim
            XParetoGP(:,j) = Mean_xpareto(:,j)*(ub(j)-lb(j)) + lb(j);
        end

        for j = 1:Opt.Gen.NoOfGPs
            YParetoGP(:,j) = Mean_pareto(:,j)*std(Y(:,j)) + mean(Y(:,j));
        end

        for j = 1:Opt.Gen.NoOfGPs
            for k = 1:Opt.pop
                YParetoGPstd(k,j) = sqrt(Opt.Mean(j).varf(Mean_xpareto(k,:)))*std(Y(:,j));
            end
        end

        %% Update log with final results
        final_log_update(Xpareto,Ypareto,X,Y,XParetoGP,YParetoGP,hypf,Opt)
    end
end
return

function create_log_file(X,Y,Opt,f,lb,ub)
try
    function_name = func2str(f);
    string1 = '';
    string2 = {};
    string3 = '';
    string4 = '';
    string5 = {};
    string6 = '';
    string7 = '';
    for i = 1:size(X,2)
        string1 = strcat(string1,'%8.4f ');
        string2 = {string2{:},strcat('x',num2str(i))};
        string3 = strcat(string3,'%+8s ');
    end
    for i = 1:size(Y,2)
        string4 = strcat(string4,'%8.4f ');
        string5 = {string5{:},strcat('f',num2str(i))};
        string6 = strcat(string6,'%+8s ');
        string7 = strcat(string7,'%8d ');
    end

    TSEMO_log = fopen( 'TSEMO_log.txt', 'w');
    fprintf(TSEMO_log,'\n %s %s \n', 'TSEMO log file created on',date);
    fprintf(TSEMO_log,'\n %s','This file shows the initial specifications of TSEMO and logs the output.');
    fprintf(TSEMO_log,'\n %s', '¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯');

    fprintf(TSEMO_log,'\n %s \n', 'License information');
    fprintf(TSEMO_log,'\n %s \n', 'BSD 2-Clause License');
    fprintf(TSEMO_log,'\n %s', 'Copyright (c) 2017, Eric Bradford, Artur M. Schweidtmann and Alexei Lapkin');
    fprintf(TSEMO_log,'\n %s \n', 'All rights reserved.');
    fprintf(TSEMO_log,'\n %s', 'Redistribution and use in source and binary forms, with or without');
    fprintf(TSEMO_log,'\n %s \n', 'modification, are permitted provided that the following conditions are met:');
    fprintf(TSEMO_log,'\n %s   ', '*Redistributions of source code must retain the above copyright notice, this');
    fprintf(TSEMO_log,'\n %s \n', ' list of conditions and the following disclaimer.');
    fprintf(TSEMO_log,'\n %s   ', '*Redistributions in binary form must reproduce the above copyright notice,');
    fprintf(TSEMO_log,'\n %s   ', ' this list of conditions and the following disclaimer in the documentation');
    fprintf(TSEMO_log,'\n %s \n', ' and/or other materials provided with the distribution.');
    fprintf(TSEMO_log,'\n %s', '¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯');

    fprintf(TSEMO_log,'\n %s \n', 'Problem specifications');
    fprintf(TSEMO_log,'\n %s %s \n', 'Function used:  ',function_name);
    fprintf(TSEMO_log,'\n %s %d', 'Number of inputs:  ',size(X,2));
    fprintf(TSEMO_log,'\n %s %d \n', 'Number of outputs: ',size(Y,2));
    fprintf(TSEMO_log,'\n %s', 'Lower bounds of decision variables:');
    fprintf(TSEMO_log,strcat('\n',string3,'\n'),string2{:});
    fprintf(TSEMO_log,string1,lb);
    fprintf(TSEMO_log,'\n \n %s', 'Upper bounds of decision variables:');
    fprintf(TSEMO_log,strcat('\n',string3,'\n'),string2{:});
    fprintf(TSEMO_log,string1,ub);
    fprintf(TSEMO_log,'\n %s', '¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯');

    fprintf(TSEMO_log,'\n %s \n', 'Algorithm options');
    fprintf(TSEMO_log,'\n %s %d', 'Maximum number of function evaluations: ',Opt.maxeval);
    fprintf(TSEMO_log,'\n %s %d', 'Sample batch size:                      ',Opt.NoOfBachSequential);
    fprintf(TSEMO_log,'\n %s %d \n', 'Number of algorithm iterations:         ',ceil(Opt.maxeval/Opt.NoOfBachSequential));
    fprintf(TSEMO_log,'\n %s %d', 'Genetic algorithm population size:       ',Opt.pop);
    fprintf(TSEMO_log,'\n %s %d \n', 'Genetic algorithm number of generations: ',Opt.Generation);
    fprintf(TSEMO_log,strcat('\n','%s',string6,'\n'),'                                         ',string5{:});
    fprintf(TSEMO_log,strcat('%s',string7,'\n'), ' Number of spectral sampling points:     ',Opt.GP(1:size(Y,2)).nSpectralpoints);
    fprintf(TSEMO_log,strcat('%s',string7), ' Type of matern function:                ',Opt.GP(1:size(Y,2)).matern);
    fprintf(TSEMO_log,strcat('\n','%s',string7), ' Direct evaluations per input dimension: ',Opt.GP(1:size(Y,2)).fun_eval);
    fprintf(TSEMO_log,'\n %s', '¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯');

    fprintf(TSEMO_log,'\n %s \n', 'Initial data set');
    fprintf(TSEMO_log,'\n %s %d \n', 'Number of initial data points: ',size(X,1));
    fprintf(TSEMO_log,'\n %s', 'Initial input data matrix:');
    fprintf(TSEMO_log,strcat('\n',string3,'\n'),string2{:});
    fprintf(TSEMO_log,strcat(string1,'\n'),X');
    fprintf(TSEMO_log,'\n %s', 'Initial output data matrix:');
    fprintf(TSEMO_log,strcat('\n',string6,'\n'),string5{:});
    fprintf(TSEMO_log,strcat(string4,'\n'),Y');
    fprintf(TSEMO_log,'\n %s', '¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯');
    fclose(TSEMO_log) ;
catch
    warning('There was an error when writing the log file. Maybe the file was currently open in another program. The algorithm continues to run but some parts of the log file are maybe not correct.') ;
    fclose('all') ;
end
return

function update_log_file(it,hv_imp,toc,xnewtrue,ytrue,Opt,Y,ub,lb)
try
    string1 = '';
    string2 = {};
    string3 = '';
    string4 = '';
    string5 = {};
    string6 = '';
    string7 = '';
    string8 = {};
    for i = 1:Opt.Gen.NoOfInputDim
        string1 = strcat(string1,'%8.4f ');
        string2 = {string2{:},strcat('x',num2str(i))};
        string3 = strcat(string3,'%+8s ');
        string8 = {string8{:},strcat('lambda',num2str(i))};
    end
    string8 = {string8{:},'sigmaf'};
    string8 = {string8{:},'sigman'};
    for i = 1:Opt.Gen.NoOfGPs
        string4     = strcat(string4,'%8.4f ');
        string5     = {string5{:},strcat('f',num2str(i))};
        string6     = strcat(string6,'%+8s ');
        string7     = strcat(string7,'%8d ');
        hypcov      = exp(Opt.GP(i).hyp.cov);
        hypmat(:,i) = [hypcov(1:end-1).*(1./(ub-lb))';hypcov(end)*std(Y(:,i));exp(Opt.GP(i).hyp.lik)*std(Y(:,i))];
    end

    TSEMO_log = fopen( 'TSEMO_log.txt', 'a');
    fprintf(TSEMO_log,'\n %s %d \n', 'Algorithm iteration',it);
    fprintf(TSEMO_log,'\n %s %8.4f', 'Predicted hypervolume improvement: ',hv_imp);
    fprintf(TSEMO_log,'\n %s %8.4f \n', 'Time taken: ',toc);
    fprintf(TSEMO_log,'\n %s', 'Proposed evaluation point(s): ');
    fprintf(TSEMO_log,strcat('\n',string3,'\n'),string2{:});
    fprintf(TSEMO_log,strcat(string1,'\n'),xnewtrue');
    fprintf(TSEMO_log,'\n %s', 'Corresponding observation(s): ');
    fprintf(TSEMO_log,strcat('\n',string6,'\n'),string5{:});
    fprintf(TSEMO_log,strcat(string4,'\n'),ytrue);
    fprintf(TSEMO_log,'\n %s', 'Current hyperparameter values: ');
    fprintf(TSEMO_log,strcat('\n','%+16s',string6,'\n'),'Hyperparameter',string5{:});
    for i = 1:Opt.Gen.NoOfInputDim+2
        fprintf(TSEMO_log,strcat('%+16s',string4,'\n'),string8{i},hypmat(i,:));
    end
    fprintf(TSEMO_log,'\n %s', '¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯');
    fclose(TSEMO_log) ;
catch
    warning('There was an error when writing the log file. Maybe the file was currently open in another program. The algorithm continues to run but some parts of the log file are maybe not correct.') ;
    fclose('all') ;
end
return

function final_log_update(Xpareto,Ypareto,X,Y,XParetoGP,YParetoGP,hypf,Opt)
try
    string1 = '';
    string2 = {};
    string3 = '';
    string4 = '';
    string5 = {};
    string6 = '';
    string7 = '';
    string8 = {};
    for i = 1:Opt.Gen.NoOfInputDim
        string1 = strcat(string1,'%8.4f ');
        string2 = {string2{:},strcat('x',num2str(i))};
        string3 = strcat(string3,'%+8s ');
        string8 = {string8{:},strcat('lambda',num2str(i))};
    end
    string8 = {string8{:},'sigmaf'};
    string8 = {string8{:},'sigman'};
    for i = 1:Opt.Gen.NoOfGPs
        string4     = strcat(string4,'%8.4f ');
        string5     = {string5{:},strcat('f',num2str(i))};
        string6     = strcat(string6,'%+8s ');
        string7     = strcat(string7,'%8d ');
    end

    TSEMO_log = fopen( 'TSEMO_log.txt', 'a');
    fprintf(TSEMO_log,'\n %s \n', 'Final algorithm output');
    fprintf(TSEMO_log,'\n %s', 'Final input data matrix:');
    fprintf(TSEMO_log,strcat('\n',string3,'\n'),string2{:});
    fprintf(TSEMO_log,strcat(string1,'\n'),X');
    fprintf(TSEMO_log,'\n %s', 'Final output data matrix:');
    fprintf(TSEMO_log,strcat('\n',string6,'\n'),string5{:});
    fprintf(TSEMO_log,strcat(string4,'\n'),Y');
    fprintf(TSEMO_log,'\n %s', 'Input data matrix of corresponding Pareto front:');
    fprintf(TSEMO_log,strcat('\n',string3,'\n'),string2{:});
    fprintf(TSEMO_log,strcat(string1,'\n'),Xpareto');
    fprintf(TSEMO_log,'\n %s', 'Output data matrix of corresponding Pareto front:');
    fprintf(TSEMO_log,strcat('\n',string6,'\n'),string5{:});
    fprintf(TSEMO_log,strcat(string4,'\n'),Ypareto');
    fprintf(TSEMO_log,'\n %s', 'Input data matrix of Pareto front of final Gaussian process model:');
    fprintf(TSEMO_log,strcat('\n',string3,'\n'),string2{:});
    fprintf(TSEMO_log,strcat(string1,'\n'),XParetoGP');
    fprintf(TSEMO_log,'\n %s', 'Output data matrix of Pareto front of final Gaussian process model:');
    fprintf(TSEMO_log,strcat('\n',string6,'\n'),string5{:});
    fprintf(TSEMO_log,strcat(string4,'\n'),YParetoGP');
    fprintf(TSEMO_log,'\n %s', 'Final hyperparameter values: ');
    fprintf(TSEMO_log,strcat('\n','%+16s',string6,'\n'),'Hyperparameter',string5{:});
    for i = 1:Opt.Gen.NoOfInputDim+2
        fprintf(TSEMO_log,strcat('%+16s',string4,'\n'),string8{i},hypf(i,:));
    end
    fclose(TSEMO_log) ;
catch
    warning('There was an error when writing the log file. Maybe the file was currently open in another program. The algorithm continues to run but some parts of the log file are maybe not correct.') ;
    fclose('all') ;
end
return

function [Xnew,Ynew] = ScaleVariables(X,Y,lb,ub,Opt)
% Copyright (c) by Eric Bradford, Artur M. Schweidtmann and Alexei Lapkin, 2017-13-12.

%% Scales input and output variabels
Xnew = zeros(size(X)) ; % scaled inputs
Ynew = zeros(size(Y)) ; % scaled outputs

%% Scale input variables to [0,1]
for i = 1 : size(X,2)
    Xnew(:,i) = (X(:,i)-lb(i)) / (ub(i)-lb(i)) ;
end

%% Scale output variables to zero mean and unit variance
MeanOfOutputs = zeros(Opt.Gen.NoOfGPs,1) ;
stdOfOutputs = zeros(Opt.Gen.NoOfGPs,1) ;
for i = 1 : size(Y,2)
    MeanOfOutputs(i) = mean(Y(:,i)); % calculate mean
    stdOfOutputs(i) = std(Y(:,i)) ; % calculate standard deviation
    Ynew(:,i) = (Y(:,i) - MeanOfOutputs(i)) / stdOfOutputs(i) ; % scale outputs
end
return

function [OptGPhyp] = TrainingOfGP(Xnew,Ynew,OptGP)
% Copyright (c) by Eric Bradford, Artur M. Schweidtmann and Alexei Lapkin, 2017-13-12.

% Function which minimizes the neg-loglikelihood to find hyperparameters
%% Initialize variables
Opt.GP = OptGP ;
[n,D] = size(Xnew) ;

%% Set initial hyperparameters
h1 = Opt.GP.h1; % number of hyperparameters from covariance
h2 = Opt.GP.h2; % number of hyperparameters from likelihood

%% Calculation of squared-distance matrix
a = Xnew' ;
K_M = zeros(n,n*D) ;
for i = 1:D
    K_M(:,(i-1)*n+1:i*n) = sqdist(a(i,:),a(i,:)) ;
end

%% Minimize log-negative likeliehood
% Objective Function
obj_fun.f = @(hypVar) NLikelihood(hypVar,Xnew,Ynew,K_M,Opt.GP);

% Define bounds
lb              = ones(h1+h2,1) *  log(sqrt(10^(-3))) ;  % see Jones paper
ub              = ones(h1+h2,1) *  log(sqrt(10^(3)))  ;  % see Jones paper
lb(h1+h2)    = -6;
ub(h1+h2)    = Opt.GP.noiselimit;
bounds = [lb,ub];
opts.maxevals = Opt.GP.fun_eval*(h1+h2);
opts.maxits =  100000*(h1+h2);
opts.maxdeep = 100000*(h1+h2);
opts.showits = 0;

% Defintion of options for global search
[~,x0] = Direct(obj_fun,bounds,opts);

% Defintion of options for fmincon solver
LSoptions.Algorithm = 'interior-point';
LSoptions.DerivativeCheck = 'off';
LSoptions.TolCon = 1e-12;
LSoptions.Display = 'off';
LSoptions.Hessian = 'bfgs';
LSoptions.TolFun = 1e-12;
LSoptions.PlotFcns = [];
LSoptions.GradConstr = 'off';
LSoptions.GradObj = 'on';
LSoptions.TolX = 1e-14;
LSoptions.UseParallel = 0;

% Solve optimization problem
hypResult = fmincon(obj_fun.f,x0,[],[],[],[],lb,ub,[],LSoptions);

%% Return optimal hyperparameters
Opt.GP.hyp.cov  = hypResult(1:h1);
Opt.GP.hyp.lik  = hypResult(h1+1:h1+h2);
OptGPhyp = Opt.GP.hyp ;
return

function [NLL,dNLL] = NLikelihood(hypVar, Xnew, Ynew, K_M, OptGP)
% Copyright (c) by Eric Bradford, Artur M. Schweidtmann and Alexei Lapkin, 2017-13-12.

% Calculates the log-negative likelihood
%% Initialize variables
[n,D]       = size(Xnew) ;
Opt.GP      = OptGP ;
if Opt.GP.cov ~= inf
    d           = Opt.GP.cov;    % type of Martern
else
    d           = 1;
end
h1          = Opt.GP.h1 ;    % number of hyperparameters from covariance
h2          = Opt.GP.h2;     % number of hyperparameters from likelihood
hyp.cov     = hypVar(1:h1);
hyp.lik     = hypVar(h1+1:h1+h2);
ell         = exp(hyp.cov(1:D));
sf2         = exp(2*hypVar(D+1));
K           = zeros(n,n) ;

%% Calculate covariance matrix
for i = 1:D
    K = K_M(:,(i-1)*n+1:i*n) * d/ell(i)^2 + K;
end

if Opt.GP.cov ~= inf
    sqrtK = sqrt(K) ;
    expnK = exp(-sqrtK) ;
else
    expnK = exp(-1/2*K);
    sqrtK = [];
end

if      Opt.GP.cov == 3, t = sqrtK ; m =  (1 + t).*expnK;
elseif  Opt.GP.cov == 1,             m =  expnK;
elseif  Opt.GP.cov == 5, t = sqrtK ; m =  (1 + t.*(1+t/3)).*expnK;
elseif  Opt.GP.cov == inf,           m  = expnK;
end
K = sf2*m;
K =  K + eye(n)*exp(hyp.lik*2) ;
K = (K+K')/2 ; % This guarantees a symmetric matrix

%% Calculate inverse of covariance matrix
try
    CH = chol(K) ;
    invK = CH\(CH'\eye(n));
catch
    CH = chol(K+eye(n)*1e-4);
    invK = CH\(CH'\eye(n));
    warning('Covariance matrix in Nlikelihood is not positive semi-definite')
end

%% Calculate determinant of covariance matrix
logDetK = 2*sum(log(abs(diag(CH)))) ;

%% Calculate hyperperpriors
logprior = 0 ;
dlogpriorcov = zeros(1,h1) ;
for i = 1 : h1
    [A, dlogpriorcov(i)] =  priorGauss(Opt.GP.priorcov(1),Opt.GP.priorcov(2), hyp.cov(i) ) ;
    logprior = logprior + A ;
end

dlogpriorlik = zeros(1,h2);
for i = 1 : h2
    [A, dlogpriorlik(i)] =  priorGauss(Opt.GP.priorlik(1),Opt.GP.priorlik(2), hyp.lik(i) ) ;
    logprior = logprior + A ;
end

%% Calculate negative log-likeliehood
NLL = n/2*log(2*pi) + 1/2 * logDetK + 1/2 * Ynew'*invK*Ynew - logprior ;

%% Gradient calculation
if nargout == 2 % do only if No of output variable is 2 (if necessary)
    dsq_M = zeros(n,n*D) ;
    for i = 1 : D
        dsq_M(:,(i-1)*n+1:i*n)  = K_M(:,(i-1)*n+1:i*n) * (d)/ell(i)^2 ;
    end

    c = invK*Ynew;
    for i = 1 : h1
        dK = covMaternanisotropic(Opt.GP.cov,hyp.cov, sqrtK, expnK, dsq_M, Xnew, [], i);
        b = invK* dK ;
        dNLL_f.cov(i) = 1/2*trace(b) - 1/2*Ynew'*b*c ;
    end

    for i = 1 : h2
        dK = 2 * exp(hyp.lik(i)) * eye(n) * exp(hyp.lik(i));
        b = invK* dK ;
        dNLL_f.lik(i) = 1/2*trace(b) - 1/2*Ynew'*b*c ;
    end

    dNLL = [dNLL_f.cov';dNLL_f.lik'] - [dlogpriorcov';dlogpriorlik'];
end

return

function f = posterior_sample(Xnew,Ynew,Opt)
% Copyright (c) by Eric Bradford, Artur M. Schweidtmann and Alexei Lapkin, 2018-08-07.

% extration of variables from problem structure
nSpectralpoints = Opt.nSpectralpoints;
[n,D] = size(Xnew);
ell = exp(Opt.hyp.cov(1:D));
sf2 = exp(2*Opt.hyp.cov(D+1));
sn2 = exp(2*Opt.hyp.lik);

% Sampling of W and b
sW1  = lhsdesign(nSpectralpoints,D,'criterion','none');
sW2  = lhsdesign(nSpectralpoints,D,'criterion','none');
if Opt.cov ~= inf
    W = repmat(1./(ell)', nSpectralpoints, 1).*norminv(sW1).*sqrt(Opt.cov./chi2inv(sW2,Opt.cov));
else
    W = randn(nSpectralpoints,D) .* repmat(1./ell', nSpectralpoints, 1);
end

b = 2*pi*lhsdesign(nSpectralpoints,1,'criterion','none');

% Calculation of phi
phi = sqrt(2 * sf2 / nSpectralpoints) * cos(W * Xnew' + repmat(b, 1, n));

% Sampling of theta according to phi
A = phi * phi' + sn2 * eye(nSpectralpoints);

invA      = invChol(A);
mu_theta  = invA*phi*Ynew;
cov_theta = sn2*invA;
cov_theta = (cov_theta+cov_theta')/2;
theta     = mvnrnd(mu_theta,cov_theta)';

% Posterior sample (function) according to theta
f = @(x) (theta' * sqrt(2 * sf2 / nSpectralpoints) * cos(W * x' + repmat(b,1,size(x,1))))';
return

function [f,varf] = mean_sample(Xnew,Ynew,Opt)
% Copyright (c) by Eric Bradford, Artur M. Schweidtmann and Alexei Lapkin, 2020-23-05.

% extration of variables from problem structure
nSpectralpoints = Opt.nSpectralpoints;
[n,D] = size(Xnew);
ell = exp(Opt.hyp.cov(1:D));
sf2 = exp(2*Opt.hyp.cov(D+1));
sn2 = exp(2*Opt.hyp.lik);

% Sampling of W and b
sW1  = lhsdesign(nSpectralpoints,D,'criterion','none');
sW2  = lhsdesign(nSpectralpoints,D,'criterion','none');
if Opt.cov ~= inf
    W = repmat(1./(ell)', nSpectralpoints, 1).*norminv(sW1).*sqrt(Opt.cov./chi2inv(sW2,Opt.cov));
else
    W = randn(nSpectralpoints,D) .* repmat(1./ell', nSpectralpoints, 1);
end

b = 2*pi*lhsdesign(nSpectralpoints,1,'criterion','none');

% Calculation of phi
phi = sqrt(2 * sf2 / nSpectralpoints) * cos(W * Xnew' + repmat(b, 1, n));

% Sampling of theta according to phi
A = phi * phi' + sn2 * eye(nSpectralpoints);

invA      = invChol(A);
mu_theta  = invA*phi*Ynew;

% Mean approximation (function) according to theta
phi_x = @(x) sqrt(2 * sf2 / nSpectralpoints) * cos(W * x' + repmat(b,1,size(x,1)));
f     = @(x) (mu_theta' * phi_x(x))';
varf  = @(x) sn2 + sn2*phi_x(x)'*invA*phi_x(x);
return

function v = hypervolumemonte(P,r,N)
% Copyright (c) 2009, Yi Cao
% All rights reserved.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are
% met:
%
% * Redistributions of source code must retain the above copyright
% notice, this list of conditions and the following disclaimer.
% * Redistributions in binary form must reproduce the above copyright
% notice, this list of conditions and the following disclaimer in
% the documentation and/or other materials provided with the distribution
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
% POSSIBILITY OF SUCH DAMAGE.

% HYPERVOUME    Hypervolume indicator as a measure of Pareto front estimate.
%   V = HYPERVOLUME(P,R,N) returns an estimation of the hypervoulme (in
%   percentage) dominated by the approximated Pareto front set P (n by d)
%   and bounded by the reference point R (1 by d). The estimation is doen
%   through N (default is 1000) uniformly distributed random points within
%   the bounded hyper-cuboid.
%
%   V = HYPERVOLUMN(P,R,C) uses the test points specified in C (N by d).
%
% See also: paretofront, paretoGroup

% Version 1.0 by Yi Cao at Cranfield University on 20 April 2008

% Example
%{
% an random exmaple
F=(randn(100,3)+5).^2;
% upper bound of the data set
r=max(F);
% Approximation of Pareto set
P=paretofront(F);
% Hypervolume
v=hypervolume(F(P,:),r,100000);
%}
% https://se.mathworks.com/matlabcentral/fileexchange/19651-hypervolume-indicator

% Check input and output
error(nargchk(2,3,nargin));
error(nargoutchk(0,1,nargout));

P=P*diag(1./r);
[n,d]=size(P);
if nargin<3
    N=1000;
end
if ~isscalar(N)
    C=N;
    N=size(C,1);
else
    C=rand(N,d);
end

fDominated=false(N,1);
lB=min(P);
fcheck=all(bsxfun(@gt, C, lB),2);

for k=1:n
    if any(fcheck)
        f=all(bsxfun(@gt, C(fcheck,:), P(k,:)),2);
        fDominated(fcheck)=f;
        fcheck(fcheck)=~f;
    end
end

v=sum(fDominated)/N;
return

function K = covMaternanisotropic(d, hyp, sqrtK,expnK, dsq_M, x, z, i)
% Copyright (c) 2005-2017 Carl Edward Rasmussen & Hannes Nickisch. All rights reserved.
%
% Redistribution and use in source and binary forms, with or without modification,
% are permitted provided that the following conditions are met:
%    1. Redistributions of source code must retain the above copyright notice,
%       this list of conditions and the following disclaimer.
%    2. Redistributions in binary form must reproduce the above copyright notice,
%      this list of conditions and the following disclaimer in the documentation
%      and/or other materials provided with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY CARL EDWARD RASMUSSEN & HANNES NICKISCH ``AS IS''
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
% WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
% IN NO EVENT SHALL CARL EDWARD RASMUSSEN & HANNES NICKISCH OR CONTRIBUTORS BE LIABLE
% FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
% DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
% LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
% ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
% (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
% EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
%
% The views and conclusions contained in the software and documentation
% are those of the authors and should not be interpreted as representing official policies,
% either expressed or implied, of Carl Edward Rasmussen & Hannes Nickisch.
%
% The code and associated documentation is available from http://gaussianprocess.org/gpml/code.</pre>

[n,D] = size(x);
sf2 = exp(2*hyp(D+1));

if nargin<7                                                        % covariances
    if      d == 3, t = sqrtK ; m =  (1 + t).*expnK;
    elseif  d == 1,             m =  expnK;
    elseif  d == 5, t = sqrtK ; m =  (1 + t.*(1+t/3)).*expnK;
    elseif  d == inf, m = expnK;
    end
    K = sf2*m;
else                                                               % derivatives
    if i<=D                                               % length scale parameter
        Ki = dsq_M(:,(i-1)*n+1:i*n) ;
        if     d == 3,             dm = expnK;
        elseif d == 1, t = sqrtK ; dm = (1./t).*expnK;
        elseif d == 5, t = sqrtK ; dm = ((1+t)/3).*expnK;
        elseif d == inf; dm = -1/2*expnK;
        end

        K = sf2*dm.*Ki;
        K(Ki<1e-12) = 0;                                    % fix limit case for d=1
    elseif i==D+1                                            % magnitude parameter
        if      d == 3, t = sqrtK ; m =  (1 + t).*expnK;
        elseif  d == 1,             m =  expnK;
        elseif  d == 5, t = sqrtK ; m =  (1 + t.*(1+t/3)).*expnK;
        elseif  d == inf,           m = expnK;
        end
        K = 2*sf2*m;
    end
end
return

function D = sqdist(X1, X2)
% Copyright (c) 2016, Mo Chen
% All rights reserved.
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are
% met:
%
% * Redistributions of source code must retain the above copyright
% notice, this list of conditions and the following disclaimer.
% * Redistributions in binary form must reproduce the above copyright
% notice, this list of conditions and the following disclaimer in
% the documentation and/or other materials provided with the distribution
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
% POSSIBILITY OF SUCH DAMAGE.

% https://se.mathworks.com/matlabcentral/fileexchange/24599-pairwise-distance-matrix
% Pairwise square Euclidean distance between two sample sets
% Input:
%   X1, X2: dxn1 dxn2 sample matrices
% Output:
%   D: n1 x n2 square Euclidean distance matrix
% Written by Mo Chen (sth4nth@gmail.com).

D = bsxfun(@plus,dot(X2,X2,1),dot(X1,X1,1)')-2*(X1'*X2);
D(D<0) = 0 ; % check due to numerical errors
return

function [lp,dlp] = priorGauss(mu,s2,x)
% Copyright (c) 2005-2017 Carl Edward Rasmussen & Hannes Nickisch. All rights reserved.
%
% Redistribution and use in source and binary forms, with or without modification,
% are permitted provided that the following conditions are met:
%    1. Redistributions of source code must retain the above copyright notice,
%       this list of conditions and the following disclaimer.
%    2. Redistributions in binary form must reproduce the above copyright notice,
%      this list of conditions and the following disclaimer in the documentation
%      and/or other materials provided with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY CARL EDWARD RASMUSSEN & HANNES NICKISCH ``AS IS''
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
% WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
% IN NO EVENT SHALL CARL EDWARD RASMUSSEN & HANNES NICKISCH OR CONTRIBUTORS BE LIABLE
% FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
% DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
% LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
% ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
% (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
% EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
%
% The views and conclusions contained in the software and documentation
% are those of the authors and should not be interpreted as representing official policies,
% either expressed or implied, of Carl Edward Rasmussen & Hannes Nickisch.
%
% The code and associated documentation is available from http://gaussianprocess.org/gpml/code.</pre>

lp  = -(x-mu).^2/(2*s2) - log(2*pi*s2)/2;
dlp = -(x-mu)/s2;
return

function [f,const] = pareto_objective(x,OptSample)
% Copyright (c) by Eric Bradford, Artur M. Schweidtmann and Alexei Lapkin, 2017-13-12.

Opt.Sample = OptSample;
f = zeros(size(x,1),size(Opt.Sample,2));
for i = 1:size(Opt.Sample,2)
    f(:,i) = Opt.Sample(i).f(x);
end
const = [];
return

function [Sample_pareto,Sample_xpareto,Sample_nadir] = Find_sample_pareto(Opt,it)
% Copyright (c) by Eric Bradford, Artur M. Schweidtmann and Alexei Lapkin, 2017-13-12.

D = Opt.Gen.NoOfInputDim;               % Number of input dimensions
options = nsgaopt();                    % create default options structure
options.popsize = Opt.pop;              % populaion size
options.maxGen  = Opt.Generation;       % max generation
options.numObj = Opt.Gen.NoOfGPs;       % number of objectives
options.numVar = D;                     % number of design variables
options.numCons = 0;                    % number of constraints
options.outputfuns = [];                % saving pop
options.lb = zeros(1,D);                % lower bound of x
options.ub = ones(1,D);                 % upper bound of x
options.objfun = @(x) pareto_objective(x,Opt.Sample);        % objective function handle
options.useParallel = 'no';             % parallel computation is non-essential here
if it > 1
options.initfun = {@(opt,pop) initpop_new(opt,pop,Opt)};
end
[~,result] = evalc('nsga2(options);');  % begin the optimization!

Sample_xpareto = zeros(Opt.pop,D);
Sample_pareto = zeros(Opt.pop,Opt.Gen.NoOfGPs);
result = result.pops(Opt.Generation,:);

for k = 1:Opt.pop
    Sample_xpareto(k,:) = result(k).var;
    Sample_pareto(k,:) = result(k).obj;
end

for k = 1:Opt.Gen.NoOfGPs
    Sample_nadir(k) = max(Sample_pareto(:,k));
end
return

function pop = initpop_new(opt,pop,Opt)
% Copyright (c) by Eric Bradford, Artur M. Schweidtmann and Alexei Lapkin, 2020-22-05.

for p = 1:opt.popsize
    pop(p).var = Opt.warmstart_pareto(p,:);
end

return

function [Mean_pareto,Mean_xpareto] = Find_mean_pareto(Opt)
% Copyright (c) by Eric Bradford, Artur M. Schweidtmann and Alexei Lapkin, 2017-13-12.

D = Opt.Gen.NoOfInputDim;               % Number of input dimensions
options = nsgaopt();                    % create default options structure
options.popsize = Opt.pop;              % populaion size
options.maxGen  = Opt.Generation;       % max generation
options.numObj = Opt.Gen.NoOfGPs;       % number of objectives
options.numVar = D;                     % number of design variables
options.numCons = 0;                    % number of constraints
options.outputfuns = [];                % saving pop
options.lb = zeros(1,D);                % lower bound of x
options.ub = ones(1,D);                 % upper bound of x
options.objfun = @(x) pareto_objective(x,Opt.Mean); % objective function handle
options.useParallel = 'no';             % parallel computation is non-essential here
[~,result] = evalc('nsga2(options);');  % begin the optimization!

Mean_xpareto = zeros(Opt.pop,D);
Mean_pareto  = zeros(Opt.pop,Opt.Gen.NoOfGPs);
result       = result.pops(Opt.Generation,:);

for k = 1:Opt.pop
    Mean_xpareto(k,:) = result(k).var;
    Mean_pareto(k,:) = result(k).obj;
end

return

function hv = hypervolume_2D(Yfront,r)
% The mex-file used follows the description of:
% 'M. Emmerich, K. Yang, A. Deutz, H. Wang and C. M. Fonseca. A
% Multicriteria Generalization of Bayesian Global Optimization.'
% Group website: http://liacs.leidenuniv.nl/~csmoda/index.php?page=code

AYfront = remove_points_above_reference(Yfront,r);
if isempty(AYfront)
    hv = 0;
else
    normvec = min(AYfront,[],1);
    A =  AYfront-repmat(normvec,size(AYfront,1),1);
    A =  A * diag(1./(r-normvec));
    A = -A + ones(size(A));
    A = sortrows(A,2);
    hyp_percentage = hypervolume2D(A,[0,0]);
    hv = prod(r-normvec)*hyp_percentage;
end
return

function hv = hypervolume_3D(Yfront,r)
% The mex-file used follows the description of:
% 'K. Yang, M. Emmerich, A. Deutz and C. M. Fonseca. A
% Computing 3-D Expected Hypervolume Improvement and Related Integrals in
% Asymptotically Optimal Time.'
% Group website: http://liacs.leidenuniv.nl/~csmoda/index.php?page=code

AYfront = remove_points_above_reference(Yfront,r);
if isempty(AYfront)
    hv = 0;
else
    normvec = min(AYfront,[],1);
    A = AYfront-repmat(normvec,size(AYfront,1),1) ;
    A = A *diag(1./(r-normvec));
    A = -A + ones(size(A)) ;
    A = sortrows(A,3);
    hyp_percentage = hypervolume3D(A,[0,0,0],[1,1,1]);
    hv = prod(r-normvec)*hyp_percentage;
end
return

function [index,hv_imp] = hypervolume_improvement_index(Ynew,Sample_nadir,Sample_pareto,Opt)
% Copyright (c) by Eric Bradford, Artur M. Schweidtmann and Alexei Lapkin, 2017-13-12.

r = Sample_nadir + 0.01*(max(Sample_pareto)-min(Sample_pareto));
index = [];

for i = 1 : Opt.NoOfBachSequential

    Yfront = Ynew(paretofront(Ynew),:);

    if size(Ynew,2) == 2
        hvY = hypervolume_2D(Yfront,r);

        for k = 1:size(Sample_pareto,1)
            A = [Ynew;Sample_pareto(k,:)];
            Afront = A(paretofront(A),:);
            hv = hypervolume_2D(Afront,r);
            hv_improvement(k) = hv-hvY;
        end

    elseif size(Ynew,2) == 3
        hvY = hypervolume_3D(Yfront,r);

        for k = 1:size(Sample_pareto,1)
            A = [Ynew;Sample_pareto(k,:)];
            Afront = A(paretofront(A),:);
            hv = hypervolume_3D(Afront,r);
            hv_improvement(k) = hv-hvY;
        end

    else
        AYfront = remove_points_above_reference(Yfront,r);
        normvec = min(AYfront,[],1);
        hyp_percentage = hypervolumemonte(AYfront-repmat(normvec,size(AYfront,1),1),r-normvec,3000);
        hvY = prod(r-normvec)*hyp_percentage;

        hv_improvement = zeros(size(Sample_pareto,1),1);
        for k = 1:size(Sample_pareto,1)
            B = [Ynew;Sample_pareto(k,:)];
            Bfront = B(paretofront(B),:);
            ABfront = remove_points_above_reference(Bfront,r);
            if isempty(ABfront)
                hv_improvement(k) = 0;
            else
                normvec = min(ABfront,[],1);
                hyp_percentage = hypervolumemonte(ABfront-repmat(normvec,size(ABfront,1),1),r-normvec,10000);
                hv = prod(r-normvec)*hyp_percentage;
                hv_improvement(k) = hv-hvY;
            end
        end
    end

    if i == 1
        hvY0 = hvY;
    end

    [~,Currentindex] = max(hv_improvement);
    Ynew = [Ynew;Sample_pareto(Currentindex,:)];
    index = [index;Currentindex];
end
hv_imp = hv_improvement(index(end))+hvY-hvY0;
return

function A = remove_points_above_reference(Afront,r)
% Copyright (c) by Eric Bradford, Artur M. Schweidtmann and Alexei Lapkin, 2017-13-12.

[A,~] = sortrows(Afront);
for p = 1:size(Afront,2)
    A = A(A(:,p)<=r(p),:);
end
return

function Opt = set_option_structure(Opt,X,Y)
% Copyright (c) by Eric Bradford, Artur M. Schweidtmann and Alexei Lapkin, 2017-13-12.

%% Extraction of input and output dimensions from X and Y
Opt.Gen.NoOfGPs = size(Y,2);           % Number of GPs to be trained
Opt.Gen.NoOfInputDim = size(X,2);      % Number of inputs dimensions
for i = 1 : Opt.Gen.NoOfGPs
    %% Set up GP options
    Opt.GP(i).cov = Opt.GP(i).matern;      % Matern type 1 / 3 / 5 / inf

    %% Set hyperpriors (MAP)
    Opt.GP(i).noiselimit = 0;                      % Upper bound on noise
    Opt.GP(i).var        = 10;                     % Upper bound on signal variance
    Opt.GP(i).h1         = Opt.Gen.NoOfInputDim+1; % Number of hyperparameters from covariance
    Opt.GP(i).h2         = 1;                      % Number of hyperparameters from likelihood

    %% priorGauss (mean, var)
    Opt.GP(i).priorlik  = [-6 ,Opt.GP(i).var];
    Opt.GP(i).priorcov  = [ 0 ,Opt.GP(i).var];

    %% Initial values for hyperparameters
    Opt.GP(i).hyp.cov       = zeros(1, Opt.GP(i).h1);
    Opt.GP(i).hyp.lik       = log(1e-2);
end
return


function uvHCurrCand = sc_get_prop_cand(uvHCurr, dx, dy)

% Get propagation neighbor

uvHCurrCand = zeros(size(uvHCurr));

Ht = [1, 0, dx; 0, 1, dy; 0 0 1];
numUvPixels = size(uvHCurr, 2);

% for i = 1: numUvPixels
%     H = reshape(uvHCurr(:,i), 3, 3);
%     Hc = H*Ht;
%     uvHCurrCand(:, i) = Hc(:);
% end
%
% uvHCurrCand = uvHCurrCand./repmat(uvHCurrCand(9,:), 9, 1);

end


function A = mycol2im(v,par1, par2)
% FUNCTION K = mycol2im(v,par1,par2)
%   AUTHOR:    Makoto Yamada
%              (myamada@ism.ac.jp)
%   DATE:       02/16/08
%
%  DESCRIPTION:
%
%   This function is for converting 1D signal to 2D signal.
%

count = 1;
A =zeros(par2(1), par2(2));

for ii = 1:par2(1)
    for jj = 1:par2(2)
        A(jj,ii) = v(count);
        count = count + 1;
    end
end


classdef TestImgHash
    %TestImgHash

    methods (Static)
        function test_1
            img1 = imread(fullfile(mexopencv.root(), 'test', 'tsukuba_l.png'));
            img2 = imread(fullfile(mexopencv.root(), 'test', 'tsukuba_r.png'));

            algs = {'AverageHash', 'BlockMeanHash', 'ColorMomentHash',
                'MarrHildrethHash', 'PHash', 'RadialVarianceHash'};
            for i=1:numel(algs)
                obj = cv.ImgHash(algs{i});
                hash1 = obj.compute(img1);
                hash2 = obj.compute(img2);
                validateattributes(hash1, {'numeric'}, {'vector'});
                validateattributes(hash1, {'numeric'}, {'vector'});
                val = obj.compare(hash1, hash2);
                validateattributes(val, {'numeric'}, {'scalar'});
            end
        end

        function test_2
            img = imread(fullfile(mexopencv.root(), 'test', 'tsukuba.png'));

            hash = cv.ImgHash.averageHash(img);
            validateattributes(hash, {'uint8'}, {'vector'}); % 'numel',8

            hash = cv.ImgHash.blockMeanHash(img, 'Mode','Mode1');
            validateattributes(hash, {'uint8'}, {'vector'}); % 'numel',32|121 (Mode0|Mode1)

            hash = cv.ImgHash.colorMomentHash(img);
            validateattributes(hash, {'double'}, {'vector'}); % 'numel',42

            hash = cv.ImgHash.marrHildrethHash(img, 'Alpha',2);
            validateattributes(hash, {'uint8'}, {'vector'}); % 'numel',72

            hash = cv.ImgHash.pHash(img);
            validateattributes(hash, {'uint8'}, {'vector'}); % 'numel',8

            hash = cv.ImgHash.radialVarianceHash(img, 'Sigma',1);
            validateattributes(hash, {'uint8'}, {'vector'}); % 'numel',40
        end
    end

end


function J=jacob_bvar(param)

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

J=zeros(length(param),1);
for jj = 1 : length(param)
    J(jj)= bound0prime(param(jj));
end
J=diag(J);

%
% function y = bound01prime(x);
% y = exp(x)/(1+exp(x))^2;
%
function y = bound0prime(x);
y = exp(x);


function n=nodesize(t,j)
%NODESIZE Node size.
%   S=NODESIZE(T) returns an N-element vector S of the sizes of the
%   nodes in the tree T, where N is the number of nodes.  The size of a
%   node is defined as the number of observations from the data used to
%   create the tree that satisfy the conditions for the node.
%
%   S=NODESIZE(T,J) takes an array J of node numbers and returns the
%   sizes for the specified nodes.
%
%   See also CLASSREGTREE, CLASSREGTREE/NUMNODES.

%   Copyright 2006 The MathWorks, Inc.
%   $Revision: 1.1.6.1 $  $Date: 2006/11/11 22:56:20 $

if nargin>=2 && ~validatenodes(t,j)
    error('stats:classregtree:nodesize:InvalidNode',...
          'J must be an array of node numbers or a logical array of the proper size.');
end

if nargin<2
    n = t.nodesize;
else
    n = t.nodesize(j,:);
end


function Istitched = vertical_stitch(I,height)
% I must be within 0 and 1
Istitched = I;
while size(Istitched,1)<height
    window_size = round(size(I,1)/4);
    up = I((size(I,1)-window_size+1):size(I,1),:);
    down = I(1:window_size,:);
    aup = zeros(window_size, size(up,2));
    adown = zeros(window_size, size(up,2));
    for i=1:window_size
        aup(i,:) = up(i,:)*(1-i/window_size);
        adown(i,:) = down(i,:)*i/window_size;
    end
    Istitched = [Istitched(1:(size(Istitched,1)-window_size),:);aup+adown;Istitched((window_size+1):size(Istitched,1),:)];
end

Istitched = Istitched(1:height,:);


function test_suite = test_grShortestPath
%TEST_GRSHORTESTPATH  Test case for the file grShortestPath
%
%   Test case for the file grShortestPath

%   Example
%   test_grShortestPath
%
%   See also
%
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2014-01-13,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2014 INRA - Cepia Software Platform.

test_suite = functiontests(localfunctions);

function test_Simple(testCase) %#ok<*DEFNU>
% propagate distance on a graph where a branch has smaller nodes but larger
% cumulated distance

% create graph structure
n0 = [10 10;20 10; 30 10;40 10;50 10;30 40];
e0 = [1 2;1 6; 2 3;3 4;4 5;5 6];
% weights of edges
l0 = [2 5 2 2 2 5]';
path = grShortestPath(n0, e0, 1, 5, l0);

expPath = [1 2 3 4 5]';
testCase.assertEqual(expPath, path, 'AbsTol', .01);


function value = ddot ( n, dx, incx, dy, incy )

%*****************************************************************************80
%
%% DDOT forms the dot product of two vectors.
%
%  Discussion:
%
%    This routine uses unrolled loops for increments equal to one.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 June 2005
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Jack Dongarra, Cleve Moler, Jim Bunch and Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979.
%
%    Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
%    Basic Linear Algebra Subprograms for Fortran Usage,
%    Algorithm 539,
%    ACM Transactions on Mathematical Software,
%    Volume 5, Number 3, September 1979, pages 308-323.
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vectors.
%
%    Input, real DX(*), the first vector.
%
%    Input, integer INCX, the increment between successive entries in DX.
%
%    Input, real DY(*), the second vector.
%
%    Input, integer INCY, the increment between successive entries in DY.
%
%    Output, real VALUE, the sum of the product of the
%    corresponding entries of DX and DY.
%
  ddot = 0.0;
  value = 0.0;

  if ( n <= 0 )
    return
  end
%
%  Code for unequal increments or equal increments
%  not equal to 1.
%
  if ( incx ~= 1 | incy ~= 1 )

    if ( 0 <= incx )
      ix = 1;
    else
      ix = ( - n + 1 ) * incx + 1;
    end

    if ( 0 <= incy )
      iy = 1;
    else
      iy = ( - n + 1 ) * incy + 1;
    end

    for i = 1 : n
      value = value + dx(ix) * dy(iy);
      ix = ix + incx;
      iy = iy + incy;
    end
%
%  Code for both increments equal to 1.
%
  else

    m = mod ( n, 5 );

    for i = 1 : m
      value = value + dx(i) * dy(i);
    end

    for i = m+1 : 5 : n

      value = value + dx(i  ) * dy(i  ) ...
                    + dx(i+1) * dy(i+1) ...
                    + dx(i+2) * dy(i+2) ...
                    + dx(i+3) * dy(i+3) ...
                    + dx(i+4) * dy(i+4);
    end

  end

  return
end


function [ ohyps ] = rotateHyps( hyps, R )
%ROTATEHYPS Rotate room layout hypothesis
%
numHyps = length(hyps);
ohyps = hyps; %repmat(hyps(1), [numHyps 1]);
for hid = 1:numHyps
    ohyps(hid).extLine = rotateLines(hyps(hid).extLine, R);
    ohyps(hid).srcLine = rotateLines(hyps(hid).srcLine, R);
    ohyps(hid).hCorner = rotatePoint(hyps(hid).hCorner, R);
end

end


function [eu,EUhm] = hmg2euc(hm)

% HMG2EUC Homogeneous to Euclidean point transform.
%   UMG2EUC(HM) is the Euclidean point corresponding to the homogeneous
%   point HM.
%
%   [e, E_hm] = HMG2EUC(HM) returns the Jacobian of the transformation.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

s = size(hm,1);
n = size(hm,2);

eu = hm(1:s-1,:)./repmat(hm(s,:),s-1,1);

if nargout > 1 % Jacobians
    if n == 1
        EUhm = [1/hm(s)*eye(s-1) -eu/hm(s)]; % this takes any dimension
    else
        error('Jacobians not computed for multiple points in HM')
    end
end

return

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function [PQ, PV, REF, NONE, BUS_I, BUS_TYPE, PD, QD, GS, BS, BUS_AREA, VM, ...
    VA, BASE_KV, ZONE, VMAX, VMIN, LAM_P, LAM_Q, MU_VMAX, MU_VMIN] = idx_bus
%IDX_BUS   Defines constants for named column indices to bus matrix.
%   Example:
%
%   [PQ, PV, REF, NONE, BUS_I, BUS_TYPE, PD, QD, GS, BS, BUS_AREA, VM, ...
%   VA, BASE_KV, ZONE, VMAX, VMIN, LAM_P, LAM_Q, MU_VMAX, MU_VMIN] = idx_bus;
%
%   Some examples of usage, after defining the constants using the line above,
%   are:
%
%    Pd = bus(4, PD);       % get the real power demand at bus 4
%    bus(:, VMIN) = 0.95;   % set the min voltage magnitude to 0.95 at all buses
%
%   The index, name and meaning of each column of the bus matrix is given
%   below:
%
%   columns 1-13 must be included in input matrix (in case file)
%    1  BUS_I       bus number (positive integer)
%    2  BUS_TYPE    bus type (1 = PQ, 2 = PV, 3 = ref, 4 = isolated)
%    3  PD          Pd, real power demand (MW)
%    4  QD          Qd, reactive power demand (MVAr)
%    5  GS          Gs, shunt conductance (MW demanded at V = 1.0 p.u.)
%    6  BS          Bs, shunt susceptance (MVAr injected at V = 1.0 p.u.)
%    7  BUS_AREA    area number, (positive integer)
%    8  VM          Vm, voltage magnitude (p.u.)
%    9  VA          Va, voltage angle (degrees)
%    10 BASE_KV     baseKV, base voltage (kV)
%    11 ZONE        zone, loss zone (positive integer)
%    12 VMAX        maxVm, maximum voltage magnitude (p.u.)
%    13 VMIN        minVm, minimum voltage magnitude (p.u.)
%
%   columns 14-17 are added to matrix after OPF solution
%   they are typically not present in the input matrix
%                   (assume OPF objective function has units, u)
%    14 LAM_P       Lagrange multiplier on real power mismatch (u/MW)
%    15 LAM_Q       Lagrange multiplier on reactive power mismatch (u/MVAr)
%    16 MU_VMAX     Kuhn-Tucker multiplier on upper voltage limit (u/p.u.)
%    17 MU_VMIN     Kuhn-Tucker multiplier on lower voltage limit (u/p.u.)
%
%   additional constants, used to assign/compare values in the BUS_TYPE column
%    1  PQ    PQ bus
%    2  PV    PV bus
%    3  REF   reference bus
%    4  NONE  isolated bus
%
%   See also DEFINE_CONSTANTS.

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

%% define bus types
PQ      = 1;
PV      = 2;
REF     = 3;
NONE    = 4;

%% define the indices
BUS_I       = 1;    %% bus number (1 to 29997)
BUS_TYPE    = 2;    %% bus type (1 - PQ bus, 2 - PV bus, 3 - reference bus, 4 - isolated bus)
PD          = 3;    %% Pd, real power demand (MW)
QD          = 4;    %% Qd, reactive power demand (MVAr)
GS          = 5;    %% Gs, shunt conductance (MW at V = 1.0 p.u.)
BS          = 6;    %% Bs, shunt susceptance (MVAr at V = 1.0 p.u.)
BUS_AREA    = 7;    %% area number, 1-100
VM          = 8;    %% Vm, voltage magnitude (p.u.)
VA          = 9;    %% Va, voltage angle (degrees)
BASE_KV     = 10;   %% baseKV, base voltage (kV)
ZONE        = 11;   %% zone, loss zone (1-999)
VMAX        = 12;   %% maxVm, maximum voltage magnitude (p.u.)      (not in PTI format)
VMIN        = 13;   %% minVm, minimum voltage magnitude (p.u.)      (not in PTI format)

%% included in opf solution, not necessarily in input
%% assume objective function has units, u
LAM_P       = 14;   %% Lagrange multiplier on real power mismatch (u/MW)
LAM_Q       = 15;   %% Lagrange multiplier on reactive power mismatch (u/MVAr)
MU_VMAX     = 16;   %% Kuhn-Tucker multiplier on upper voltage limit (u/p.u.)
MU_VMIN     = 17;   %% Kuhn-Tucker multiplier on lower voltage limit (u/p.u.)


function h = noise3dPlot(noise, plotType, CX, CY, CZ, CZVar, varargin)

% NOISE3DPLOT Draw a 3D or contour plot for the relevant noise model.
% FORMAT
% DESC draws a 3D or contour plot for the relevant noise model.
% ARG noise : the noise structure for which the plot is required.
% ARG plotType : string containing the name of the plotting function (for example mesh, contour).
% ARG X : the input X data in the form of a 'mesh' matrix.
% ARG Y : the input Y data in the form of a 'mesh' matrix.
% ARG mu : the input mean in the form of a 'mesh' matrix.
% ARG varSigma : the input variance in the form of a 'mesh' matrix.
% ARG P1, P2, P3 ... : optional additional arguments for the given plot type.
% RETURN h : the gradients of the diagonal with respect to each element
% of X. The returned matrix has the same dimensions as X.
%
% SEEALSO : noiseParamInit, noise3dPlot,
%
% COPYRIGHT : Neil D. Lawrence, 2004, 2005

% NOISE

functionName = [noise.type 'Noise3dPlot'];
if exist(functionName) == 2
  fhandle = str2func(functionName);
  h = fhandle(noise, plotType, CX, CY, CZ, CZVar, varargin{:});
end


function [h]=m_triplot(long,lat,tri)
%  M_CONTOURF Adds filled contours to a map
%    M_CONTOUR(LONG,LAT,DATA,...)
%
global MAP_PROJECTION

% Have to have initialized a map first

if isempty(MAP_PROJECTION)
  disp('No Map Projection initialized - call M_PROJ first!');
  return;
end
bcol=[.8,.9,1];
[X,Y]=m_ll2xy(long,lat,'clip','on');

hold on; trimesh(tri,X,Y,0*X,'facecolor',bcol,'edgecolor','k');
view(2)
%triplot(tri,X,Y,'k','facecolor',bcol);

%m_coast('patch','red') ;

end


% PURPOSE  : 	Restore messy event code values.
%                 pop_setcodebit sets the bit(s) at position(s) "bitindex" in each EEG.event(i).type value to 0 (off).
%                 "bitindex" must contain number(s) between 1 and 16.
%
% FORMAT   :
%
% EEG = pop_setcodebit(EEG, bitindex, newvalue)
%
% or
%
% pop_rt2text(ERPLAB, varargin)
%
%
% INPUTS   :
%
%    EEG           - continuous dataset
%    bitindex      - bit position(s). From 1-16
%    newvalue      - 1 or 0 (zero).
%
%
% OUTPUTS :
%
%    EEG           - updated continuous dataset%
%
%
% Example:
%
% In Biosemi system you have a 16-bit word for sending your event codes. However, you generally only use event codes from 1 to 255
% (8-bit numbers). In this case, the upper byte (bits 9 to 16) should be silent, and these bits should be zero.
% Unfortunately, sometimes this does not happen and you get different and larger event codes.
% You may find some discussions and proposed solutions about this subject in many blogs on the internet.
% However, as a general way to deal with this issue, setcodebit.m will help you to control every single bit from this 16-bit word.
% Hence, you will be able to set each bit, or a group of them, to either "0" or "1", using a single line command.
% For example, if you want to assure that your event codes keep values from 1 to 255, you must set the upper byte
% (bits 9 to 16) to zero (cleaning any spuriously activated bit), using a command like the following:
%
%
% EEG = pop_setcodebit(EEG, 9:16, 0);
%
%
% Note, EEG = pop_setcodebit(EEG, 9:16);  will work as well, since "0" is the default value for "newvalue".
%
%
% See also setcodebitGUI.m setcodebit.m
%
% *** This function is part of ERPLAB Toolbox ***
% Author: Javier Lopez-Calderon & Johanna Kreither
% Center for Mind and Brain
% University of California, Davis,
% Davis, CA
% 2011
%
% Thanks to Eric Foo for helping with testing and the help section.

function [EEG, com ] = pop_setcodebit(EEG, bitindex, newvalue, varargin)
com = '';
if nargin<1
        help setcodebit
        return
end
if isobject(EEG) % eegobj
        whenEEGisanObject % calls a script for showing an error window
        return
end
if ~isempty(EEG.epoch)
        msgboxText = 'pop_setcodebit.m only works for continuous dataset.';
        title = 'ERPLAB: setcodebitGUI few inputs';
        errorfound(msgboxText, title);
        return
end
if ischar(EEG.event(1).type)
        msgboxText = 'Your event codes are not numeric.';
        title = 'ERPLAB: pop_setcodebit few inputs';
        errorfound(msgboxText, title);
        return
end
if nargin==1
        % call GUI
        answer = setcodebitGUI;
        if isempty(answer)
                disp('User selected Cancel')
                return
        end

        todo =  answer{1};
        if strcmpi(todo, 'all')
                bitindex = 1:16;
                newvalue = 0;
        elseif strcmpi(todo, 'lower')
                bitindex = 1:8;
                newvalue = 0;
        elseif strcmpi(todo, 'upper')
                bitindex = 9:16;
                newvalue = 0;
        else
                if ~isempty(answer{2})
                        bitindex = answer{2};
                        newvalue = 0;
                        [EEG, com ] = pop_setcodebit(EEG, bitindex, newvalue, 'History', 'off');
                end
                if ~isempty(answer{3})
                        bitindex = answer{3};
                        newvalue = 1;
                        [EEG, com2 ] = pop_setcodebit(EEG, bitindex, newvalue, 'History', 'off');
                        com = sprintf('%s\n%s', com, com2);
                end
                return
        end

        %
        % Somersault
        %
        [EEG, com ] = pop_setcodebit(EEG, bitindex, newvalue, 'History', 'gui');
        return
end
p = inputParser;
p.FunctionName  = mfilename;
p.CaseSensitive = false;
p.addRequired('EEG');
p.addRequired('bitindex', @isnumeric);
p.addRequired('newvalue', @isnumeric);
p.addParamValue('History', 'script', @ischar);             % history from scripting
p.parse(EEG, bitindex, newvalue, varargin{:});

if strcmpi(p.Results.History,'implicit')
        shist = 3; % implicit
elseif strcmpi(p.Results.History,'script')
        shist = 2; % script
elseif strcmpi(p.Results.History,'gui')
        shist = 1; % gui
else
        shist = 0; % off
end
if isempty(bitindex)
        msgboxText = 'Error: you must specify one bit index, at least.';
        title = 'ERPLAB: pop_setcodebit few inputs';
        errorfound(msgboxText, title);
        return
else
        if isnumeric(bitindex)
                if min(bitindex)<1 || max(bitindex)>16
                        msgboxText = 'Error: bit index must be a positive integer between 1 and 16.';
                        title = 'ERPLAB: setcodebitGUI few inputs';
                        errorfound(msgboxText, title);
                        return
                end
        else
                msgboxText = 'Error: bit index must be numeric.';
                title = 'ERPLAB: pop_setcodebit few inputs';
                errorfound(msgboxText, title);
                return
        end
        bitindex = unique_bc2(bitindex);
end
if ischar(newvalue)
        msgboxText = 'Error: new value must be numeric.';
        title = 'ERPLAB: setcodebitGUI few inputs';
        errorfound(msgboxText, title);
        return
else
        if length(newvalue)~=1
                msgboxText = 'Error: new value must be a single value, either 0 or 1.';
                title = 'ERPLAB: pop_setcodebit few inputs';
                errorfound(msgboxText, title);
                return
        end
        if newvalue~=0 && newvalue~=1
                msgboxText = 'Error: new value must be a single value, either 0 or 1.';
                title = 'ERPLAB: pop_setcodebit few inputs';
                errorfound(msgboxText, title);
                return
        end
end

EEG = setcodebit(EEG, bitindex, newvalue);

% History
bitindexstr = vect2colon(bitindex);
newvalue    = vect2colon(newvalue);
com = sprintf('%s = pop_setcodebit( %s, %s, %s);', inputname(1), inputname(1), bitindexstr, newvalue);

% get history from script. EEG
switch shist
        case 1 % from GUI
                com = sprintf('%s %% GUI: %s', com, datestr(now));
                %fprintf('%%Equivalent command:\n%s\n\n', com);
                displayEquiComERP(com);
        case 2 % from script
                EEG = erphistory(EEG, [], com, 1);
        case 3
                % implicit
                % EEG = erphistory(EEG, [], com, 1);
                % fprintf('%%Equivalent command:\n%s\n\n', com);
        otherwise %off or none
                com = '';
                return
end

%
% Completion statement
%
msg2end
return


function [hog_data, valid_data, vid_id] = Read_HOG_files_dynamic(users, hog_data_dir)

    hog_data = [];
    vid_id = {};
    valid_data = [];

    for i=1:numel(users)

        hog_files = dir([hog_data_dir, '/' users{i} '*.hog']);

        hog_data_curr_p = [];
        feats_filled = 0;

        for f_num=1:numel(hog_files)
            f = fopen([hog_data_dir,  '/', hog_files(f_num).name], 'r');

            curr_data = [];
            curr_ind = 0;

            while(~feof(f))

                if(curr_ind == 0)
                    num_cols = fread(f, 1, 'int32');
                    if(isempty(num_cols))
                        break;
                    end

                    num_rows = fread(f, 1, 'int32');
                    num_chan = fread(f, 1, 'int32');

                    curr_ind = curr_ind + 1;

                    % preallocate some space
                    if(curr_ind == 1)
                        curr_data = zeros(5000, 1 + num_rows * num_cols * num_chan);
                        num_feats =  1 + num_rows * num_cols * num_chan;
                    end

                    if(curr_ind > size(curr_data,1))
                        curr_data = cat(1, curr_data, zeros(6000, 1 + num_rows * num_cols * num_chan));
                    end
                    feature_vec = fread(f, [1, 1 + num_rows * num_cols * num_chan], 'float32');
                    curr_data(curr_ind, :) = feature_vec;
                else

                    % Reading in batches of 5000

                    feature_vec = fread(f, [4 + num_rows * num_cols * num_chan, 5000], 'float32');
                    feature_vec = feature_vec(4:end,:)';

                    num_rows_read = size(feature_vec,1);

                    curr_data(curr_ind+1:curr_ind+num_rows_read,:) = feature_vec;

                    curr_ind = curr_ind + size(feature_vec,1);

                end

            end

            fclose(f);

            curr_data = curr_data(1:curr_ind,:);
            vid_id_curr = cell(curr_ind,1);
            vid_id_curr(:) = users(i);

            vid_id = cat(1, vid_id, vid_id_curr);

            % Assume same number of frames per video
            if(f_num==1)
                hog_data_curr_p = zeros(curr_ind*numel(users), num_feats);
            end

            if(size(hog_data_curr_p,1) < feats_filled+curr_ind)
               hog_data_curr_p = cat(1, hog_data_curr_p, zeros(size(hog_data_curr_p,1), num_feats));
            end

            hog_data_curr_p(feats_filled+1:feats_filled+curr_ind,:) = curr_data;

            feats_filled = feats_filled + curr_ind;
        end

        hog_data_curr_p(1:feats_filled,2:end) = bsxfun(@plus, hog_data_curr_p(1:feats_filled,2:end), -median(hog_data_curr_p(1:feats_filled,2:end)));
        hog_data = cat(1, hog_data, hog_data_curr_p(1:feats_filled,:));

    end

    if(numel(users) > 0)
        valid_data = hog_data(:,1) > 0;
        hog_data = hog_data(:,2:end);
    end
end


function [trQYtY, YQ] = evaluate_objective(Y, problem_data, use_Cholesky)
% function [trQYtY, YQ] = evaluate_objective(Y, problem_data, use_Cholesky)
%
% This function computes and returns the value of the objective function
% tr(Q Y^T Y).  Optionally, it returns the product YQ as the second
% argument

% Copyright (C) 2016 by David M. Rosen

if nargin < 3
    use_Cholesky = true;
end

Yt = Y';
YQ = Qproduct(Yt, problem_data, use_Cholesky)';

trQYtY = trace(YQ * Yt);

end


function [x,D_struc,problem,r,res,solvertime,prob] = call_mosek_geometric(model);

x = [];
D_struc = [];
r = [];
res = [];
solvertime = 0;
[prob,problem] = yalmip2geometric(model.options,model.F_struc,model.c,model.Q,model.K,model.ub,model.lb,model.monomtable,model.linear_variables,model.extended_variables);
if problem == 0

    % Mosek does not support equalities
    if ~isempty(prob.G)
        prob.A = [prob.A;prob.G;-prob.G];
        prob.b = [prob.b;prob.h;1./prob.h];
        prob.map = [prob.map;max(prob.map) + (1:2*length(prob.h))'];
    end
    if model.options.savedebug
        save mosekdebug prob
    end

    param = model.options.mosek;

    % Call MOSEK
    showprogress('Calling MOSEK',model.options.showprogress);
    if model.options.verbose == 0
        solvertime = tic;
        res = mskgpopt(prob.b,prob.A,prob.map,param,'minimize echo(0)');
        solvertime = toc(solvertime);
    else
        solvertime = tic;
    	res = mskgpopt(prob.b,prob.A,prob.map,param,'minimize');
        solvertime = toc(solvertime);
    end
    sol = res.sol;

    x = zeros(length(model.c),1);
    x(model.linear_variables)=exp(res.sol.itr.xx);
    D_struc = [];

    % Check, currently not exhaustive...
    switch res.sol.itr.prosta
        case 'PRIMAL_AND_DUAL_FEASIBLE'
            problem = 0;
        case 'PRIMAL_INFEASIBLE'
            problem = 1;
        case 'DUAL_INFEASIBLE'
            problem = 2;
        case 'UNKNOWN'
            problem = 9;
        otherwise
            problem = -1;
    end
end


function r = lt(p,q)
% MEAS/LT  Implement p < q for meas.

% find the difference between the two
diff = meas();
diff = p - q;

% see if p < q at the 1-sigma level
if (diff.value < diff.error)
    r = 1;
else
    r = 0;
end


function [x,fval,exitflag,info] = opti_lpsolve(f,A,b,Aeq,beq,lb,ub,int,sos,opts)
%OPTI_LPSOLVE Solve a LP or MILP using LP_SOLVE
%
%   [x,fval,exitflag,info] = opti_lpsolve(f,A,b,Aeq,beq,lb,ub,int,sos) solves
%   the linear program min f'x where A,b are the inequality constraints, Aeq,
%   beq are the equality constraints, lb,ub are the bounds and int is a
%   string of the integer variables in the form 'CIB' (Continuous / Integer
%   Binary). sos is a structure with fields sostype, sosind, soswt for SOS.
%
%   THIS IS A WRAPPER FOR LP_SOLVE USING THE MEX INTERFACE
%   See supplied LGPL License

%   Copyright (C) 2011 Jonathan Currie (IPL)

% Modified Version of lp_solve.m supplied with LP_SOLVE distribution

t = tic;

% Handle missing arguments
if nargin < 10, opts = optiset; end
if nargin < 9, sos = []; end
if nargin < 8, int = []; end
if nargin < 7, ub = []; end
if nargin < 6, lb = []; end
if nargin < 5, beq = []; end
if nargin < 4, Aeq = []; end
if nargin < 3, error('You must supply at least 3 arguments to opti_lpsolve'); end

%Check for linear constraints & augment as required
if(~isempty(A) || ~isempty(Aeq))
    %All <= for A,b
    con_type = ones(1,size(A,1)); %1 <=
    %Augment A, Aeq
    if(~isempty(Aeq))
        A = [A;Aeq]; b = [b;beq];
        con_type = [con_type 3*ones(1,size(Aeq,1))]; %3 ==
    end
    %Warn re sparse
    if(~issparse(A))
        if(strcmpi(opts.warnings,'all'))
            warning('opti:sparse','The A matrix should be sparse, correcting: [sparse(A)]');
        end
        A = sparse(A);
    end
end

%Load LP_SOLVE Problem
n = length(f);
m = size(A,1);
lp = lp_solve('make_lp', m, n);
%Load Objective
lp_solve('set_obj_fn', lp, f);
lp_solve('set_minim', lp); % default is solving minimum lp.
%Load Linear Constraints
if(~isempty(A))
    lp_solve('set_mat', lp, A);
    lp_solve('set_rh_vec', lp, b);
    lp_solve('set_constr_type', lp, con_type);
end
%Load Bounds
if(~isempty(lb)), lp_solve('set_lowbo', lp, lb); end
if(~isempty(ub)), lp_solve('set_upbo', lp, ub); end
%Set Integer Constraints
ind = find(int ~= 'C');
ismip = 0;
if(any(ind))
    for i = 1:length(ind)
      if(int(ind(i)) == 'B')
          lp_solve('set_binary', lp, ind(i), 1);
      elseif(int(ind(i)) == 'I')
          lp_solve('set_int', lp, ind(i), 1);
      end
    end
    ismip = 1;
end
%Set SOS
if(~isempty(sos) && isfield(sos,'type') && ~isempty(sos.type))
    if(length(sos.type) > 1)
        [r,c] = size(sos.type);
        if(~iscell(sos.index) || ~iscell(sos.weight))
            error('When adding multiple SOS they must be supplied as cell arrays!');
        end
        if(r > c)
            tp = str2num(sos.type); %#ok<*ST2NM>
        else
            tp = str2num(sos.type');
        end
        for i = 1:length(t) %not sure if this is actually working?
            lp_solve('add_SOS', lp, ['set' num2str(i)], tp(i), i, sos.index{i}, sos.weight{i});
        end
    else
        lp_solve('add_SOS', lp, 'set1', str2num(sos.type), 1, sos.index, sos.weight);
    end
end
%Set Options
lp_solve('set_verbose', lp, dispLevel(opts.display));
lp_solve('set_timeout', lp, opts.maxtime); %converts internally to long?
lp_solve('set_bb_depthlimit', lp, opts.maxnodes); %not exactly number of nodes..
lp_solve('set_epsint', lp, opts.tolint);
%Hard Coded Options
lp_solve('set_scaling', lp, 7); %curtis-reid scaling
lp_solve('set_presolve', lp, 1+2+4); %presolve rows + cols + elim linear dependent rows
%Solve
exitflag = lp_solve('solve', lp);
%Get Outputs
[fval, x] = lp_solve('get_solution', lp);
iter = lp_solve('get_total_iter', lp);
msg = lp_solve('get_statustext', lp, exitflag);
if(~ismip)
    try
        lam = lp_solve('get_dual_solution',lp);
    catch %#ok<CTCH>
        lam = struct('ineqlin',[],'eqlin',[],'bounds',[]);
    end
end
%Delete Problem
lp_solve('delete_lp', lp);
%Check for deleted vars
if(length(x) ~= length(f))
    optiwarn('LP_SOLVE presolve has removed %d variable(s) from the optimization problem, thus the returned decision variable vector will be shorter than expected. OPTI does not currently insert the missing variables.',abs(length(x) ~= length(f)));
end

%Assign Outputs
info.Iterations = iter;
info.Time = toc(t);
info.Algorithm = 'LP_SOLVE: Simplex';
info.Status = msg;

switch(exitflag)
    case {0,1} %suboptimal as well...?
        exitflag = 1;
    case {2,3} %infeasible
        exitflag = -1;
    case 7 %timeout
        exitflag = 0;
    otherwise %unknown
        exitflag = -2;
end

%Add constant objective term
if(~isempty(fval) && isfield(opts,'objbias') && ~isempty(opts.objbias))
    fval = fval + opts.objbias;
end

%Assign Lambda
if(~ismip)
    eq = con_type == 3;
    info.Lambda = struct('ineqlin',lam(~eq),'eqlin',lam(eq),'bounds',lam(length(b)+1:end));
else
    info.Lambda = [];
end

function  print_level = dispLevel(lev)
%Return LPSOLVE compatible display level
switch(lower(lev))
    case'off'
        print_level = 3;
    case 'iter'
        print_level = 6;
    case 'final'
        print_level = 4;
end


function [SampEn, ApEn] = EvalEntropyMetrics(rr, t_rr, m, r, HRVparams, tWin, sqi)

% [SampEn, ApEn] = EvalEntropyMetrics(rr, t_rr, m, r, HRVparams, tWin, sqi)
%
% Overview
%	Calculates Sample Entropy and Approximate Entropy values of input data.
%
% Input
%   rr          - (seconds) rr intervals
%   t_rr        - (seconds) time of the rr intervals
%   m           - pattern length
%   r           - radius of similarity
%   HRVparams   - struct of settings for hrv_toolbox analysis
%   sqi         - Signal Quality Index; Requires a matrix with at least two
%                 columns. Column 1 should be timestamps of each sqi
%                 measure, and Column 2 should be SQI on a scale from 0 to 1
%   tWin        - vector containing the starting time of each
%                                windows (in seconds)
% Output
%   SampEn      -
%   ApEn        -
%
% Written by Giulia Da Poian <giulia.dap@gmail.com>
%	REPO:
%       https://github.com/cliffordlab/PhysioNet-Cardiovascular-Signal-Toolbox
%	COPYRIGHT (C) 2016
%   LICENSE:
%       This software is offered freely and without warranty under
%       the GNU (v3 or later) public license. See license file for
%       more information
%

if nargin<3
    Error('Not enough input arguments!');
end
if nargin<4 || isempty(HRVparams)
    windowlength = length(data);
    SQI_th = 0.9;          % SQI threshold
    WinQuality_th = 0.20;  % Low quality windows threshold
else
    windowlength = HRVparams.windowlength;
    SQI_th = HRVparams.sqi.LowQualityThreshold;        % SQI threshold
    WinQuality_th = HRVparams.RejectionThreshold;  % Low quality windows threshold
end
if nargin<5 || isempty(tWin)
    tWin = 0;
end
if nargin <6 || isempty(sqi)
    sqi(:,1) = t_rr;
    sqi(:,2) = ones(length(t_rr),1);
end

% Preallocation (all NaN)
SampEn = nan(length(tWin),1);
ApEn = nan(length(tWin),1);

% What Sample Entropy function?
SampEnType = 'Maxim'; % Initialize default SampEn method
if windowlength < 34000; SampEnType = 'Fast'; end

% Loop through each window of RR data
for iWin = 1:length(tWin)
    if ~isnan(tWin(iWin))
        % Isolate data in this window
        sqi_win = sqi( sqi(:,1) >= tWin(iWin) & sqi(:,1) < tWin(iWin) + windowlength,:);
        nn_win = rr( t_rr >= tWin(iWin) & t_rr < tWin(iWin) + windowlength );
        lowqual_idx = find(sqi_win(:,2) < SQI_th);  % Analysis of SQI for the window
        % If enough data has an adequate SQI, perform the calculations
        if numel(lowqual_idx)/length(sqi_win(:,2)) < WinQuality_th

            nn_win = zscore(nn_win);  % normalization of the signal that replace the common
                                      % practice of expressing the tolerance as r times the std
            % 1. Sample Entropy
            switch SampEnType
                case 'Fast'
                   SampEn(iWin) = fastSampen(nn_win, m, r);
                otherwise
                   SampEn(iWin) = sampenMaxim(nn_win, m, r);
            end
            % 2. Approximate Entropy
            ApEn(iWin) =  ApproxEntropy( nn_win, m, r);
        end
    end
end


function ADimInfo = get_affine_matrix(this)
% Computes an affine transformation matrix based on the resolution and the
% first sampling point.
%
%   Y = MrDimInfo()
%   ADimInfo = Y.get_affine_matrix()
%
%   Note: The brackets after get_affine_matrix are necessary to prevent an
%   error in numArgumentsFromSubscript used for subsasign.
%
% This is a method of class MrDimInfo.
%
% DETAILS
%   - translation is the position of the first sampling point
%   - zoom is resolution or, if nan, samplingWidths
%
% IN
%
% OUT
%       Affine transformation matrix that describes the scaling
%       (resolution) and translation (given by the sampling points) that is
%       defined by the dimInfo.
%       Main use is within the geometry operations of MrImageGeometry.
%
% EXAMPLE
%
%   See also MrDimInfo MrImageGemeotry

% Author:   Saskia Bollmann & Lars Kasper
% Created:  2018-10-19
% Copyright (C) 2018 Institute for Biomedical Engineering
%                    University of Zurich and ETH Zurich

% This file is part of the TAPAS UniQC Toolbox, which is released
% under the terms of the GNU General Public License (GPL), version 3.
% You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version).
% For further details, see the file COPYING or
%  <http://www.gnu.org/licenses/>.

% create template affine matrix (in case x, y, and z are not defined)
ADimInfo =  [1   0   0   0;
    0   1   0   0;
    0   0   1   0;
    0   0   0   1];

% check whether x, y, or z are specified
dimLabels = this.dimLabels;
hasX = ismember('x', dimLabels);
hasY = ismember('y', dimLabels);
hasZ = ismember('z', dimLabels);
if hasX, dimIndexX = this.get_dim_index('x'); end
if hasY, dimIndexY = this.get_dim_index('y'); end
if hasZ, dimIndexZ = this.get_dim_index('z'); end
% populate fields
% translation is the position of the first sampling point
% zoom is resolution or, if nan, samplingWidths
% x
if hasX
    ADimInfo(1,4) = this.samplingPoints{dimIndexX}(1);
    if ~isnan(this.resolutions(dimIndexX))
        ADimInfo(1,1) = this.resolutions(dimIndexX);
    elseif ~isnan(this.samplingWidths(dimIndexX))
        ADimInfo(1,1) = this.samplingWidths(dimIndexX);
    end
end

% y
if hasY
    ADimInfo(2,4) = this.samplingPoints{dimIndexY}(1);
    if ~isnan(this.resolutions(dimIndexY))
        ADimInfo(2,2) = this.resolutions(dimIndexY);
    elseif ~isnan(this.samplingWidths(dimIndexY))
        ADimInfo(2,2) = this.samplingWidths(dimIndexY);
    end
end

% z
if hasZ
    ADimInfo(3,4) = this.samplingPoints{dimIndexZ}(1);
    if ~isnan(this.resolutions(dimIndexZ))
        ADimInfo(3,3) = this.resolutions(dimIndexZ);
    elseif ~isnan(this.samplingWidths(dimIndexZ))
        ADimInfo(3,3) = this.samplingWidths(dimIndexZ);
    end
end
end


function S = Polygons_intersection_Polygon_cleanup(S,accuracy)
if nargin == 1
    accuracy = 1e-9;
end
if accuracy<eps
    error('Polygons_intersection_Polygon_cleanup:e0',...
          'Accuracy must be a positive real');
end

if isfield(S,'P')
    S_area = struct('A', {});
    for i=1:numel(S)
        S(i).area = 0;
        for j=1:numel(S(i).P)
            S_area(i).A(j) = polyarea(S(i).P(j).x,S(i).P(j).y);
            S(i).area      = S(i).area + (1-2*S(i).P(j).hole) * S_area(i).A(j);
        end
        To_delete = S_area(i).A < accuracy;
        S(i).P(To_delete) = [];
        S_area(i).A(To_delete) = [];
    end
elseif isfield(S,'x')
    S_area = zeros(numel(S),1);
    for i=1:numel(S)
        S_area(i) = polyarea(S(i).x,S(i).y);
    end
    To_delete = S_area < accuracy;
    S(To_delete) = [];
    S_area(To_delete) = [];
end


% Local Regression and Likelihood, Figure 7.4.
%
% Censored Local Likelihood
%
% Author: Catherine Loader
%
% NEED: this is given problems for unknown reasons. something
% to do with censored observations and the ibeta routine.
% if i replace this by
% res[ZLIK] = th*y-y*log(p)
% res[ZDLL] = y*p
% res[ZDDLL] = y*p*(1-p)
% works fine. Memory leak somewhere??
% also works if i ignore censoring.

load border;
fit = locfit(day,runs,'cens',no,'family','geom','alpha',0.7);
figure('Name','fig7_4: censored local likelihood;' );
lfplot(fit);
xlabel('Date');
ylabel('Runs');


function test_ft_inside_headmodel

% MEM 1gb
% WALLTIME 00:10:00
% DEPENDENCY ft_inside_headmodel

if nargout
  % assume that this is called by RUNTESTS
  tests = functiontests(localfunctions);
else
  % assume that this is called from the command line
  func = localfunctions;
  for i=1:numel(func)
    fprintf('evaluating %s\n', func2str(func{i}));
    feval(func{i});
  end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function testSphere(testCase)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

headmodel = [];
headmodel.r = 120;
headmodel.o = [0 0 40];
headmodel.unit = 'mm';

assert( ft_inside_headmodel([  0   0  40], headmodel));
assert( ft_inside_headmodel([119   0  40], headmodel));
assert(~ft_inside_headmodel([121   0  40], headmodel)); % outside
assert( ft_inside_headmodel([  0   0 -79], headmodel));
assert(~ft_inside_headmodel([  0   0 -81], headmodel)); % outside

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function testMesh(testCase)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[pos, tri] = mesh_sphere(162);

headmodel = [];
headmodel.bnd.pos = pos*120;
headmodel.bnd.pos(:,3) = headmodel.bnd.pos(:,3) + 40;
headmodel.bnd.tri = tri;
headmodel.unit = 'mm';

assert( ft_inside_headmodel([  0   0  40], headmodel));
assert( ft_inside_headmodel([119   0  40], headmodel));
assert(~ft_inside_headmodel([121   0  40], headmodel)); % outside
assert( ft_inside_headmodel([  0   0 -79], headmodel));
assert(~ft_inside_headmodel([  0   0 -81], headmodel)); % outside


function P = Predict(Theta, X)

n = size(X, 1);

P = zeros(n, 1);

for i = 1 : n,

    if Sigmoid(X * Theta)(i) >= 0.5

        P(i) = 1;

    end

end

end


function [ears,ear_opp,flops,flop_opp] = find_ears(F)
  % FIND_EARS  Find all ears (faces with two boundary edges) in a given mesh
  %
  % [ears,ear_opp,flops,flop_opp] = find_ears(F)
  %
  % Inputs:
  %   F  #F by 3 list of triangle mesh indices
  % Outputs:
  %   ears  #ears list of indices into F of ears
  %   ear_opp  #ears list of indices indicating which edge is non-boundary
  %     (connecting to flops)
  %   flops  #ears list of indices into F of faces incident on ears
  %   flop_opp  #ears list of indices indicating which edge is non-boundary
  %     (connecting to ears)
  %
  % Known limitation: (V,F) must be manifold near ears for flops and flop_opp
  % outputs.
  %

  if nargout<=2
    % Works on non-manifold meshes...
    [~,B] = on_boundary(F);
    ears = find(sum(B,2) == 2);
    [~,ear_opp] = min(B(ears,:),[],2);
  else
    % Must be manifold (actually only ears need to be)
    [Fp, Fi] = triangle_triangle_adjacency(F);
    % Definition: an ear has 2 boundary **edges**
    ears = find(sum(Fp==-1,2)==2);
    [flops,ear_opp] = max(Fp(ears,:),[],2);
    flop_opp = Fi(sub2ind(size(Fp),ears,ear_opp));
  end
end