function ll = robOneDynamicsLogLikelihood(model)

% ROBONEDYNAMICSLOGLIKELIHOOD Give the log likelihood of the robot one dynamics part.

% FGPLVM

thetaDiff = model.theta(2:end)-model.theta(1:end-1);
while any(thetaDiff>pi)
  ind = find(thetaDiff>pi);
  thetaDiff(ind) = thetaDiff(ind) - 2*pi;
end
while any(thetaDiff<-pi)
  ind = find(thetaDiff<-pi);
  thetaDiff(ind) = thetaDiff(ind) + 2*pi;
end

logLikeTheta = log(2*pi*model.sigma2)...
    +(thetaDiff.*thetaDiff)/model.sigma2;
likeTheta = exp(-0.5*logLikeTheta)*model.mixTheta;
likeTheta = likeTheta + (1-model.mixTheta)/(2*pi);
ll = sum(log(likeTheta));

logLikeR1 = model.a*log(model.b) - gammaln(model.a) ...
    +(model.a-1)*log(model.r) - model.b*model.r + log(model.mixR);
logLikeR2 = log(model.b) - model.b*model.r + log(1-model.mixR);

logLikeR1(find(logLikeR1<-316))=-316;
logLikeR2(find(logLikeR2<-316))=-316;

ind = find(logLikeR1>logLikeR2);
logLikeR(ind) = log(1+exp(logLikeR2(ind))./exp(logLikeR1(ind)))+logLikeR1(ind);

ind2 = find(logLikeR1<=logLikeR2);
logLikeR(ind2) = log(1+exp(logLikeR1(ind2))./exp(logLikeR2(ind2)))+logLikeR2(ind2);

ll = ll + sum(logLikeR);


% LSADM (Goldfarb et al. 2010)
% process_video('RPCA', 'LSADM', 'dataset/demo.avi', 'output/demo_LSADM.avi');
opts.D = M;
opts.mu = norm(M)/1.25;
[n1,n2] = size(M);
opts.Xs = M;
opts.Ys = M;
opts.n1 = n1;
opts.n2 = n2;
opts.sigma = 1e-6;
opts.maxitr = 500;
opts.rho = 1/sqrt(n1);
opts.eta_mu = 2/3;
opts.eta_sigma = 2/3;
opts.muf = 1e-6;
opts.sigmaf = 1e-6;
opts.epsilon = 1e-7;
opts.sv = 100;
out_ALM = ALM_SADAL_smoothed(opts.D,opts);
L = out_ALM.X;
S = out_ALM.Y;


function Q_gpfa = test_gpfa(seed)

if nargin >= 1
  randn('state', seed);
  rand('state', seed);
end

%
% Generate some data
%

D = 3;

in_x = 1:200;
N_x = length(in_x);
X = zeros(D,N_x);
X(1,:) = in_x/N_x;%sin(2*pi*in_w/10);
X(2,:) = cos(2*pi*in_x/7);
X(3,:) = randn(N_x,1);

in_w = 1:100;
N_w = length(in_w);
W = zeros(D,N_w);
W(1,:) = cos(2*pi*in_w/20);
W(2,:) = cos(2*pi*in_w/10);
W(3,:) = randn(N_w,1);

s = 0.5;
Y_noiseless = W'*X;
Y = Y_noiseless + s*randn(N_w,N_x);

Imv = (rand(size(Y)) < 0.4);
Imv(:,15) = true;
Y(Imv) = NaN;

%
% GP module for X
%

N_p = 10;
pseudo_x = linspace(min(in_x),max(in_x), N_p);
is_pseudo_x = false(D,1);

D2_xx = sq_dist(in_x);
D2_pp = sq_dist(pseudo_x);
D2_px = sq_dist(pseudo_x, in_x);

% Covariance functions
covfunc = @(D2) gp_cov_se(D2);
covfunc_x = cell(D,1);
% $$$ covfunc_x{1} = gp_cov_jitter(gp_cov_se(D2_xx));
covfunc_x{1} = gp_cov_pseudo(gp_cov_jitter(gp_cov_se(D2_pp)), ...
                             gp_cov_se(D2_px), ...
                             gp_cov_se(diag(D2_xx)));
is_pseudo_x(1) = true;
theta_x{1} = 30;

covfunc_x{2} = gp_cov_jitter(covfunc(D2_xx));
theta_x{2} = 3;

covfunc_x{3} = gp_cov_jitter(gp_cov_pp(sqrt(D2_xx),1));
theta_x{3} = 1.1;

% $$$ figure
% $$$ imagesc(covfunc_x{1}(theta_x{1}));
% $$$ figure
% $$$ imagesc(covfunc_x{2}(theta_x{2}));
% $$$ return

%
% GP module for W
%

D2_ww = sq_dist(in_w);

% Covariance functions
covfunc_w = cell(D,1);
covfunc_w{1} = gp_cov_scale(gp_cov_jitter(gp_cov_se(D2_ww)));
theta_w{1} = [1; 3];
covfunc_w{2} = gp_cov_scale(gp_cov_jitter(gp_cov_se(D2_ww)));
theta_w{2} = [1; 2];
covfunc_w{3} = gp_cov_scale(gp_cov_jitter(gp_cov_pp(sqrt(D2_ww),1)));
theta_w{3} = [1; 1.1];

%
% Isotropic noise module
%

%noise_module = noise_module_fixed(1/s^2 * ones(N_w, N_x));
noise_module = noise_module_isotropic(N_w, N_x, 1e-3, 1e-3, 'init', 100);

%
% VB inference
%

%
% TODO:
%
% - pseudo inputs
%
% - put the noise to Q(W)
%
% - test more components
%
% - rotation, optimize the hyperparameters jointly?
%
% - weighted noise
%

%
% GPFA
%

% GP modules with component-wise factorization

% NOTES:
%
% The algorithm can be quite sensitive to hyperparameter initialization and
% update schedule. Also, it seems that it's best to do rotations rarely and
% after the hyperparameters have been updated at least once. The rotations
% in GPFA are approximately optimized, so some problems can be caused by
% that. So it might be a good idea NOT to use rotations..

% Update only the smooth components at the beginning
maxiter = 30;
update_schedule = cell(maxiter,1);
update_schedule(:) = {1:D};
update_schedule(1:10) = {1};
%update_schedule(6:10) = {1:2};

X_module = factor_module_gp_factorized(N_x, covfunc_x, theta_x, ...
                                       'is_pseudo', is_pseudo_x, ...
                                       'update_hyperparameters', [5:5:1000], ...
                                       'update_schedule', {update_schedule});
W_module = factor_module_gp_factorized(N_w, covfunc_w, theta_w, ...
                                       'update_hyperparameters', [5:5:1000], ...
                                       'update_schedule', {update_schedule});

Q_gpfa = vbfa(D, Y, W_module, X_module, noise_module, ...
              'maxiter', maxiter, ...
              'update_noise', 1, ...
              'rotate', false, ...
              'rotation_checkgrad', false);

figure
plot(Q_gpfa.loglikelihood)

%
% PCA
%

X_module = factor_module_iid(D, N_x);
W_module = factor_module_ard(D, N_w);

Q_pca = vbfa(D, Y, W_module, X_module, noise_module, ...
         'maxiter', 30, ...
         'update_noise', 1, ...
         'rotate', 1);

%
% Results
%

hax = tsplot(X);
title(hax(1), 'True X');
hax = tsplot(W);
title(hax(1), 'True W');

hax = tsplot(Q_gpfa.X);
title(hax(1), 'GPFA X');
hax = tsplot(Q_gpfa.W);
title(hax(1), 'GPFA W');

hax = tsplot(Q_pca.X);
title(hax(1), 'PCA X');
hax = tsplot(Q_pca.W);
title(hax(1), 'PCA W');

