function s=compressShape(w, forceCompression)
%compressShape Compress a gradient or pulse shape.
%   s=compressShape(w) Compress the waveform using a run-length compression
%   scheme on the derivative. This strategy encodes constant and linear
%   waveforms with very few samples. A structure is returned with the
%   fields:
%     num_samples - the number of samples in the uncompressed waveform
%     data - containing the compressed waveform
%
%   See also decompressShape

if nargin<2
    forceCompression=false;
end

if any(~isfinite(w))
    error('compressShape() received infinite samples');
end

if ~forceCompression && length(w) <= 4 % avoid compressing very short shapes
    s.num_samples=length(w);
    s.data = w(:)';
    return;
end

% %MZ: old code with implicit quantization
% data = [w(1); diff(w(:))];
% maskChanges = [true; abs(diff(data))>1e-8];   % TRUE if values change
% vals = data(maskChanges);                     % Elements without repetitions

% MZ: explicit quantization with error correction
quant_fac=1e-7; % single precision floating point has ~7.25 decimal places
ws=w./quant_fac;
datq=round([ws(1); diff(ws(:))]);
qerr=ws(:)-cumsum(datq);
qcor=[0; diff(round(qerr))];
datd=datq+qcor;
maskChanges=[true; diff(datd)~=0];
vals=datd(maskChanges).*quant_fac;            % Elements without repetitions

