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 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 [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 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 K = nddisimXndsimKernCompute(disimKern, simKern, t1, t2)

% NDDISIMXNDSIMKERNCOMPUTE Compute a cross kernel between DISIM and SIM kernels with no decay in the SIM part.
% FORMAT
% DESC computes cross kernel terms between DISIM and SIM kernels for
% the multiple output kernel.
% ARG disimKern : the kernel structure associated with the DISIM
% kernel.
% ARG simKern : the kernel structure associated with the SIM
% kernel.
% ARG t : inputs for which kernel is to be computed.
% RETURN k : block of values from kernel matrix.
%
% FORMAT
% DESC computes cross kernel terms between DISIM and SIM kernels for
% the multiple output kernel.
% ARG disimKern : the kernel structure associated with the DISIM
% kernel.
% ARG simKern : the kernel structure associated with the SIM
% kernel.
% ARG t1 : row inputs for which kernel is to be computed.
% ARG t2 : column inputs for which kernel is to be computed.
% RETURN k : block of values from kernel matrix.
%
% SEEALSO : multiKernParamInit, multiKernCompute, disimKernParamInit, simKernParamInit
%
% COPYRIGHT : Neil D. Lawrence, 2006
%
% COPYRIGHT : Antti Honkela, 2007-2009
%
% COPYRIGHT : Jaakko Peltonen, 2011

% KERN

%fprintf(1,'nddisimXndsimKernCompute step1\n');
%t1
%t2

if nargin < 4
  t2 = t1;
end
if size(t1, 2) > 1 | size(t2, 2) > 1
  error('Input can only have one column');
end

if (isempty(t1)) || (isempty(t2)),
  K = zeros(size(t1,1),size(t2,1));
  return;
end;

if isfield(disimKern,'delay'),
  t1=t1-disimKern.delay;

  % crude way to handle times below zero, just truncate to zero
  % since the cross-kernel value at t1=0 is zero, which is the same
  % as cross-kernel values for negative t.
  I=find(t1<0);
  t1(I)=0;
end;

if disimKern.inverseWidth ~= simKern.inverseWidth
  error('Kernels cannot be cross combined if they have different inverse widths.')
end
%if disimKern.di_decay ~= simKern.decay
%  error('Kernels cannot be cross combined if they have different driving input decays.');
%end
if disimKern.di_variance ~= simKern.variance
  error('Kernels cannot be cross combined if they have different driving input variances.');
end

dim1 = size(t1, 1);
dim2 = size(t2, 1);
%size(t1)
%t1
%size(t2)
%t2
t1Mat = t1(:, ones(1, dim2));
t2Mat = t2(:, ones(1, dim1))';
diffT = (t1Mat - t2Mat);

l = sqrt(2/disimKern.inverseWidth);
D_i = disimKern.decay;
%delta = disimKern.di_decay;

sigma = sqrt(2/simKern.inverseWidth);
if isfield(simKern, 'isNegativeS') && (simKern.isNegativeS == true)
    variancemultiplier = (simKern.sensitivity*simKern.sensitivity);
else
    variancemultiplier = simKern.variance;
end
variancemultiplier=variancemultiplier*sqrt(disimKern.variance);

k1a=-sqrt(pi)*variancemultiplier*sigma/(2*D_i*D_i)*...
    (exp((D_i*sigma/2)^2-D_i*t1Mat+D_i*t2Mat+lnDiffErfs(D_i*sigma/2+t2Mat/sigma,D_i*sigma/2+t2Mat/sigma-t1Mat/sigma))...
     +exp((D_i*sigma/2)^2-D_i*t1Mat+lnDiffErfs(D_i*sigma/2-t1Mat/sigma,D_i*sigma/2)));
k2a=-variancemultiplier*sigma*sigma/(2*D_i)*...
    (exp(-(diffT/sigma).^2)-exp(-(t1Mat/sigma).^2)+1-exp(-(t2Mat/sigma).^2));
k3a=-sqrt(pi)*variancemultiplier*sigma/(2*D_i)*...
    ((diffT-1/D_i).*erf(diffT/sigma)-(t1Mat-1/D_i).*erf(t1Mat/sigma)-(t2Mat+exp(-D_i*t1Mat)/D_i).*erf(t2Mat/sigma));

%
%k1b
%k2b
%pause
K=real(k1a+k2a+k3a);


%SerialLink.ikine6s Analytical inverse kinematics
%
% Q = R.ikine6s(T) is the joint coordinates (1xN) corresponding to the robot
% end-effector pose T represented by an SE(3) homogenenous transform (4x4).  This
% is a analytic solution for a 6-axis robot with a spherical wrist (the most
% common form for industrial robot arms).
%
% If T represents a trajectory (4x4xM) then the inverse kinematics is
% computed for all M poses resulting in Q (MxN) with each row representing
% the joint angles at the corresponding pose.
%
% Q = R.IKINE6S(T, CONFIG) as above but specifies the configuration of the arm in
% the form of a string containing one or more of the configuration codes:
%
% 'l'   arm to the left (default)
% 'r'   arm to the right
% 'u'   elbow up (default)
% 'd'   elbow down
% 'n'   wrist not flipped (default)
% 'f'   wrist flipped (rotated by 180 deg)
%
% Notes::
% - Treats a number of specific cases:
%  - Robot with no shoulder offset
%  - Robot with a shoulder offset (has lefty/righty configuration)
%  - Robot with a shoulder offset and a prismatic third joint (like Stanford arm)
%  - The Puma 560 arms with shoulder and elbow offsets (4 lengths parameters)
%  - The Kuka KR5 with many offsets (7 length parameters)
% - The inverse kinematic solution is generally not unique, and
%   depends on the configuration string.
% - Joint offsets, if defined, are added to the inverse kinematics to
%   generate Q.
% - Only applicable for standard Denavit-Hartenberg parameters
%
% Reference::
% - Inverse kinematics for a PUMA 560,
%   Paul and Zhang,
%   The International Journal of Robotics Research,
%   Vol. 5, No. 2, Summer 1986, p. 32-44
%
% Author::
% - The Puma560 case: Robert Biro with Gary Von McMurray,
%   GTRI/ATRP/IIMB, Georgia Institute of Technology, 2/13/95
% - Kuka KR5 case: Gautam Sinha,
%   Autobirdz Systems Pvt. Ltd.,  SIDBI Office,
%   Indian Institute of Technology Kanpur, Kanpur, Uttar Pradesh.
%
% See also SerialLink.FKINE, SerialLink.IKINE.

function theta = ikine6s(robot, T, varargin)

    if robot.mdh ~= 0
        error('RTB:ikine:notsupported','Solution only applicable for standard DH conventions');
    end

    if robot.n ~= 6
        error('RTB:ikine:notsupported','Solution only applicable for 6-axis robot');
    end

    % recurse over all poses in a trajectory
    if ndims(T) == 3
        theta = zeros(size(T,3),robot.n);
        for k=1:size(T,3)
            theta(k,:) = ikine6s(robot, T(:,:,k), varargin{:});
        end
        return;
    end

    if ~ishomog(T)
        error('RTB:ikine:badarg', 'T is not a homog xform');
    end

    L = robot.links;

    if ~robot.isspherical()
        error('RTB:ikine:notsupported', 'wrist is not spherical');
    end

    % The configuration parameter determines what n1,n2,n4 values are used
    % and how many solutions are determined which have values of -1 or +1.

    if nargin < 3
        configuration = '';
    else
        configuration = lower(varargin{1});
    end

    % default configuration

    sol = [1 1 1];  % left, up, noflip

    for c=configuration
        switch c
            case 'l'
                sol(1) = 1;
            case 'r'
                sol(1) = 2;
            case 'u'
                sol(2) = 1;
            case 'd'
                sol(2) = 2;
            case 'n'
                sol(3) = 1;
            case 'f'
                sol(3) = 2;
        end
    end

    % determine the arm structure and the relevant solution to use
    if isempty(robot.ikineType)
        if is_simple(L)
            robot.ikineType = 'nooffset';
        elseif is_puma(L)
            robot.ikineType = 'puma';
        elseif is_offset(L)
            robot.ikineType = 'offset';
        elseif is_rrp(L)
            robot.ikineType = 'rrp';
        else
            error('RTB:ikine6s:badarg', 'This kinematic structure not supported');
        end
    end

    % undo base and tool transformations
    T = inv(robot.base) * T * inv(robot.tool);

    %% now solve for the first 3 joints, based on position of the spherical wrist centre

    switch robot.ikineType
        case 'puma'
            % Puma model with shoulder and elbow offsets
            %
            % - Inverse kinematics for a PUMA 560,
            %   Paul and Zhang,
            %   The International Journal of Robotics Research,
            %   Vol. 5, No. 2, Summer 1986, p. 32-44
            %
            % Author::
            % Robert Biro with Gary Von McMurray,
            % GTRI/ATRP/IIMB,
            % Georgia Institute of Technology
            % 2/13/95

            a2 = L(2).a;
            a3 = L(3).a;
            d1 = L(1).d;
            d3 = L(3).d;
            d4 = L(4).d;

            % The following parameters are extracted from the Homogeneous
            % Transformation as defined in equation 1, p. 34

            Ox = T(1,2);
            Oy = T(2,2);
            Oz = T(3,2);

            Ax = T(1,3);
            Ay = T(2,3);
            Az = T(3,3);

            Px = T(1,4);
            Py = T(2,4);
            Pz = T(3,4) - d1;

            %
            % Solve for theta(1)
            %
            % r is defined in equation 38, p. 39.
            % theta(1) uses equations 40 and 41, p.39,
            % based on the configuration parameter n1
            %

            r=sqrt(Px^2 + Py^2);
            if sol(1) == 1
                theta(1)= atan2(Py,Px) + pi - asin(d3/r);
            else
                theta(1)= atan2(Py,Px) + asin(d3/r);
            end

            %
            % Solve for theta(2)
            %
            % V114 is defined in equation 43, p.39.
            % r is defined in equation 47, p.39.
            % Psi is defined in equation 49, p.40.
            % theta(2) uses equations 50 and 51, p.40, based on the configuration
            % parameter n2
            %
            if sol(2) == 1
                n2 = -1;
            else
                n2 = 1;
            end
            if sol(1) == 2
                n2 = -n2;
            end

            V114= Px*cos(theta(1)) + Py*sin(theta(1));
            r=sqrt(V114^2 + Pz^2);
            Psi = acos((a2^2-d4^2-a3^2+V114^2+Pz^2)/(2.0*a2*r));
            if ~isreal(Psi)
                warning('RTB:ikine6s:notreachable', 'point not reachable');
                theta = [NaN NaN NaN NaN NaN NaN];
                return
            end
            theta(2) = atan2(Pz,V114) + n2*Psi;

            %
            % Solve for theta(3)
            %
            % theta(3) uses equation 57, p. 40.
            %
            num = cos(theta(2))*V114+sin(theta(2))*Pz-a2;
            den = cos(theta(2))*Pz - sin(theta(2))*V114;
            theta(3) = atan2(a3,d4) - atan2(num, den);

        case 'nooffset'
            a2 = L(2).a;
            a3 = L(3).a;
            d1 = L(1).d;

            px = T(1,4); py = T(2,4); pz = T(3,4);

            %%% autogenerated code
            if L(1).alpha < 0
                if sol(1) == 1
                    q(1) = angle(-px-py*1i);
                else
                    q(1) = angle(px+py*1i);
                end
                S1 = sin(q(1));
                C1 = cos(q(1));

                if sol(2) == 1
                    q(2) = -angle(a2*d1*-2.0+a2*pz*2.0-C1*a2*px*2.0i-S1*a2*py*2.0i)+angle(d1*pz*2.0i-a2^2*1i+a3^2*1i-d1^2*1i-pz^2*1i-C1^2*px^2*1i-sqrt((a2*d1*2.0-a2*pz*2.0)^2+(C1*a2*px*2.0+S1*a2*py*2.0)^2-(d1*pz*-2.0+a2^2-a3^2+d1^2+pz^2+C1^2*px^2+S1^2*py^2+C1*S1*px*py*2.0)^2)-S1^2*py^2*1i-C1*S1*px*py*2.0i);
                else
                    q(2) = -angle(a2*d1*-2.0+a2*pz*2.0-C1*a2*px*2.0i-S1*a2*py*2.0i)+angle(d1*pz*2.0i-a2^2*1i+a3^2*1i-d1^2*1i-pz^2*1i-C1^2*px^2*1i+sqrt((a2*d1*2.0-a2*pz*2.0)^2+(C1*a2*px*2.0+S1*a2*py*2.0)^2-(d1*pz*-2.0+a2^2-a3^2+d1^2+pz^2+C1^2*px^2+S1^2*py^2+C1*S1*px*py*2.0)^2)-S1^2*py^2*1i-C1*S1*px*py*2.0i);
                end
                S2 = sin(q(2));
                C2 = cos(q(2));

                if sol(3) == 1
                    q(3) = -angle(a2*-1i+C2*d1-C2*pz+S2*d1*1i-S2*pz*1i+C1*C2*px*1i-C1*S2*px+C2*S1*py*1i-S1*S2*py)+angle(a3*1i-sqrt((-a2+S2*d1-S2*pz+C1*C2*px+C2*S1*py)^2+(-C2*d1+C2*pz+C1*S2*px+S1*S2*py)^2-a3^2));
                else
                    q(3) = -angle(a2*-1i+C2*d1-C2*pz+S2*d1*1i-S2*pz*1i+C1*C2*px*1i-C1*S2*px+C2*S1*py*1i-S1*S2*py)+angle(a3*1i+sqrt((-a2+S2*d1-S2*pz+C1*C2*px+C2*S1*py)^2+(-C2*d1+C2*pz+C1*S2*px+S1*S2*py)^2-a3^2));
                end
            else
                if sol(1) == 1
                    q(1) = angle(px+py*1i);
                else
                    q(1) = angle(-px-py*1i);
                end
                S1 = sin(q(1));
                C1 = cos(q(1));

                if sol(2) == 1
                    q(2) = -angle(a2*d1*2.0-a2*pz*2.0-C1*a2*px*2.0i-S1*a2*py*2.0i)+angle(d1*pz*2.0i-a2^2*1i+a3^2*1i-d1^2*1i-pz^2*1i-C1^2*px^2*1i-sqrt((a2*d1*2.0-a2*pz*2.0)^2+(C1*a2*px*2.0+S1*a2*py*2.0)^2-(d1*pz*-2.0+a2^2-a3^2+d1^2+pz^2+C1^2*px^2+S1^2*py^2+C1*S1*px*py*2.0)^2)-S1^2*py^2*1i-C1*S1*px*py*2.0i);
                else
                    q(2) = -angle(a2*d1*2.0-a2*pz*2.0-C1*a2*px*2.0i-S1*a2*py*2.0i)+angle(d1*pz*2.0i-a2^2*1i+a3^2*1i-d1^2*1i-pz^2*1i-C1^2*px^2*1i+sqrt((a2*d1*2.0-a2*pz*2.0)^2+(C1*a2*px*2.0+S1*a2*py*2.0)^2-(d1*pz*-2.0+a2^2-a3^2+d1^2+pz^2+C1^2*px^2+S1^2*py^2+C1*S1*px*py*2.0)^2)-S1^2*py^2*1i-C1*S1*px*py*2.0i);
                end
                S2 = sin(q(2));
                C2 = cos(q(2));

                if sol(3) == 1
                    q(3) = -angle(a2*-1i-C2*d1+C2*pz-S2*d1*1i+S2*pz*1i+C1*C2*px*1i-C1*S2*px+C2*S1*py*1i-S1*S2*py)+angle(a3*1i-sqrt((-a2-S2*d1+S2*pz+C1*C2*px+C2*S1*py)^2+(C2*d1-C2*pz+C1*S2*px+S1*S2*py)^2-a3^2));
                else
                    q(3) = -angle(a2*-1i-C2*d1+C2*pz-S2*d1*1i+S2*pz*1i+C1*C2*px*1i-C1*S2*px+C2*S1*py*1i-S1*S2*py)+angle(a3*1i+sqrt((-a2-S2*d1+S2*pz+C1*C2*px+C2*S1*py)^2+(C2*d1-C2*pz+C1*S2*px+S1*S2*py)^2-a3^2));
                end
            end

            theta(1:3) = q;

        case'offset'
            % general case with 6 length parameters
            a1 = L(1).a;
            a2 = L(2).a;
            a3 = L(3).a;
            d1 = L(1).d;
            d2 = L(2).d;
            d3 = L(3).d;

            px = T(1,4); py = T(2,4); pz = T(3,4);

            %%% autogenerated code
            if L(1).alpha < 0

                if sol(1) == 1
                    q(1) = -angle(-px+py*1i)+angle(d2*1i+d3*1i-sqrt(d2*d3*-2.0-d2^2-d3^2+px^2+py^2));
                else
                    q(1) = angle(d2*1i+d3*1i+sqrt(d2*d3*-2.0-d2^2-d3^2+px^2+py^2))-angle(-px+py*1i);
                end
                S1 = sin(q(1));
                C1 = cos(q(1));

                if sol(2) == 1
                    q(2) = angle(d1*pz*2.0i-sqrt(d1*pz^3*4.0+d1^3*pz*4.0-a1^4-a2^4-a3^4-d1^4-py^4-pz^4-C1^4*px^4+C1^2*py^4*2.0-C1^4*py^4+a1^2*a2^2*2.0+a1^2*a3^2*2.0+a2^2*a3^2*2.0-a1^2*d1^2*2.0+a2^2*d1^2*2.0+a3^2*d1^2*2.0-a1^2*py^2*6.0-a2^2*py^2*2.0+a3^2*py^2*2.0-a1^2*pz^2*2.0+a2^2*pz^2*2.0+a3^2*pz^2*2.0-d1^2*py^2*2.0-d1^2*pz^2*6.0-py^2*pz^2*2.0+d1*py^2*pz*4.0+C1^3*a1*px^3*4.0+S1^3*a1*py^3*4.0-C1^2*a1^2*px^2*6.0+C1^2*a2^2*px^2*2.0+C1^2*a3^2*px^2*2.0+C1^2*a1^2*py^2*6.0+C1^2*a2^2*py^2*1.0e1-C1^2*a3^2*py^2*2.0-C1^4*a2^2*py^2*1.2e1+C1^6*a2^2*py^2*4.0-C1^2*d1^2*px^2*2.0+C1^2*d1^2*py^2*2.0-C1^2*px^2*py^2*6.0+C1^4*px^2*py^2*6.0-C1^2*px^2*pz^2*2.0+C1^2*py^2*pz^2*2.0+S1^6*a2^2*py^2*4.0+C1*a1^3*px*4.0+S1*a1^3*py*4.0+a1^2*d1*pz*4.0-a2^2*d1*pz*4.0-a3^2*d1*pz*4.0-C1*a1*a2^2*px*4.0-C1*a1*a3^2*px*4.0-C1*S1*px^3*py*4.0+C1*a1*d1^2*px*4.0+C1*a1*px*py^2*1.2e1+C1*a1*px*pz^2*4.0+S1*a1*a2^2*py*4.0-S1*a1*a3^2*py*4.0+S1*a1*d1^2*py*4.0+S1*a1*px^2*py*4.0+S1*a1*py*pz^2*4.0-C1*S1^3*px*py^3*4.0+C1*S1^3*px^3*py*4.0-C1^3*a1*px*py^2*1.2e1-S1^3*a1*a2^2*py*8.0+C1^2*d1*px^2*pz*4.0-C1^2*d1*py^2*pz*4.0-S1^3*a1*px^2*py*4.0-C1*a1*d1*px*pz*8.0-S1*a1*d1*py*pz*8.0-C1*S1*a1^2*px*py*1.2e1-C1*S1*a2^2*px*py*4.0+C1*S1*a3^2*px*py*4.0-C1*S1*d1^2*px*py*4.0-C1*S1*px*py*pz^2*4.0-C1^2*S1*a1*a2^2*py*8.0+C1^2*S1*a1*px^2*py*8.0+C1*S1^3*a2^2*px*py*8.0+C1^3*S1*a2^2*px*py*8.0+C1*S1*d1*px*py*pz*8.0)-a1^2*1i-a2^2*1i+a3^2*1i-d1^2*1i-py^2*1i-pz^2*1i-C1^2*px^2*1i+C1^2*py^2*1i+C1*a1*px*2.0i+S1*a1*py*2.0i-C1*S1*px*py*2.0i)-angle(-a2*(a1*-1i+d1-pz+C1*px*1i+S1^3*py*1i+C1^2*S1*py*1i));
                else
                    q(2) = angle(d1*pz*2.0i+sqrt(d1*pz^3*4.0+d1^3*pz*4.0-a1^4-a2^4-a3^4-d1^4-py^4-pz^4-C1^4*px^4+C1^2*py^4*2.0-C1^4*py^4+a1^2*a2^2*2.0+a1^2*a3^2*2.0+a2^2*a3^2*2.0-a1^2*d1^2*2.0+a2^2*d1^2*2.0+a3^2*d1^2*2.0-a1^2*py^2*6.0-a2^2*py^2*2.0+a3^2*py^2*2.0-a1^2*pz^2*2.0+a2^2*pz^2*2.0+a3^2*pz^2*2.0-d1^2*py^2*2.0-d1^2*pz^2*6.0-py^2*pz^2*2.0+d1*py^2*pz*4.0+C1^3*a1*px^3*4.0+S1^3*a1*py^3*4.0-C1^2*a1^2*px^2*6.0+C1^2*a2^2*px^2*2.0+C1^2*a3^2*px^2*2.0+C1^2*a1^2*py^2*6.0+C1^2*a2^2*py^2*1.0e1-C1^2*a3^2*py^2*2.0-C1^4*a2^2*py^2*1.2e1+C1^6*a2^2*py^2*4.0-C1^2*d1^2*px^2*2.0+C1^2*d1^2*py^2*2.0-C1^2*px^2*py^2*6.0+C1^4*px^2*py^2*6.0-C1^2*px^2*pz^2*2.0+C1^2*py^2*pz^2*2.0+S1^6*a2^2*py^2*4.0+C1*a1^3*px*4.0+S1*a1^3*py*4.0+a1^2*d1*pz*4.0-a2^2*d1*pz*4.0-a3^2*d1*pz*4.0-C1*a1*a2^2*px*4.0-C1*a1*a3^2*px*4.0-C1*S1*px^3*py*4.0+C1*a1*d1^2*px*4.0+C1*a1*px*py^2*1.2e1+C1*a1*px*pz^2*4.0+S1*a1*a2^2*py*4.0-S1*a1*a3^2*py*4.0+S1*a1*d1^2*py*4.0+S1*a1*px^2*py*4.0+S1*a1*py*pz^2*4.0-C1*S1^3*px*py^3*4.0+C1*S1^3*px^3*py*4.0-C1^3*a1*px*py^2*1.2e1-S1^3*a1*a2^2*py*8.0+C1^2*d1*px^2*pz*4.0-C1^2*d1*py^2*pz*4.0-S1^3*a1*px^2*py*4.0-C1*a1*d1*px*pz*8.0-S1*a1*d1*py*pz*8.0-C1*S1*a1^2*px*py*1.2e1-C1*S1*a2^2*px*py*4.0+C1*S1*a3^2*px*py*4.0-C1*S1*d1^2*px*py*4.0-C1*S1*px*py*pz^2*4.0-C1^2*S1*a1*a2^2*py*8.0+C1^2*S1*a1*px^2*py*8.0+C1*S1^3*a2^2*px*py*8.0+C1^3*S1*a2^2*px*py*8.0+C1*S1*d1*px*py*pz*8.0)-a1^2*1i-a2^2*1i+a3^2*1i-d1^2*1i-py^2*1i-pz^2*1i-C1^2*px^2*1i+C1^2*py^2*1i+C1*a1*px*2.0i+S1*a1*py*2.0i-C1*S1*px*py*2.0i)-angle(-a2*(a1*-1i+d1-pz+C1*px*1i+S1^3*py*1i+C1^2*S1*py*1i));
                end
                S2 = sin(q(2));
                C2 = cos(q(2));

                if sol(3) == 1
                    q(3) = angle(a3*1i-sqrt(d1*pz*-2.0+a1^2+a2^2-a3^2+d1^2+py^2+pz^2+C1^2*px^2-C1^2*py^2+C2*a1*a2*2.0-C1*a1*px*2.0-S2*a2*d1*2.0-S1*a1*py*2.0+S2*a2*pz*2.0-C1*C2*a2*px*2.0-C2*S1*a2*py*2.0+C1*S1*px*py*2.0))-angle(a2*-1i-C2*a1*1i+C2*d1-C2*pz+S2*a1+S2*d1*1i-S2*pz*1i+C1*C2*px*1i-C1*S2*px+C2*S1*py*1i-S1*S2*py);
                else
                    q(3) = -angle(a2*-1i-C2*a1*1i+C2*d1-C2*pz+S2*a1+S2*d1*1i-S2*pz*1i+C1*C2*px*1i-C1*S2*px+C2*S1*py*1i-S1*S2*py)+angle(a3*1i+sqrt(d1*pz*-2.0+a1^2+a2^2-a3^2+d1^2+py^2+pz^2+C1^2*px^2-C1^2*py^2+C2*a1*a2*2.0-C1*a1*px*2.0-S2*a2*d1*2.0-S1*a1*py*2.0+S2*a2*pz*2.0-C1*C2*a2*px*2.0-C2*S1*a2*py*2.0+C1*S1*px*py*2.0));
                end
            else
                if sol(1) == 1
                    q(1) = -angle(px-py*1i)+angle(d2*1i+d3*1i-sqrt(d2*d3*-2.0-d2^2-d3^2+px^2+py^2));
                else
                    q(1) = -angle(px-py*1i)+angle(d2*1i+d3*1i+sqrt(d2*d3*-2.0-d2^2-d3^2+px^2+py^2));
                end
                S1 = sin(q(1));
                C1 = cos(q(1));

                if sol(2) == 1
                    q(2) = angle(d1*pz*2.0i-sqrt(d1*pz^3*4.0+d1^3*pz*4.0-a1^4-a2^4-a3^4-d1^4-py^4-pz^4-C1^4*px^4+C1^2*py^4*2.0-C1^4*py^4+a1^2*a2^2*2.0+a1^2*a3^2*2.0+a2^2*a3^2*2.0-a1^2*d1^2*2.0+a2^2*d1^2*2.0+a3^2*d1^2*2.0-a1^2*py^2*6.0-a2^2*py^2*2.0+a3^2*py^2*2.0-a1^2*pz^2*2.0+a2^2*pz^2*2.0+a3^2*pz^2*2.0-d1^2*py^2*2.0-d1^2*pz^2*6.0-py^2*pz^2*2.0+d1*py^2*pz*4.0+C1^3*a1*px^3*4.0+S1^3*a1*py^3*4.0-C1^2*a1^2*px^2*6.0+C1^2*a2^2*px^2*2.0+C1^2*a3^2*px^2*2.0+C1^2*a1^2*py^2*6.0+C1^2*a2^2*py^2*1.0e1-C1^2*a3^2*py^2*2.0-C1^4*a2^2*py^2*1.2e1+C1^6*a2^2*py^2*4.0-C1^2*d1^2*px^2*2.0+C1^2*d1^2*py^2*2.0-C1^2*px^2*py^2*6.0+C1^4*px^2*py^2*6.0-C1^2*px^2*pz^2*2.0+C1^2*py^2*pz^2*2.0+S1^6*a2^2*py^2*4.0+C1*a1^3*px*4.0+S1*a1^3*py*4.0+a1^2*d1*pz*4.0-a2^2*d1*pz*4.0-a3^2*d1*pz*4.0-C1*a1*a2^2*px*4.0-C1*a1*a3^2*px*4.0-C1*S1*px^3*py*4.0+C1*a1*d1^2*px*4.0+C1*a1*px*py^2*1.2e1+C1*a1*px*pz^2*4.0+S1*a1*a2^2*py*4.0-S1*a1*a3^2*py*4.0+S1*a1*d1^2*py*4.0+S1*a1*px^2*py*4.0+S1*a1*py*pz^2*4.0-C1*S1^3*px*py^3*4.0+C1*S1^3*px^3*py*4.0-C1^3*a1*px*py^2*1.2e1-S1^3*a1*a2^2*py*8.0+C1^2*d1*px^2*pz*4.0-C1^2*d1*py^2*pz*4.0-S1^3*a1*px^2*py*4.0-C1*a1*d1*px*pz*8.0-S1*a1*d1*py*pz*8.0-C1*S1*a1^2*px*py*1.2e1-C1*S1*a2^2*px*py*4.0+C1*S1*a3^2*px*py*4.0-C1*S1*d1^2*px*py*4.0-C1*S1*px*py*pz^2*4.0-C1^2*S1*a1*a2^2*py*8.0+C1^2*S1*a1*px^2*py*8.0+C1*S1^3*a2^2*px*py*8.0+C1^3*S1*a2^2*px*py*8.0+C1*S1*d1*px*py*pz*8.0)-a1^2*1i-a2^2*1i+a3^2*1i-d1^2*1i-py^2*1i-pz^2*1i-C1^2*px^2*1i+C1^2*py^2*1i+C1*a1*px*2.0i+S1*a1*py*2.0i-C1*S1*px*py*2.0i)-angle(-a2*(a1*-1i-d1+pz+C1*px*1i+S1^3*py*1i+C1^2*S1*py*1i));
                else
                    q(2) = angle(d1*pz*2.0i+sqrt(d1*pz^3*4.0+d1^3*pz*4.0-a1^4-a2^4-a3^4-d1^4-py^4-pz^4-C1^4*px^4+C1^2*py^4*2.0-C1^4*py^4+a1^2*a2^2*2.0+a1^2*a3^2*2.0+a2^2*a3^2*2.0-a1^2*d1^2*2.0+a2^2*d1^2*2.0+a3^2*d1^2*2.0-a1^2*py^2*6.0-a2^2*py^2*2.0+a3^2*py^2*2.0-a1^2*pz^2*2.0+a2^2*pz^2*2.0+a3^2*pz^2*2.0-d1^2*py^2*2.0-d1^2*pz^2*6.0-py^2*pz^2*2.0+d1*py^2*pz*4.0+C1^3*a1*px^3*4.0+S1^3*a1*py^3*4.0-C1^2*a1^2*px^2*6.0+C1^2*a2^2*px^2*2.0+C1^2*a3^2*px^2*2.0+C1^2*a1^2*py^2*6.0+C1^2*a2^2*py^2*1.0e1-C1^2*a3^2*py^2*2.0-C1^4*a2^2*py^2*1.2e1+C1^6*a2^2*py^2*4.0-C1^2*d1^2*px^2*2.0+C1^2*d1^2*py^2*2.0-C1^2*px^2*py^2*6.0+C1^4*px^2*py^2*6.0-C1^2*px^2*pz^2*2.0+C1^2*py^2*pz^2*2.0+S1^6*a2^2*py^2*4.0+C1*a1^3*px*4.0+S1*a1^3*py*4.0+a1^2*d1*pz*4.0-a2^2*d1*pz*4.0-a3^2*d1*pz*4.0-C1*a1*a2^2*px*4.0-C1*a1*a3^2*px*4.0-C1*S1*px^3*py*4.0+C1*a1*d1^2*px*4.0+C1*a1*px*py^2*1.2e1+C1*a1*px*pz^2*4.0+S1*a1*a2^2*py*4.0-S1*a1*a3^2*py*4.0+S1*a1*d1^2*py*4.0+S1*a1*px^2*py*4.0+S1*a1*py*pz^2*4.0-C1*S1^3*px*py^3*4.0+C1*S1^3*px^3*py*4.0-C1^3*a1*px*py^2*1.2e1-S1^3*a1*a2^2*py*8.0+C1^2*d1*px^2*pz*4.0-C1^2*d1*py^2*pz*4.0-S1^3*a1*px^2*py*4.0-C1*a1*d1*px*pz*8.0-S1*a1*d1*py*pz*8.0-C1*S1*a1^2*px*py*1.2e1-C1*S1*a2^2*px*py*4.0+C1*S1*a3^2*px*py*4.0-C1*S1*d1^2*px*py*4.0-C1*S1*px*py*pz^2*4.0-C1^2*S1*a1*a2^2*py*8.0+C1^2*S1*a1*px^2*py*8.0+C1*S1^3*a2^2*px*py*8.0+C1^3*S1*a2^2*px*py*8.0+C1*S1*d1*px*py*pz*8.0)-a1^2*1i-a2^2*1i+a3^2*1i-d1^2*1i-py^2*1i-pz^2*1i-C1^2*px^2*1i+C1^2*py^2*1i+C1*a1*px*2.0i+S1*a1*py*2.0i-C1*S1*px*py*2.0i)-angle(-a2*(a1*-1i-d1+pz+C1*px*1i+S1^3*py*1i+C1^2*S1*py*1i));
                end
                S2 = sin(q(2));
                C2 = cos(q(2));

                if sol(3) == 1
                    q(3) = angle(a3*1i-sqrt(d1*pz*-2.0+a1^2+a2^2-a3^2+d1^2+py^2+pz^2+C1^2*px^2-C1^2*py^2+C2*a1*a2*2.0-C1*a1*px*2.0+S2*a2*d1*2.0-S1*a1*py*2.0-S2*a2*pz*2.0-C1*C2*a2*px*2.0-C2*S1*a2*py*2.0+C1*S1*px*py*2.0))-angle(a2*-1i-C2*a1*1i-C2*d1+C2*pz+S2*a1-S2*d1*1i+S2*pz*1i+C1*C2*px*1i-C1*S2*px+C2*S1*py*1i-S1*S2*py);
                else
                    q(3) = -angle(a2*-1i-C2*a1*1i-C2*d1+C2*pz+S2*a1-S2*d1*1i+S2*pz*1i+C1*C2*px*1i-C1*S2*px+C2*S1*py*1i-S1*S2*py)+angle(a3*1i+sqrt(d1*pz*-2.0+a1^2+a2^2-a3^2+d1^2+py^2+pz^2+C1^2*px^2-C1^2*py^2+C2*a1*a2*2.0-C1*a1*px*2.0+S2*a2*d1*2.0-S1*a1*py*2.0-S2*a2*pz*2.0-C1*C2*a2*px*2.0-C2*S1*a2*py*2.0+C1*S1*px*py*2.0));
                end
            end

            theta(1:3) = q;

        case 'rrp'
            % RRP (Stanford arm like)

            px = T(1,4); py = T(2,4); pz = T(3,4);
            d1 = L(1).d;
            d2 = L(2).d;

            %%% autogenerated code
            if L(1).alpha < 0
                if sol(1) == 1
                    q(1) = -angle(-px+py*1i)+angle(d2*1i-sqrt(-d2^2+px^2+py^2));
                else
                    q(1) = angle(d2*1i+sqrt(-d2^2+px^2+py^2))-angle(-px+py*1i);
                end
                S1 = sin(q(1));
                C1 = cos(q(1));

                if sol(2) == 1
                    q(2) = angle(d1-pz-C1*px*1i-S1*py*1i);
                else
                    q(2) = angle(-d1+pz+C1*px*1i+S1*py*1i);
                end
                S2 = sin(q(2));
                C2 = cos(q(2));

                q(3) = -C2*d1+C2*pz+C1*S2*px+S1*S2*py;

            else
                if sol(1) == 1
                    q(1) = -angle(px-py*1i)+angle(d2*1i-sqrt(-d2^2+px^2+py^2));
                else
                    q(1) = -angle(px-py*1i)+angle(d2*1i+sqrt(-d2^2+px^2+py^2));
                end
                S1 = sin(q(1));
                C1 = cos(q(1));

                if sol(2) == 1
                    q(2) = angle(-d1+pz-C1*px*1i-S1*py*1i);
                else
                    q(2) = angle(d1-pz+C1*px*1i+S1*py*1i);
                end
                S2 = sin(q(2));
                C2 = cos(q(2));

                q(3) = -C2*d1+C2*pz-C1*S2*px-S1*S2*py;
            end
            theta(1:3) = q;

        case 'kr5'
            %Given function will calculate inverse kinematics for KUKA KR5 robot

            % Equations are calculated and implemented by
            % Gautam Sinha
            % Autobirdz Systems Pvt. Ltd.
            % SIDBI Office,
            % Indian Institute of Technology Kanpur, Kanpur, Uttar Pradesh
            % 208016
            % India
            %email- gautam.sinha705@gmail.com

            % get the a1, a2 and a3-- link lenghts for link no 1,2,3
            L = robot.links;
            a1 = L(1).a;
            a2 = L(2).a;
            a3 = L(3).a;

            % Check wether wrist is spherical or not
            if ~robot.isspherical()
                error('wrist is not spherical')
            end

            % get d1,d2,d3,d4---- Link offsets for link no 1,2,3,4
            d1 = L(1).d;
            d2 = L(2).d;
            d3 = L(3).d;
            d4 = L(4).d;

            % Get the parameters from transformation matrix
            Ox = T(1,2);
            Oy = T(2,2);
            Oz = T(3,2);

            Ax = T(1,3);
            Ay = T(2,3);
            Az = T(3,3);

            Px = T(1,4);
            Py = T(2,4);
            Pz = T(3,4);

            % Set the parameters n1, n2 and n3 to get required configuration from
            % solution
            n1 = -1;   % 'l'
            n2 = -1;   % 'u'
            n4 = -1;   % 'n'
            if ~isempty(strfind(configuration, 'l'))
                n1 = -1;
            end
            if ~isempty(strfind(configuration, 'r'))
                n1 = 1;
            end
            if ~isempty(strfind(configuration, 'u'))
                if n1 == 1
                    n2 = 1;
                else
                    n2 = -1;
                end
            end
            if ~isempty(strfind(configuration, 'd'))
                if n1 == 1
                    n2 = -1;
                else
                    n2 = 1;
                end
            end
            if ~isempty(strfind(configuration, 'n'))
                n4 = 1;
            end
            if ~isempty(strfind(configuration, 'f'))
                n4 = -1;
            end

            % Calculation for theta(1)
            r=sqrt(Px^2+Py^2);

            if (n1 == 1)
                theta(1)= atan2(Py,Px) + asin((d2-d3)/r);
            else
                theta(1)= atan2(Py,Px)+ pi - asin((d2-d3)/r);
            end

            % Calculation for theta(2)
            X= Px*cos(theta(1)) + Py*sin(theta(1)) - a1;
            r=sqrt(X^2 + (Pz-d1)^2);
            Psi = acos((a2^2-d4^2-a3^2+X^2+(Pz-d1)^2)/(2.0*a2*r));

            if ~isreal(Psi)
                warning('RTB:ikine6s:notreachable', 'point not reachable');
                theta = [NaN NaN NaN NaN NaN NaN];
                return
            end

            theta(2) = atan2((Pz-d1),X) + n2*Psi;

            % Calculation for theta(3)
            Nu = cos(theta(2))*X + sin(theta(2))*(Pz-d1) - a2;
            Du = sin(theta(2))*X - cos(theta(2))*(Pz-d1);
            theta(3) = atan2(a3,d4) - atan2(Nu, Du);

            % Calculation for theta(4)
            Y = cos(theta(1))*Ax + sin(theta(1))*Ay;
            M2 = sin(theta(1))*Ax - cos(theta(1))*Ay ;
            M1 =  ( cos(theta(2)-theta(3)) )*Y + ( sin(theta(2)-theta(3)) )*Az;
            theta(4) = atan2(n4*M2,n4*M1);

            % Calculation for theta(5)
            Nu =  -cos(theta(4))*M1 - M2*sin(theta(4));
            M3 =  -Az*( cos(theta(2)-theta(3)) ) + Y*( sin(theta(2)-theta(3)) );
            theta(5) = atan2(Nu,M3);

            % Calculation for theta(6)
            Z = cos(theta(1))*Ox + sin(theta(1))*Oy;
            L2 = sin(theta(1))*Ox - cos(theta(1))*Oy;
            L1 = Z*( cos(theta(2)-theta(3) )) + Oz*( sin(theta(2)-theta(3)));
            L3 = Z*( sin(theta(2)-theta(3) )) - Oz*( cos(theta(2)-theta(3)));
            A1 = L1*cos(theta(4)) + L2*sin(theta(4));
            A3 = L1*sin(theta(4)) - L2*cos(theta(4));
            Nu =  -A1*cos(theta(5)) - L3*sin(theta(5));
            Du =  -A3;
            theta(6) = atan2(Nu,Du);

        otherwise
            error('RTB:ikine6s:badarg', 'Unknown solution type [%s]', robot.ikineType);
    end
    % Solve for the wrist rotation

    % we need to account for some random translations between the first and last 3
    % joints (d4) and also d6,a6,alpha6 in the final frame.

    T13 = robot.A(1:3, theta(1:3));  % transform of first 3 joints

    % T = T13 * Tz(d4) * R * Tz(d6) Tx(a5)
    Td4 = transl(0, 0, L(4).d);      % Tz(d4)
    Tt = transl(L(6).a, 0, L(6).d) * trotx(L(6).alpha);  % Tz(d6) Tx(a5) Rx(alpha6)

    R = inv(Td4) * inv(T13) * T * inv(Tt);

    % the spherical wrist implements Euler angles

if sol(3) == 1
        theta(4:6) = tr2eul(R, 'flip');
    else
        theta(4:6) = tr2eul(R);

    end
    if L(4).alpha > 0
        theta(5) = -theta(5);
    end

    % remove the link offset angles
    for i=1:robot.n   %#ok<*AGROW>
        theta(i) = theta(i) - L(i).offset;
    end
end

% predicates to determine which kinematic solution to use
function s = is_simple(L)
    alpha = [-pi/2 0 pi/2];
    s =     all([L(2:3).d] == 0) && ...
            (all([L(1:3).alpha] == alpha) || all([L(1:3).alpha] == -alpha)) && ...
            all([L(1:3).sigma] == 0) && ...
            (L(1).a == 0);
end

function s = is_offset(L)
    alpha = [-pi/2 0 pi/2];
    s =     (all([L(1:3).alpha] == alpha) || all([L(1:3).alpha] == -alpha)) && ...
            all([L(1:3).sigma] == 0);
end

function s = is_rrp(L)
    alpha = [-pi/2 pi/2 0];
    s =     all([L(2:3).a] == 0) && ...
            (all([L(1:3).alpha] == alpha) || all([L(1:3).alpha] == -alpha)) && ...
            all([L(1:3).sigma] == [0 0 1]);
end

function s = is_puma(L)
    alpha = [pi/2 0 -pi/2];
    s =     (L(2).d == 0) && (L(1).a == 0) && ...
            (L(3).d ~= 0) && (L(3).a ~= 0) && ...
            all([L(1:3).alpha] == alpha) && ...
            all([L(1:3).sigma] == 0);
end


function sgmga_size_table

%*****************************************************************************80
%
%% MAIN is the main program for SGMGA_SIZE_TABLE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 August 2010
%
%  Author:
%
%    John Burkardt
%
  addpath ( '../sandia_rules' );

  timestamp ( );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'SGMGA_SIZE_TABLE:\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Make tables of point counts.\n' );
  fprintf ( 1, '  Measure the CPU time for each table.\n' );