Yh_gpfa = Q_gpfa.W'*Q_gpfa.X;
Yh_pca = Q_pca.W'*Q_pca.X;
hax = tsplot([Y_noiseless(:,15)';
              Yh_gpfa(:,15)';
              Yh_pca(:,15)']);
title(hax(1), 'Reconstructions of observations');

% Comparison
%noise_std_gpfa = Q_gpfa.Tau(1)^(-0.5);
%noise_std_pca = Q_pca.Tau(1)^(-0.5);
recon_error_gpfa = rmse(Y_noiseless, Yh_gpfa)
recon_error_pca = rmse(Y_noiseless, Yh_pca)
test_error_gpfa = rmse(Y_noiseless(Imv), Yh_gpfa(Imv))
test_error_pca = rmse(Y_noiseless(Imv), Yh_pca(Imv))


%mmcfindls(lambda,mu,c)
%   This function finds the average system size
%   for an M/M/c queueing system.

function out = mmcfindls(lambda,mu,c)

pc = lambda/mu;
lq = mmcfindlq(lambda,mu,c);

ls = lq + pc;

out = ls;


function [MPa] = inH2O2MPa(inH2O)
% Convert pressure from inches of water column at 4 degrees to megapascals
% Chad Greene 2012
MPa = inH2O*0.000249089;


function [bor,F0,F1] = spm_BMS_bor(L,posterior,priors,C)
% Compute Bayes Omnibus Risk
% FORMAT [bor,F0,F1] = spm_BMS_bor(L,posterior,priors,C)
%
% L         Log model evidence table (models x  subjects)
% posterior .a model counts, .r model-subject probs
% priors    .a model counts
% C         if this field is specified then BOR under family prior
%           is computed, otherwise BOR under model prior is computed.
%           C(k,f) = 1 if model k belongs to family f (0 otherwise)
%
% REFERENCES:
%
% Rigoux, L, Stephan, KE, Friston, KJ and Daunizeau, J. (2014)
% Bayesian model selection for group studies - Revisited.
% NeuroImage 84:971-85. doi: 10.1016/j.neuroimage.2013.08.065
%__________________________________________________________________________
% Copyright (C) 2015 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: spm_BMS_bor.m 6444 2015-05-21 11:15:48Z guillaume $

if nargin < 4
    options.families = 0;
    % Evidence of null (equal model freqs)
    F0 = FE_null(L,options);
else
    options.families = 1;
    options.C = C;
    % Evidence of null (equal model freqs) under family prior
    [tmp,F0] = FE_null(L,options);
end

% Evidence of alternative
F1 = FE(L,posterior,priors);

% Implied by Eq 5 (see also p39) in Rigoux et al.
% See also, last equation in Appendix 2
bor = 1/(1+exp(F1-F0));

function [F,ELJ,Sqf,Sqm] = FE(L,posterior,priors)
% derives the free energy for the current approximate posterior
% This routine has been copied from the VBA_groupBMC function
% of the VBA toolbox http://code.google.com/p/mbb-vb-toolbox/
% and was written by Lionel Rigoux and J. Daunizeau
%
% See equation A.20 in Rigoux et al. (should be F1 on LHS)

[K,n] = size(L);
a0 = sum(posterior.a);
Elogr = psi(posterior.a) - psi(sum(posterior.a));
Sqf = sum(gammaln(posterior.a)) - gammaln(a0) - sum((posterior.a-1).*Elogr);
Sqm = 0;
for i=1:n
    Sqm = Sqm - sum(posterior.r(:,i).*log(posterior.r(:,i)+eps));
end
ELJ = gammaln(sum(priors.a)) - sum(gammaln(priors.a)) + sum((priors.a-1).*Elogr);
for i=1:n
    for k=1:K
        ELJ = ELJ + posterior.r(k,i).*(Elogr(k)+L(k,i));
    end
end
F = ELJ + Sqf + Sqm;

function [F0m,F0f] = FE_null (L,options)
% Free energy of the 'null' (H0: equal frequencies)
%
% F0m       Evidence for null (ie. equal probs) over models
% F0f       Evidence for null (ie. equal probs) over families
%
% This routine derives from the VBA_groupBMC function
% of the VBA toolbox http://code.google.com/p/mbb-vb-toolbox/
% written by Lionel Rigoux and J. Daunizeau
%
% See Equation A.17 in Rigoux et al.

[K,n] = size(L);
if options.families
    f0 = options.C*sum(options.C,1)'.^-1/size(options.C,2);
    F0f = 0;
else
    F0f = [];
end
F0m = 0;
for i=1:n
    tmp = L(:,i) - max(L(:,i));
    g = exp(tmp)./sum(exp(tmp));
    for k=1:K
        F0m = F0m + g(k).*(L(k,i)-log(K)-log(g(k)+eps));
        if options.families
            F0f = F0f + g(k).*(L(k,i)-log(g(k)+eps)+log(f0(k)));
        end
    end
end


function varargout=wt(d,varargin)
%% Continous Wavelet Transform
% Creates a figure of wavelet power in units of
% normalized variance.
%
% USAGE: [wave,period,scale,coi,sig95]=wt(d[,params])
%
% d: a time series
% wave: the wavelet transform of d
% period: a vector of "Fourier" periods associated with wave
% scale: a vector of wavelet scales associated with wave
% coi: the cone of influence
%
% Settings: Pad: pad the time series with zeros?
% .         Dj: Octaves per scale (default: '1/12')
% .         S0: Minimum scale
% .         J1: Total number of scales
% .         Mother: Mother wavelet (default 'morlet')
% .         MaxScale: An easier way of specifying J1
% .         MakeFigure: Make a figure or simply return the output.
% .         BlackandWhite: Create black and white figures
% .         AR1: the ar1 coefficient of the series
% .              (default='auto' using a naive ar1 estimator. See ar1nv.m)
%
% Settings can also be specified using abbreviations. e.g. ms=MaxScale.
% For detailed help on some parameters type help wavelet.
%
%
% Example:
%      wt([0:200;sin(0:200)],'dj',1/20,'bw','maxscale',32)
%
% (C) Aslak Grinsted 2002-2014
%
% http://www.glaciology.net/wavelet-coherence

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

% ------validate and reformat timeseries.
[d,dt]=formatts(d);

n=size(d,1);
sigma2=var(d(:,2));

%----------default arguments for the wavelet transform-----------
Args=struct('Pad',1,...      % pad the time series with zeroes (recommended)
    'Dj',1/12, ...    % this will do 12 sub-octaves per octave
    'S0',2*dt,...    % this says start at a scale of 2 years
    'J1',[],...
    'Mother','Morlet', ...
    'MaxScale',[],...   %a more simple way to specify J1
    'MakeFigure',(nargout==0),...
    'AR1','auto');
Args=parseArgs(varargin,Args,{'BlackandWhite'});
if isempty(Args.J1)
    if isempty(Args.MaxScale)
        Args.MaxScale=(n*.17)*2*dt; %automaxscale
    end
    Args.J1=round(log2(Args.MaxScale/Args.S0)/Args.Dj);
end

if strcmpi(Args.AR1,'auto')
    Args.AR1=ar1nv(d(:,2));

    if any(isnan(Args.AR1))
        error('Automatic AR1 estimation failed. Specify it manually (use arcov or arburg).')
    end
end

%----------------::::::::---------- Analyze: ---------:::::::::::::-----------------

[wave,period,scale,coi] = wavelet(d(:,2),dt,Args.Pad,Args.Dj,Args.S0,Args.J1,Args.Mother);

t=d(:,1);
power = (abs(wave)).^2 ;        % compute wavelet power spectrum
signif = wave_signif(1.0,dt,scale,0,Args.AR1,-1,-1,Args.Mother);
sig95 = (signif')*(ones(1,n));  % expand signif --> (J+1)x(N) array
sig95 = power ./ (sigma2*sig95);
Yticks = 2.^(fix(log2(min(period))):fix(log2(max(period))));

if Args.MakeFigure
    H=imagesc(t,log2(period),log2(abs(power/sigma2)));%#ok,log2(levels));  %*** or use 'contourfill'
    %logpow=log2(abs(power/sigma2));
    %[c,H]=contourf(t,log2(period),logpow,[min(logpow(:)):.25:max(logpow(:))]);
    %set(H,'linestyle','none')

    clim=get(gca,'clim'); %center color limits around log2(1)=0
    clim=[-1 1]*max(clim(2),3);
    set(gca,'clim',clim)

    HCB=colorbar;
    set(HCB,'ytick',-7:7);
    barylbls=rats(2.^(get(HCB,'ytick')'));
    barylbls([1 end],:)=' ';
    barylbls(:,all(barylbls==' ',1))=[];
    set(HCB,'yticklabel',barylbls);

    set(gca,'YLim',log2([min(period),max(period)]), ...
        'YDir','reverse', ...
        'YTick',log2(Yticks(:)), ...
        'YTickLabel',num2str(Yticks'), ...
        'layer','top')
    %xlabel('Time')
    ylabel('Period')
    hold on

    [c,h] = contour(t,log2(period),sig95,[1 1],'k'); %#ok
    set(h,'linewidth',2)
    %plot(t,log2(coi),'k','linewidth',3)
    tt=[t([1 1])-dt*.5;t;t([end end])+dt*.5];
    hcoi=fill(tt,log2([period([end 1]) coi period([1 end])]),'w');
    set(hcoi,'alphadatamapping','direct','facealpha',.5)

    hold off
    set(gca,'box','on','layer','top');
end
varargout={wave,period,scale,coi,sig95};
varargout=varargout(1:nargout);


function imResult = blendMode_Lighten(A, B, offsetW, offsetH)
%% Lighten blending mode: compares the color information for each pixel of
%   the base and the blend color and applies the lighter color as the
%   result.
%   Any pixels in the base image that are darker than the blend color are
%   replaced, and pixels that are lighter are left unchanged. No part of
%   the image will become darker.
%
% Input:
%       A       -       Base Image
%       B       -       Top Image
%   offsetW     -   move picture B horizontally in respect to the top-left
%                   corner of picture A. Default value = 1.
%   offsetH     -   move picture B vertically in respect to the top-left
%                   corner of picture A. Default value = 1.
%
% Output:
%       imResult    -   Result of the blending, having the same size of the
%                       Base Image A.
%

%% Check Input
a = size(A);
b = size(B);
blendMode_checkInput(nargin, a, b, func2str(@blendMode_Lighten));

if nargin < 3
    offsetW = 1;
    offsetH = 1;
end

if nargin < 4
    offsetH = 1;
end

%% Implementation
imResult = A;

if (((offsetW ~= 1) || (offsetH ~= 1)) || (sum(a == b) ~= length(a)))
    [A, B] = blendMode_ResizeImages(A, B, a, b, offsetW, offsetH);
end

ind = B > A;
C = abs(ind - 1) .* A + ind .* B;

if (((offsetW ~= 1) || (offsetH ~= 1)) || (sum(a == b) ~= length(a)))
    imResult = blendMode_CreateResult(imResult, C, offsetW, offsetH);
else
    imResult = C;
end

end


function [Y, optinf] = bpdn(D, S, lambda, opt)

% bpdn -- Basis Pursuit DeNoising
%
%         argmin_x (1/2)||D*x - s||_2^2 + lambda*||x||_1
%
%         The solution of the BPDN problem (see chen-1998-atomic) is
%         computed using the ADMM approach (see boyd-2010-distributed).
%
% Usage:
%       [Y, optinf] = bpdn(D, S, lambda, opt)
%
% Input:
%       D           Dictionary matrix
%       S           Signal vector (or matrix)
%       lambda      Regularization parameter
%       opt         Options/algorithm parameters structure (see below)
%
% Output:
%       Y           Dictionary coefficient vector (or matrix)
%       optinf      Details of optimisation
%
%
% Options structure fields:
%   Verbose           Flag determining whether iteration status is displayed.
%                     Fields are iteration number, functional value,
%                     data fidelity term, l1 regularisation term, and
%                     primal and dual residuals (see Sec. 3.3 of
%                     boyd-2010-distributed). The value of rho is also
%                     displayed if options request that it is automatically
%                     adjusted.
%   MaxMainIter       Maximum main iterations
%   AbsStopTol        Absolute convergence tolerance (see Sec. 3.3.1 of
%                     boyd-2010-distributed)
%   RelStopTol        Relative convergence tolerance (see Sec. 3.3.1 of
%                     boyd-2010-distributed)
%   L1Weight          Weighting array for coefficients in l1 norm of X
%   Y0                Initial value for Y
%   U0                Initial value for U
%   rho               ADMM penalty parameter
%   AutoRho           Flag determining whether rho is automatically updated
%                     (see Sec. 3.4.1 of boyd-2010-distributed)
%   AutoRhoPeriod     Iteration period on which rho is updated
%   RhoRsdlRatio      Primal/dual residual ratio in rho update test
%   RhoScaling        Multiplier applied to rho when updated
%   AutoRhoScaling    Flag determining whether RhoScaling value is
%                     adaptively determined (see wohlberg-2015-adaptive). If
%                     enabled, RhoScaling specifies a maximum allowed
%                     multiplier instead of a fixed multiplier.
%   RhoRsdlTarget     Residual ratio targeted by auto rho update policy.
%   StdResiduals      Flag determining whether standard residual definitions
%                     (see Sec 3.3 of boyd-2010-distributed) are used instead
%                     of normalised residuals (see wohlberg-2015-adaptive)
%   RelaxParam        Relaxation parameter (see Sec. 3.4.3 of
%                     boyd-2010-distributed)
%   NonNegCoef        Flag indicating whether solution should be forced to
%                     be non-negative
%   AuxVarObj         Flag determining whether objective function is computed
%                     using the auxiliary (split) variable
%
%
% Author: Brendt Wohlberg <brendt@lanl.gov>  Modified: 2015-07-23
%
% This file is part of the SPORCO library. Details of the copyright
% and user license can be found in the 'License' file distributed with
% the library.

if nargin < 4,
  opt = [];
end
checkopt(opt, defaultopts([]));
opt = defaultopts(opt);

% Default lambda is 1/10 times the lambda value beyond which the
% solution is a zero vector
if nargin < 3 | isempty(lambda),
  lambda = 0.1*max(vec(abs(D'*S)));
end

% Set up status display for verbose operation
hstr = 'Itn   Fnc       DFid      l1        r         s      ';
sfms = '%4d %9.2e %9.2e %9.2e %9.2e %9.2e';
nsep = 54;
if opt.AutoRho,
  hstr = [hstr '   rho   '];
  sfms = [sfms ' %9.2e'];
  nsep = nsep + 10;
end
if opt.Verbose && opt.MaxMainIter > 0,
  disp(hstr);
  disp(char('-' * ones(1,nsep)));
end

% Start timer
tstart = tic;

% Set up algorithm parameters and initialise variables
rho = opt.rho;
if isempty(rho), rho = 50*lambda+1; end;
[Nr, Nc] = size(D);
Nm = size(S,2);
Nx = Nc*Nm;
DTS = D'*S;
[luL, luU] = factorise(D, rho);
optinf = struct('itstat', [], 'opt', opt);
r = Inf;
s = Inf;
epri = 0;
edua = 0;

% Initialise main working variables
X = [];
if isempty(opt.Y0),
  Y = zeros(Nc,Nm);
else
  Y = opt.Y0;
end
Yprv = Y;
if isempty(opt.U0),
  if isempty(opt.Y0),
    U = zeros(Nc,Nm);
  else
    U = (lambda/rho)*sign(Y);
  end
else
  U = opt.U0;
end

% Main loop
k = 1;
while k <= opt.MaxMainIter && (r > epri | s > edua),

  % Solve X subproblem
  X = linsolve(D, rho, luL, luU, DTS + rho*(Y - U));

  % See pg. 21 of boyd-2010-distributed
  if opt.RelaxParam == 1,
    Xr = X;
  else
    Xr = opt.RelaxParam*X + (1-opt.RelaxParam)*Y;
  end

  % Solve Y subproblem
  Y = shrink(Xr + U, (lambda/rho)*opt.L1Weight);
  if opt.NonNegCoef,
    Y(Y < 0) = 0;
  end

  % Update dual variable
  U = U + Xr - Y;

  % Objective function and convergence measures
  if opt.AuxVarObj,
    Jdf = sum(vec(abs(D*Y - S).^2))/2;
    Jl1 = sum(abs(vec(bsxfun(@times, opt.L1Weight, Y))));
  else
    Jdf = sum(vec(abs(D*X - S).^2))/2;
    Jl1 = sum(abs(vec(bsxfun(@times, opt.L1Weight, X))));
  end
  Jfn = Jdf + lambda*Jl1;

  nX = norm(X(:)); nY = norm(Y(:)); nU = norm(U(:));
  if opt.StdResiduals,
    % See pp. 19-20 of boyd-2010-distributed
    r = norm(vec(X - Y));
    s = norm(vec(rho*(Yprv - Y)));
    epri = sqrt(Nx)*opt.AbsStopTol+max(nX,nY)*opt.RelStopTol;
    edua = sqrt(Nx)*opt.AbsStopTol+rho*nU*opt.RelStopTol;
  else
    % See wohlberg-2015-adaptive
    r = norm(vec(X - Y))/max(nX,nY);
    s = norm(vec(Yprv - Y))/nU;
    epri = sqrt(Nx)*opt.AbsStopTol/max(nX,nY)+opt.RelStopTol;
    edua = sqrt(Nx)*opt.AbsStopTol/(rho*nU)+opt.RelStopTol;
  end

  % Record and display iteration details
  tk = toc(tstart);
  optinf.itstat = [optinf.itstat; [k Jfn Jdf Jl1 r s epri edua rho tk]];
  if opt.Verbose,
    if opt.AutoRho,
      disp(sprintf(sfms, k, Jfn, Jdf, Jl1, r, s, rho));
    else
      disp(sprintf(sfms, k, Jfn, Jdf, Jl1, r, s));
    end
  end

  % See wohlberg-2015-adaptive and pp. 20-21 of boyd-2010-distributed
  if opt.AutoRho,
    if k ~= 1 && mod(k, opt.AutoRhoPeriod) == 0,
      if opt.AutoRhoScaling,
        rhomlt = sqrt(r/(s*opt.RhoRsdlTarget));
        if rhomlt < 1, rhomlt = 1/rhomlt; end
        if rhomlt > opt.RhoScaling, rhomlt = opt.RhoScaling; end
      else
        rhomlt = opt.RhoScaling;
      end
      rsf = 1;
      if r > opt.RhoRsdlTarget*opt.RhoRsdlRatio*s, rsf = rhomlt; end
      if s > (opt.RhoRsdlRatio/opt.RhoRsdlTarget)*r, rsf = 1/rhomlt; end
      rho = rsf*rho;
      U = U/rsf;
      if rsf ~= 1,
        [luL, luU] = factorise(D, rho);
      end
    end
  end

  Yprv = Y;
  k = k + 1;

end

% Record run time and working variables
optinf.runtime = toc(tstart);
optinf.X = X;
optinf.Y = Y;
optinf.U = U;
optinf.lambda = lambda;
optinf.rho = rho;

% End status display for verbose operation
if opt.Verbose && opt.MaxMainIter > 0,
  disp(char('-' * ones(1,nsep)));
end

return

function u = vec(v)

  u = v(:);

return

function u = shrink(v, lambda)

  if isscalar(lambda),
    u = sign(v).*max(0, abs(v) - lambda);
  else
    u = sign(v).*max(0, bsxfun(@minus, abs(v), lambda));
  end

return

function [L,U] = factorise(A, c)

  [N,M] = size(A);
  % If N < M it is cheaper to factorise A*A' + cI and then use the
  % matrix inversion lemma to compute the inverse of A'*A + cI
  if N >= M,
    [L,U] = lu(A'*A + c*eye(M,M));
  else
    [L,U] = lu(A*A' + c*eye(N,N));
  end

return

function x = linsolve(A, c, L, U, b)

  [N,M] = size(A);
  if N >= M,
    x = U \ (L \ b);
  else
    x = (b - A'*(U \ (L \ (A*b))))/c;
  end

return

function opt = defaultopts(opt)

  if ~isfield(opt,'Verbose'),
    opt.Verbose = 0;
  end
  if ~isfield(opt,'MaxMainIter'),
    opt.MaxMainIter = 1000;
  end
  if ~isfield(opt,'AbsStopTol'),
    opt.AbsStopTol = 0;
  end
  if ~isfield(opt,'RelStopTol'),
    opt.RelStopTol = 1e-4;
  end
  if ~isfield(opt,'L1Weight'),
    opt.L1Weight = 1;
  end
  if ~isfield(opt,'Y0'),
    opt.Y0 = [];
  end
  if ~isfield(opt,'U0'),
    opt.U0 = [];
  end
  if ~isfield(opt,'rho'),
    opt.rho = [];
  end
  if ~isfield(opt,'AutoRho'),
    opt.AutoRho = 1;
  end
  if ~isfield(opt,'AutoRhoPeriod'),
    opt.AutoRhoPeriod = 10;
  end
  if ~isfield(opt,'RhoRsdlRatio'),
    opt.RhoRsdlRatio = 1.2;
  end
  if ~isfield(opt,'RhoScaling'),
    opt.RhoScaling = 100;
  end
  if ~isfield(opt,'AutoRhoScaling'),
    opt.AutoRhoScaling = 1;
  end
  if ~isfield(opt,'RhoRsdlTarget'),
    opt.RhoRsdlTarget = 1;
  end
  if ~isfield(opt,'StdResiduals'),
    opt.StdResiduals = 0;
  end
  if ~isfield(opt,'RelaxParam'),
    opt.RelaxParam = 1.8;
  end
  if ~isfield(opt,'NonNegCoef'),
    opt.NonNegCoef = 0;
  end
  if ~isfield(opt,'AuxVarObj'),
    opt.AuxVarObj = 1;
  end

return


function a = p03_a ( m, n )

%*****************************************************************************80
%
%% P03_A returns the matrix A for problem 3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 April 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Cleve Moler,
%    Numerical Computing with MATLAB,
%    SIAM, 2004,
%    ISBN13: 978-0-898716-60-3,
%    LC: QA297.M625,
%    ebook: http://www.mathworks.com/moler/chapters.html
%
%  Parameters:
%
%    Input, integer M, the number of equations.
%
%    Input, integer N, the number of variables.
%
%    Output, real A(M,N), the matrix.
%
  a = [  1.0,  2.0,  3.0; ...
         4.0,  5.0,  6.0; ...
         7.0,  8.0,  9.0; ...
        10.0, 11.0, 12.0; ...
        13.0, 14.0, 15.0 ];

  return
end


function lambda = pei_eigenvalues ( alpha, n )

%*****************************************************************************80
%
%% PEI_EIGENVALUES returns the eigenvalues of the PEI matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real ALPHA, the scalar that defines the Pei matrix.  A
%    typical value of ALPHA is 1.0.
%
%    Input, integer N, the order of A.
%
%    Output, real LAMBDA(N,1), the eigenvalues.
%
  lambda = zeros ( n, 1 );

  lambda(1:n-1,1) = alpha;
  lambda(n,1) = alpha + n;

  return
end


function y = ExtendArray(x,s,fill)

%ExtendArray - Extend existing array to a given (larger) size.
%
%  USAGE
%
%    y = ExtendArray(x,s)
%
%    x              input array
%    s              output size, must have the same number of dimensions as x
%    fill           optional value for new elements (default: nan)
%

% Copyright (C) 2013 by Michaël Zugaro
%
% 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.

% Check inputs
if nargin < 2,
	error('Incorrect number of parameters (type ''help ExtendArray'' for details).');
end
currentSize = size(x);
if length(currentSize) ~= length(s),
	error('Array and size do not have the same number of dimensions (type ''help ExtendArray'' for details).');
end

% Create extended array, fill with requested value
if nargin < 3,
	y = nan(s);
else
	if ~isscalar(fill),
		error('Incorrect fill value (type ''help ExtendArray'' for details).');
	end
	switch(fill),
		case 0,
			y = zeros(s);
		case 1,
			y = ones(s);
		case inf,
			y = inf(s);
		otherwise,
			y = fill*ones(s);
	end
end

% Copy old array into new array
indices = arrayfun(@(x) 1:x,currentSize,'uniformoutput',0);
y(indices{:}) = x;


function i4_gcd_test ( )

%*****************************************************************************80
%
%% I4_GCD_TEST tests I4_GCD.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 April 2009
%
%  Author:
%
%    John Burkardt
%
  test_num = 7;

  i_test = [ 36, 49, 0, 12, 36, 1, 91 ];
  j_test = [ 30, -7, 71, 12, 49, 42, 28 ];

  fprintf ( 1, '\n' );
  fprintf ( 1, 'I4_GCD_TEST\n' );
  fprintf ( 1, '  I4_GCD computes the greatest common factor\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '     I     J   I4_GCD\n' );
  fprintf ( 1, '\n' );

  for test = 1 : test_num
    i = i_test(test);
    j = j_test(test);
    fprintf ( 1, '  %6d  %6d  %6d\n', i, j, i4_gcd ( i, j ) );
  end

  return
end


function [mm] = um2mm(um)
% Convert length from micrometers (or microns) to millimeters.
% Chad A. Greene 2012
mm = um*0.001;


classdef MaF14 < PROBLEM
% <multi/many> <real> <large/none>
% LSMOP3

%------------------------------- Reference --------------------------------
% R. Cheng, M. Li, Y. Tian, X. Zhang, S. Yang, Y. Jin, and X. Yao, A
% benchmark test suite for evolutionary many-objective optimization,
% Complex & Intelligent Systems, 2017, 3(1): 67-81.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    properties(Access = private)
        sublen;	% Number of variables in each subcomponent
        len;    % Cumulative sum of lengths of variable groups
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            % Parameter setting
            if isempty(obj.M); obj.M = 3; end
            if isempty(obj.D); obj.D = 20*obj.M; end
            obj.lower    = zeros(1,obj.D);
            obj.upper    = [ones(1,obj.M-1),10.*ones(1,obj.D-obj.M+1)];
            obj.encoding = ones(1,obj.D);
            % Calculate the number of variables in each subcomponent
            nk = 2;
            c  = 3.8*0.1*(1-0.1);
            for i = 1 : obj.M-1
                c = [c,3.8.*c(end).*(1-c(end))];
            end
            obj.sublen = floor(c./sum(c).*(obj.D-obj.M+1)/nk);
            obj.len    = [0,cumsum(obj.sublen*nk)];
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            [N,D] = size(PopDec);
            M     = obj.M;
            nk    = 2;
            PopDec(:,M:D) = (1+repmat((M:D)./D,N,1)).*PopDec(:,M:D) - repmat(PopDec(:,1)*10,1,D-M+1);
            G = zeros(N,M);
            for i = 1 : 2 : M
                for j = 1 : nk
                    G(:,i) = G(:,i) + Rastrigin(PopDec(:,obj.len(i)+M-1+(j-1)*obj.sublen(i)+1:obj.len(i)+M-1+j*obj.sublen(i)));
                end
            end
            for i = 2 : 2 : M
                for j = 1 : nk
                    G(:,i) = G(:,i) + Rosenbrock(PopDec(:,obj.len(i)+M-1+(j-1)*obj.sublen(i)+1:obj.len(i)+M-1+j*obj.sublen(i)));
                end
            end
            G      = G./repmat(obj.sublen,N,1)./nk;
            PopObj = (1+G).*fliplr(cumprod([ones(N,1),PopDec(:,1:M-1)],2)).*[ones(N,1),1-PopDec(:,M-1:-1:1)];
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R = UniformPoint(N,obj.M);
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            if obj.M == 2
                R = obj.GetOptimum(100);
            elseif obj.M == 3
                a = linspace(0,1,10)';
                R = {a*a',a*(1-a'),(1-a)*ones(size(a'))};
            else
                R = [];
            end
        end
    end
end

function f = Rastrigin(x)
    f = sum(x.^2-10.*cos(2.*pi.*x)+10,2);
end

function f = Rosenbrock(x)
    f = sum(100.*(x(:,1:size(x,2)-1).^2-x(:,2:size(x,2))).^2+(x(:,1:size(x,2)-1)-1).^2,2);
end


function f = crowding_distance(x,problem)
% This function calculates the crowding distance

%
%  Copyright (c) 2009, Aravind Seshadri
%  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.

[N,M] = size(x);
switch problem
    case 1
        M = 2;
        V = 6;
    case 2
        M = 3;
        V = 12;
end

% Crowding distance for each front
for i = 1 : length(F(front).f)
    y(i,:) = x(F(front).f(i),:);
end
for i = 1 : M
    [sorted(i).individual,sorted(i).index] = sort(y(:,V + i));
    distance(sorted(i).index(1)).individual = Inf;
    distance(sorted(i).index(length(sorted(i).index))).individual = Inf;
end

[num,len] = size(y);
% Initialize all the distance of individuals as zero.
for i = 1 : M
    for j = 2 : num - 1
        distance(j).individual = 0;
    end
    objective(i).range = ...
                sorted(i).individual(length(sorted(i).individual)) - ...
                sorted(i).individual(1);
        % Maximum and minimum objectives value for the ith objective
end
% Caluclate the crowding distance for front one.
for i = 1 : M
    for j = 2 : num - 1
        distance(j).individual = distance(j).individual + ...
            (sorted(i).individual(j + 1) - sorted(i).individual(j - 1))/...
            objective(i).range;
        y(sorted(i).index(j),M + V + 2) = distance(j).individual;
    end
end


function x = r8bb_sl ( n1, n2, ml, mu, a_lu, pivot, b )

%*****************************************************************************80
%
%% R8BB_SL solves a R8BB system factored by R8BB_FA.
%
%  Discussion:
%
%    Note that in C++ and FORTRAN, we can look at A as an abstract
%    vector, but then look at parts of A as storing a two dimensional
%    array.  MATLAB assigns an inherent dimensionality to a data object,
%    and gets very unhappy when you try to manipulate the data yourself.
%    This means that the MATLAB implementation of this routine requires
%    the use of temporary 2D arrays.
%
%    The R8BB storage format is for a border banded matrix.  Such a
%    matrix has the logical form:
%
%      A1 | A2
%      ---+---
%      A3 | A4
%
%    with A1 a (usually large) N1 by N1 banded matrix, while A2, A3 and A4
%    are dense rectangular matrices of orders N1 by N2, N2 by N1, and N2 by N2,
%    respectively.
%
%    A should be defined as a vector.  The user must then store
%    the entries of the four blocks of the matrix into the vector A.
%    Each block is stored by columns.
%
%    A1, the banded portion of the matrix, is stored in
%    the first (2*ML+MU+1)*N1 entries of A, using standard LINPACK
%    general band format.  The reason for the factor of 2 in front of
%    ML is to allocate space that may be required if pivoting occurs.
%
%    The following formulas should be used to determine how to store
%    the entry corresponding to row I and column J in the original matrix:
%
%    Entries of A1:
%
%      1 <= I <= N1, 1 <= J <= N1, (J-I) <= MU and (I-J) <= ML.
%
%      Store the I, J entry into location
%      (I-J+ML+MU+1)+(J-1)*(2*ML+MU+1).
%
%    Entries of A2:
%
%      1 <= I <= N1, N1+1 <= J <= N1+N2.
%
%      Store the I, J entry into location
%      (2*ML+MU+1)*N1+(J-N1-1)*N1+I.
%
%    Entries of A3:
%
%      N1+1 <= I <= N1+N2, 1 <= J <= N1.
%
%      Store the I, J entry into location
%      (2*ML+MU+1)*N1+N1*N2+(J-1)*N2+(I-N1).
%
%    Entries of A4:
%
%      N1+1 <= I <= N1+N2, N1+1 <= J <= N1+N2
%
%      Store the I, J entry into location
%      (2*ML+MU+1)*N1+N1*N2+(J-1)*N2+(I-N1).
%      (same formula used for A3).
%
%    The linear system A * x = b is decomposable into the block system:
%
%      ( A1 A2 ) * (X1) = (B1)
%      ( A3 A4 )   (X2)   (B2)
%
%    All the arguments except B are input quantities only, which are
%    not changed by the routine.  They should have exactly the same values
%    they had on exit from R8BB_FA.
%
%    If more than one right hand side is to be solved, with the same matrix,
%    R8BB_SL should be called repeatedly.  However, R8BB_FA only needs to be
%    called once to create the factorization.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 March 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N1, N2, the order of the banded and dense blocks.
%    N1 and N2 must be nonnegative, and at least one must be positive.
%
%    Input, integer ML, MU, the lower and upper bandwidths.
%    ML and MU must be nonnegative and no greater than N1-1.
%
%    Input, real A_LU( (2*ML+MU+1)*N1 + 2*N1*N2 + N2*N2), the factor information
%    computed by R8BB_FA.
%
%    Input, integer PIVOT(N1+N2), the pivoting information from R8BB_FA.
%
%    Input, real B(N1+N2), the right hand side of the linear system.
%
%    Output, real X(N1+N2), the solution.
%
  nband = (2*ml+mu+1)*n1;
%
%  Set B1 := inverse(A1) * B1.
%  Copy the banded matrix out of A_LU and into A1_LU.
%
  if ( 0 < n1 )

    a1_lu(1:2*ml+mu+1,1:n1) = r8vec_to_r8gb ( n1, n1, ml, mu, a_lu(1:nband) );

    job = 0;

    x(1:n1) = r8gb_sl ( n1, ml, mu, a1_lu, pivot, b, job );

  end
%
%  Modify the right hand side of the second linear subsystem.
%  Set B2 := B2 - A3*B1.
%
  for i = 1 : n2
    for j = 1 : n1
      ij = nband + n1*n2 + (j-1)*n2 + i;
      b(n1+i) = b(n1+i) - a_lu(ij) * x(j);
    end
  end
%
%  Set B2 := inverse(A4) * B2.
%  Copy the dense matrix out of A_LU and into A4_LU.
%
  if ( 0 < n2 )

    a4_lu(1:n2,1:n2) = r8vec_to_r8ge ( ...
      n2, n2, a_lu(nband+2*n1*n2+1:nband+2*n1*n2+n2*n2) );

    job = 0;
    x(n1+1:n1+n2) = r8ge_sl ( n2, a4_lu, pivot(n1+1:n1+n2), b(n1+1:n1+n2), job );

  end
%
%  Modify the first subsolution.
%  Set B1 := B1 + A2*B2.
%
  for i = 1 : n1
    for j = 1 : n2
      ij = nband + (j-1)*n1 + i;
      x(i) = x(i) + a_lu(ij) * x(n1+j);
    end
  end

  return
end


% [descriptors, locs] = sift(img)
%
% This function returns IMAGE's SIFT keypoints.
%   Input parameters:
%     img: the image.
%
%   Returned:
%     descriptors: a K-by-128 matrix, where each row gives an invariant
%         descriptor for one of the K keypoints.  The descriptor is a vector
%         of 128 values normalized to unit length.
%     locs: K-by-4 matrix, in which each row has the 4 values for a
%         keypoint location (row, column, scale, orientation).  The
%         orientation is in the range [-PI, PI] radians.
%
% Credits: Thanks for initial version of this program to D. Alvaro and
%          J.J. Guerrero, Universidad de Zaragoza (modified by D. Lowe)

function [descriptors, locs] = sift(img)

% If you have the Image Processing Toolbox, you can uncomment the following
%   lines to allow input of color images, which will be converted to grayscale.
if isrgb(img)
   img = rgb2gray(img);
end

[rows, cols] = size(img);

% Convert into PGM imagefile, readable by "keypoints" executable
f = fopen('tmp.pgm', 'w');
if f == -1
    error('Could not create file tmp.pgm.');
end
fprintf(f, 'P5\n%d\n%d\n255\n', cols, rows);
fwrite(f, img', 'uint8');
fclose(f);

% Call keypoints executable
if isunix
    command = '!./sift ';
else
    command = '!siftWin32 ';
end
command = [command ' <tmp.pgm >tmp.key'];
eval(command);

% Open tmp.key and check its header
g = fopen('tmp.key', 'r');
if g == -1
    error('Could not open file tmp.key.');
end
[header, count] = fscanf(g, '%d %d', [1 2]);
if count ~= 2
    error('Invalid keypoint file beginning.');
end
num = header(1);
len = header(2);
if len ~= 128
    error('Keypoint descriptor length invalid (should be 128).');
end

% Creates the two output matrices (use known size for efficiency)
locs = double(zeros(num, 4));
descriptors = double(zeros(num, 128));

% Parse tmp.key
for i = 1:num
    [vector, count] = fscanf(g, '%f %f %f %f', [1 4]); %row col scale ori
    if count ~= 4
        error('Invalid keypoint file format');
    end
    locs(i, :) = vector(1, :);

    [descrip, count] = fscanf(g, '%d', [1 len]);
    if (count ~= 128)
        error('Invalid keypoint file value.');
    end
    % Normalize each input vector to unit length
    descrip = descrip / sqrt(sum(descrip.^2));
    descriptors(i, :) = descrip(1, :);
end
fclose(g);

delete('tmp.pgm');


function kern = linard2KernParamInit(kern)

% LINARD2KERNPARAMINIT LINARD2 kernel parameter initialisation.
% The automatic relevance determination version of the linear
% kernel (LINARD2) is the simple inner product kernel with feature
% selection applied.
%
% k(x_i, x_j) = x_i'*A* x_j
%
% where A is a diagonal matrix of values constrained to positve. These
% parameters are stored in the field 'inputScales'.
%
% SEEALSO : linKernParamInit, rbfardKernParamInit
%
% FORMAT
% DESC initialises the automatic relevance determination linear
%  kernel structure with some default parameters.
% ARG kern : the kernel structure which requires initialisation.
% RETURN kern : the kernel structure with the default parameters placed in.
%
% SEEALSO : kernCreate, kernParamInit
%
% COPYRIGHT : Neil D. Lawrence, 2004, 2005, 2006
%
% COPYRIGHT : Michalis K. Titsias, 2009

% KERN

% These parameters are restricted to positive
kern.inputScales = 0.999*ones(1, kern.inputDimension);
kern.nParams = kern.inputDimension;

kern.transforms(1).index = [1:kern.nParams];
kern.transforms(1).type = optimiDefaultConstraint('positive');

kern.isStationary = false;


function [ehat,v_e,etahat,v_eta] = VBA_getNoise(posterior,out)
% returns the Laplace approximation to the innovations posterior density
% function [ehat,v_e,etahat,v_eta] = VBA_getNoise(posterior)
% This may be useful if one is interested in recovering, e.g., the state
% noise that enters and perturbs the system. In particular, posterior
% covariances are attached to state noise, which means that statistical
% inference can be performed in the usual way...
% IN:
%   - posterior/out: output structures of VBA_NLStateSpaceModel.m
% OUT:
%   - ehat/etahat: the 1st-order moment of the posterior density on the
%   measurement (resp. state) noise
%   - v_e/v_eta: the second-order moment of the posterior density on the
%   measurement (resp. state) noise

dim = out.dim;
options = out.options;
u = out.u;
y = out.y;

ehat = zeros(dim.p,dim.n_t);
v_e = cell(1,dim.n_t);
etahat = zeros(dim.n,dim.n_t);
v_eta = cell(1,dim.n_t);

% initial condition
if dim.n > 0
    [fx,dfdx,dfdp] = VBA_evalFun('f',posterior.muX0,posterior.muTheta,u(:,1),options,dim,1);
    etahat(:,1) = posterior.muX(:,1) - fx;
    if isinf(posterior.a_alpha) && isequal(posterior.b_alpha,0)
        v_eta{1} = zeros(dim.n,dim.n);
    else
        v_eta{1} = posterior.SigmaX.current{1} ...
            + dfdx'*posterior.SigmaX0*dfdx;
        if dim.n_theta > 0
            v_eta{1} = v_eta{1} + dfdp'*posterior.SigmaTheta*dfdp;
        end
    end
end
[gx,dgdx,dgdp] = VBA_evalFun('g',posterior.muX(:,1),posterior.muPhi,u(:,1),options,dim,1);
ehat(:,1) = y(:,1) - gx;
v_e{1} = zeros(dim.p,dim.p);
if dim.n > 0
    v_e{1} = dgdx'*posterior.SigmaX.current{1}*dgdx;
end
if dim.n_phi > 0
    v_e{1} = v_e{1} + dgdp'*posterior.SigmaPhi*dgdp;
end

% loop over time samples
for t = 2:dim.n_t
    if dim.n > 0
        [fx,dfdx,dfdp] = VBA_evalFun('f',posterior.muX(:,t-1),posterior.muTheta,u(:,t),options,dim,t);
        etahat(:,t) = posterior.muX(:,t) - fx;
        if isinf(posterior.a_alpha) && isequal(posterior.b_alpha,0)
            v_eta{t} = zeros(dim.n,dim.n);
        else
            P = [-dfdx',eye(dim.n)];
            jointCov = ...
                [ posterior.SigmaX.current{t}  posterior.SigmaX.inter{t-1}'
                posterior.SigmaX.inter{t-1}  posterior.SigmaX.current{t-1} ];
            v_eta{t} = P*jointCov*P';
            if dim.n_theta > 0
                v_eta{t} = v_eta{t} + dfdp'*posterior.SigmaTheta*dfdp;
            end
        end
    end
    [gx,dgdx,dgdp] = VBA_evalFun('g',posterior.muX(:,t),posterior.muPhi,u(:,t),options,dim,t);
    ehat(:,t) = y(:,t) - gx;
    v_e{t} = zeros(dim.p,dim.p);
    if dim.n > 0
        v_e{t} = dgdx'*posterior.SigmaX.current{t}*dgdx;
    end
    if dim.n_phi > 0
        v_e{t} = v_e{t} + dgdp'*posterior.SigmaPhi*dgdp;
    end
end


function [R,current_eps]=DT_USTM(snrdB,T,L,Mt,Mr,epsilon,prec,filename)
%
% Function to compute the DT lower bound for a Rayleigh block-fading
% channel with no CSI at transmitter and receiver. The bound assumes that
% USTM is chosen as input distribution
% snrdB: SNR in dB
% Mt: number of transmit antennas
% Mr: number of receive antennas (Mt<=Mr);
% T: size of coherence interval
% L: number of independent coherence intervals; L*T is the blocklength
% epsilon: maximal block error probability
% prec: it controls the number of samples for the Monte Carlo simulation; Note nsamples=2^prec; One should have nsamples>> 100 x 1/epsilon
% filename: data file where the samples of the information density are saved for possible future refinements.
%
% The outputs of the program are
% R: R^*(T*L,current_eps,snrdB)
% current_eps: actual upper bound on the maximal error probability
%
%-------------------------------------------------------------------
%                       SET-UP PARAMETERS
%-------------------------------------------------------------------

SAVE=0; % if this flag is active the samples of the information density are saved for possibe further refinemes
MAT=0; % save with .mat extension

K = 2^prec; % number of monte carlo simulations (it should be at least 100 x 1/epsilon)
rho = 10.^(snrdB/10); % SNR in linear scale

%-------------------------------------------------------------------
%                       MONTE CARLO SIMULATION
%-------------------------------------------------------------------
I = zeros(K,1); %allocate for the montecarlo runs
%-------------------------------------------------------------------
%                       CONSTANTS
%-------------------------------------------------------------------
rho_tilde = T*rho/Mt;
D= [sqrt(1+rho_tilde)*eye(Mt), zeros(Mt,T-Mt);
    zeros(T-Mt,Mt), eye(T-Mt)]; % D matrix (covariance matrix of equivalent noise)

lambda=1+rho_tilde;
lambda1=1/lambda;
lambda2=rho_tilde*lambda1;
c2 = logComplexGammaRatio(Mt,Mr,T); %gamma constant
c1 = Mt*(T-Mt)*log(lambda2); % SNR constant
const = c1+c2;

noise_norm=sqrt(.5);

%-------------------------------------------------------------------
%                       MONTE CARLO
%-------------------------------------------------------------------

for k = 1:K %do K montecarlo runs
        i_L = 0;
        Z = (randn(T,Mr,L)+1i*randn(T,Mr,L))*noise_norm;
        for l = 1:L %Create each realization
            Sigma = svd(D*Z(:,:,l)).^2;
            if (Mt==1)
                M = createMalt(Mt,Mr,T,Sigma,lambda2); %create  matrix
                logdetM=log(det(M))+Sigma(1)*lambda2-(T-Mr)*log(Sigma(1));
                partial_sum=sum(Sigma)-logdetM;
            elseif Mr >= Mt
                M = createM_rx_larger(Mt,Mr,T,Sigma,lambda2);
                logdetM=logdet(M);
                logdetSigma = (T-Mr)*sum(log(Sigma));% compute logdet(Sigma^(T-M))
                partial_sum=lambda1*sum(Sigma) - logdetM +logdetSigma;
            else
                logdetM = createM_tx_larger(Mt, Mr, T, Sigma, lambda2); %already in log domain
                logdetSigma = (T-Mr)*sum(log(Sigma));% compute logdet(Sigma^(T-M))
                const2 = sum(gammaln(T-((Mt+1):T)+1)) - sum(gammaln(1:(T-Mr)));
                partial_sum = sum(Sigma) -logdetM + logdetSigma - const2;
            end

            vanderTerm = det(vander(Sigma)); %get the determinant of the vandermode matrix
            TraceZ=real(trace(Z(:,:,l)'*Z(:,:,l)));

            i_temp = const- TraceZ  +partial_sum + log(vanderTerm);  %Information density for time l (i(x_l, y_l)
            i_L = i_L + i_temp; %add it to the total i_L
        end

        I(k) =  i_L; %put all computations on a pile to compute the average later

end

if (SAVE==1)
  if (MAT==1)
    save(filename,'I')
  else
    save(filename,'I','-ascii','-append')
  end
end

%---------------------------------------
%   START SEARCHING FOR THE RATE
%---------------------------------------

I=sort(I);

Kcurrent=length(I); % redefine K to account for append

current_prec=floor(log2(Kcurrent)); % actual precision

K=2^(current_prec); % round off K to avoid search errors

step=K/2;
index=step;

onevec=ones(K,1);

while(step>1),

   th=I(index);

   current_eps=sum(exp(-max(0,I-th)))/K;

   step=step/2;

   if current_eps> epsilon,

       index=index-step;

   else

       index=index+step;

   end

end

current_eps=sum(exp(-max(0,I-I(index))))/K;

if (I(index)>500),

  R=I(index)/(L*T*log(2));

else

  R=log2(exp(I(index))+1)/(L*T); % factor 2 removed from DT bound to account for max error probability

end

end

function val = logComplexGammaRatio(Mt,Mr,T)
    k=T-Mt+1:1:T;
    val = -sum(gammaln(k));
    r=1:1:Mt;
    val=val+sum(gammaln(r));
end

function M = createMalt(Mt,Mr,T,Sigma,lambda)
M = nan(Mr,Mr); %(l is the row, k is the column)
for l = 1:Mr
    for k=1:Mt
        M(l,k)= exp((Mt-k)*log(Sigma(l)) + log(gammainc(lambda*Sigma(l),T+k-Mt-Mr)) + lambda*(Sigma(l)-Sigma(1)) + (T-Mr)*log(Sigma(1)/Sigma(l)));
    end

    for k=Mt+1:1:Mr
        M(l,k)= Sigma(l)^(Mr-k);
    end
end
end

function M = createM_rx_larger(Mt,Mr,T,Sigma,lambda)
M = nan(Mr,Mr); %(l is the row, k is the column)
P=Mr;

for l = 1:Mr
    for k=1:Mt
        M(l,k)=(Sigma(l)^(Mt-k))*gammainc(lambda*Sigma(l),T+k-Mt-P);
    end

    for k=Mt+1:Mr
        M(l,k)=exp((T-k)*log(Sigma(l)) -Sigma(l)*lambda); % case Mt<Mr
    end
end

end

function C = createM_tx_larger(M,N,T,Sigma,lambda)
inf_flag = 0;
A = zeros(max(M,N), max(M,N));

for i = 1:M
    for j = 1:M
        if j <= N
            if Sigma(j) == 0 && M-i == 0
                A(i,j) = exp(lambda*Sigma(j) + log(gammainc(Sigma(j)*lambda, T-N-M+i-1)));
            else
                if (T-N-M+i-1) > 0
                    A(i,j) = exp((M-i)*log(Sigma(j))  + lambda*Sigma(j) + log(gammainc(Sigma(j)*lambda, T-N-M+i-1)));
                else
                    A(i,j) = exp((M-i)*log(Sigma(j))  + lambda*Sigma(j));
                end
            end
            if isinf(A(i,j)) == 1
                inf_flag = 1;
                break;
            end
            %A(i,j) = exp((M-i)*log(lambda(j))  + gammaln(T-M) +  log(gammainc(lambda(j)*p, T+i-2*M)));
        else
            A(i,j) = exp((T-j-(M-i))*log(lambda) + sum(log(T-j-(0:(M-i-1)))));
            %A(i,j) = exp((T-j-(M-i))*log(p)  + sum(log(T-j-(0:(M-i-1)))));
        end
    end
    if inf_flag == 1
        break;
    end
end
if inf_flag == 1
    for i = 1:M
        for j = 1:M
            if j <= N
                if (T-N-M+i-1) > 0
                    A(i,j) = exp((M-i)*log(Sigma(j))  +  log(gammainc(Sigma(j)*lambda, (T-N-M+i-1))));
                else
                    A(i,j) = exp((M-i)*log(Sigma(j)));
                end
            else
                A(i,j) = exp((T-j-(M-i))*log(lambda)  + sum(log(T-j-(0:(M-i-1)))));
            end
        end
    end
end

if inf_flag == 0
    C = logdet(A);
else
    C = logdet(A) + lambda*sum(Sigma);
end
end

function v = logdet(A, op)

assert(isfloat(A) && ndims(A) == 2 && size(A,1) == size(A,2), ...
    'logdet:invalidarg', ...
    'A should be a square matrix of double or single class.');

if nargin < 2
    use_chol = 0;
else
    assert(strcmpi(op, 'chol'), ...
        'logdet:invalidarg', ...
        'The second argument can only be a string ''chol'' if it is specified.');
    use_chol = 1;
end

%% computation

if use_chol
    v = 2 * sum(log(diag(chol(A))));
else
    [L, U, P] = lu(A);
    du = diag(U);
    c = det(P) * prod(sign(du));
    v = log(c) + sum(log(abs(du)));
end
end


function [accuracy,predictlabel,elapse] = CSRKDApredict(fea, gnd, model)
% SRKDApredict: Spectral Regression Kernel Discriminant Analysis Prediction
%               SRKDApredict use SRKDA as a classifier. It used the nearest
%               center rule in the SRKDA subspace for classification.
%
%       [predictlabel,accuracy,elapse] = SRKDApredict(fea, gnd, model);
%
%             Input:
%
%               fea     - data matrix. Each row is a data point.
%               gnd     - Label vector of fea.
%             model     - model trained by SRKDAtrain.m
%
%             Output:
%
%            accuracy   - classification accuracy
%         predictlabel  - predict label for fea
%             elapse    - running time.
%
%    Examples:
%
%
% See also SRKDAtrain, KSR, KSR_caller
%
%Reference:
%
%   [1] Deng Cai, Xiaofei He, and Jiawei Han. "Speed Up Kernel Discriminant
%   Analysis", The VLDB Journal, vol. 20, no. 1, pp. 21-33, January, 2011.
%
%   [2] Deng Cai, Xiaofei He and Jiawei Han, "SRDA: An Efficient Algorithm for
%   Large Scale Discriminant Analysis" IEEE Transactions on Knowledge and
%   Data Engineering, vol. 20, no. 1, pp. 1-12, January, 2008.
%
%   [3] V. Sindhwani, P. Niyogi, M. Belkin, "Beyond  the  Point  Cloud:  from
%   Transductive  to  Semi-supervised  Learning", ICML 2005.
%
%   version 2.0 --December/2011
%   version 1.0 --May/2006
%
%   Written by Deng Cai (dengcai AT gmail.com)
%

MAX_MATRIX_SIZE = 8000; % You can change this number based on your memory.

nTrain = size(model.Landmark,1);
nTest = size(fea,1);
nBlock = ceil(MAX_MATRIX_SIZE*MAX_MATRIX_SIZE/nTrain);
Embed_Test = zeros(nTest,size(model.projection,2));
for i = 1:ceil(nTest/nBlock)
    if i == ceil(nTest/nBlock)
        smpIdx = (i-1)*nBlock+1:nTest;
    else
        smpIdx = (i-1)*nBlock+1:i*nBlock;
    end
    KTest= constructKernel(fea(smpIdx,:),model.Landmark,model.options);
    Embed_Test(smpIdx,:) = KTest*model.projection;
    clear KTest;
end

D = EuDist2(Embed_Test,model.ClassCenter,0);
[dump, idx] = min(D,[],2);
predictlabel = model.ClassLabel(idx);

accuracy = 1 - length(find(predictlabel-gnd))/nTest;


function value = givens_condition ( n )

%*****************************************************************************80
%
%% GIVENS_CONDITION returns the L1 condition of the GIVENS matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 April 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, real VALUE, the L1 condition.
%
  a_norm = n * n;

  if ( n == 1 )
    b_norm = 1.0;
  else
    b_norm = 2.0;
  end

  value = a_norm * b_norm;

  return
end


function [ n_data, x, fx ] = sqrt_values ( n_data )

%*****************************************************************************80
%
%% SQRT_VALUES returns some values of the square root function.
%
%  Discussion:
%
%    SQRT(X) = positive real number Y such that Y * Y = X.
%
%    In Mathematica, the function can be evaluated by:
%
%      Sqrt[x]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real X, the argument of the function.
%
%    Output real FX, the value of the function.
%
  n_max = 14;

  fx_vec = [ ...
     0.0000000000000000E+00, ...
     0.9000000040950000E-04, ...
     0.3000000000000000E+00, ...
     0.3162277660168379E+00, ...
     0.6324555320336759E+00, ...
     0.1000000000000000E+01, ...
     0.1414213562373095E+01, ...
     0.1732050807568877E+01, ...
     0.1772453850905516E+01, ...
     0.4358898943540674E+01, ...
     0.5385164807134504E+01, ...
     0.8426149773176359E+01, ...
     0.9848857801796105E+01, ...
     0.1111111106055556E+05 ];

  x_vec = [ ...
     0.0000000000000000E+00, ...
     0.8100000073710001E-08, ...
     0.9000000000000000E-01, ...
     0.1000000000000000E+00, ...
     0.4000000000000000E+00, ...
     0.1000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.3000000000000000E+01, ...
     0.3141592653589793E+01, ...
     0.1900000000000000E+02, ...
     0.2900000000000000E+02, ...
     0.7100000000000000E+02, ...
     0.9700000000000000E+02, ...
     0.1234567890000000E+09 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    x = 0.0;
    fx = 0.0;
  else
    x = x_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


function varargout = ellipseAsPolygon(ellipse, N)
%ELLIPSEASPOLYGON Convert an ellipse into a series of points.
%
%   Deprecated, use ellipseToPolygon instead.
%
%   P = ellipseAsPolygon(ELL, N);
%   converts ELL given as [x0 y0 a b] or [x0 y0 a b theta] into a polygon
%   with N edges. The result P is (N+1)-by-2 array containing coordinates
%   of the N+1 vertices of the polygon.
%   The resulting polygon is closed, i.e. the last point is the same as the
%   first one.
%
%   P = ellipseAsPolygon(ELL);
%   Use a default number of edges equal to 72. This result in one piont for
%   each 5 degrees.
%
%   [X Y] = ellipseAsPolygon(...);
%   Return the coordinates o fvertices in two separate arrays.
%
%   See also:
%   ellipses2d, circleAsPolygon, rectAsPolygon, drawEllipse

% ------
% Author: David Legland
% e-mail: david.legland@inrae.fr
% Created: 2005-04-06
% Copyright 2005 INRA - TPV URPOI - BIA IMASTE

warning('matGeom:deprecated', ...
    'function "ellipseAsCurve" is deprecated, use "ellipseToPolygon" instead');

% format output
if nargout <= 1
    varargout = {ellipseToPolygon(ellipse, N)};
else
    [x, y] = ellipseToPolygon(ellipse, N);
    varargout = {x, y};
end


% Measure PSS correlations in the presence of a frequency offset.
%n_id_2=0;

% Copyright 2012 Evrytania LLC (http://www.evrytania.com)
%
% Written by James Peroulas <james@evrytania.com>
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU Affero 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 Affero General Public License for more details.
%
% You should have received a copy of the GNU Affero General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

freq_off_set=linspace(-30e3,30e3,1001);
time_off_set=-3000:3000;

n_freqs=length(freq_off_set);
n_times=length(time_off_set);

pss_td=NaN(3,144+2048+0*2048);
for n_id_2=0:2
  pss_freq=pss(n_id_2);
  pss_freq=[0 pss_freq(32:end) zeros(1,2048-73+5+5) pss_freq(1:31)];
  pss_td(n_id_2+1,145:end)=[idft(pss_freq)*sqrt(2048/62) zeros(1,0*2048)];
  pss_td(n_id_2+1,1:144)=pss_td(n_id_2+1,end-143:end);
  pss_td(n_id_2+1,:)=pss_td(n_id_2+1,:)/sqrt(sigpower(pss_td(n_id_2+1,:)));
end

% Frequency domain correlations
figure(1);
log_xc_freq=NaN(3,n_freqs);
for n_id_2=0:2
  for t=1:n_freqs
    freq=freq_off_set(t);
    log_xc_freq(n_id_2+1,t)=sum(conj(pss_td(n_id_2+1,:)).*fshift(pss_td(n_id_2+1,:),freq/(fs_lte/2)))/(2048+144);
  end
end
plot(freq_off_set,db20(abs(transpose(log_xc_freq))));
%ylim([-20 5]);
drawnow;
zgo;

% Time domain auto correlations
pss_td_ext=[pss_td zeros(3,(2048+144)*2)];
log_xc_td=NaN(3,n_times);
figure(2);
for n_id_2=0:2
  for t=1:n_times
    to=time_off_set(t);
    log_xc_td(n_id_2+1,t)=sum(conj(pss_td_ext(n_id_2+1,:)).*tshift(pss_td_ext(n_id_2+1,:),to))/(2048+144);
  end
end
plot(time_off_set,db20(abs(transpose(log_xc_td))));
%ylim([-50 5]);
drawnow;
zgo;

% Time domain cross correlations
log_xc_td_cross=NaN(3,n_times);
log_xc_td_cross_max16=NaN(3,n_times);
figure(3);
xc_set=[1 2;1 3;2 3]-1;
for k=1:3
  n_id_2_1=xc_set(k,1);
  n_id_2_2=xc_set(k,2);
  for t=1:n_times
    to=time_off_set(t);
    log_xc_td_cross(k,t)=sum(conj(pss_td_ext(n_id_2_1+1,:)).*tshift(pss_td_ext(n_id_2_2+1,:),to))/(2048+144);
  end
  for t=1:n_times
    log_xc_td_cross_max16(k,t)=max(log_xc_td_cross(k,max([t-32 1]):min([t+80 n_times])));
  end
end
plot(time_off_set,db20(abs(transpose(log_xc_td_cross))), ...
time_off_set,db20(abs(transpose(log_xc_td_cross_max16))));
legend('0 1','0 2','2 3','location','se');
%ylim([-50 5]);
drawnow;
zgo;


%%%% Create animation for a 2-link serial chain manipiulator

% Course: Robotic Manipulation and Mobility
% Advisor: Dr. V. Krovi
%
% Homework Number: MIDTERM
%
% Names: Sourish Chakravarty
% 	Hrishi Lalit Shah

function [aviobj]= CREATE_ANIME_1(aviobj,T,th1,x2,y2,th2,h)

global l1 lc1 l2 lc2
figure(h)
Z0=[0 0]; %%% Point of suspension
for i=1:length(T)

    c1=cos(th1(i));
    c2=cos(th2(i));
    s1=sin(th1(i));
    s2=sin(th2(i));
    %%% Plotting Link - 1
    Z1= Z0 + [l1*c1, l1*s1];% Coordinate of the hanging end link 1
    plot([Z0(1),Z1(1)],[Z0(2),Z1(2)],'b','linewidth',4);
    hold on
    Z1m= Z0 + [lc1*c1, lc1*s1];% Coordinate of the CM of link 1
    plot(Z1m(1), Z1m(2),'r*'); %Plots mid point
    plot(Z0(1),Z0(2),'k*'); % Plots point of suspension

    %%% Plotting Link - 2
    Z2m = [x2(i), y2(i)];% Coordinate of the CM of link 2
    Z2l = Z2m - [lc2*c2, lc2*s2]; % Coordinate of source-end of link - 2
    Z2r = Z2m + [(l2-lc2)*c2, (l2-lc2)*s2]; % Coordinate of effector-end of link-2
    plot([Z2l(1),Z2r(1)],[Z2l(2),Z2r(2)],'m','linewidth',4);
    plot(Z2m(1), Z2m(2),'r*'); %Plots mid point
    xlim([-4,4]);
    ylim([-4,4]);
%     title('Midterm Animation');
    xlabel('X-axis');
    ylabel('Y-axis');
    grid on
    hold off
    pause(0.01);     %Stop execution for 0.01 to make animation visible
    frame= getframe(gcf);   %Step 2: Grab the frame
    aviobj = addframe(aviobj,frame); % Step 3: Add frame to avi object
end


function test_stepsize_alg_demo()
% demonstration file for original stepsize algorithm.
%
% This file illustrates how to set user's own stepsize algorithm in case of linear
% regression problem. This demonstrates SGD and SVRG algorithms.
%
% This file is part of SGDLibrary.
%
% Created by H.Kasai on Sep. 25, 2017

    clc;
    clear;
    close all;

    %% generate synthetic data
    % set number of dimensions
    d = 10;
    % set number of samples
    n = 1000;
    % generate data
    data = logistic_regression_data_generator(n, d);

    %% define problem definitions
    problem = logistic_regression(data.x_train, data.y_train, data.x_test, data.y_test);

    %% perform algorithms SGD and SVRG
    options.w_init = data.w_init;
    options.step_init = 0.01;
    options.verbose = 2;

    options.step_alg = 'fix';
    [w_sgd_fix, info_sgd_fix] = sgd(problem, options);

    options.step_alg = 'decay';
    [w_sgd_decay, info_sgd_decay] = sgd(problem, options);

    options.step_alg = 'decay-2';
    [w_sgd_decay2, info_sgd_decay2] = sgd(problem, options);

    options.stepsizefun = @my_stepalg;  % set my_stepalg (user-defined stepsize algorithm)
    [w_sgd_my, info_sgd_my] = sgd(problem, options);

    %% display cost/optimality gap vs number of gradient evaluations
    display_graph('grad_calc_count','cost', {'SGD (fix)','SGD (decay)', 'SGD (decay-2)', 'SGD (My stepsize algorithm)'}, ...
            {w_sgd_fix, w_sgd_decay w_sgd_decay2, w_sgd_my}, {info_sgd_fix, info_sgd_decay, info_sgd_decay2, info_sgd_my});

end

%% define user-defined stepsize algorithm
function step = my_stepalg(iter, options)
    step = options.step_init / (10 + iter*0.5);
end


function tetrahedron_arbq_rule_test02 ( degree, n, header )

%*****************************************************************************80
%
%% TETRAHEDRON_ARBQ_RULE_TEST02 gets a rule and writes it to a file.
%
%  Licensing:
%
%    This code is distributed under the GNU GPL license.
%
%  Modified:
%
%    08 July 2014
%
%  Author:
%
%    Original FORTRAN77 version by Hong Xiao, Zydrunas Gimbutas.
%    This MATLAB version by John Burkardt.
%
%  Reference:
%
%    Hong Xiao, Zydrunas Gimbutas,
%    A numerical algorithm for the construction of efficient quadrature
%    rules in two and higher dimensions,
%    Computers and Mathematics with Applications,
%    Volume 59, 2010, pages 663-676.
%
%  Parameters:
%
%    Input, integer DEGREE, the desired total polynomial degree exactness
%    of the quadrature rule.  0 <= DEGREE <= 15.
%
%    Input, integer N, the number of nodes to be used by the rule.
%
%    Input, string HEADER, an identifier for the filenames.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TETRAHEDRON_ARBQ_RULE_TEST02\n' );
  fprintf ( 1, '  Get a quadrature rule for the tetrahedron.\n' );
  fprintf ( 1, '  Then write it to a file.\n' );
  fprintf ( 1, '  Polynomial exactness degree DEGREE = %d\n', degree );
%
%  Retrieve a symmetric quadrature rule.
%
  [ x, w ] = tetrahedron_arbq ( degree, n );
%
%  Write the points and weights to a file.
%
  rule_filename = strcat ( header, '.txt' );

  rule_unit = fopen ( rule_filename, 'wt' );
  for i = 1 : n
    fprintf ( rule_unit, '%g  %g  %g  %g\n', x(1,i), x(2,i), x(3,i), w(i) );
  end
  fclose ( rule_unit );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Quadrature rule written to file "%s"\n', rule_filename );

  return
end


function [x,state] = struct_ctranspose(z,task)
%STRUCT_CTRANSPOSE Complex conjugate transpose.
%   [x,state] = struct_ctranspose(z) computes x as the complex conjugate
%   transpose of z. The structure state stores information which is reused
%   in computing the right and left Jacobian-vector products.
%
%   struct_ctranspose(z,task) computes the right or left Jacobian-vector
%   product of this transformation, depending on the structure task. Use
%   the structure state and add the field 'r' of the same shape as z or the
%   field 'l' of the same shape as x to obtain the structure task for
%   computing the right and left Jacobian-vector products
%
%      (dF(:)/dz(:).')*task.r(:) and
%      (dF(:)/dz(:).')'*task.l(:) + conj((dF(:)/dconj(z(:)).')'*task.l(:)),
%
%   respectively. Here, F(z) represents this transormation, (:) signifies
%   vectorization and the derivative w.r.t. z (conj(z)) is a partial
%   derivative which treats conj(z) (z) as constant. The output has the
%   same shape as x or z for the right and left Jacobian-vector products,
%   respectively.
%
%   See also struct_conj, struct_transpose.

%   Authors: Laurent Sorber (Laurent.Sorber@cs.kuleuven.be)
%            Marc Van Barel (Marc.VanBarel@cs.kuleuven.be)
%            Lieven De Lathauwer (Lieven.DeLathauwer@kuleuven-kulak.be)
%
%   References:
%   [1] L. Sorber, M. Van Barel, L. De Lathauwer, "Structured data fusion,"
%       ESAT-SISTA Internal Report 13-177, KU Leuven, 2013.

if nargin < 2, task = []; end
state = [];

if isempty(task) || (isempty(task.l) && isempty(task.r))
    x = z';
elseif ~isempty(task.r)
    if ~isreal(z) || ~isreal(task.r)
        error('struct_ctranspose:nonanalytic',['Nonanalytic objective ' ...
            'functions are currently not supported in sdf_nls, please ' ...
            'use sdf_minf instead.']);
    end
    x = task.r.';
elseif ~isempty(task.l)
    x = task.l';
end

end


function rf=v_lpcaa2rf(aa)
%V_LPCAA2RF LPC: Convert vocal tract areas to reflection coefficients RF=(AA)

%      Copyright (C) Mike Brookes 1998
%      Version: $Id: v_lpcaa2rf.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   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 2 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 can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[nf,p2]=size(aa);
rf = (aa(:,2:p2)-aa(:,1:p2-1))./(aa(:,2:p2)+aa(:,1:p2-1));


function mColormap = gui_Colormap_HSVCut(nSize)

% If size is not specified, set it to 256
if nargin < 1
  nSize = 256;
end

nTmpSize = floor(nSize/0.85);
mColormap = hsv(nTmpSize);
mColormap = mColormap(1:nSize,:);


function [sys,x0,str,ts]=NL_PID_3fal(t,x,u,flag,r0,h0,r1,h1,h,Bet,A,D)
switch flag,
    case 0,
        [sys,x0,str,ts]=mdlInitializeSizes(h);
    case 2,
        sys=mdlUpdates(x,u,r0,r1,h0,h1,h);
    case 3,
        sys=mdlOutputs(x,Bet,A,D);
    case 4,
        sys=mdlGetTimeOfNextVarHit(t,h);
    case {1,9},
        sys=[];
    otherwise
        error(['Unhandled flag=',num2str(flag)]);
end
function [sys,x0,str,ts]=mdlInitializeSizes(h)
    sizes=simsizes;
    sizes.NumContStates=0;
    sizes.NumDiscStates=5;
    sizes.NumOutputs=2;
    sizes.NumInputs=2;
    sizes.DirFeedthrough=1;
    sizes.NumSampleTimes=1;
    sys=simsizes(sizes);
    x0=[0;0;0;0;0];
    str=[];
    ts=[h 0];
function sys=mdlUpdates(x,u,r0,r1,h0,h1,h)
    fh0=fhan(x(1)-u(1),x(2),r0,h0);
    x(1)=x(1)+h*x(2);
    x(2)=x(2)+h*fh0;
    fh1=fhan(x(3)-u(2),x(4),r1,h1);
    x(3)=x(3)+h*x(4);
    x(4)=x(4)+h*fh1;
    x(5)=x(5)+h*(x(1)-x(3));
    sys=x;
function sys=mdlOutputs(x,Bet,A,D)
    sys(1)=Bet(1)*fal(x(5),A(1),D)+Bet(2)*fal(x(1)-x(3),A(2),D)+Bet(3)*fal(x(2)-x(4),A(3),D);
    sys(2)=x(1);
function sys=mdlGetTimeOfNextVarHit(t,h)
    sys=t+h;
function y=fhan(x1,x2,r,h)
d=r*h;
d0=h*d;
y=x1+h*x2;
a0=sqrt(d^2+8*r*abs(y));
if abs(y)>d0
    a=x2+(a0-d)*sign(y)/2;
else
    a=x2+y/h;
end

if abs(a)>d
    y=-r*sign(a);
else
    y=-r*a/d;
end
function f=fal(e,a,d)
    if abs(e)<d
        f=e*d^(a-1);
    else f=(abs(e))^a*sign(e);
    end


function Lms = kappabeta_ach(Ns, epsil, P, hack)
%
% Compute achievability (lower) bound for log M. This is just a glue between kappa() and betaq_up_v2()
%

if (nargin < 4) || isempty(hack)
	hack = 1;
end

taus = linspace(0,1,40).*epsil; taus = taus(3:end-2);

Lms = [];

for n = Ns;
	disp(sprintf('kappabeta_ach(): n = %d', n));
	temp_lbs = [];
	for tau = taus;
		temp_lb = log2(kappa_inf(tau, P)) - betaq_up_v2(1-epsil+tau, n, P);
		temp_lbs = [temp_lbs temp_lb];
	end;
	[bb ind] = max(temp_lbs);
	tau = taus(ind);
	if (hack)
		kap = log2(kappa_inf(tau, P));
	else
		kap = log2(kappa(tau, n, P));
	end
	clb = kap - betaq_up_v2(1 - epsil + tau, n, P);

	Lms = [Lms clb];

end


% This is a simplest implementation of the proposed RIFT algorithm. In this implementation,...
% rotation invariance part and corner point detection are not included.

clc;clear;close all;
warning('off')

addpath sar-optical   % type of multi-modal data

str1='pair1.jpg';   % image pair
str2='pair2.jpg';
im1 = im2uint8(imread(str1));
im2 = im2uint8(imread(str2));

if size(im1,3)==1
    temp=im1;
    im1(:,:,1)=temp;
    im1(:,:,2)=temp;
    im1(:,:,3)=temp;
end

if size(im2,3)==1
    temp=im2;
    im2(:,:,1)=temp;
    im2(:,:,2)=temp;
    im2(:,:,3)=temp;
end

disp('RIFT feature detection and description')
% RIFT feature detection and description
[des_m1,des_m2] = RIFT_no_rotation_invariance(im1,im2,4,6,96);

disp('nearest matching')
% nearest matching
[indexPairs,matchmetric] = matchFeatures(des_m1.des,des_m2.des,'MaxRatio',1,'MatchThreshold', 100);
matchedPoints1 = des_m1.kps(indexPairs(:, 1), :);
matchedPoints2 = des_m2.kps(indexPairs(:, 2), :);
[matchedPoints2,IA]=unique(matchedPoints2,'rows');
matchedPoints1=matchedPoints1(IA,:);

disp('outlier removal')
%outlier removal
H=FSC(matchedPoints1,matchedPoints2,'affine',2);
Y_=H*[matchedPoints1';ones(1,size(matchedPoints1,1))];
Y_(1,:)=Y_(1,:)./Y_(3,:);
Y_(2,:)=Y_(2,:)./Y_(3,:);
E=sqrt(sum((Y_(1:2,:)-matchedPoints2').^2));
inliersIndex=E<3;
cleanedPoints1 = matchedPoints1(inliersIndex, :);
cleanedPoints2 = matchedPoints2(inliersIndex, :);

disp('Show matches')
% Show results
figure; showMatchedFeatures(im1, im2, cleanedPoints1, cleanedPoints2, 'montage');

disp('registration result')
% registration
image_fusion(im2,im1,double(H));


function [Q, R] = gson(X)
% Gram-Schmidt orthonormalization which produces the same result as [Q,R]=qr(X,0)
% Written by Mo Chen (sth4nth@gmail.com).
[d,n] = size(X);
m = min(d,n);
R = zeros(m,n);
Q = zeros(d,0);
for i = 1:m
    R(1:i-1,i) = Q'*X(:,i);
    v = X(:,i)-Q*R(1:i-1,i);
    R(i,i) = norm(v);
    Q(:,i) = v/R(i,i);
end
R(:,m+1:n) = Q'*X(:,m+1:n);


% Calculate channel taps based on the 4th or 6th OFDM symbol (both are ZC sequences)
%
% There is almost certainly a better way to do this, but this gets the job done
%
% @param zc_seq Frequency domain ZC sequence from OFDM symbol number 4 or 6 (all FFT bins, no cyclic prefix)
% @param sample_rate Sample rate (in Hz) of `zc_seq`
% @param symbol_idx Which symbol (must be 4 or 6) is in the `zc_seq` vector
% @return taps Result of dividing `zc_seq` into a golden reference copy of the selected ZC sequence
function [taps] = calculate_channel(zc_seq, sample_rate, symbol_idx)
    assert(symbol_idx == 4 || symbol_idx == 6, "Symbol index must be 4 or 6");
    fft_size = get_fft_size(sample_rate);

    % The golden reference needs to be in the frequency domain
    gold_seq = fftshift(fft(reshape(create_zc(fft_size, symbol_idx), size(zc_seq))));

%     figure(400);
%     subplot(1, 2, 1);
%     plot(abs(gold_seq).^2)
%     subplot(1, 2, 2);
%     plot(abs(zc_seq).^2)

    taps = gold_seq ./ zc_seq;
end


function lik = lik_binomial(varargin)
%LIK_BINOMIAL  Create a Binomial likelihood structure
%
%  Description
%    LIK = LIK_BINOMIAL creates Binomial likelihood structure.
%
%    The likelihood is defined as follows:
%                  __ n
%      p(y|f, z) = || i=1 [ p_i^(y_i)*(1-p_i)^(z_i-y_i)) *
%                           gamma(z_i+1)/(gamma(y_i+1)*gamma(z_i-y_i+1))]
%    where p_i = exp(f_i)/ (1+exp(f_i)) is the succes probability,
%    which is a function of the latent variable f_i and z is a
%    vector of numbers of trials.
%
%    When using Binomial likelihood you need to give the vector z
%    as an extra parameter to each function that requires y also.
%    For example, you should call gpla_e as follows
%      gpla_e(w, gp, x, y, 'z', z)
%
%  See also
%    GP_SET, LIK_*
%

% Copyright (c) 2009-2010 Jaakko Riihim�ki & Jarno Vanhatalo
% Copyright (c) 2010-2011 Aki Vehtari

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

  ip=inputParser;
  ip.FunctionName = 'LIK_BINOMIAL';
  ip.addOptional('lik', [], @isstruct);
  ip.parse(varargin{:});
  lik=ip.Results.lik;

  if isempty(lik)
    init=true;
    lik.type = 'Binomial';
  else
    if ~isfield(lik,'type') || ~isequal(lik.type,'Binomial')
      error('First argument does not seem to be a valid likelihood function structure')
    end
    init=false;
  end

  if init
    % Set the function handles to the subfunctions
    lik.fh.pak = @lik_binomial_pak;
    lik.fh.unpak = @lik_binomial_unpak;
    lik.fh.ll = @lik_binomial_ll;
    lik.fh.llg = @lik_binomial_llg;
    lik.fh.llg2 = @lik_binomial_llg2;
    lik.fh.llg3 = @lik_binomial_llg3;
    lik.fh.tiltedMoments = @lik_binomial_tiltedMoments;
    lik.fh.predy = @lik_binomial_predy;
    lik.fh.predprcty = @lik_binomial_predprcty;
    lik.fh.invlink = @lik_binomial_invlink;
    lik.fh.recappend = @lik_binomial_recappend;
  end

end

function [w,s] = lik_binomial_pak(lik)
%LIK_BINOMIAL_PAK  Combine likelihood parameters into one vector.
%
%  Description
%    W = LIK_BINOMIAL_PAK(LIK) takes a likelihood structure LIK
%    and returns an empty verctor W. If Binomial likelihood had
%    parameters this would combine them into a single row vector
%    W (see e.g. likelih_negbin). This is a mandatory subfunction
%    used for example in energy and gradient computations.
%
%  See also
%    LIK_NEGBIN_UNPAK, GP_PAK

  w = []; s = {};
end

function [lik, w] = lik_binomial_unpak(lik, w)
%LIK_BINOMIAL_UNPAK  Extract likelihood parameters from the vector.
%
%  Description
%    W = LIK_BINOMIAL_UNPAK(W, LIK) Doesn't do anything.
%
%    If Binomial likelihood had parameters this would extracts
%    them parameters from the vector W to the LIK structure.
%    This is a mandatory subfunction used for example in energy
%    and gradient computations.
%
%  See also
%    LIK_BINOMIAL_PAK, GP_UNPAK

  lik=lik;
  w=w;

end

function ll = lik_binomial_ll(lik, y, f, z)
%LIK_BINOMIAL_LL  Log likelihood
%
%  Description
%    LL = LIK_BINOMIAL_LL(LIK, Y, F, Z) takes a likelihood
%    structure LIK, succes counts Y, numbers of trials Z, and
%    latent values F. Returns the log likelihood, log p(y|f,z).
%    This subfunction is needed when using Laplace approximation
%    or MCMC for inference with non-Gaussian likelihoods. This
%    subfunction is also used in information criteria (DIC, WAIC)
%    computations.
%
%  See also
%    LIK_BINOMIAL_LLG, LIK_BINOMIAL_LLG3, LIK_BINOMIAL_LLG2, GPLA_E

  if isempty(z)
    error(['lik_binomial -> lik_binomial_ll: missing z!'...
           'Binomial likelihood needs the expected number of   '...
           'occurrences as an extra input z. See, for         '...
           'example, lik_binomial and gpla_e.             ']);
  end

  expf = exp(f);
  p = expf ./ (1+expf);
  N = z;
  ll =  sum(gammaln(N+1)-gammaln(y+1)-gammaln(N-y+1)+y.*log(p)+(N-y).*log(1-p));
end

function llg = lik_binomial_llg(lik, y, f, param, z)
%LIK_BINOMIAL_LLG    Gradient of the log likelihood
%
%  Description
%    LLG = LIK_BINOMIAL_LLG(LIK, Y, F, PARAM) takes a likelihood
%    structure LIK, succes counts Y, numbers of trials Z and
%    latent values F. Returns the gradient of the log likelihood
%    with respect to PARAM. At the moment PARAM can be 'param' or
%    'latent'. This subfunction is needed when using Laplace
%    approximation or MCMC for inference with non-Gaussian
%    likelihoods.
%
%  See also
%    LIK_BINOMIAL_LL, LIK_BINOMIAL_LLG2, LIK_BINOMIAL_LLG3, GPLA_E

  if isempty(z)
    error(['lik_binomial -> lik_binomial_llg: missing z!'...
           'Binomial likelihood needs the expected number of   '...
           'occurrences as an extra input z. See, for         '...
           'example, lik_binomial and gpla_e.             ']);
  end

  switch param
    case 'latent'
      expf = exp(f);
      N = z;

      llg = y./(1+expf) - (N-y).*expf./(1+expf);
  end
end

function llg2 = lik_binomial_llg2(lik, y, f, param, z)
%LIK_BINOMIAL_LLG2  Second gradients of the log likelihood
%
%  Description
%    LLG2 = LIK_BINOMIAL_LLG2(LIK, Y, F, PARAM) takes a
%    likelihood structure LIK, succes counts Y, numbers of trials
%    Z, and latent values F. Returns the Hessian of the log
%    likelihood with respect to PARAM. At the moment PARAM can be
%    only 'latent'. G2 is a vector with diagonal elements of the
%    Hessian matrix (off diagonals are zero). This subfunction
%    is needed when using Laplace approximation or EP for inference
%    with non-Gaussian likelihoods.
%
%  See also
%    LIK_BINOMIAL_LL, LIK_BINOMIAL_LLG, LIK_BINOMIAL_LLG3, GPLA_E

  if isempty(z)
    error(['lik_binomial -> lik_binomial_llg2: missing z!'...
           'Binomial likelihood needs the expected number of    '...
           'occurrences as an extra input z. See, for          '...
           'example, lik_binomial and gpla_e.              ']);
  end

  switch param
    case 'latent'
      expf = exp(f);
      N = z;

      llg2 = -N.*expf./(1+expf).^2;
  end
end

function llg3 = lik_binomial_llg3(lik, y, f, param, z)
%LIK_BINOMIAL_LLG3  Third gradients of the log likelihood
%
%  Description
%    LLG3 = LIK_BINOMIAL_LLG3(LIK, Y, F, PARAM) takes a
%    likelihood structure LIK, succes counts Y, numbers of trials
%    Z and latent values F and returns the third gradients of the
%    log likelihood with respect to PARAM. At the moment PARAM
%    can be only 'latent'. G3 is a vector with third gradients.
%    This subfunction is needed when using Laplace appoximation
%    for inference with non-Gaussian likelihoods.
%
%  See also
%    LIK_BINOMIAL_LL, LIK_BINOMIAL_LLG, LIK_BINOMIAL_LLG2, GPLA_E, GPLA_G

  if isempty(z)
    error(['lik_binomial -> lik_binomial_llg3: missing z!'...
           'Binomial likelihood needs the expected number of    '...
           'occurrences as an extra input z. See, for          '...
           'example, lik_binomial and gpla_e.              ']);
  end

  switch param
    case 'latent'
      expf = exp(f);
      N = z;
      llg3 = N.*(expf.*(expf-1))./(1+expf).^3;
  end
end

function [logM_0, m_1, sigm2hati1] = lik_binomial_tiltedMoments(lik, y, i1, sigm2_i, myy_i, z)
%LIK_BINOMIAL_TILTEDMOMENTS  Returns the marginal moments for EP algorithm
%
%  Description
%    [M_0, M_1, M2] = LIK_BINOMIAL_TILTEDMOMENTS(LIK, Y, I, S2,
%    MYY, Z) takes a likelihood structure LIK, succes counts Y,
%    numbers of trials Z, index I and cavity variance S2 and mean
%    MYY. Returns the zeroth moment M_0, mean M_1 and variance
%    M_2 of the posterior marginal (see Rasmussen and Williams
%    (2006): Gaussian processes for Machine Learning, page 55).
%    This subfunction is needed when using EP for inference with
%    non-Gaussian likelihoods.
%
%  See also
%    GPEP_E

%  if isempty(z)
%    error(['lik_binomial -> lik_binomial_tiltedMoments: missing z!'...
%           'Binomial likelihood needs the expected number of               '...
%           'occurrences as an extra input z. See, for                     '...
%           'example, lik_binomial and gpla_e.                         ']);
%  end

  yy = y(i1);
  N = z(i1);
  logM_0=zeros(size(yy));
  m_1=zeros(size(yy));
  sigm2hati1=zeros(size(yy));

  for i=1:length(i1)
    % Create function handle for the function to be integrated
    % (likelihood * cavity) and useful integration limits
    [tf,minf,maxf]=init_binomial_norm(yy(i),myy_i(i),sigm2_i(i),N(i));

    % Integrate with quadrature
    RTOL = 1.e-6;
    ATOL = 1.e-10;
    [m_0, m_1(i), m_2] = quad_moments(tf,minf, maxf, RTOL, ATOL);
    sigm2hati1(i) = m_2 - m_1(i).^2;

    % If the second central moment is less than cavity variance
    % integrate more precisely. Theoretically for log-concave
    % likelihood should be sigm2hati1 < sigm2_i.
    if sigm2hati1(i) >= sigm2_i(i)
      ATOL = ATOL.^2;
      RTOL = RTOL.^2;
      [m_0, m_1(i), m_2] = quad_moments(tf, minf, maxf, RTOL, ATOL);
      sigm2hati1 = m_2 - m_1(i).^2;
      %    if sigm2hati1 >= sigm2_i
      %      error('lik_binomial_tilted_moments: sigm2hati1 >= sigm2_i');
      %    end
    end
    logM_0(i) = log(m_0);
  end
end

function [lpy, Ey, Vary] = lik_binomial_predy(lik, Ef, Varf, yt, zt)
%LIK_BINOMIAL_PREDY  Returns the predictive mean, variance and density of y
%
%  Description
%    [LPY] = LIK_BINOMIAL_PREDY(LIK, EF, VARF YT, ZT)
%    Returns logarithm of the predictive density PY of YT, that is
%        p(yt | y, zt) = \int p(yt | f, zt) p(f|y) df.
%    This requires also the succes counts YT, numbers of trials ZT.
%    This subfunction is needed when computing posterior predictive
%    distributions for future observations.
%
%    [LPY, EY, VARY] = LIK_BINOMIAL_PREDY(LIK, EF, VARF) takes a
%    likelihood structure LIK, posterior mean EF and posterior
%    Variance VARF of the latent variable and returns the
%    posterior predictive mean EY and variance VARY of the
%    observations related to the latent variables. This subfunction
%    is needed when computing posterior predictive distributions for
%    future observations.
%
%
%  See also
%    GPEP_PRED, GPLA_PRED, GPMC_PRED

  if isempty(zt)
    error(['lik_binomial -> lik_binomial_predy: missing z!'...
           'Binomial likelihood needs the expected number of       '...
           'occurrences as an extra input z. See, for             '...
           'example, lik_binomial and gpla_e.                 ']);
  end

  if nargout > 1
    nt=length(Ef);
    Ey=zeros(nt,1);
    EVary = zeros(nt,1);
    VarEy = zeros(nt,1);
    for i1=1:nt
      ci = sqrt(Varf(i1));
      F  = @(x)zt(i1)./(1+exp(-x)).*norm_pdf(x,Ef(i1),sqrt(Varf(i1)));
      Ey(i1) = quadgk(F,Ef(i1)-6*ci,Ef(i1)+6*ci);

      F2  = @(x)zt(i1)./(1+exp(-x)).*(1-1./(1+exp(-x))).*norm_pdf(x,Ef(i1),sqrt(Varf(i1)));
      EVary(i1) = quadgk(F2,Ef(i1)-6*ci,Ef(i1)+6*ci);

      F3  = @(x)(zt(i1)./(1+exp(-x))).^2.*norm_pdf(x,Ef(i1),sqrt(Varf(i1)));
      VarEy(i1) = quadgk(F3,Ef(i1)-6*ci,Ef(i1)+6*ci) - Ey(i1).^2;
    end
    Vary = EVary+VarEy;
  end

  nt=length(yt);
  lpy=zeros(nt,1);
  for i1=1:nt
    ci = sqrt(Varf(i1));
    F  = @(x)exp(gammaln(zt(i1)+1)-gammaln(yt(i1)+1)-gammaln(zt(i1)-yt(i1)+1) + yt(i1).*log(1./(1+exp(-x))) + (zt(i1)-yt(i1)).*log(1-(1./(1+exp(-x))))).*norm_pdf(x,Ef(i1),sqrt(Varf(i1)));
    lpy(i1) = log(quadgk(F,Ef(i1)-6*ci,Ef(i1)+6*ci));
  end

end

function prctys = lik_binomial_predprcty(lik, Ef, Varf, zt, prcty)
%LIK_BINOMIAL_PREDPRCTY  Returns the percentiled of predictive density of y
%
%  Description
%    PRCTY = LIK_BINOMIAL_PREDPRCTY(LIK, EF, VARF YT, ZT)
%    Returns percentiles of the predictive density PY of YT, that is
%    This requires also the succes counts YT, numbers of trials ZT. This
%    subfunction is needed when using function gp_predprcty.
%
%  See also
%    GP_PREDPCTY

  if isempty(zt)
    error(['lik_binomial -> lik_binomial_predprcty: missing z!'...
           'Binomial likelihood needs the expected number of       '...
           'occurrences as an extra input z. See, for             '...
           'example, lik_binomial and gpla_e.                 ']);
  end

  opt=optimset('TolX',.5,'Display','off');
  nt=size(Ef,1);
  prctys = zeros(nt,numel(prcty));
  prcty=prcty/100;
  for i1=1:nt
    ci = sqrt(Varf(i1));
    for i2=1:numel(prcty)
      a=floor(fminbnd(@(a) (quadgk(@(f) binocdf(a,zt(i1),logitinv(f)).*norm_pdf(f,Ef(i1),ci),Ef(i1)-6*ci,Ef(i1)+6*ci,'AbsTol',1e-4)-prcty(i2)).^2,binoinv(prcty(i2),zt(i1),logitinv(Ef(i1)-1.96*ci)),binoinv(prcty(i2),zt(i1),logitinv(Ef(i1)+1.96*ci)),opt));
      if quadgk(@(f) binocdf(a,zt(i1),logitinv(f)).*norm_pdf(f,Ef(i1),ci),Ef(i1)-6*ci,Ef(i1)+6*ci,'AbsTol',1e-4)<prcty(i2)
        a=a+1;
      end
      prctys(i1,i2)=a;
    end
  end
end

function [df,minf,maxf] = init_binomial_norm(yy,myy_i,sigm2_i,N)
%INIT_LOGIT_NORM
%
%  Description
%    Return function handle to a function evaluating Binomial *
%    Gaussian which is used for evaluating (likelihood * cavity)
%    or (likelihood * posterior) Return also useful limits for
%    integration. This is private function for lik_binomial. This
%    subfunction is needed by subfunctions tiltedMoments and predy.
%
% See also
%   LIK_BINOMIAL_TILTEDMOMENTS, LIK_BINOMIAL_PREDY

% avoid repetitive evaluation of constant part
  ldconst = gammaln(N+1)-gammaln(yy+1)-gammaln(N-yy+1) - log(sigm2_i)/2 - log(2*pi)/2;
%   ldconst = log(factorial(N)/(factorial(yy)*factorial(N-yy))-log(sigm2_i)/2 -log(2*pi)/2;

 % Create function handle for the function to be integrated
  df = @binomial_norm;
 % use log to avoid underflow, and derivates for faster search
  ld = @log_binomial_norm;
  ldg = @log_binomial_norm_g;
  ldg2 = @log_binomial_norm_g2;

  % Set the limits for integration
  % Binomial likelihood is log-concave so the binomial_norm
  % function is unimodal, which makes things easier
  if yy==0 || yy==N
    % with yy==0 or yy==N the mode of the likelihood is not defined
    % use the mode of the Gaussian (cavity or posterior) as a first guess
    modef = myy_i;
  else
    % use precision weighted mean of the Gaussian approximation of the
    % binomial likelihood and Gaussian
    mean_app = log(yy./(N-yy));
    ld0=1/(1+exp(-mean_app));
    ld1=(1-ld0)*ld0;
    ld2=ld0-3*ld0^2+2*ld0^3;
    var_app=inv(-( yy*(ld2*ld0-ld1^2)/ld0^2 + (N-yy)*(ld2*(ld0-1)-ld1^2)/(ld0-1)^2 ));

    modef = (myy_i/sigm2_i + mean_app/var_app)/(1/sigm2_i + 1/var_app);
%     sigm_app = sqrt((1/sigm2_i + 1/var_app)^-1);
  end
  % find the mode of the integrand using Newton iterations
  % few iterations is enough, since the first guess in the right direction
  niter=3;       % number of Newton iterations
  mindelta=1e-6; % tolerance in stopping Newton iterations
  for ni=1:niter
      g = ldg(modef);
      h = ldg2(modef);
      delta=-g/h;
      modef=modef+delta;
      if abs(delta)<mindelta
          break
      end
  end
  % integrand limits based on Gaussian approximation at mode
  modes=sqrt(-1/h);
  minf=modef-4*modes;
  maxf=modef+4*modes;
  modeld=ld(modef);
  iter=0;
  % check that density at end points is low enough
  lddiff=12; % min difference in log-density between mode and end-points
  minld=ld(minf);
  step=1;
  while minld>(modeld-lddiff)
    minf=minf-step*modes;
    minld=ld(minf);
    iter=iter+1;
    step=step*2;
    if iter>100
      error(['lik_negbin -> init_negbin_norm: ' ...
             'integration interval minimun not found ' ...
             'even after looking hard!'])
    end
  end
  maxld=ld(maxf);
  step=1;
  while maxld>(modeld-lddiff)
    maxf=maxf+step*modes;
    maxld=ld(maxf);
    iter=iter+1;
    step=step*2;
    if iter>100
      error(['lik_negbin -> init_negbin_norm: ' ...
             'integration interval maximun not found ' ...
             'even after looking hard!'])
    end
  end

  function integrand = binomial_norm(f)
  % Logit * Gaussian
    integrand = exp(ldconst + yy*log(1./(1.+exp(-f)))+(N-yy)*log(1-1./(1.+exp(-f)))...
                   - 0.5 * (f-myy_i).^2./sigm2_i);
%     integrand = exp(ldconst ...
%                     +yy*log(x)+(N-yy)*log(1-x) ...
%                     -0.5*(f-myy_i).^2./sigm2_i);
    integrand(isnan(integrand))=0;
  end

  function log_int = log_binomial_norm(f)
  % log(Binomial * Gaussian)
  % log_binomial_norm is used to avoid underflow when searching
  % integration interval

    log_int = ldconst + yy*log(1./(1.+exp(-f)))+(N-yy)*log(1-1./(1.+exp(-f)))...
                   - 0.5 * (f-myy_i).^2./sigm2_i;
%     log_int = ldconst ...
%               -log(1+exp(-yy.*f)) ...
%               -0.5*(f-myy_i).^2./sigm2_i;
  end

  function g = log_binomial_norm_g(f)
  % d/df log(Binomial * Gaussian)
  % derivative of log_logit_norm
    g = -(f-myy_i)./sigm2_i - exp(-f).*(N-yy)./((1+exp(-f)).^2.*(1-1./(1+exp(-f)))) ...
        + exp(-f).*yy./(1+exp(-f));
%     g = yy./(exp(f*yy)+1)...
%         + (myy_i - f)./sigm2_i;
  end

  function g2 = log_binomial_norm_g2(f)
  % d^2/df^2 log(Binomial * Gaussian)
  % second derivate of log_logit_norm
    g2 = - (1+exp(2.*f)+exp(f).*(2+N*sigm2_i)./((1+exp(f))^2*sigm2_i));
%     a=exp(f*yy);
%     g2 = -a*(yy./(a+1)).^2 ...
%          -1/sigm2_i;
  end

end

function p = lik_binomial_invlink(lik, f, z)
%LIK_BINOMIAL_INVLINK  Returns values of inverse link function
%
%  Description
%    P = LIK_BINOMIAL_INVLINK(LIK, F) takes a likelihood structure LIK and
%    latent values F and returns the values of inverse link function P.
%    This subfunction is needed when using gp_predprctmu.
%
%     See also
%     LIK_BINOMIAL_LL, LIK_BINOMIAL_PREDY

  p = logitinv(f);
end

function reclik = lik_binomial_recappend(reclik, ri, lik)
%RECAPPEND  Append the parameters to the record
%
%  Description
%    RECLIK = GPCF_BINOMIAL_RECAPPEND(RECLIK, RI, LIK) takes a
%    likelihood record structure RECLIK, record index RI and
%    likelihood structure LIK with the current MCMC samples of
%    the parameters. Returns RECLIK which contains all the old
%    samples and the current samples from LIK. This subfunction
%    is needed when using MCMC sampling (gp_mc).
%
%  See also
%    GP_MC

  if nargin == 2
    reclik.type = 'Binomial';

    % Set the function handles
    reclik.fh.pak = @lik_binomial_pak;
    reclik.fh.unpak = @lik_binomial_unpak;
    reclik.fh.ll = @lik_binomial_ll;
    reclik.fh.llg = @lik_binomial_llg;
    reclik.fh.llg2 = @lik_binomial_llg2;
    reclik.fh.llg3 = @lik_binomial_llg3;
    reclik.fh.tiltedMoments = @lik_binomial_tiltedMoments;
    reclik.fh.invlink = @lik_binomial_invlink;
    reclik.fh.predprcty = @lik_binomial_predprcty;
    reclik.fh.predy = @lik_binomial_predy;
    reclik.fh.recappend = @likelih_binomial_recappend;
    return
  end

end


function a = sin(a)
%SIN          Gradient sine sin(a)
%

% written  10/16/98     S.M. Rump
% modified 10/14/00     S.M. Rump  use Tony's trick
% modified 04/04/04     S.M. Rump  set round to nearest for safety
%                                    accelaration for sparse input
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
% modified 10/08/08     S.M. Rump  improved sparse multiplication: not using intval data type
% modified 08/26/12     S.M. Rump  global variables removed
%

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  N = getappdata(0,'INTLAB_GRADIENT_NUMVAR');

  % use full(a.x(:)): cures Matlab V6.0 bug
  % a=7; i=[1 1]; x=a(i), b=sparse(a); y=b(i)  yields row vector x but column vector y
  % ax is full anyway
  ax = cos(full(a.x(:)));
  a.x = sin(a.x);
  if issparse(a.dx)
    sizeax = size(a.dx,1);
    [ia,ja,sa] = find(a.dx);
    if isa(a.x,'intval')
      adx = times(ax(ia),sa,0);
      if adx.complex
        a.dx = intval( sparse(ia,ja,adx.mid,sizeax,N) , sparse(ia,ja,adx.rad,sizeax,N) , 'midrad' );
      else
        a.dx = intval( sparse(ia,ja,adx.inf,sizeax,N) , sparse(ia,ja,adx.sup,sizeax,N) , 'infsup' );
      end
    else
      a.dx = sparse(ia,ja,ax(ia).*sa,sizeax,N);
    end
  else
    a.dx = a.dx .* ax(:,ones(1,N));
  end

  if rndold
    setround(rndold)
  end


function C = encode(p,R)
%
% Compute vector of transmit costs for each stage of the KD-tree
%

N = getNpts(p);  bw = getBW(p,1);
if (size(p.bandwidth,2) > 2*N)
  error('Encoding of variable bandwidths not yet supported...');
end;
if (any( abs(getWeights(p) - 1/N) > 2*eps ))
  error('Encoding of variable weights not yet supported...');
end;
C = zeros(1,2*N); C(1) = 2*getDim(p)*R;		% Root node...

for i=1:N-1,            % calc costs of splitting each node; 1/2 cost to each side
 if (~isLeaf(i,N))
  mu0 = p.means(:,i);				% get means of parent, children
  mu1 = p.means(:, double(p.leftch(i))+1);
  mu2 = p.means(:, double(p.rightch(i))+1);
  sig0= sqrt( p.bandwidth(:,i) );		% and bw's of parent, children
  sig1= sqrt( p.bandwidth(:,double(p.leftch(i))+1) );
  sig2= sqrt( p.bandwidth(:,double(p.rightch(i))+1) );
  sig0UB = sqrt(max( sig0.^2 - bw.^2, 2^-R));	% don't round down too far...

  [tmp,splitDim] = max( sig0UB );		% which dimension are we splitting on...
  %[tmp,splitDim2] = max( abs(mu0-mu1) + abs(mu0-mu2) );
  %if (splitDim ~= splitDim2) warning('Disagreement?'); end;

  % COMPUTE COST OF SENDING MEANS
  muMax = max(mu1,mu2); sigDiff = sig0.^2 - (mu1.^2 + mu2.^2 - mu0.^2);
  for j=splitDim,
    costMu = gauss( muMax(j), mu0(j), sig0UB(j).^2 ,1,R );
  end;
  for j = [1:splitDim-1,splitDim+1:getDim(p)];
    costMu = costMu + gauss2( mu1(j), mu0(j), 1*sig0UB(j).^2 ,1,R );
  end;

  % COMPUTE COST OF SENDING VARIANCES
  if (isLeaf(p.leftch(i),N) && isLeaf(p.rightch(i),N)), costSig = 0; costMu = 0;
  elseif (isLeaf(p.leftch(i),N) || isLeaf(p.rightch(i),N)), costSig = 0;
  else
    for j=splitDim,
      costSig = gauss2( sig1(j).^2 , sig0(j).^2/2, sig0(j).^2/4, 1, R);
    end;
    for j = [1:splitDim-1,splitDim+1:getDim(p)]
      costSig = costSig + gauss2( sig1(j).^2 , sig0(j).^2, sig0(j).^2/2, 1, R);
    end;
  end;

  %[costMu,costSig]
  C(double(p.leftch(i))+1) = .5 * (C(i) + costMu + costSig);
  C(double(p.rightch(i))+1) = .5 * (C(i) + costMu + costSig);
 end;
end;

function v = gauss(x,mu,sig2,sigOut2,R)		% SIMPLE 1-SIDED GAUSSIAN
  v = R-log2(  2*1/sqrt(2*pi*sig2) .* exp(-(x-mu).^2 ./(2*sig2)) );

%function v = gauss(x,mu,sig2,sigOut2,R)	% 1-SIDED W/ OUTLIER PROCESS
%  v = R-log2(  2*1/sqrt(2*pi*sig2) .* exp(-(x-mu).^2 ./(2*sig2)) ); + ...
%               1/sqrt(2*pi*sig2) .* exp(-(x-mu).^2 ./(2*sigOut2)) );

function v = gauss2(x,mu,sig2,sigOut2,R)	% SIMPLE 2-SIDED GAUSSIAN
  v = R-log2(  1/sqrt(2*pi*sig2) .* exp(-(x-mu).^2 ./(2*sig2)) );

%function v = gauss2(x,mu,sig2,sigOut2,R)	% 2-SIDED W/ OUTLIER PROCESS
%  v = R-log2(  1/sqrt(2*pi*sig2) .* exp(-(x-mu).^2 ./(2*sig2))  + ...
%               1/sqrt(2*pi*sig2) .* exp(-(x-mu).^2 ./(2*sigOut2)) );

function b = isLeaf(ind,N)
  ind = double(ind);
  if (ind <= 0 || ind > 2*N) b = 0;
  elseif (ind <= N-1) b = 0;
  else b = 1;
  end;