k = find([maskChanges', true]);               % Indices of changes
n = diff(k)';                                 % Number of repetitions

% Encode in Pulseq format
nExtra=n-2;
vals2=vals;
vals2(nExtra<0)=nan;
nExtra(nExtra<0)=nan;
v=[vals vals2 nExtra]';
v=v(isfinite(v));
v(abs(v)<1e-10)=0;
s.num_samples = length(w);
% decide whether compression makes sense, otherwise store the original
if forceCompression || s.num_samples > length(v)
    s.data = v';
else
    s.data = w(:)';
end


function scr_lin = linear_calibrate_scores(scr,key,obj_func,niters,prior,addzeros)
% A function for doing 'cheating' calibration on scores.  It trains
% a linear calibration (scaling and offset) on the scores and then
% applies the calibration to the same scores.
% Inputs:
%   scr: The object (of class Scores) containing the scores to be
%     calibrated.
%   key: The Key indicating target and non-target trials.
%   obj_func: The objective function for the calibration training.  The
%     default is cllr objective.
%   niters: The maximum number of iterations in the calibration
%     training.
%   prior: The effective target prior.
%   addzeros: If true, missing scores (required by key but not
%     present in scr) are added (with value zero).
% Outputs:
%   scr_lin: The calibrated version of the input scores.

assert(nargin==6)
assert(isa(scr,'Scores'))
assert(isa(key,'Key'))
assert(scr.validate())
assert(key.validate())

[tar,non] = scr.get_tar_non(key);
logprint(Logger.Info,'training calibration\n')
[flin,nxe,w_lin] = train_linear_calibration(tar,non,prior,obj_func,niters,true);

logprint(Logger.Info,'calibrating scores\n')
scr_lin = scr.align_with_ndx(key);
scr_lin = scr_lin.transform(flin);

logprint(Logger.Info,'displaying calibration results\n')
logprint(Logger.Info,'weights: scaling %f, offset %f\n',w_lin(1),w_lin(2))
res = Results(scr_lin,key);
logprint(Logger.Info,'norm_act_dcf = %g, norm_min_dcf = %g, prbep = %g, nxe = %g\n',res.get_norm_act_dcf(prior),res.get_norm_min_dcf(prior),res.get_prbep(),nxe);

if addzeros
    scr_lin = scr_lin.set_missing_to_value(key,0.0);
end

assert(scr_lin.validate())

end


function [X_den,P]=denoise_TV_One(Xobs,lambda,l,u,P_init,pars)
%This function implements the FISTA method for TV denoising problems.
%
% INPUT
% Xobs ..............................an observed noisy image.
% lambda ........................ parameter
% pars.................................parameters structure
% pars.MAXITER ..................... maximum number of iterations
%                                                      (Default=100)
% pars.epsilon ..................... tolerance for relative error used in
%                                                       the stopping criteria (Default=1e-4)
% pars.print ..........................  1 if a report on the iterations is
%                                                       given, 0 if the  report is silenced
% pars.tv .................................. type of total variation
%                                                      penatly.  'iso' for isotropic (default)
%                                                      and 'l1' for nonisotropic
%
% OUTPUT
% X_den ........................... The solution of the problem
%                                            min{||X-Xobs||^2+2*lambda*TV(X)}

%%% Written by Chen Chen and Junzhou Huang at UT Arlington
%%% April. 13, 2012

%%% Related paper: Chen Chen and Junzhou Huang, "Compressive Sensing MRI
%%% with Wavelet Tree Sparsity"

%Define the Projection onto the box
if((l==-Inf)&&(u==Inf))
    project=@(x)x;
elseif (isfinite(l)&&(u==Inf))
    project=@(x)(((l<x).*x)+(l*(x<=l)));
elseif (isfinite(u)&&(l==-Inf))
     project=@(x)(((x<u).*x)+((x>=u)*u));
elseif ((isfinite(u)&&isfinite(l))&&(l<u))
    project=@(x)(((l<x)&(x<u)).*x)+((x>=u)*u)+(l*(x<=l));
else
    error('lower and upper bound l,u should satisfy l<u');
end

% Assigning parameres according to pars and/or default values
flag=exist('pars', 'var');
if (flag&&isfield(pars,'MAXITER'))
    MAXITER=pars.MAXITER;
else
    MAXITER=100;
end
% if (flag&&isfield(pars,'epsilon'))
%     epsilon=pars.epsilon;
% else
%     epsilon=1e-4;
% end
% if(flag&&isfield(pars,'print'))
%     prnt=pars.print;
% else
%     prnt=1;
% end
if(flag&&isfield(pars,'tv'))
    tv=pars.tv;
else
    tv='iso';
end

[m,n]=size(Xobs);
% clear P; clear R;
if(isempty(P_init))
    P{1}=zeros(m-1,n);    P{2}=zeros(m,n-1);
    R{1}=zeros(m-1,n);    R{2}=zeros(m,n-1);
else
    P{1}=P_init{1};    P{2}=P_init{2};
    R{1}=P_init{1};    R{2}=P_init{2};
end
tk=1;tkp1=1;count=0;i=0;

D=zeros(m,n);%fval=inf;fun_all=[];
while((i<MAXITER)&&(count<5))
%    fold=fval;
    i=i+1;
%     Dold=D;
    Pold=P;
    tk=tkp1;
    D=project(Xobs-lambda*Lforward(R, m, n));
    Q=Ltrans(D, m, n);
    %%%%%%%%%%
    % Taking a step towards minus of the gradient
    P{1}=R{1}+1/(8*lambda)*Q{1};
    P{2}=R{2}+1/(8*lambda)*Q{2};

    %%%%%%%%%%
    % Peforming the projection step
    switch tv
        case 'iso'
            A=[P{1}.^2;zeros(1,n)]+[P{2}.^2,zeros(m,1)];
            A=sqrt(max(A,1));
            P{1}=P{1}./A(1:m-1,:); P{2}=P{2}./A(:,1:n-1);
        case 'l1'
            P{1}=P{1}./(max(abs(P{1}),1));
            P{2}=P{2}./(max(abs(P{2}),1));
        otherwise
            error('unknown type of total variation. should be iso or l1');
    end

    %%%%%%%%%%
    %Updating R and t
    tkp1=(1+sqrt(1+4*tk^2))/2;

    R{1}=P{1}+(tk-1)/(tkp1)*(P{1}-Pold{1});
    R{2}=P{2}+(tk-1)/tkp1*(P{2}-Pold{2});

%     re=norm(D-Dold,'fro')/norm(D,'fro');
%     if (re<epsilon)
%         count=count+1;
%     else
%         count=0;
%     end
    C=Xobs-lambda*Lforward(P, m, n);
    PC=project(C);
%     fval=-norm(C-PC,'fro')^2+norm(C,'fro')^2;
%     fun_all=[fun_all;fval];
end
X_den=D;iter=i;

function X=Lforward(P, m, n)

%       [m2,n2]=size(P{1});
%       [m1,n1]=size(P{2});
%
%       if (n2~=n1+1)
%           error('dimensions are not consistent')
%       end
%       if(m1~=m2+1)
%           error('dimensions are not consistent')
%       end
%
%       m=m2+1;
%       n=n2;

      X=zeros(m,n);
      X(1:m-1,:)=P{1};
      X(:,1:n-1)=X(:,1:n-1)+P{2};
      X(2:m,:)=X(2:m,:)-P{1};
      X(:,2:n)=X(:,2:n)-P{2};
   end

   function P=Ltrans(X, m, n)
%       [m,n]=size(X);
      P{1}=X(1:m-1,:)-X(2:m,:);
      P{2}=X(:,1:n-1)-X(:,2:n);
   end
end


function [gKern, gVarmeans, gVarcovars, gInd] = linard2VardistPsi2Gradient(linard2Kern, vardist, Z, covGrad, learnInducing)

% LINARD2VARDISTPSI2GRADIENT description.

% VARGPLVM

if nargin < 5
    learnInducing = 1;
end

% inverse variances
A = linard2Kern.inputScales;

Psi1 = linard2VardistPsi1Compute(linard2Kern, vardist, Z);

Amat = sparse(diag(A));
K1 = (vardist.means'*vardist.means + diag(sum(vardist.covars,1)));
K = Amat*K1*Amat;

% TYPICAL WAY
%sumS = sum(vardist.covars,1);
%for q=1:vardist.latentDimension
%   sc = sum(vardist.means(:,q)*ones(1,size(Z,1)).*Psi1,1);
%
%   ZZ = Z(:,q)*Z(:,q)';
%
%   gKern(q)  = sum(sum( (Z(:,q)*sc + (sumS(q)*A(q))*ZZ).*covGrad ));
%
%   gVarmeans(:,q) =  A(q)*sum(Psi1*((ones(size(Z,1),1)*Z(:,q)').*covGrad),2);
%
%   gVarcovars(q) = sum(sum((ZZ.*covGrad)));
%end
%gKern = 2*gKern(:)';
%gVarmeans = 2*gVarmeans(:)';
%gVarcovars = (A.^2).*gVarcovars;
%gVarcovars = ones(size(Psi1,1),1)*gVarcovars;
%gVarcovars = gVarcovars(:)';
%gInd = covGrad*Z*K;
%gInd = 2*gInd(:)';
%%% end of typical way

% FAST WAY
AZ = Z*Amat;
AZK = AZ*K1;
gKern = sum((covGrad*Z).*AZK,1);
gVarmeans = (Psi1*(covGrad*Z))*Amat;
AZ = AZ*Amat;
gVarcovars = ones(size(Psi1,1),1)*sum((covGrad*Z).*AZ,1);
if learnInducing
    gInd = covGrad*Z*K;
else
    [M Q] = size(Z);
    gInd = zeros(M, Q);
end
%
gKern = 2*gKern(:)';
gVarmeans = 2*gVarmeans(:)';
gVarcovars = gVarcovars(:)';
gInd = 2*gInd(:)';
%%%  end of FAST WAY

%sum(sum(abs(gKern1-gKern)))
%sum(sum(abs(gVarmeans1 - gVarmeans)))
%sum(sum(abs(gVarcovars1 - gVarcovars)))
%sum(sum(abs(gInd1 - gInd)))
%pause


function [V]=quad_smooth(V,Ev,IND_V_not_Ev,Lc,n,Ls,w1)

L1=Ev>0;
L2=IND_V_not_Ev>0;
w2=1-w1;

p=V;

for i=1:n
    Xp=NaN(size(Ev));  Yp=NaN(size(Ev));  Zp=NaN(size(Ev));
    Xp(L1)=p(Ev(L1),1); Yp(L1)=p(Ev(L1),2); Zp(L1)=p(Ev(L1),3);
    Vp1=[gnanmean(Xp,2) gnanmean(Yp,2) gnanmean(Zp,2)];

    Xp=NaN(size(Ev));  Yp=NaN(size(Ev));  Zp=NaN(size(Ev));
    Xp(L2)=p(IND_V_not_Ev(L2),1); Yp(L2)=p(IND_V_not_Ev(L2),2); Zp(L2)=p(IND_V_not_Ev(L2),3);
    Vp2=[gnanmean(Xp,2) gnanmean(Yp,2) gnanmean(Zp,2)];

    Vp=(w1.*Vp1+w2.*Vp2)./(w1+w2);
    p=p+Ls.*(Vp-p);

    p(Lc,:)=V(Lc,:);
    if i>1
        D=gnansum((p-p_old).^2,2);
        SSQD_new=gnansum(D(:));
        if i>2
            SSQD_ratio=SSQD_new./SSQD_old;
            disp(num2str(SSQD_ratio));
        end
        SSQD_old=SSQD_new;
    end
    p_old=p;

end
V=p;

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


function [mu, s2, MU, SIG2] = simulGPmc(hyp, inf, meanfunc, cov, lik, input, target, test, lag, Nsamples)
% simulGPmc - Simulation of the dynamic GP model, where the output variance is
% propagated using the Monte Carlo method
%
%% Syntax
%  [mu, s2, MU, SIG2] = simulGPmc(hyp, inf, mean, cov, lik, input, target, test,
%  lag, Nsamples)
%
%% Description
% Idea: at every time step the output of GP model is approximated with
% Nsamples samples, which are used as the future inputs of the GP model.
% Samples are re-used if necessary (ie. y(k-1) for y(k-2) if lag=2 etc.)
% Uses routines gpx and gmx_sample.
%
% Input:
% * hyp      ... the structure of optimized hyperparameters
% * inf      ... the function specifying the inference method
% * meanfunc ... the prior mean function
% * cov      ... the specified covariance function, see help covFun for more info
% * lik      ... the likelihood function
% * input    ... the input part of the training data,  NxD matrix
% * target   ... the output part of the training data (ie. target), Nx1 vector
% * test     ... the input matrix for simulation, kxD vector, see
%                construct.m for more info
% * lag      ... the order of the model (number of used lagged outputs)
% * Nsamples ... the number of samples used in algorithm (ie. runs of simulation)
%
% Output:
% * mu    ... the mean predicted output
% * s2    ... the associated variances (with noise variances)
% * MU    ... the matrix of all predicted means, kxNsamples
% * SIG2  ... the associated predicted variances
%
% See also:
% gpx, gmx_sample, simulGPnaive
%
% Examples:
% demo_example_gp_simulation
%
%%
% * Written by J. Prikryl, November 2010
% * Based on the work of C.E. Rasmussen, A. Girard, K. Azman.
%

% meanfunc ... 'mean' is used as a matlab core function in this file

Ndx = 800;
DSig = 3;
num_iters = length(test);
sum_time  = 0;

[N, D] = size(input);
PDF = zeros(Nsamples,lag);

% Preallocate mu and s2
mu = zeros ( num_iters, 1 );
s2 = zeros ( num_iters, 1 );

% 1st step - input is a point
test_ = test(1,:);
[mu(1), s2(1), post] = gpx(hyp, inf, meanfunc, cov, lik, input, target, test_);

MU(1,:) = mu(1)*ones(1,Nsamples);
SIG2(1,:) = s2(1)*ones(1,Nsamples);

pdf = gmx_sample(mu(1),s2(1),Nsamples);
PDF(:,lag) = pdf;
% instead of a cycle, create one monstrous test_ matrix
test_ = repmat( test(2,:), Nsamples, 1 );
test_(:,lag) = pdf;
[MU(2,:), SIG2(2,:), post] = gpx(hyp, inf, meanfunc, cov, lik, input, target, test_, post);
mu(2,1) = mean(MU(2,:));
s2(2,1) = mean(SIG2(2,:)) + mean((MU(2,:)-mu(2)).^2);

% steps from 3 on ...
for k=3:num_iters

    if(mod(k,50)==0 || k==3)
        disp(['simulGPmc, step: ',int2str(k),'/',int2str(length(test))]);
    end

    if(k>lag)
        % samples of previous distributions
        for jj=1:lag-1
            PDF(:,jj) = PDF(:,jj+1);
        end

    else  % k <= lag
        % part after 'else' not yet tested for lag>=3
        col0 = lag-(k-1);
        PDF(:,1:col0) = repmat(test(k,1:col0),Nsamples,1);
        for jj=col0+1:lag-1
            PDF(:,jj) = PDF(:,jj+1);
        end
    end

    pdf = gmx_sample(MU(k-1,:),SIG2(k-1,:),Nsamples);
    PDF(:,lag) = pdf;

    % simulate for all cases, again using the matrix version
    t0 = tic;
    test_ = repmat ( test(k,:), Nsamples, 1 );
    test_(:,1:lag) = PDF;
    [MU(k,:), SIG2(k,:), post] = gpx(hyp, inf, meanfunc, cov, lik, input, target, test_, post);
    calltime = toc(t0);
%     fprintf ( 'gpr_simul() ....... %f sec\n\n', calltime ); % uncomment
%     if you wish
    sum_time = sum_time + calltime;

    % approximate output distribution with gauss - calculate m and v
    mu(k,1) = mean(MU(k,:));
    s2(k,1) = mean(SIG2(k,:)) + mean((MU(k,:)-mu(k)).^2);

end

%     fprintf ( 'average time needed for gpr_simul() .... %f sec\n', sum_time/(num_iters-2) );
%     uncomment if you wish
return;


function [kPa] = dynpcm22kPa(dynpcm2)
% Convert pressure from dynes per square centimeter to kilopascals
% Chad Greene 2012
kPa = dynpcm2*0.000100000;


function out = DN_Unique(x)
% DN_Unique     The proportion of the time series that are unique values
%
%---INPUTS:
%
% x, the input data vector

% ------------------------------------------------------------------------------
% Copyright (C) 2020, Ben D. Fulcher <ben.d.fulcher@gmail.com>,
% <http://www.benfulcher.com>
%
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This function is free software: you can redistribute it and/or modify it under
% the terms of the GNU General Public License as published by the Free Software
% Foundation, either version 3 of the License, or (at your option) any later
% version.
%
% This program is distributed in the hope that it will be useful, but WITHOUT
% ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
% FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
% details.
%
% You should have received a copy of the GNU General Public License along with
% this program. If not, see <http://www.gnu.org/licenses/>.
% ------------------------------------------------------------------------------

out = length(unique(x))/length(x);

end


 function [data M m] =scale_func(data,M,m)
%
% function data =rescale(data)
%
% This function rescale the input data between -1 and 1
%
% INPUT
%
% data: the data to rescale
% max: the maximum value of the ouput data
% min: the minimum value of the output data
%
% OUTPUT
%
% data: the rescaled data
[Nb_s Nb_b]=size(data);
if nargin==1
    M=max(data,[],1);
    m=min(data,[],1);
end

data = 2*(data-repmat(m,Nb_s,1))./(repmat(M-m,Nb_s,1))-1;


function gamma_inc_values_test ( )

%*****************************************************************************80
%
%% GAMMA_INC_VALUES_TEST demonstrates the use of GAMMA_INC_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 February 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'GAMMA_INC_VALUES_TEST:\n' );
  fprintf ( 1, '  GAMMA_INC_VALUES stores values of\n' );
  fprintf ( 1, '  the incomplete Gamma function.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      A            X            GAMMA_INC(A)(X)\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, a, x, fx ] = gamma_inc_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %12f  %12f  %24.16f\n', a, x, fx );

  end

  return
end


function [vert, fac] = voxel_image( pts, vox_sz, color, alpha, edgec )
%VOXEL_IMAGE Creates a 3D voxel image
%   Parameters:
%   pts    - n x 3 matrix with 3D points
%   vox_sz - 1 x 3 vector with voxel size - if vox_sz is a scalar, all
%            edges will have the same length
%   color  - face color
%   alpha  - face alpha (opacity)
%   edgec  - edge color
%
%   Return values:
%   vert   - 8n x 3 matrix containing the vertices of the voxels
%   fac    - 6n x 4 matrixes containing the indexes of vert that form a
%            face

% Example:
%
% pts = [1,1,1; 2,2,2];
% vs = [0.5, 0.5, 0.5];
% voxel_image(pts, vs);
% view([-37.5, 30]);
%
% This example creates two voxels (at (1,1,1) and (2,2,2)) with edges of
% length 0.5
%
% Author: Stefan Schalk, 11 Feb 2011

if (nargin < 1)
    error('No input arguments given');
end
if (nargin < 2)
    vox_sz = [1,1,1];
end
if (nargin < 3)
    color = 'b';
end
if (nargin < 4)
    alpha = 1;
end
if (nargin < 5)
    edgec = 'k';
end

if (size(pts,2) ~= 3)
    error('pts should be an n x 3 matrix');
end
if (isscalar(vox_sz))
    vox_sz = vox_sz*ones(1,3);
end
if (size(vox_sz,1) ~= 1 || size(vox_sz,2) ~= 3)
    error('vox_sz should be an 1 x 3 vector');
end

np = size(pts,1);
vert = zeros(8*np,3);
fac = zeros(6*np,4,'uint32');
vert_bas = [...
        -0.5,-0.5,-0.5;
        0.5,-0.5,-0.5;
        0.5,0.5,-0.5;
        -0.5,0.5,-0.5;
        -0.5,-0.5,0.5;
        0.5,-0.5,0.5;
        0.5,0.5,0.5;
        -0.5,0.5,0.5];
vert_bas = vert_bas.*([vox_sz(1).*ones(8,1), vox_sz(2).*ones(8,1), vox_sz(3).*ones(8,1)]);
fac_bas = [...
        1,2,3,4;
        1,2,6,5;
        2,3,7,6;
        3,4,8,7;
        4,1,5,8;
        5,6,7,8];
for vx = 1:np
    a = ((vx-1)*8+1):vx*8;
    for dim = 1:3
        vert( a,dim ) = vert_bas(:,dim) + pts(vx,dim);
    end
    fac ( ((vx-1)*6+1):vx*6,: ) = (vx - 1)*8*ones(6,4) + fac_bas;
end
patch('Vertices',vert,'Faces',fac,'FaceColor',color,'FaceAlpha',alpha,'Edgecolor',edgec);

end


function npts = plotObj(prob,xb,data)
%PLOTOBJ Plot the objective function contour
%   plotObj(prob,xb,data)

%   Copyright (C) 2011 Jonathan Currie (I2C2)

%Contour Colour
dkg = [0.4 0.4 0.4];
%Gather Commonly Used Inputs
npts = data.npts;
scale = data.scale;
idx = data.idx;
fval = [];

%Detail based on problem type, only if not specified by the user
if(isempty(npts))
    switch(lower(prob.type))
        case {'lp','milp','bilp'}
            npts = 5;
        case {'qp','qcqp','miqp','miqcqp','sdp','misdp'}
            npts = 30;
        case {'nls','uno','nlp','minlp'}
            npts = 50;
        otherwise
            npts = 50;
    end
end

%Determine if we have a 1D or 2D problem
if((~isempty(prob.lb) && length(prob.lb) == 1) || (~isempty(xb) && length(xb) == 1))
    %Generate vector Based On Mode
    switch(data.mode)
        case {'normal','usex0','bounded_scale'}
            if(length(scale) == 2)
                x = linspace(scale(1),scale(2),npts);
            elseif(length(scale) == 1)
                x = linspace(xb(1)-scale,xb(1)+scale,npts);
            else
                error('Input ''scale'' should be a vector with two inputs [xmin,xmax] for 1D problems');
            end
        case {'multi','bounded'}
            x = linspace(prob.lb(1),prob.ub(1),npts);
    end
    obj = zeros(npts,1);
    %Get Objective (created in buildConfig)
    fun = prob.objective;
    %Create Objective Line
    for i = 1:npts
    	obj(i) = fun(x(i));
    end
    %Do Log Plot if asked
    if(data.dolog)
        obj = log(obj);
    end
    %Draw Plot
    plot(x,obj.*prob.sense,'-','color',dkg);
    yl = ylim; axis([x(1) x(end) yl]);
    %Get Min
    if(~isempty(xb))
        fval = fun(xb).*prob.sense;
    end

else %2D or higher Problem
    %Generate Grid Based On Mode
    switch(data.mode)
        case {'normal','usex0','bounded_scale'}
            if(length(scale) == 4)
                [x1,x2] = meshgrid(linspace(scale(1),scale(2),npts),linspace(scale(3),scale(4),npts));
            elseif(length(scale) == 1)
                [x1,x2] = meshgrid(linspace(xb(idx(1))-scale,xb(idx(1))+scale,npts),linspace(xb(idx(2))-scale,xb(idx(2))+scale,npts));
            else
                error('Input ''scale'' should be a vector with four inputs [x1min, x1max, x2min, x2max] for 2D problems');
            end
        case {'multi','bounded'}
            [x1,x2] = meshgrid(linspace(prob.lb(idx(1)),prob.ub(idx(1)),npts),linspace(prob.lb(idx(2)),prob.ub(idx(2)),npts));
    end
    nox = size(x1);
    noy = size(x2);
    obj = zeros(nox(1),noy(2));
    %Get Objective (created in buildConfig)
    fun = prob.objective;
    x = data.fixval;
    %Create Objective Surface
    for npts = 1:nox(1)
        for m = 1:noy(2)
            x(idx) = [x1(npts,m) x2(npts,m)]';
            obj(npts,m) = fun(x);
        end
    end
    %Do Log Plot if asked
    if(data.dolog)
        obj = log(obj);
    end
    %Draw Contour Plot
    [hc,hl] = contour(x1,x2,obj.*prob.sense,':','color',dkg);
    if(~isempty(hc))
        clabel(hc,hl);
        hold on;
        %Plot Minimum Contour
        if(~isempty(xb))
            fval = fun(xb).*prob.sense;
            contour(x1,x2,obj.*prob.sense,':','color',dkg,'levellist',fval);
        end
    else
        optiwarn('OPTI:EmptyContour','Objective Contour Data is Empty - Cannot Plot Objective!');
        %Manually set axis of interest
        try
            axis([min(min(x1)) max(max(x1)) min(min(x2)) max(max(x2))]);
        catch
        end
    end

    xlabel(sprintf('x_%d',idx(1))); ylabel(sprintf('x_%d',idx(2)));
end

%Plot Optimum + Title
data.fval = fval;
title(plotTitle(prob,xb,data));
hold off;
end


function D = sparseDiag(d)

% SPARSEDIAG Create a diagonal matrix that is sparse from a vector.
% FORMAT
% DESC creates a diagonal matrix that is sparse from a vector.
% ARG d : the diagonal vector from which the sparse diagonal matrix
% is formed.
% RETURN D : the sparse diagonal matrix containing the vector as
% its diagonal.
%
% SEEALSO : diag, spdiags
%
% COPYRIGHT : Neil D. Lawrence, 2005

% NDLUTIL

if length(size(d)) ~=2
  error('Input must be a vector.');
end
if size(d, 1) ~= 1 & size(d, 2) ~=1
  error('Input must be a vector.');
end

D = spdiags(d, 0, length(d), length(d));
% % Can be made more efficient.
% n = length(d);
% D = spalloc(n, n, n);
% for i = 1:n
%   D(i, i) = d(i);
% end


function b = r8to_to_r8ge ( n, a )

%*****************************************************************************80
%
%% R8TO_TO_R8GE copies a R8TO matrix to a R8GE matrix.
%
%  Discussion:
%
%    The R8TO storage format is used for a Toeplitz matrix, which is constant
%    along diagonals.  Thus, in an N by N Toeplitz matrix, there are at most
%    2*N-1 distinct entries.  The format stores the N elements of the first
%    row, followed by the N-1 elements of the first column (skipping the
%    entry in the first row).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 February 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, real A(2*N-1), the R8TO matrix.
%
%    Output, real B(N,N), the R8GE matrix.
%
  for i = 1 : n
    b(i,1:i-1) = a(n+i-1:-1:n+1,1);
    b(i,i:n) = a(1:n-i+1,1);
  end

  return
end


function [cdstr, utstr] = jd2str(jdate)

% convert Julian date to string equivalent
% calendar date and universal time

% input

%  jdate = Julian date

% output

%  cdstr = calendar date string
%  utstr = universal time string

% Orbital Mechanics with Matlab

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[month, day, year] = gdate(jdate);

% serial date number

sdn = datenum(year, month, day);

% create calendar date string

cdstr = datestr(sdn, 1);

% create universal time string

utstr = datestr(day - fix(day), 'HH:MM:SS.FFF');


function [near,in,on,B,L] = near_mesh(V,F,Q,epsilon)
  % NEAR_MESH test whether a list of points are in or near a given mesh
  %
  % [near] = near_mesh(V,F,Q)
  % [near,in,on,B,L] = near_mesh(V,F,Q)
  %
  % Inputs:
  %   V  #V by dim list of vertex positions
  %   F  #F by 3 list of face indices
  %   Q  #Q by dim list of query points
  %   epsilon  minium distance allowed after collapses are complete, default is to
  %     use fraction of maximum edge length
  % Outputs:
  %   near #Q list of flags revealing whether queries are near (V,F)
  %   in #Q list of flags revealing whether queries are in (V,F)
  %   on #Q list of flags revealing whether queries are on boundary of (V,F)
  %   B  #B by 1 list of mesh outline edges
  %   L  #loops+1 by 1 list of boundary loop start indices into B, the last
  %     entries is (by tradition) always the numel of B + 1
  %
  % See in_mesh, inpolygon
  %

  if ~exist('epsilon','var') || isempty(epsilon)
    EE = edges(F);
    % maximum edge length
    %maxD = max(sqrt(sum((V(EE(:,1),:) - V(EE(:,2),:)).^2,2)));
    minD = min(sqrt(sum((V(EE(:,1),:) - V(EE(:,2),:)).^2,2)));
    epsilon = minD/2;
  end

  dim = size(V,2);
  % only works in 2D
  assert(dim == 2);

  % first determine points strictly in or on mesh
  [in,on,B,L] = in_mesh(V,F,Q);

  % avoid sqrts
  sqr_eps = epsilon.^2;

  % boundary edges
  BE = [B; B(2:end) B(1)]';
  % compute projection of each point to each boundary line segment
  [T,sqrD] = project_to_lines(Q,V(BE(:,1),:),V(BE(:,2),:));
  % each vertex seen by each edge
  QBE = repmat(Q,[1 1 size(BE,1)]);
  % edge start positions
  S = V(BE(:,1),:);
  % edge destination positions
  D = V(BE(:,2),:);
  % distance of each point to each edge start
  sqrDS = ...
    squeeze(sum((QBE - permute(repmat(S,[1 1 size(Q,1)]),[3 2 1])).^2,2));
  % distance of each point to each edge dest
  sqrDD = ...
    squeeze(sum((QBE - permute(repmat(D,[1 1 size(Q,1)]),[3 2 1])).^2,2));
  % replace distances to edges when point is closest to start or dest endpoints
  % respectively
  sqrD(T<0) = sqrDS(T<0);
  sqrD(T>1) = sqrDD(T>1);
  % compute minimum distance to boundary
  [minD] = min(sqrD,[],2);
  % mask telling whether closest point for each edge is close enough
  near = minD<sqr_eps;
  % all strictly in points are also close
  near = in | near;
end


classdef comparison

methods(Static)

        function result = matching_atoms_ratio(original, new, options)
            % Returns distance betweeen two dictionaries
            % in terms of how many atoms from the original
            % dictionary could be
            % found in the new dictionary
            % Assumes that all atoms in dictionary are normalized.
            % Works well if the dictionaries have low coherence.
            distance_threshold=0.01;
            if nargin > 2
                if isfield(options, 'distance_threshold')
                    distance_threshold = options.distance_threshold;
                end
            end
            % Number of atoms which could be
            % identified properly
            atomsFound=0;
            % We compute inner product of each
            % atom in one dictionary with all
            % atoms in other dictionary
            innerProducts =abs(original'*new);
            % Find the number of atoms in original dictionary
            numAtoms = size(original,2);
            for i=1:1:numAtoms
                %  Trying to find a match for the i-th atom in original dictionary.
                % We find its highest inner product
                % with atoms in new dictionary
                max_similarity = max(innerProducts(i,:));
                % The distance is 1  - max_similarity
                min_distance =1- max_similarity;
                % If distance is less than the threshold
                % then the atom has been found.
                match_found = (min_distance<distance_threshold);
                atomsFound=atomsFound + match_found;
            end;
            % Find the ratio of found atoms with total atoms
            result= atomsFound/numAtoms;
        end

end

end


function [mach] = ftps2mach(ftps)
% Convert speed from feet per second to mach number (at STP!)
% Chad A. Greene 2012
mach = ftps*0.0008886297376093 ;


%VOCEVALSEG Evaluates a set of segmentation results.
% VOCEVALSEG(VOCopts,ID); prints out the per class and overall
% segmentation accuracies. Accuracies are given using the intersection/union
% metric:
%   true positives / (true positives + false positives + false negatives)
%
% [ACCURACIES,AVACC,CONF] = VOCEVALSEG(VOCopts,ID) returns the per class
% percentage ACCURACIES, the average accuracy AVACC and the confusion
% matrix CONF.
%
% [ACCURACIES,AVACC,CONF,RAWCOUNTS] = VOCEVALSEG(VOCopts,ID) also returns
% the unnormalised confusion matrix, which contains raw pixel counts.
function [accuracies,avacc,conf,rawcounts] = tvg_VOCevalseg(VOCopts,id)

% image test set
[gtids,t]=textread(sprintf(VOCopts.seg.imgsetpath,VOCopts.testset),'%s %d');

% number of labels = number of classes plus one for the background
num = VOCopts.nclasses+1;
confcounts = zeros(num);
count=0;
tic;
for i=1:length(gtids)
    % display progress
    if toc>1
        fprintf('test confusion: %d/%d\n',i,length(gtids));
        drawnow;
        tic;
    end

    imname = gtids{i};

    % ground truth label file
    gtfile = sprintf(VOCopts.seg.clsimgpath,imname);
    [gtim,map] = imread(gtfile);
    gtim = double(gtim);

    % results file
    resfile = sprintf(VOCopts.seg.clsrespath,id,VOCopts.testset,imname);
    [resim,map] = imread(resfile);
    resim = double(resim);

    % Check validity of results image
    maxlabel = max(resim(:));
    if (maxlabel>VOCopts.nclasses),
        error('Results image ''%s'' has out of range value %d (the value should be <= %d)',imname,maxlabel,VOCopts.nclasses);
    end

    szgtim = size(gtim); szresim = size(resim);
    if any(szgtim~=szresim)
        error('Results image ''%s'' is the wrong size, was %d x %d, should be %d x %d.',imname,szresim(1),szresim(2),szgtim(1),szgtim(2));
    end

    %pixel locations to include in computation
    locs = gtim<255;

    % joint histogram
    sumim = 1+gtim+resim*num;
    hs = histc(sumim(locs),1:num*num);
    count = count + numel(find(locs));
    confcounts(:) = confcounts(:) + hs(:);
end

% confusion matrix - first index is true label, second is inferred label
%conf = zeros(num);
conf = 100*confcounts./repmat(1E-20+sum(confcounts,2),[1 size(confcounts,2)]);
rawcounts = confcounts;

% Percentage correct labels measure is no longer being used.  Uncomment if
% you wish to see it anyway
%overall_acc = 100*sum(diag(confcounts)) / sum(confcounts(:));
%fprintf('Percentage of pixels correctly labelled overall: %6.3f%%\n',overall_acc);

accuracies = zeros(VOCopts.nclasses,1);
fprintf('Accuracy for each class (intersection/union measure)\n');
for j=1:num

   gtj=sum(confcounts(j,:));
   resj=sum(confcounts(:,j));
   gtjresj=confcounts(j,j);
   % The accuracy is: true positive / (true positive + false positive + false negative)
   % which is equivalent to the following percentage:
   accuracies(j)=100*gtjresj/(gtj+resj-gtjresj);

   clname = 'background';
   if (j>1), clname = VOCopts.classes{j-1};end;
   fprintf('  %14s: %6.3f%%\n',clname,accuracies(j));
end
accuracies = accuracies(1:end);
avacc = mean(accuracies);
fprintf('-------------------------\n');
fprintf('Average accuracy: %6.3f%%\n',avacc);


function K = covPPERard(cov, hyp, x, z, i)

% Stationary partially periodic covariance function for a stationary
% covariance function k0 such as covMaternard, covPPard, covRQard and covSEard.
% Partially periodic means that the covariance function is periodic only
% along some dimensions (as opposed to all dimensions).
% Stationary means that the covariance function k0(x,z) depends on the
% data points x,z only through the squared distance
% dxz = (x-z)'*inv(P)*(x-z) where the P matrix is diagonal with ARD parameters
% ell_1^2,...,ell_D^2, where D is the dimension of the input space.
% The covariance function is parameterized as:
%
% k(x,z) = k0(u(x),u(z)),
% [u(x)]([i,i+D]) = [sin(pi*x/p); cos(pi*x/p)]      if p_i is finite
% [u(x)]([i,i+D]) = [x; 0]                          if p_i is not finite
%
% where the period p belongs to covPPERard and hyp0 belong to k0:
%
% hyp = [ log(p_1)
%         log(p_2)
%          .
%         log(p_D)
%         hyp0 ]
%
% The first D hyperparameters of k0 are the log lengthscales such that
% hyp0(i) = log(ell(i)) for i=1..D.
% Note that for k0 = covSEard and D = 1, a faster alternative is covPeriodic.
%
% Copyright (c) by Luigi Acerbi, 2015-12-20.
% Original code by Hannes Nickisch.
%
% See also COVFUNCTIONS, COVPERARD.

nocov = false;                   % default case when no cov argument is provided
if nargin==0, cov = {@covSEard}; nocov = true; end                % default case
if isnumeric(cov)       % detect old version where the cov parameter was missing
  % i <- z, z <- x, x <- hyp, hyp <- cov
  if nargin>3, i = z; end
  if nargin>2, z = x; end
  if nargin>1, x = hyp; end
  hyp = cov; cov = {@covSEard}; nocov = true;
end

if nocov && nargin<2 || ~nocov && nargin<3         % report number of parameters
  K = ['(D+',feval(cov{:}),')']; return
end
if nocov && nargin<3 || ~nocov && nargin<4, z = []; end    % make sure, z exists
xeqz = isempty(z); dg = strcmp(z,'diag');                       % determine mode

[n,D] = size(x);
p = exp(hyp(1:D));
lell = hyp(D+(1:D));
hyp0 =  [lell; lell; hyp(2*D+1:end)];

if nocov && nargin<4 || ~nocov && nargin<5
  [x,z] = u(x,z,p,dg);                      % apply the embedding u:IR^D->IR^2*D
  K = feval(cov{:},hyp0,x,z);
else
  if i<=D
    if isfinite(p(i))                             % periodic dimension
        if dg                                                   % compute distance d
          di = zeros([n,1]);
        else
          if xeqz                                             % symmetric matrix Kxx
            di = repmat(reshape(x(:,i),n, 1),[1, n])...
                -repmat(reshape(x(:,i),1, n),[n, 1]);
          else                                               % cross covariances Kxz
            nz = size(z,1);
            di = repmat(reshape(x(:,i),n, 1),[1,nz])...
                -repmat(reshape(z(:,i),1,nz),[n, 1]);
          end
        end
        di = 2*pi*di/p(i); dD2_dlpi = -2*sin(di).*di;   % derivative dD2i/dlog(p(i))
        [x,z] = u(x,z,p,dg);                    % apply the embedding u:IR^D->IR^2*D
        if dg                                            % compute squared distances
          D2 = zeros(n,1);
        else
          if xeqz
            D2 = sq_dist(x(:,[i,i+D])');
          else
            D2 = sq_dist(x(:,[i,i+D])',z(:,[i,i+D])');
          end
        end
        % reconstruct derivative w.r.t. D2i from derivative w.r.t. log(ell(i))
        dK_dD2 = feval(cov{:},hyp0,x,z,i) + feval(cov{:},hyp0,x,z,i+D);
        dK_dD2 = dK_dD2./(-2*D2); dK_dD2(D2<1e-12) = 0;
        K = dK_dD2.*dD2_dlpi;                                     % apply chain rule
    else                                            % non-periodic dimension
      K = zeros(n);
    end
  else
    [x,z] = u(x,z,p,dg);                    % apply the embedding u:IR^D->IR^2*D
    if i<=2*D
        if isfinite(p(i-D))
          K = feval(cov{:},hyp0,x,z,i-D) + feval(cov{:},hyp0,x,z,i);
        else
          K = feval(cov{:},hyp0,x,z,i-D);
        end
    else
      K = feval(cov{:},hyp0,x,z,i);
    end
  end
end

function [x,z] = u(x,z,p,dg)                % apply the embedding u:IR^D->IR^2*D
  [~,D] = size(x);
  pd = find(isfinite(p));     % periodic dimensions
  x(:,pd) = 2*pi*x(:,pd)*diag(1./p(pd));
  x = [x, zeros(size(x))];
  x(:,pd+D) = cos(x(:,pd)); x(:,pd) = sin(x(:,pd));
  if numel(z)>0 && ~dg
    z(:,pd) = 2*pi*z(:,pd)*diag(1./p(pd));
    z = [z, zeros(size(z))];
    z(:,pd+D) = cos(z(:,pd)); z(:,pd) = sin(z(:,pd));
  end


% Observation models and vision library.
%
% Sensor management
%   composeRobSen                   - Compose robot and sensor frames and parameters.
%
% Pin-hole model functions
%   distort                         - Distort projected point with radial distortion.
%   undistort                       - Undistorts projected point with radial distortion.
%   pixellise                       - Metric to pixellic conversion
%   depixellise                     - Pixellic to metric conversion
%   intrinsic                       - Build intrinsic matrix
%   invIntrinsic                    - Build inverse intrinsic matrix
%   essential                       - Essential matrix from frame specification.
%   fundamental                     - Fundamental matrix between 2 cameras.
%   pinHole                         - Pin-hole camera model, with optional radial distortion.
%   invPinHole                      - Inverse pin-hole camera model, with radial distortion correction.
%   pinHoleIdp                      - Pin-hole camera model for Inverse depth points, with radial distortion.
%   invPinHoleIdp                   - Inverse pin-hole camera model for IDP, with radial distortion correction.
%   pinHolePlucker                  - Projects plucker line.
%   invPinHolePlucker               - Retro-projects plucker line
%   aInvPinHolePlucker              - Inverse pin hole model for Plucker lines
%   pinHoleSegment                  - Pin hole projection of a segment.
%   pluckerInvCamera                - Inverse Plucker projection matrix
%   isVisible                       - Points visible from pinHole camera.
%   invDistortion                   - Radial distortion correction calibration.
%   invPinHoleHmg                   - Retro-project anchored homogeneous point AHP.
%   visibleSegment                  - Visible segment.
%   invPinHoleAPlucker              - Retro-projects anchored plucker line
%   invPinHoleIdpLin                - IDP line retro projection.
%   invPinHoleAhm                   - Retro-project anchored homogeneous point AHP.
%   persp_project                   - Project point into plane using pin-hole camera model
%   persp_retro                     - Retroproject pixel into 3D space.
%   pinHoleHmg                      - Pin-hole camera model for HMG points, with optional radial distortion.
%   projAhmPntIntoPinHole           - Project AHM pnt into pinhole camera.
%   projIdpPntIntoPinHole           - Project Idp pnt into pinhole.
%   invPinHoleDepth                 -
%   pinHoleDepth                    - Pin hole projection with distance measurement
%
% Functions related to Omnicam camera model
%   depixelliseOmniCam              - TODO: can inverse A up front - will save calculation esp in Jac part
%   invOmniCam                      - Inverse omnidirectional camera model
%   invOmniCamAhm                   - Retro-project anchored homogeneous point AHP.
%   invPinHoleAhmLin                - AHM line retro projection.
%   invPinHoleHmgLin                - IDP line retro projection.
%   omniCam                         - Gives projected pixel u of 3D point p for Omnidirectional Camera model
%   omni_project                    - Gives projected pixel u of 3D point p for Omnidirectional Camera model
%   omni_retro                      - Retroproject pixel into 3D space.
%   pixelliseOmniCam                - Omnicam Affine correction step
%
% Observation models, with 2 frame transforms
%   projEucPntIntoPinHoleOnRob      - Project Euc pnt into pinhole on robot.
%   projIdpPntIntoPinHoleOnRob      - Project Idp pnt into pinhole on robot.
%   projHmgPntIntoPinHoleOnRob      - Project Hmg pnt into pinhole on robot.
%   projSegLinIntoPinHoleOnRob      - Project segment line into pinhole on robot.
%   projPlkLinIntoPinHoleOnRob      - Project Plucker line into pinhole on robot.
%   projAplLinIntoPinHoleOnRob      - Project anchored Plucker line into pinhole on robot.
%   projIdpLinIntoPinHoleOnRob      - Project Idp line into pinhole on robot.
%   retroProjIdpPntFromPinHoleOnRob - Retro-project idp from pinhole on robot.
%   retroProjHmgPntFromPinHoleOnRob - Retro-proj. Hmg pnt from pinhole on rob.
%   retroProjPlkLinFromPinHoleOnRob - Retro-project Plucker line from pinhole on robot.
%   retroProjPlkEndPnts             - Retro project Plucker endpoints.
%   retroProjAplLinFromPinHoleOnRob - Retro-project anchored Plucker line from pinhole on robot.
%   retroProjAplEndPnts             - Retro project anchored Plucker endpoints.
%   retroProjIdpLinFromPinHoleOnRob - retroprj Idp Line from pinhole on robot.
%   projAhmPntIntoPinHoleOnRob      - Project Ahm pnt into pinhole on robot.
%   retroProjAhmPntFromPinHoleOnRob - Retro-project ahm from pinhole on robot.
%   projAhmLinIntoPinHoleOnRob      - Project Ahm line into pinhole on robot.
%   projHmgLinIntoPinHoleOnRob      - Project Hmg line into pinhole on robot.
%   retroProjAhmLinFromPinHoleOnRob - retroprj Ahm Line from pinhole on robot.
%   retroProjHmgLinFromPinHoleOnRob - retroprj Hmg Line from pinhole on robot.
%   projAhmPntIntoOmniCamOnRob      - Project Ahm pnt into omnidirectional camera on robot.
%   projEucPntIntoOmniCamOnRob      - Project Euc pnt into omnidirec cam on robot.
%   retroProjAhmPntFromOmniCamOnRob - Retro-project ahm from omnicam on robot.
%   projEucPntIntoPhdOnRob          - Project Eucliden point into Pinhole-depth in Robot
%   retroProjEucPntFromPhdOnRob     - Retro-proj Euc. pnt. from Pinhole-depth in Rob


%SNR
function Sn=SNR(Im_original,Im_modified)

if (size(Im_original)~=size(Im_modified))
    error ('error:image sizes do not agree')
end

else
    A=double(Im_original);
    B=double(Im_modified);
end

[m,n]=size(A);
sumaI=0;
sumaDif=0;
for u=1:m
    for v=1:n
        sumaI=sumaI+A(u,v)^2;
        sumaDif=sumaDif+(A(u,v)-B(u,v))^2;
    end
end

if (sumaDif==0)
    sumaDif=1;
end

Sn=sumaI/sumaDif;
Sn=10*log10(Sn);


function x = elastix_transf_imcoord2(x, t)
% elastix_transf_imcoord  Convert 2D pixel coordinates using
% elastix/transformix transform.
%
% Let x be the (x, y)-coordinates of a pixel in an image (instead of
% row/column). This function computes the coordinates that the same pixel
% will have in the new image after transformix or elastix transforms.
%
% XT = elastix_transf_imcoord2(X, T)
%
%   X is a two-column matrix with the Cartesian coordinates of one or more
%   points in the input image.
%
%   T is a struct produced by elastix with the details of the image
%   transformation. See help elastix for details.
%
%   XT has the same size as X, and contains the coordinates of the same
%   points in the output image.
%
% See also: elastix, transformix, elastix_read_file2param,
% elastix_read_reg_output, elastix_write_param2file,
% elastix_compose_afftransf.

% Author: Ramon Casero <rcasero@gmail.com>
% Copyright © 2014 University of Oxford
% Version: 0.1.1
%
% University of Oxford means the Chancellor, Masters and Scholars of
% the University of Oxford, having an administrative office at
% Wellington Square, Oxford OX1 2JD, UK.
%
% This file is part of Gerardus.
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program 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. The offer of this
% program under the terms of the License is subject to the License
% being interpreted in accordance with English Law and subject to any
% action against the University of Oxford being under the jurisdiction
% of the English Courts.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

% check arguments
narginchk(2, 2);
nargoutchk(0, 1);

if (size(x, 2) ~= 2)
    error('X must have two columns (2D points)')
end
if (~isstruct(t))
    error('T must be a struct with the transform parameters')
end
if (isfield(t, 'Direction') && any(t.Direction ~= [1 0 0 1]))
    error('Not implemented for Direction ~= [1 0 0 1]')
end

% defaults
if (~isfield(t, 'CenterOfRotationPoint'))
    t.CenterOfRotationPoint = [0 0];
end

% select type of transform
switch (t.Transform)

    case 'SimilarityTransform' % similarity transform

        if (length(t.TransformParameters) ~= 4)
            error('SimilarityTransform must have 4 parameters')
        end

        % transformation nomenclature
        s =     t.TransformParameters(1);
        theta = t.TransformParameters(2);
        tx =    t.TransformParameters(3);
        ty =    t.TransformParameters(4);
        cx =    t.CenterOfRotationPoint(1);
        cy =    t.CenterOfRotationPoint(2);

        % transform points
        %
        % note that this is the inverse of the similarity transform. The
        % reason is the way that ITK, and by extension elastix/transformix,
        % implement image transformations
        x = [cos(theta) sin(theta); -sin(theta) cos(theta)] ...
            * [x(:, 1)' - tx - cx; x(:, 2)' - ty - cy] / s;
        x = x';
        x(:, 1) = x(:, 1) + cx;
        x(:, 2) = x(:, 2) + cy;

    otherwise

        error('Transform not implemented')

end


function quality_test11 ( dim_num, n, z )

%*****************************************************************************80
%
%% TEST11 tests POINTSET_SPACING.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 November 2008
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n');
  fprintf ( 1, 'TEST11\n');
  fprintf ( 1, '  POINTSET_SPACING computes pointset spacing parameters.\n');

  gamma = pointset_spacing ( dim_num, n, z );

  gamma_min = min ( gamma(1:n) );
  gamma_max = max ( gamma(1:n) );

  gamma_ave = sum ( gamma(1:n) ) / n;

  if ( 1 < n )
    gamma_std = sqrt ( sum ( ( gamma(1:n) - gamma_ave ).^2 )  / ( n - 1 ) );
  else
    gamma_std = 0.0;
  end

  fprintf ( 1, '\n');
  fprintf ( 1, '  Minimum spacing          GAMMA_MIN = %f\n', gamma_min );
  fprintf ( 1, '  Average spacing          GAMMA_AVE = %f\n', gamma_ave );
  fprintf ( 1, '  Maximum spacing          GAMMA_MAX = %f\n', gamma_max );
  fprintf ( 1, '  Spacing standard dev     GAMMA_STD = %f\n', gamma_std );

  return
end


function exact = p03_exact ( )

%*****************************************************************************80
%
%% P03_EXACT returns the exact integral for problem 3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 January 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real EXACT, the value of the integral.
%
  exact = ( 16.0 / 3.0 ) * ( 2.0 - sqrt ( 2.0 ) );

  return
end


function [wl,wr] = cvwindow(x)

  wr = zeros(size(x));
  wl = zeros(size(x));

  eps = 1e-16;
  sml = find(x<eps); %too small
  mid = find(x>=eps & x<=1-eps); %just right
  lrg = find(x>1-eps); %too large

  wl(sml) = 0;  wr(sml) = 1;
  wl(lrg) = 1;  wr(lrg) = 0;

  xmid = x(mid);
  a = exp(1-1./(1-exp(1-1./(1-xmid))));
  b = exp(1-1./(1-exp(1-1./xmid)));
  n = sqrt(a.^2 + b.^2);
  wl(mid) = a./n;  wr(mid) = b./n;


function net = mlpunpak(net, w)
%MLPUNPAK Separates weights vector into weight and bias matrices.
%
%	Description
%	NET = MLPUNPAK(NET, W) takes an mlp network data structure NET and  a
%	weight vector W, and returns a network data structure identical to
%	the input network, except that the first-layer weight matrix W1, the
%	first-layer bias vector B1, the second-layer weight matrix W2 and the
%	second-layer bias vector B2 have all been set to the corresponding
%	elements of W.
%
%	See also
%	MLP, MLPPAK, MLPFWD, MLPERR, MLPBKP, MLPGRAD
%

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

% Check arguments for consistency
errstring = consist(net, 'mlp');
if ~isempty(errstring);
  error(errstring);
end

if net.nwts ~= length(w)
  error('Invalid weight vector length')
end

nin = net.nin;
nhidden = net.nhidden;
nout = net.nout;

mark1 = nin*nhidden;
net.w1 = reshape(w(1:mark1), nin, nhidden);
mark2 = mark1 + nhidden;
net.b1 = reshape(w(mark1 + 1: mark2), 1, nhidden);
mark3 = mark2 + nhidden*nout;
net.w2 = reshape(w(mark2 + 1: mark3), nhidden, nout);
mark4 = mark3 + nout;
net.b2 = reshape(w(mark3 + 1: mark4), 1, nout);


function M = prepare_mesh_fittemplate(headshape,template)

% PREPARE_MESH_FITTEMPLATE computes an affine transformation matrix between 2 point clouds
%
% This function relies on cpd toolbox from  Myronenko, see https://sites.google.com/site/myronenko/research/cpd
%
%
% See also FT_PREPARE_MESH

% Copyright (C) 2019, Simon Homoelle
%
% 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$

% add toolbox cpd
ft_hastoolbox('cpd', 2);

%
opt.corresp = 0;
opt.method  = 'affine';
opt.max_it = 100;
opt.fgt = 0;
opt.tol = 10e-12;
opt.outliers = 0.0;
opt.outliers = 0;
[transform,~] = cpd_register(headshape,template, opt);

M = eye(4,4);
M(1:3,1:3) = transform.R;
M(1:3,4)   = transform.t;

end


classdef Optimizer_MMA < Optimizer

    properties (GetAccess = public, SetAccess = protected)
        type = 'MMA'
    end

    properties (Access = private)
        kkttol
        maxoutit
        x
        xold1
        xold2
        xmin
        xmax
        low
        outit = 0;
        outeriter = 0;
        upp
        m
        c
        d
        a0
        a
        n
        f0val
        df0dx
        fval
        dfdx
        upperBound
        lowerBound
        hasFinished
        incrementalScheme
        hasConverged
        historicalVariables
       % targetParameters
        KKTnorm
    end

    methods (Access = public)

        function obj = Optimizer_MMA(cParams)
            obj.initOptimizer(cParams);
            obj.init(cParams);
            obj.outputFunction.monitoring.create(cParams);
%             obj.upperBound = cParams.uncOptimizerSettings.ub;
%             obj.lowerBound = cParams.uncOptimizerSettings.lb;
            obj.maxoutit = 1e4;
        end

       function solveProblem(obj)
            % obj.cost.computeFunctionAndGradient();
            % obj.constraint.computeFunctionAndGradient();
%             obj.printOptimizerVariable();
            obj.hasFinished = false;
            obj.printOptimizerVariable();
            while ~obj.hasFinished
                obj.update();
                obj.updateIterInfo();
                obj.updateMonitoring();
                obj.printOptimizerVariable();
            end
%             obj.printOptimizerVariable();
%             obj.printHistory();
            obj.hasConverged = 0;
%             obj.printHistoryFinalValues();
       end

        function update(obj)
            x = obj.designVariable.value;
            obj.cost.computeFunctionAndGradient();
            obj.constraint.computeFunctionAndGradient();
            obj.checkInitial(x);
            obj.outit = obj.outit+1;
            obj.outeriter = obj.outeriter+1;
            %%%% The MMA subproblem is solved at the point xval:
            [xmma,ymma,zmma,lam,xsi,eta,mu,zet,s,obj.low,obj.upp] = ...
                obj.mmasub(obj.m,obj.n,obj.outeriter,x,obj.xmin,obj.xmax,obj.xold1,obj.xold2, ...
                obj.f0val,obj.df0dx,obj.fval,obj.dfdx,obj.low,obj.upp,obj.a0,obj.a,obj.c,obj.d);
            %%%% Some vectors are updated:
            obj.xold2 = obj.xold1;
            obj.xold1 = x;
            x = xmma;
            %%%% The user should now calculate function values and gradients
            %%%% of the objective- and constraint functions at xval.
            %%%% The results should be put in f0val, df0dx, fval and dfdx.
            obj.designVariable.update(x);
            obj.cost.computeFunctionAndGradient();
            obj.constraint.computeFunctionAndGradient();

            [obj.f0val,obj.df0dx,obj.fval,obj.dfdx] = obj.funmma();
            %%%% The residual vector of the KKT conditions is calculated:
            [~,kktnorm] = obj.kktcheck(obj.m,obj.n,xmma,ymma,zmma,lam,xsi,eta,mu,zet,s, ...
                obj.xmin,obj.xmax,obj.df0dx,obj.fval,obj.dfdx,obj.a0,obj.a,obj.c,obj.d);

            obj.historicalVariables.kktnorm = kktnorm;
            obj.dualVariable.value = lam;
            obj.updateConvergenceStatus();
            obj.KKTnorm     = kktnorm;
        end

    end

    methods (Access = private)

        function updateMonitoring(obj)
            s.hasFinished          = obj.hasFinished;
            s.nIter                = obj.nIter;
            s.KKTnorm              = obj.KKTnorm;
            s.outitFrac            = obj.outit/obj.maxoutit;
            obj.outputFunction.monitoring.compute(s);
        end

        function init(obj,cParams)
            obj.outputFunction         = cParams.outputFunction.monitoring;
            obj.upperBound             = cParams.uncOptimizerSettings.ub;
            obj.lowerBound             = cParams.uncOptimizerSettings.lb;
            obj.incrementalScheme      = cParams.incrementalScheme;
            obj.hasConverged           = false;
            obj.constraintCase         = cParams.constraintCase;
            obj.targetParameters       = cParams.targetParameters;
        end

        function [f,df,c,dc] = funmma(obj)
            f  = obj.cost.value;
            df = obj.cost.gradient;
            c  = obj.constraint.value;
            dc = obj.constraint.gradient;
            dc = dc';

            [c,dc] = obj.checkConstraintCase(c,dc);

            %% Re-scale constraints
            % In many applications, the constraints are on the form yi(x) < =  ymaxi
            % The user should then preferably scale the constraints in such a way that 1 < =  ymaxi < =  100 for each i
            % (and not ymaxi = 10^10 for example).

            % kconstr = 100;
            kconstr = 1;
            cconstr = 0;
            c = kconstr*c;
            c(c > 0) = c(c > 0) + cconstr;
            c(c < 0) = c(c < 0) - cconstr;
            % dc = kconstr*dc;

            %% Re-scale objective function
            % The objective function f(x) should preferably be scaled such that
            % 1 < =  f0(x) < =  100 for reasonable values on the variables.
            kfun = 1;
            cfun = 0;
            f = kfun*f + cfun;
            df = kfun*df;
        end

        function checkInitial(obj,x0)
            if isempty(obj.x)
                obj.x = x0;
                obj.xold1 = obj.x;
                obj.xold2 = obj.xold1;
                obj.xmin = obj.lowerBound*ones(length(x0),1);
                obj.xmax = obj.upperBound*ones(length(x0),1);
                % obj.low = obj.xmin;
                obj.low = zeros(length(x0),1);
                % obj.upp = obj.xmax;
                obj.upp = ones(length(x0),1);
                [obj.f0val,obj.df0dx,obj.fval,obj.dfdx] = obj.funmma();
                obj.m = length(obj.fval);
                obj.c = 1000*ones(obj.m,1);
                obj.d = 0*ones(obj.m,1);
                obj.a0 = 1;
                obj.a = 0*ones(obj.m,1);
                obj.n = length(obj.x);
            end
        end

        function [xmma,ymma,zmma,lam,xsi,eta,mu,zet,s,low,upp] = ...
                mmasub(obj,m,n,iter,xval,xmin,xmax,xold1,xold2, ...
                ~,df0dx,fval,dfdx,low,upp,a0,a,c,d)
            epsimin = sqrt(m+n)*10^(-9); %10^(-7);
            raa0 = 1e-5;
            move = 1.0;
            albefa = 0.1;
            asyinit = 0.1 ; %0.2 % 0.5;
            asyincr = 1.1; % 1.2;
            asydecr = 0.65; % 0.7
            eeen = ones(n,1);
            eeem = ones(m,1);
            zeron = zeros(n,1);

            % Calculation of the asymptotes low and upp :
            if iter < 2.5
                low = xval - asyinit*(xmax-xmin);
                upp = xval + asyinit*(xmax-xmin);
            else
                zzz = (xval-xold1).*(xold1-xold2);
                factor = eeen;
                factor(find(zzz > 0)) = asyincr;
                factor(find(zzz < 0)) = asydecr;
                low = xval - factor.*(xold1 - low);
                upp = xval + factor.*(upp - xold1);
                lowmin = xval - 10*(xmax-xmin);
                lowmax = xval - 0.01*(xmax-xmin);
                uppmin = xval + 0.01*(xmax-xmin);
                uppmax = xval + 10*(xmax-xmin);
                low = max(low,lowmin);
                low = min(low,lowmax);
                upp = min(upp,uppmax);
                upp = max(upp,uppmin);
            end

            % Calculation of the bounds alfa and beta :

            zzz1 = low + albefa*(xval-low);
            zzz2 = xval - move*(xmax-xmin);
            zzz  = max(zzz1,zzz2);
            alfa = max(zzz,xmin);
            zzz1 = upp - albefa*(upp-xval);
            zzz2 = xval + move*(xmax-xmin);
            zzz  = min(zzz1,zzz2);
            beta = min(zzz,xmax);

            % Calculations of p0, q0, P, Q and b.

            xmami = xmax-xmin;
            xmamieps = 1e-5*eeen;
            xmami = max(xmami,xmamieps);
            xmamiinv = eeen./xmami;
            ux1 = upp-xval;
            ux2 = ux1.*ux1;
            xl1 = xval-low;
            xl2 = xl1.*xl1;
            uxinv = eeen./ux1;
            xlinv = eeen./xl1;
            %
            p0 = zeron;
            q0 = zeron;
            p0 = max(df0dx,0);
            q0 = max(-df0dx,0);
            %p0(find(df0dx > 0)) = df0dx(find(df0dx > 0));
            %q0(find(df0dx < 0)) = -df0dx(find(df0dx < 0));
            pq0 = 0.001*(p0 + q0) + raa0*xmamiinv;
            p0 = p0 + pq0;
            q0 = q0 + pq0;
            p0 = p0.*ux2;
            q0 = q0.*xl2;
            %
            dfdx = sparse(dfdx);
            P = max(dfdx,0);
            Q = max(-dfdx,0);
            %P(find(dfdx > 0)) = dfdx(find(dfdx > 0));
            %Q(find(dfdx < 0)) = -dfdx(find(dfdx < 0));
            PQ = 0.001*(P + Q) + raa0*(eeem*xmamiinv');
            %PQ = 0.001*(P + Q) + raa0*eeem*xmamiinv;
            P = P + PQ;
            Q = Q + PQ;
            P = P * spdiags(ux2,0,n,n);
            Q = Q * spdiags(xl2,0,n,n);
            b = P*uxinv + Q*xlinv - fval ;
            %
            %%% Solving the subproblem by a primal-dual Newton method
            [xmma,ymma,zmma,lam,xsi,eta,mu,zet,s] = ...
                obj.subsolv(m,n,epsimin,low,upp,alfa,beta,p0,q0,P,Q,a0,a,b,c,d);
        end

        function updateConvergenceStatus(obj)
            kktnorm = obj.historicalVariables.kktnorm;
            has_not_converged = kktnorm > obj.kkttol && obj.outit < obj.maxoutit;
            obj.hasConverged = ~has_not_converged;
        end

        function [c,dc] = checkConstraintCase(obj,c,dc)
            if strcmp(obj.constraintCase,'EQUALITY')
                c  = [c;-c];
                dc = [dc;-dc];
            end
        end

        function updateIterInfo(obj)
            obj.increaseIter();
            obj.updateStatus();
        end

        function increaseIter(obj)
            obj.nIter = obj.nIter + 1;
        end

        function updateStatus(obj)
            obj.hasFinished = obj.hasConverged || obj.hasExceededStepIterations();
        end

        function itHas = hasExceededStepIterations(obj)
            iStep = obj.incrementalScheme.iStep;
            nStep = obj.incrementalScheme.nSteps;
            itHas = obj.nIter >= obj.maxIter*(iStep/nStep);
        end

    end

    methods (Access = private, Static)

        function [xmma,ymma,zmma,lamma,xsimma,etamma,mumma,zetmma,smma] = ...
                subsolv(m,n,epsimin,low,upp,alfa,beta,p0,q0,P,Q,a0,a,b,c,d)
            %
            % This function subsolv solves the MMA subproblem:
            %
            % minimize   SUM[ p0j/(uppj-xj) + q0j/(xj-lowj) ] + a0*z +
            %          + SUM[ ci*yi + 0.5*di*(yi)^2 ],
            %
            % subject to SUM[ pij/(uppj-xj) + qij/(xj-lowj) ] - ai*z - yi < =  bi,
            %            alfaj < =   xj < =   betaj,  yi > =  0,  z > =  0.
            %
            % Input:  m, n, low, upp, alfa, beta, p0, q0, P, Q, a0, a, b, c, d.
            % Output: xmma,ymma,zmma, slack variables and Lagrange multiplers.
            %
            een = ones(n,1);
            eem = ones(m,1);
            epsi = 1;
            epsvecn = epsi*een;
            epsvecm = epsi*eem;
            x = 0.5*(alfa+beta);
            y = eem;
            z = 1;
            lam = eem;
            xsi = een./(x-alfa);
            xsi = max(xsi,een);
            eta = een./(beta-x);
            eta = max(eta,een);
            mu  = max(eem,0.5*c);
            zet = 1;
            s = eem;
            itera = 0;
            while epsi > epsimin
                epsvecn = epsi*een;
                epsvecm = epsi*eem;
                ux1 = upp-x;
                xl1 = x-low;
                ux2 = ux1.*ux1;
                xl2 = xl1.*xl1;
                uxinv1 = een./ux1;
                xlinv1 = een./xl1;
                plam = p0 + P'*lam ;
                qlam = q0 + Q'*lam ;
                gvec = P*uxinv1 + Q*xlinv1;
                dpsidx = plam./ux2 - qlam./xl2 ;
                rex = dpsidx - xsi + eta;
                rey = c + d.*y - mu - lam;
                rez = a0 - zet - a'*lam;
                relam = gvec - a*z - y + s - b;
                rexsi = xsi.*(x-alfa) - epsvecn;
                reeta = eta.*(beta-x) - epsvecn;
                remu = mu.*y - epsvecm;
                rezet = zet*z - epsi;
                res = lam.*s - epsvecm;
                residu1 = [rex' rey' rez]';
                residu2 = [relam' rexsi' reeta' remu' rezet res']';
                residu = [residu1' residu2']';
                residunorm = sqrt(residu'*residu);
                residumax = max(abs(residu));
                ittt = 0;
                while residumax > 0.9*epsi & ittt < 200 % 100
                    ittt = ittt + 1;
                    itera = itera + 1;
                    ux1 = upp-x;
                    xl1 = x-low;
                    ux2 = ux1.*ux1;
                    xl2 = xl1.*xl1;
                    ux3 = ux1.*ux2;
                    xl3 = xl1.*xl2;
                    uxinv1 = een./ux1;
                    xlinv1 = een./xl1;
                    uxinv2 = een./ux2;
                    xlinv2 = een./xl2;
                    plam = p0 + P'*lam ;
                    qlam = q0 + Q'*lam ;
                    gvec = P*uxinv1 + Q*xlinv1;
                    GG = P*spdiags(uxinv2,0,n,n) - Q*spdiags(xlinv2,0,n,n);
                    dpsidx = plam./ux2 - qlam./xl2 ;
                    delx = dpsidx - epsvecn./(x-alfa) + epsvecn./(beta-x);
                    dely = c + d.*y - lam - epsvecm./y;
                    delz = a0 - a'*lam - epsi/z;
                    dellam = gvec - a*z - y - b + epsvecm./lam;
                    diagx = plam./ux3 + qlam./xl3;
                    diagx = 2*diagx + xsi./(x-alfa) + eta./(beta-x);
                    diagxinv = een./diagx;
                    diagy = d + mu./y;
                    diagyinv = eem./diagy;
                    diaglam = s./lam;
                    diaglamyi = diaglam+diagyinv;
                    if m < n
                        blam = dellam + dely./diagy - GG*(delx./diagx);
                        bb = [blam' delz]';
                        Alam = spdiags(diaglamyi,0,m,m) + GG*spdiags(diagxinv,0,n,n)*GG';
                        AA = [Alam     a
                            a'    -zet/z ];
                        solut = AA\bb;
                        dlam = solut(1:m);
                        dz = solut(m+1);
                        dx = -delx./diagx - (GG'*dlam)./diagx;
                    else
                        diaglamyiinv = eem./diaglamyi;
                        dellamyi = dellam + dely./diagy;
                        Axx = spdiags(diagx,0,n,n) + GG'*spdiags(diaglamyiinv,0,m,m)*GG;
                        azz = zet/z + a'*(a./diaglamyi);
                        axz = -GG'*(a./diaglamyi);
                        bx = delx + GG'*(dellamyi./diaglamyi);
                        bz  = delz - a'*(dellamyi./diaglamyi);
                        AA = [Axx   axz
                            axz'  azz ];
                        bb = [-bx' -bz]';
                        solut = AA\bb;
                        dx  = solut(1:n);
                        dz = solut(n+1);
                        dlam = (GG*dx)./diaglamyi - dz*(a./diaglamyi) + dellamyi./diaglamyi;
                    end
                    %
                    dy = -dely./diagy + dlam./diagy;
                    dxsi = -xsi + epsvecn./(x-alfa) - (xsi.*dx)./(x-alfa);
                    deta = -eta + epsvecn./(beta-x) + (eta.*dx)./(beta-x);
                    dmu  = -mu + epsvecm./y - (mu.*dy)./y;
                    dzet = -zet + epsi/z - zet*dz/z;
                    ds   = -s + epsvecm./lam - (s.*dlam)./lam;
                    xx  = [ y'  z  lam'  xsi'  eta'  mu'  zet  s']';
                    dxx = [dy' dz dlam' dxsi' deta' dmu' dzet ds']';
                    %
                    stepxx = -1.01*dxx./xx;
                    stmxx  = max(stepxx);
                    stepalfa = -1.01*dx./(x-alfa);
                    stmalfa = max(stepalfa);
                    stepbeta = 1.01*dx./(beta-x);
                    stmbeta = max(stepbeta);
                    stmalbe  = max(stmalfa,stmbeta);
                    stmalbexx = max(stmalbe,stmxx);
                    stminv = max(stmalbexx,1);
                    steg = 1/stminv;
                    %
                    xold   =   x;
                    yold   =   y;
                    zold   =   z;
                    lamold =  lam;
                    xsiold =  xsi;
                    etaold =  eta;
                    muold  =  mu;
                    zetold =  zet;
                    sold   =   s;
                    %
                    itto = 0;
                    resinew = 2*residunorm;
                    while resinew > residunorm & itto < 50
                        itto = itto+1;
                        x   =   xold + steg*dx;
                        y   =   yold + steg*dy;
                        z   =   zold + steg*dz;
                        lam = lamold + steg*dlam;
                        xsi = xsiold + steg*dxsi;
                        eta = etaold + steg*deta;
                        mu  = muold  + steg*dmu;
                        zet = zetold + steg*dzet;
                        s   =   sold + steg*ds;
                        ux1 = upp-x;
                        xl1 = x-low;
                        ux2 = ux1.*ux1;
                        xl2 = xl1.*xl1;
                        uxinv1 = een./ux1;
                        xlinv1 = een./xl1;
                        plam = p0 + P'*lam ;
                        qlam = q0 + Q'*lam ;
                        gvec = P*uxinv1 + Q*xlinv1;
                        dpsidx = plam./ux2 - qlam./xl2 ;
                        rex = dpsidx - xsi + eta;
                        rey = c + d.*y - mu - lam;
                        rez = a0 - zet - a'*lam;
                        relam = gvec - a*z - y + s - b;
                        rexsi = xsi.*(x-alfa) - epsvecn;
                        reeta = eta.*(beta-x) - epsvecn;
                        remu = mu.*y - epsvecm;
                        rezet = zet*z - epsi;
                        res = lam.*s - epsvecm;
                        residu1 = [rex' rey' rez]';
                        residu2 = [relam' rexsi' reeta' remu' rezet res']';
                        residu = [residu1' residu2']';
                        resinew = sqrt(residu'*residu);
                        steg = steg/2;
                    end
                    residunorm = resinew;
                    residumax = max(abs(residu));
                    steg = 2*steg;
                end
                if ittt > 198
                    epsi;
                    ittt;
                end
                epsi = 0.1*epsi;
            end
            xmma   =   x;
            ymma   =   y;
            zmma   =   z;
            lamma =  lam;
            xsimma =  xsi;
            etamma =  eta;
            mumma  =  mu;
            zetmma =  zet;
            smma   =   s;
        end

        function [residu,residunorm,residumax] = ...
                kktcheck(~,~,x,y,z,lam,xsi,eta,mu,zet,s, ...
                xmin,xmax,df0dx,fval,dfdx,a0,a,c,d)

            rex   = df0dx + dfdx'*lam - xsi + eta;
            rey   = c + d.*y - mu - lam;
            rez   = a0 - zet - a'*lam;
            relam = fval - a*z - y + s;
            rexsi = xsi.*(x-xmin);
            reeta = eta.*(xmax-x);
            remu  = mu.*y;
            rezet = zet*z;
            res   = lam.*s;
            %
            residu1 = [rex' rey' rez]';
            residu2 = [relam' rexsi' reeta' remu' rezet res']';
            residu = [residu1' residu2']';
            residunorm = sqrt(residu'*residu);
            residumax = max(abs(residu));
        end

    end

    methods

        function kkttol = get.kkttol(obj)
            kkttol = obj.targetParameters.optimality_tol;
        end

    end
end


function x = combin_eigen_right ( alpha, beta, n )

%*****************************************************************************80
%
%% COMBIN_EIGEN_RIGHT returns the right eigenvectors of the COMBIN matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 November 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real ALPHA, BETA, scalars that define A.
%
%    Input, integer N, the order of the matrix.
%
%    Output, real X(N,N), the right eigenvectors.
%
  x(1:n,1:n) = 0.0;

  for j = 1 : n - 1
    x(  1,j) = +1.0;
    x(j+1,j) = -1.0;
  end

  j = n;
  x(1:n,j) = 1.0;

  return
end


function [mmHg] = psi2mmHg(psi)
% Convert units of pressure from pounds per square inch to millimeters of mercury.
% Chad Greene 2012
mmHg = psi*51.7149;


function out = tangentspace(A, B)

% Get tangent space between A and B

for i = 1:size(A, 3)
    out(:, :, i) = sqrtm(B) * logm(B ^ (-1 / 2) * A(:, :, i) * B ^ (-1 / 2)) * sqrtm(B);
end


function [b a]=get_high_shelving_filter(g,Q,f,Fs)

A=10^(g/40);
w=2*pi*f/Fs;
sn=sin(w);
cs=cos(w);
al=sn/(2*Q);
bt=sqrt(A)/Q;

b=A*[(A+1)+(A-1)*cs+bt*sn,...
   -2*((A-1)+(A+1)*cs),...
   (A+1)+(A-1)*cs-bt*sn];

a=[(A+1)-(A-1)*cs+bt*sn,...
   2*((A-1)-(A+1)*cs),...
   (A+1)-(A-1)*cs-bt*sn];

b=b/a(1);
a=a/a(1);


function [Cbe_new, Ve_new, ecef_new]=strapdown_ecef_dcm(Cbe, Ve, ecef, a, w, dt)
%Gravity (most time consuming part of ecef implementations)
Llh=ecef2geo_v000(ecef,0);
[Rn, Re, g, sL, cL, WIE_E]=geoparam_v000(Llh);
Cne=pos2Cne_v000(Llh(1), Llh(2));
ge=-Cne(:,3)*g;

%Update attitude
rot=w*dt;
rot_norm=norm(rot);
sr_a=1-(rot_norm^2/6)+(rot_norm^4/120);
sr_b=(1/2)-(rot_norm^2/24)+(rot_norm^4/720);
mx_a=eye(3)+sr_a*skew(rot)+sr_b*skew(rot)*skew(rot);

rot=-([0;0;WIE_E])*dt;
rot_norm=norm(rot);
sr_a=1-(rot_norm^2/6)+(rot_norm^4/120);
sr_b=(1/2)-(rot_norm^2/24)+(rot_norm^4/720);
mx_b=eye(3)+sr_a*skew(rot)+sr_b*skew(rot)*skew(rot);

Cbe_new=mx_b*Cbe*mx_a;

%%Update Velocity
%Update Vel
vel_inc1=Cbe*a*dt;
vel_inc2=(-ge+2*cross(Ve,[0;0;WIE_E]))*dt;
Ve_new=Ve+vel_inc1+vel_inc2;

%Update_pos
ecef_new=ecef+Ve*dt;


function varargout = log10(varargin)
%log10 (overloaded)

varargout{1} = log(varargin{1})/log(10);