%
%  Clenshaw-Curtis Grid (1), slow exponential growth (4).
%
  rule_1d = 1;
  growth_1d = 4;
  np_1d = 0;
  p_1d = [];
  dim_min = 1;
  dim_max = 5;
  level_max_min = 0;
  level_max_max = 5;
  tic ( );
  sgmga_size_tabulate ( rule_1d, growth_1d, np_1d, p_1d, dim_min, dim_max, ...
    level_max_min, level_max_max );
  ctime = toc ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  CPU Time = %f\n', ctime );
%
%  Clenshaw Curtis Grid (1), exponential growth (6).
%
  rule_1d = 1;
  growth_1d = 6;
  np_1d = 0;
  p_1d = [];
  dim_min = 1;
  dim_max = 5;
  level_max_min = 0;
  level_max_max = 5;
  tic ( );
  sgmga_size_tabulate ( rule_1d, growth_1d, np_1d, p_1d, dim_min, dim_max, ...
    level_max_min, level_max_max );
  ctime = toc ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  CPU Time = %f\n', ctime );
%
%  Clenshaw Curtis Grid (1), exponential growth (6).
%
  if ( 0 )
    rule_1d = 1;
    growth_1d = 6;
    np_1d = 0;
    p_1d = [];
    dim_min = 6;
    dim_max = 10;
    level_max_min = 0;
    level_max_max = 5;
    sgmga_size_tabulate ( rule_1d, growth_1d, np_1d, p_1d, dim_min, dim_max, ...
      level_max_min, level_max_max );
    ctime = toc ( );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  CPU Time = %f\n', ctime );
  end
%
%  Clenshaw Curtis Grid (1), exponential growth (6).
%
  rule_1d = 1;
  growth_1d = 6;
  np_1d = 0;
  p_1d = [];
  dim_min = 100;
  dim_max = 100;
  level_max_min = 0;
  level_max_max = 2;
  tic ( );
  sgmga_size_tabulate ( rule_1d, growth_1d, np_1d, p_1d, dim_min, dim_max, ...
    level_max_min, level_max_max );
  ctime = toc ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  CPU Time = %f\n', ctime );
%
%  Gauss Patterson Grid (3), slow exponential growth (4).
%
  rule_1d = 3;
  growth_1d = 4;
  np_1d = 0;
  p_1d = [];
  dim_min = 1;
  dim_max = 5;
  level_max_min = 0;
  level_max_max = 5;
  tic ( );
  sgmga_size_tabulate ( rule_1d, growth_1d, np_1d, p_1d, dim_min, dim_max, ...
    level_max_min, level_max_max );
  ctime = toc ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  CPU Time = %f\n', ctime );
%
%  Gauss Patterson Grid (3), moderate exponential growth (5).
%
  rule_1d = 3;
  growth_1d = 5;
  np_1d = 0;
  p_1d = [];
  dim_min = 1;
  dim_max = 5;
  level_max_min = 0;
  level_max_max = 5;
  tic ( );
  sgmga_size_tabulate ( rule_1d, growth_1d, np_1d, p_1d, dim_min, dim_max, ...
    level_max_min, level_max_max );
  ctime = toc ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  CPU Time = %f\n', ctime );
%
%  Gauss Patterson Grid (3), exponential growth (6).
%
  rule_1d = 3;
  growth_1d = 6;
  np_1d = 0;
  p_1d = [];
  dim_min = 1;
  dim_max = 5;
  level_max_min = 0;
  level_max_max = 5;
  tic ( );
  sgmga_size_tabulate ( rule_1d, growth_1d, np_1d, p_1d, dim_min, dim_max, ...
    level_max_min, level_max_max );
  ctime = toc ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  CPU Time = %f\n', ctime );
%
%  Gauss Legendre Grid (4), slow linear odd growth (2).
%
  rule_1d = 4;
  growth_1d = 2;
  np_1d = 0;
  p_1d = [];
  dim_min = 1;
  dim_max = 5;
  level_max_min = 0;
  level_max_max = 5;
  tic ( );
  sgmga_size_tabulate ( rule_1d, growth_1d, np_1d, p_1d, dim_min, dim_max, ...
    level_max_min, level_max_max );
  ctime = toc ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  CPU Time = %f\n', ctime );
%
%  Gauss Legendre Grid (4), moderate linear growth (3).
%
  rule_1d = 4;
  growth_1d = 3;
  np_1d = 0;
  p_1d = [];
  dim_min = 1;
  dim_max = 5;
  level_max_min = 0;
  level_max_max = 5;
  tic ( );
  sgmga_size_tabulate ( rule_1d, growth_1d, np_1d, p_1d, dim_min, dim_max, ...
    level_max_min, level_max_max );
  ctime = toc ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  CPU Time = %f\n', ctime );
%
%  Gauss Laguerre Grid (7), moderate linear growth (3).
%
  rule_1d = 7;
  growth_1d = 3;
  np_1d = 0;
  p_1d = [];
  dim_min = 1;
  dim_max = 5;
  level_max_min = 0;
  level_max_max = 5;
  tic ( );
  sgmga_size_tabulate ( rule_1d, growth_1d, np_1d, p_1d, dim_min, dim_max, ...
    level_max_min, level_max_max );
  ctime = toc ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  CPU Time = %f\n', ctime );
%
%  Hermite Genz Keister (10), slow exponential growth (4).
%
  rule_1d = 10;
  growth_1d = 4;
  np_1d = 0;
  p_1d = [];
  dim_min = 1;
  dim_max = 5;
  level_max_min = 0;
  level_max_max = 5;
  tic ( );
  sgmga_size_tabulate ( rule_1d, growth_1d, np_1d, p_1d, dim_min, dim_max, ...
    level_max_min, level_max_max );
  ctime = toc ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  CPU Time = %f\n', ctime );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SGMGA_SIZE_TABLE:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );

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

  rmpath ( '../sandia_rules' );

  return
end