%
% LibQPEP: A Library for Globally Optimal Solving Quadratic Pose Estimation Problems (QPEPs),
%          It also gives highly accurate uncertainty description of the solutions.
%
%
% Article:
%      Wu, J., Zheng, Y., Gao, Z., Jiang, Y., Hu, X., Zhu, Y., Jiao, J., Liu, M. (2020)
%           Quadratic Pose Estimation Problems: Globally Optimal Solutions,
%           Solvability/Observability Analysis and Uncertainty Description.
%           IEEE Transactions on Robotics.
%           https://doi.org/10.1109/TRO.2022.3155880
%
%
% Authors:      Jin Wu and Ming Liu
% Affiliation:  Hong Kong University of Science and Technology (HKUST)
% Emails:       jin_wu_uestc@hotmail.com; eelium@ust.hk
% Websites:     https://zarathustr.github.io
%               https://ram-lab.com
%
%
% syms_pnp.m: Generating symbolic expressions for solving PnP problem

clear all
close all
clc

addpath('func_files');
addpath('utils');

syms b1 b2 real
syms r1 r2 r3 real

image_pt = [b1, b2];
world_pt = [r1, r2, r3];

syms fx fy cx cy real
K = [fx, 0, 0;
      0, fy, 0;
     cx, cy, 1];

syms q0 q1 q2 q3 real
syms t1 t2 t3 lambda scale real
q = [q0; q1; q2; q3];
tt = [t1; t2; t3];
RR = q2R(q);
XX = [
    RR, tt;
    zeros(1, 3), 1];
J_pure = J_pnp_loss(image_pt, world_pt, K, RR, tt) * scale;
[coef_J_pure, mon_J_pure] = coeffs(J_pure, [q; tt]);
generateFuncFile(coef_J_pure, fullfile('func_files', 'coef_J_pure_pnp_func_new.m'), {image_pt, world_pt, [fx, fy, cx, cy], scale});
generateFuncFile(mon_J_pure, fullfile('func_files', 'mon_J_pure_pnp_func_new.m'), {q, tt});
J = J_pure - 0.5 * lambda * (q.' * q - 1);
x = [q; tt; lambda];
Jacob = jacobian(J, x);
Jacob = expand(Jacob.');

coef_len = length(coef_J_pure);
str = sprintf('coef_J = sym(''coef_J'', [1, %d]);', coef_len);
eval(str);
showSyms(symvar(coef_J));
J = coef_J * mon_J_pure.' - 0.5 * lambda * (q.' * q - 1);
x = [q; tt; lambda];
Jacob = jacobian(J, x);
Jacob = expand(Jacob.');

ts = solveSyms(Jacob(5 : 7), tt);
ss = Jacob(1 : 4);
ss = subs(ss, q0^3, (q0 * (1 - q1^2 - q2^2 - q3^2)));
ss = subs(ss, q0^2, (1 - q1^2 - q2^2 - q3^2));
Jacob(1 : 4) = ss;

ss = subs(ss, t1, ts.t1);
ss = subs(ss, t2, ts.t2);
ss = subs(ss, t3, ts.t3);

[coeft1, mont1] = coeffs(Jacob(5), tt);
g1_ = coeft1(1 : 3).';
g1 = vpa(zeros(3, 1));
for i = 1 : 3
    idx = sscanf(char(mont1(i)), 't%d');
    if(~isempty(idx))
        g1(idx) = g1_(i);
    end
end
[coeft2, mont2] = coeffs(Jacob(6), tt);
g2_ = coeft2(1 : 3).';
g2 = vpa(zeros(3, 1));
for i = 1 : 3
    idx = sscanf(char(mont2(i)), 't%d');
    if(~isempty(idx))
        g2(idx) = g2_(i);
    end
end
[coeft3, mont3] = coeffs(Jacob(7), tt);
g3_ = coeft3(1 : 3).';
g3 = vpa(zeros(3, 1));
for i = 1 : 3
    idx = sscanf(char(mont3(i)), 't%d');
    if(~isempty(idx))
        g3(idx) = g3_(i);
    end
end

[coeftq1, montq1] = coeffs(expand(Jacob(5) - g1.' * tt ), q);
generateFuncFile(coeftq1, fullfile('func_files', 'coeftq1_pnp_func_new.m'), {coef_J});
[coeftq2, montq2] = coeffs(expand(Jacob(6) - g2.' * tt ), q);
generateFuncFile(coeftq2, fullfile('func_files', 'coeftq2_pnp_func_new.m'), {coef_J});
[coeftq3, montq3] = coeffs(expand(Jacob(7) - g3.' * tt ), q);
generateFuncFile(coeftq3, fullfile('func_files', 'coeftq3_pnp_func_new.m'), {coef_J});
G = - [g1.'; g2.'; g3.'];
generateFuncFile(G, fullfile('func_files', 'G_pnp_func_new.m'), {coef_J});

pinvG = sym('pinvG', [3, 3]);
showSyms(symvar(pinvG));
coefs_tq = sym('coefs_tq', [3, 10]);
showSyms(symvar(coefs_tq));
ts = pinvG * coefs_tq * montq1.';
t1 = ts(1);
t2 = ts(2);
t3 = ts(3);
generateFuncFile(t1, fullfile('func_files', 't1_pnp_func_new.m'), {pinvG, coefs_tq, q});
generateFuncFile(t2, fullfile('func_files', 't2_pnp_func_new.m'), {pinvG, coefs_tq, q});
generateFuncFile(t3, fullfile('func_files', 't3_pnp_func_new.m'), {pinvG, coefs_tq, q});

[coef_Jacob1_qt, mon_Jacob1_qt] = coeffs(Jacob(1) + lambda * q0, [q; tt]);
generateFuncFile(coef_Jacob1_qt, fullfile('func_files', 'coef_Jacob1_qt_pnp_func_new.m'), {coef_J});
[coef_Jacob2_qt, mon_Jacob2_qt] = coeffs(Jacob(2) + lambda * q1, [q; tt]);
generateFuncFile(coef_Jacob2_qt, fullfile('func_files', 'coef_Jacob2_qt_pnp_func_new.m'), {coef_J});
[coef_Jacob3_qt, mon_Jacob3_qt] = coeffs(Jacob(3) + lambda * q2, [q; tt]);
generateFuncFile(coef_Jacob3_qt, fullfile('func_files', 'coef_Jacob3_qt_pnp_func_new.m'), {coef_J});
[coef_Jacob4_qt, mon_Jacob4_qt] = coeffs(Jacob(4) + lambda * q3, [q; tt]);
generateFuncFile(coef_Jacob4_qt, fullfile('func_files', 'coef_Jacob4_qt_pnp_func_new.m'), {coef_J});

coef_Jacob1_qt_syms = sym('coef_Jacob1_qt_syms', [1, 32]);
coef_Jacob2_qt_syms = sym('coef_Jacob2_qt_syms', [1, 32]);
coef_Jacob3_qt_syms = sym('coef_Jacob3_qt_syms', [1, 32]);
coef_Jacob4_qt_syms = sym('coef_Jacob4_qt_syms', [1, 32]);
showSyms(symvar(coef_Jacob1_qt_syms));
showSyms(symvar(coef_Jacob2_qt_syms));
showSyms(symvar(coef_Jacob3_qt_syms));
showSyms(symvar(coef_Jacob4_qt_syms));
coef_Jacob_qt_syms = [
    coef_Jacob1_qt_syms;
    coef_Jacob2_qt_syms;
    coef_Jacob3_qt_syms;
    coef_Jacob4_qt_syms;
    ];
Jacob_ = coef_Jacob_qt_syms * mon_Jacob1_qt.';

eqs = expand(eval([Jacob_; Jacob(8)]));

eqs_ = eqs;
eqs_(1 : 4) = eval(subs(eqs_(1 : 4), q0^2, (1 - q3 * q3 - q1 * q1 - q2 * q2)));
eqs_(1 : 4) = eval(subs(eqs_(1 : 4), q0^3, q0 * (1 - q3 * q3 - q1 * q1 - q2 * q2)));

f0 = eqs(1);
f1 = eqs(2);
f2 = eqs(3);
f3 = eqs(4);
[coef_f0_q, mon_f0_q] = coeffs(f0, q);
[coef_f1_q, mon_f1_q] = coeffs(f1, q);
[coef_f2_q, mon_f2_q] = coeffs(f2, q);
[coef_f3_q, mon_f3_q] = coeffs(f3, q);
coef_f0_q_sym = sym('coef_f0_q_sym', [1, 24]);
coef_f1_q_sym = sym('coef_f1_q_sym', [1, 24]);
coef_f2_q_sym = sym('coef_f2_q_sym', [1, 24]);
coef_f3_q_sym = sym('coef_f3_q_sym', [1, 24]);
showSyms(symvar(coef_f0_q_sym));
showSyms(symvar(coef_f1_q_sym));
showSyms(symvar(coef_f2_q_sym));
showSyms(symvar(coef_f3_q_sym));
f0 = coef_f0_q_sym * mon_f0_q.';
f1 = coef_f1_q_sym * mon_f1_q.';
f2 = coef_f2_q_sym * mon_f2_q.';
f3 = coef_f3_q_sym * mon_f3_q.';
coef_f_q_sym = [
    coef_f0_q;
    coef_f1_q;
    coef_f2_q;
    coef_f3_q;
    ];
generateFuncFile(coef_f_q_sym, fullfile('func_files', 'coef_f_q_sym_pnp_func_new.m'), {pinvG, coefs_tq, coef_Jacob_qt_syms});

eq = expand([
     f0 * q1 - f1 * q0;
     f0 * q2 - f2 * q0;
     f0 * q3 - f3 * q0;
     q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3 - 1;
     ]);
generateFuncFile(eq, fullfile('func_files', 'eq_pnp_func_new.m'), {coef_f0_q_sym, coef_f1_q_sym, coef_f2_q_sym, coef_f3_q_sym, q});

eq_ = eq;
[coefeq1, moneq1] = coeffs(eq_(1), q);
[coefeq2, moneq2] = coeffs(eq_(2), q);
[coefeq3, moneq3] = coeffs(eq_(3), q);

v1 = moneq1.';
generateFuncFile(v1, fullfile('func_files', 'v1_func_pnp_new.m'), {q});
v2 = moneq2.';
generateFuncFile(v2, fullfile('func_files', 'v2_func_pnp_new.m'), {q});
v3 = moneq3.';
generateFuncFile(v3, fullfile('func_files', 'v3_func_pnp_new.m'), {q});

D = [
    coefeq1;
    coefeq2;
    coefeq3;
    ];
generateFuncFile(D, fullfile('func_files', 'D_func_pnp_new.m'), {coef_f0_q_sym, coef_f1_q_sym, coef_f2_q_sym, coef_f3_q_sym});

HH = jacobian(eq, q);
generateFuncFile(HH, fullfile('func_files', 'Jacob_pnp_func_new.m'), {coef_f0_q_sym, coef_f1_q_sym, coef_f2_q_sym, coef_f3_q_sym, q});
gradient = expand(HH.' * eq);

[coef1, mon1] = coeffs(eqs_(1), x);
[coef2, mon2] = coeffs(eqs_(2), x);
[coef3, mon3] = coeffs(eqs_(3), x);
[coef4, mon4] = coeffs(eqs_(4), x);

Q_sym = [
    coef1(7), coef1(14), coef1(18), coef1(20);
    coef2(7), coef2(14), coef2(18), coef2(20);
    coef3(7), coef3(14), coef3(18), coef3(20);
    coef4(7), coef4(14), coef4(18), coef4(20);
    ];
generateFuncFile(Q_sym, fullfile('func_files', 'Q_pnp_func_new.m'), {pinvG, coefs_tq, coef_Jacob_qt_syms});

res = expand(eqs_(1 : 4) - Q_sym * q);
H = jacobian(res, q);
h1 = H(:, 1);
h2 = H(:, 2);
h3 = H(:, 3);
h4 = H(:, 4);
P1 = jacobian(h1, q);
P2 = jacobian(h2, q);
P3 = jacobian(h3, q);
P4 = jacobian(h4, q);

for i = 1 : 4
    for j = 1 : 4
        str = sprintf('W%d%d = [jacobian(P%d(1, :), q%d).''; jacobian(P%d(2, :), q%d).''; jacobian(P%d(3, :), q%d).''; jacobian(P%d(4, :), q%d).''];', ...
                        i, j, i, j - 1, i, j - 1, i, j - 1, i, j - 1);
        eval(str);
    end
end

W1 = [W11, W12, W13, W14];
W2 = [W21, W22, W23, W24];
W3 = [W31, W32, W33, W34];
W4 = [W41, W42, W43, W44];
v = kron(q, q);
u = kron(v, q);
W = - [W1, W2, W3, W4] / 6;
generateFuncFile(W, fullfile('func_files', 'W_pnp_func_new.m'), {pinvG, coefs_tq, coef_Jacob_qt_syms});


%DEMO_REGRESSION1  Regression problem demonstration for 2-input
%                  function with Gaussian process
%
%  Description
%    The regression problem consist of a data with two input
%    variables and one output variable with Gaussian noise. The
%    model constructed is following:
%
%    The observations y are assumed to satisfy
%
%         y = f + e,    where e ~ N(0, s^2)
%
%    where f is an underlying function, which we are interested in.
%    We place a zero mean Gaussian process prior for f, which
%    implies that at the observed input locations latent values
%    have prior
%
%         f ~ N(0, K),
%
%    where K is the covariance matrix, whose elements are given as
%    K_ij = k(x_i, x_j | th). The function k(x_i, x_j | th) is
%    covariance function and th its parameters.
%
%    Since both likelihood and prior are Gaussian, we obtain a
%    Gaussian marginal likelihood
%
%        p(y|th) = N(0, K + I*s^2).
%
%   By placing a prior for parameters, p(th), we can find
%   the maximum a posterior (MAP) estimate for them by maximizing
%
%       argmax   log p(y|th) + log p(th).
%         th
%
%   An approximation for the posterior of the parameters, can be
%   found using Markov chain Monte Carlo (MCMC) methods. We can
%   integrate over the parameters also with other integration
%   approximations such as grid integration.
%
%   After finding MAP estimate or posterior samples of
%   parameters, we can use them to make predictions for f_new:
%
%       p(f_new | y, th) = N(m, S),
%
%          m = K_nt*(K + I*s^2)^(-1)*y
%          S = K_new - K_nt*(K + I*s^2)^(-1)*K_tn
%
%   where K_new is the covariance matrix of new f, and K_nt between
%   new f and training f.
%
%   For more detailed discussion of Gaussian process regression see,
%   for example, Rasmussen and Williams (2006) or Vanhatalo and
%   Vehtari (2008)
%
%   The demo is organised in three parts:
%     1) data analysis with MAP estimate for the parameters
%     2) data analysis with grid integration over the parameters
%     3) data analysis with MCMC integration over the parameters
%
%  See also DEMO_*
%
%  References:
%    Rasmussen, C. E. and Williams, C. K. I. (2006). Gaussian
%    Processes for Machine Learning. The MIT Press.
%
%    Vanhatalo, J. and Vehtari, A. (2008). Modelling local and global
%    phenomena with sparse Gaussian processes. Proceedings of the 24th
%    Conference on Uncertainty in Artificial Intelligence,

% Copyright (c) 2008-2010 Jarno Vanhatalo
% Copyright (c) 2010 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.

%========================================================
% PART 1 data analysis with full GP model
%========================================================
disp('GP with Gaussian noise model')

% Load the data
S = which('demo_regression1');
L = strrep(S,'demo_regression1.m','demodata/dat.1');
data=load(L);
x = [data(:,1) data(:,2)];
y = data(:,3);
[n, nin] = size(x);

% Now 'x' consist of the inputs and 'y' of the output.
% 'n' and 'nin' are the number of data points and the
% dimensionality of 'x' (the number of inputs).

% ---------------------------
% --- Construct the model ---
%
% First create structures for Gaussian likelihood and squared
% exponential covariance function with ARD
lik = lik_gaussian('sigma2', 0.2^2);
gpcf = gpcf_sexp('lengthScale', [1.1 1.2], 'magnSigma2', 0.2^2)

% Set some priors
pn = prior_logunif();
lik = lik_gaussian(lik,'sigma2_prior', pn);
pl = prior_unif();
pm = prior_sqrtunif();
gpcf = gpcf_sexp(gpcf, 'lengthScale_prior', pl, 'magnSigma2_prior', pm);

% Following lines do the same since the default type is FULL
%gp = gp_set('type','FULL','lik',lik,'cf',gpcf);
gp = gp_set('lik', lik, 'cf', gpcf);

% Demostrate how to evaluate covariance matrices.
% K contains the covariance matrix without noise variance
%  at the diagonal (the prior covariance)
% C contains the covariance matrix with noise variance at
% the diagonal (the posterior covariance)
example_x = [-1 -1 ; 0 0 ; 1 1];
[K, C] = gp_trcov(gp, example_x)

% What has happend this far is the following
% - we created structures 'gpcf' and 'lik', which describe
%   the properties of the covariance function and Gaussian likelihood (see
%   gpcf_sexp and lik_gaussian for more details)
% - we created structures that describe the prior of the length-scale
%   and magnitude of the squared exponential covariance function and
%   the prior of the noise variance. These structures were set into
%   'gpcf' and 'lik' (see prior_* for more details)
% - we created a GP structure 'gp', which has among others 'gpcf'
%   and 'lik' structures.  (see gp_set for more details)

% -----------------------------
% --- Conduct the inference ---
%
% We will make the inference first by finding a maximum a posterior
% estimate for the parameters via gradient based optimization.
% After this we will use grid integration and Markov chain Monte
% Carlo sampling to integrate over the parameters.

% --- MAP estimate  ---
disp(' MAP estimate for the parameters')
% Set the options for the optimization
opt=optimset('TolFun',1e-3,'TolX',1e-3);
% Optimize with the scaled conjugate gradient method
gp=gp_optim(gp,x,y,'opt',opt);

% get optimized parameter values for display
[w,s]=gp_pak(gp);
% display exp(w) and labels
disp(s), disp(exp(w))

% For last, make predictions of the underlying function on a dense
% grid and plot it. Below Eft_map is the predictive mean and
% Varf_map the predictive variance.
[xt1,xt2]=meshgrid(-1.8:0.1:1.8,-1.8:0.1:1.8);
xt=[xt1(:) xt2(:)];
[Eft_map, Varft_map] = gp_pred(gp, x, y, xt);

% Plot the prediction and data
figure(1)
clf
mesh(xt1, xt2, reshape(Eft_map,37,37));
hold on
plot3(x(:,1), x(:,2), y, '*')
axis on;
title('The predicted underlying function and the data points (MAP solution)');

% --- Grid integration ---
disp(' Grid integration over the parameters')
% Perform the grid integration and make predictions
% We can also get predictions for marginals of f, which are not
% Gaussian due to integration over the hyperparameters
[gp_array, P_TH, th, Eft_ia, Varft_ia, fx_ia, x_ia] = ...
    gp_ia(gp, x, y, xt, 'int_method', 'grid');

% Plot the predictions for two input locations
figure(2)
clf
subplot(2,1,1)
plot(x_ia(100,:), fx_ia(100,:))
title('p(f|D) at input location (-1.6, 0.7)');
subplot(2,1,2)
plot(x_ia(400,:), fx_ia(400,:))
title('p(f|D) at input location (-0.8, 1.1)');

% --- MCMC ---
disp(' MCMC integration over the parameters')
[gp_rec,g,opt] = gp_mc(gp, x, y, 'nsamples', 220,'display',20);

% After sampling we delete the burn-in and thin the sample chain
gp_rec = thin(gp_rec, 21, 2);

% Make the predictions
[Eft_mc, Varft_mc] = gp_pred(gp_rec, x, y, xt);

figure(1)
clf
subplot(1,2,1)
mesh(xt1, xt2, reshape(Eft_map,37,37));
hold on
plot3(x(:,1), x(:,2), y, '*')
axis on;
title(['The predicted underlying function ';
       'and the data points (MAP solution)']);
subplot(1,2,2)
mesh(xt1, xt2, reshape(Eft_mc,37,37));
hold on
plot3(x(:,1), x(:,2), y, '*')
axis on;
title(['The predicted underlying function  ';
       'and the data points (MCMC solution)']);
set(gcf,'pos',[93 511 1098 420])

% We can compare the posterior samples of the parameters to the MAP
% estimate that we got from optimization
figure(3)
clf
subplot(1,2,1)
plot(gp_rec.cf{1}.lengthScale)
title('The sample chain of length-scales')
subplot(1,2,2)
plot(gp_rec.cf{1}.magnSigma2)
title('The sample chain of magnitude')
set(gcf,'pos',[93 511 1098 420])

figure(4)
clf
subplot(1,4,1)
hist(gp_rec.cf{1}.lengthScale(:,1))
hold on
plot(gp.cf{1}.lengthScale(1), 0, 'rx', 'MarkerSize', 11, 'LineWidth', 2)
title('Length-scale 1')
subplot(1,4,2)
hist(gp_rec.cf{1}.lengthScale(:,2))
hold on
plot(gp.cf{1}.lengthScale(2), 0, 'rx', 'MarkerSize', 11, 'LineWidth', 2)
title('Length-scale 2')
subplot(1,4,3)
hist(gp_rec.cf{1}.magnSigma2)
hold on
plot(gp.cf{1}.magnSigma2, 0, 'rx', 'MarkerSize', 11, 'LineWidth', 2)
title('magnitude')
subplot(1,4,4)
hist(gp_rec.lik.sigma2)
hold on
plot(gp.lik.sigma2, 0, 'rx', 'MarkerSize', 11, 'LineWidth', 2)
title('Noise variance')
legend('MCMC samples', 'MAP estimate')
set(gcf,'pos',[93 511 1098 420])

% Sample from two posterior marginals and plot them alongside
% with the MAP and grid integration results
% gpmc_preds returns the predictive mean of the latent function
% with every sampled parameter value.
[Eft_mcs, Varft_mcs] = gpmc_preds(gp_rec, x, y, xt);
sf = normrnd(Eft_mcs(100,:), sqrt(Varft_mcs(100,:)));
sf2 = normrnd(Eft_mcs(400,:), sqrt(Varft_mcs(400,:)));

figure(2)
subplot(1,2,1)
[N,X] = hist(sf);
hist(sf)
hold on
plot(x_ia(100,:), max(N)/max(fx_ia(100,:))*fx_ia(100,:), 'k')
ff = norm_pdf(x_ia(100,:)', Eft_map(100), sqrt(Varft_map(100)));
plot(x_ia(100,:), max(N)/max(ff)*ff, 'r', 'lineWidth', 2)
set(gca, 'Ytick', [])
title('p(f|D) at input location (-1.6, 0.7)');
xlim([0 1])

subplot(1,2,2)
[N,X] = hist(sf2);
hist(sf2)
hold on
plot(x_ia(400,:), max(N)/max(fx_ia(400,:))*fx_ia(400,:), 'k')
ff = norm_pdf(x_ia(400,:)', Eft_map(400), sqrt(Varft_map(400)));
plot(x_ia(400,:), max(N)/max(ff)*ff, 'r', 'lineWidth', 2)
set(gca, 'Ytick', [])
title('p(f|D) at input location (-0.8, 1.1)');
xlim([-1.2 -0.6])

disp('Done')

% ========================
% Print figures for manual
% ========================
% $$$ sf = normrnd(Eft_mc(100,:), sqrt(Varft_mc(100,:)));
% $$$ sf2 = normrnd(Eft_mc(400,:), sqrt(Varft_mc(400,:)));
% $$$
% $$$ figure
% $$$ subplot(1,2,1)
% $$$ [N,X] = hist(sf);
% $$$ hist(sf)
% $$$ h = findobj(gca,'Type','patch');
% $$$ set(h,'FaceColor','w','EdgeColor','k')
% $$$ hold on
% $$$ plot(x_ia(100,:), max(N)/max(fx_ia(100,:))*fx_ia(100,:), 'k')
% $$$ ff = norm_pdf(x_ia(100,:)', Eft_map(100), sqrt(Varft_map(100)));
% $$$ plot(x_ia(100,:), max(N)/max(ff)*ff, 'k', 'lineWidth', 2)
% $$$ set(gca, 'Ytick', [])
% $$$ xlim([0 1])
% $$$ ylim([0 110])
% $$$
% $$$ subplot(1,2,2)
% $$$ [N,X] = hist(sf2);
% $$$ hist(sf2)
% $$$ h = findobj(gca,'Type','patch');
% $$$ set(h,'FaceColor','w','EdgeColor','k')
% $$$ hold on
% $$$ plot(x_ia(400,:), max(N)/max(fx_ia(400,:))*fx_ia(400,:), 'k')
% $$$ ff = norm_pdf(x_ia(400,:)', Eft_map(400), sqrt(Varft_map(400)));
% $$$ plot(x_ia(400,:), max(N)/max(ff)*ff, 'k', 'lineWidth', 2)
% $$$ set(gca, 'Ytick', [])
% $$$ xlim([-1.2 -0.5])
% $$$
% $$$
% $$$ set(gcf,'units','centimeters');
% $$$ set(gcf,'pos',[15 14 7 5])
% $$$ set(gcf,'paperunits',get(gcf,'units'))
% $$$ set(gcf,'paperpos',get(gcf,'pos'))
% $$$ print -depsc2 /proj/bayes/jpvanhat/software/doc/GPstuffDoc/pics/demo_regression1_fig3.eps
% $$$
% $$$
% $$$ figure(4)
% $$$ clf, subplot(1,4,1)
% $$$ hist(gp_rec.cf{1}.lengthScale(:,1))
% $$$ h = findobj(gca,'Type','patch');
% $$$ set(h,'FaceColor','w','EdgeColor','k')
% $$$ hold on
% $$$ plot(gp.cf{1}.lengthScale(1), 0, 'kx', 'MarkerSize', 11, 'LineWidth', 2)
% $$$ xlabel('Length-s 1')
% $$$ xlim([0.3 1.6])
% $$$
% $$$ subplot(1,4,2)
% $$$ hist(gp_rec.cf{1}.lengthScale(:,2))
% $$$ h = findobj(gca,'Type','patch');
% $$$ set(h,'FaceColor','w','EdgeColor','k')
% $$$ hold on
% $$$ plot(gp.cf{1}.lengthScale(2), 0, 'kx', 'MarkerSize', 11, 'LineWidth', 2)
% $$$ xlabel('Length-s 2')
% $$$ xlim([0.4 1.4])
% $$$
% $$$ subplot(1,4,3)
% $$$ hist(gp_rec.cf{1}.magnSigma2)
% $$$ h = findobj(gca,'Type','patch');
% $$$ set(h,'FaceColor','w','EdgeColor','k')
% $$$ hold on
% $$$ plot(gp.cf{1}.magnSigma2, 0, 'kx', 'MarkerSize', 11, 'LineWidth', 2)
% $$$ xlabel('magnitude')
% $$$ xlim([0.5 6])
% $$$
% $$$ subplot(1,4,4)
% $$$ hist(gp_rec.lik.sigma2)
% $$$ h = findobj(gca,'Type','patch');
% $$$ set(h,'FaceColor','w','EdgeColor','k')
% $$$ hold on
% $$$ plot(gp.lik.sigma2, 0, 'kx', 'MarkerSize', 11, 'LineWidth', 2)
% $$$ xlabel('Noise variance')
% $$$ xlim([0.03 0.06])
% $$$ set(gca, 'Xtick', [0.03 0.06])
% $$$
% $$$ set(gcf,'units','centimeters');
% $$$ set(gcf,'pos',[15 14 11 5])
% $$$ set(gcf,'paperunits',get(gcf,'units'))
% $$$ set(gcf,'paperpos',get(gcf,'pos'))
% $$$
% $$$ print -depsc2 /proj/bayes/jpvanhat/software/doc/GPstuffDoc/pics/demo_regression1_fig2.eps


function value = r8poly_value ( n, a, x )

%*****************************************************************************80
%
%% R8POLY_VALUE evaluates an R8 polynomial.
%
%  Discussion:
%
%    For sanity's sake, the value of N indicates the NUMBER of
%    coefficients, or more precisely, the ORDER of the polynomial,
%    rather than the DEGREE of the polynomial.  The two quantities
%    differ by 1, but cause a great deal of confusion.
%
%    Given N and A, the form of the polynomial is:
%
%      p(x) = a(1) + a(2) * x + ... + a(n-1) * x^(n-2) + a(n) * x^(n-1)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 April 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the polynomial.
%
%    Input, real A(1:N), the coefficients of the polynomial.
%    A(1) is the constant term.
%
%    Input, real X, the point at which the polynomial is to be evaluated.
%
%    Output, real VALUE, the value of the polynomial at X.
%
  value = 0.0;
  for i = n : -1 : 1
    value = value * x + a(i);
  end

  return
end


% Isoparametric Formulation Implementation
% clear memory
clear all
close all
clc
% E: modulus of elasticity
% A: area of cross section
% L: length of bar
E=2e5; A=12.5e-4; L=1.5;
s_exact=@(x)1/2*(80000*x)*x/(A);
hold on;
ezplot(s_exact,[0 1.5])
title('Exact solution v.s. FEM','interpreter','latex');
xlabel('x','interpreter','latex');
ylabel('Axial stress, Pa','interpreter','latex');

for i=1:4   %For NEL = 1, 2, 4, 8
NNOD=3;
fprintf( '\nNumber of elements:%d\n\n',2^(i-1) );
% numberElements: number of elements
numberElements=2^(i-1);
% numberNodes: number of nodes
numberNodes=2*numberElements+1;
% generation of coordinates and connectivities

nodeCoordinates=linspace(0,L,numberNodes);
%Generate element length vector
for i=1:numberElements
    Le(i)=L/numberElements;
    elementNodes(i,:)=[(i-1)*2+1 (i-1)*2+2 (i-1)*2+3];
end
% for structure:
   % displacements: displacement vector
   % force : force vector
   % stiffness: stiffness matrix
force=zeros(numberNodes,1);
stiffness=zeros(numberNodes,numberNodes);
% computation of the system stiffness matrix and force vector
for e=1:numberElements;
  % elementDof: element degrees of freedom (Dof)
  elementDof=elementNodes(e,:) ;
  detJacobian=Le(e)/2;
  invJacobian=1/detJacobian;
  ngp = 3;
  [w,xi]=gauss1d(ngp);
  xc=0.5*(nodeCoordinates(elementDof(1))+nodeCoordinates(elementDof(3)));
  for ip=1:ngp;
      [shape,naturalDerivatives]=shapeFunctionL3(xi(ip));
      B=naturalDerivatives*invJacobian;
      stiffness(elementDof,elementDof)=...
      stiffness(elementDof,elementDof)+ B'*B*w(ip)*detJacobian*E*A;
      force(elementDof)=force(elementDof)+...
          -80000*(xc+detJacobian*xi(ip))*shape'*detJacobian*w(ip);
  end
end

% boundary conditions and solution
% prescribed dofs
prescribedDof=[numberNodes];
% solution
GDof=numberNodes;
displacements=solution(GDof,prescribedDof,stiffness,force);
% output displacements/reactions
outputDisplacementsReactionsPretty(displacements,stiffness, ...
    numberNodes,prescribedDof,force)
fprintf('Axial stress\n')
fprintf('element\t\taxial stress\n')
%Stress and strain recovery
ngp = 2;

elementNodeCoor=zeros(numberElements*NNOD,1);
elementNodeStr=zeros(numberElements*NNOD,1);
for e=1:numberElements;
  % elementDof: element degrees of freedom (Dof)
  elementDof=elementNodes(e,:);
  detJacobian=Le(e)/2;
  invJacobian=1/detJacobian;
  xi=[-1 0 1];
  for ip=1:NNOD;
      [shape,naturalDerivatives]=shapeFunctionL3(xi(ip));
      B=naturalDerivatives*invJacobian;
      elementNodeCoor(ip+(e-1)*NNOD,1)=nodeCoordinates(elementDof(ip));
      elementNodeStr(ip+(e-1)*NNOD,1)=E*B*displacements(elementDof,1);
      fprintf('%2.0f\t%2.0fth node\t%10.4e\n', e, ip,elementNodeStr(ip+(e-1)*NNOD,1))
  end
end

%post process
switch numberElements
    case 1
        str='rs--';
    case 2
        str='gs--';
    case 4
        str='ks--';
    case 8
        str='ms--';
end
switch numberElements
    case 1
        strg='r*';
    case 2
        strg='g*';
    case 4
        strg='k*';
    case 8
        strg='m*';
end
plot(elementNodeCoor,elementNodeStr,str)
hold on;
%plot(elementIpCoor,elementAxiStr,strg)
end
legend('Exact solution','NEL=1','NEL=2','NEL=4','NEL=8');
title('Stress','interpreter','latex','FontSize',18);
xlabel('$\it{x}$','interpreter','latex','FontSize',14);
ylabel({'Stress, $\it{\sigma}$'},'interpreter','latex','FontSize',14);
box on;


function b = r83_vxm ( n, a, x )

%*****************************************************************************80
%
%% R83_VXM multiplies a vector by an R83 matrix.
%
%  Discussion:
%
%    The r83 storage format is used for a tridiagonal matrix.
%    The superdiagonal is stored in entries (1,2:N), the diagonal in
%    entries (2,1:N), and the subdiagonal in (3,1:N-1).  Thus, the
%    original matrix is "collapsed" vertically into the array.
%
%  Example:
%
%    Here is how a R83 matrix of order 5 would be stored:
%
%       *  A12 A23 A34 A45
%      A11 A22 A33 A44 A55
%      A21 A32 A43 A54  *
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 January 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the linear system.
%
%    Input, real A(3,N), the R83 matrix.
%
%    Input, real X(N), the vector to be multiplied by A'.
%
%    Output, real B(N), the product A' * x.
%
  b(1:n)   =            a(2,1:n)   .* x(1:n);
  b(1:n-1) = b(1:n-1) + a(3,1:n-1) .* x(2:n);
  b(2:n)   = b(2:n)   + a(1,2:n)   .* x(1:n-1);

  return
end


function ydot = lorenzeq(t,y)
%LORENZEQ Equation of the Lorenz chaotic attractor.
%   ydot = lorenzeq(t,y).
%   The differential equation is written in almost linear form.

SIGMA = 10.;
RHO = 28.;
BETA = 8./3.;

A = [ -BETA    0     y(2)
    0  -SIGMA   SIGMA
    -y(2)   RHO    -1  ];

ydot = A*y;


function [Series, Sigmas] = garchsim(parameters, model, distr, p, q, H, y, NumSamples, NumPaths)
%{
-----------------------------------------------------------------------
 PURPOSE:
 Simulation of GARCH models
-----------------------------------------------------------------------
  USAGE:
  [Series, Sigmas] = garchsim(parameters, model, distr, p, q, H, NumSamples, NumPaths)

 INPUTS:
 parameters:    a vector of parameters (i.e. constant, ARCH, GARCH, Gamma, Delta, DoF)
 model:         'GARCH', 'GJR', 'EGARCH', 'NARCH', 'NGARCH, 'AGARCH', 'APGARCH',
                'NAGARCH'
 distr:         'GAUSSIAN', 'T', 'GED', 'CAUCHY', 'HANSEN' and 'GC'
 p:             positive scalar integer representing the order of ARCH
 q:             positive scalar integer representing the order of GARCH
 H:             a vector of time series of positive pre-sample
                conditional standard deviations.
 y:             a vector of factors for the volatility process, must be positive!
 NumSamples:    The number of samples. If is left empty 100 samples will be generated.
 NumPaths:      The number of paths. If it is left empty 1 path will be estimated.

 OUTPUTS:
 Series:       (NumSamples x NumPaths) simulated series with GARCH variances
 Sigmas:       (NumSamples x NumbPaths) vector of conditional standard deviations.

-----------------------------------------------------------------------
 Author:
 Alexandros Gabrielsen, a.gabrielsen@city.ac.uk
 Date:     08/2011
-----------------------------------------------------------------------
%}

if nargin == 0
    error('Parameters, GARCH Model, Distribution, ARCH, GARCH, Sigmas, Number of Samples,  Number of Paths')
end

if nargin < 8 | isempty(NumPaths)
    NumPaths=1;
end

% Verifying that the vector of parameters is a column vector
[r,c]=size(parameters);
if r<c
    parameters=parameters';
end

m  =  max([p,q]);

if strcmp(distr,'GAUSSIAN')
    RandomNumbers = randn(NumSamples+m, NumPaths);
elseif strcmp(distr,'T')
    RandomNumbers = trnd(parameters(end), NumSamples+m, NumPaths);
elseif strcmp(distr,'GED')
     error('Sorry: this distribution is not yet supported, it will be shortly!')
elseif strcmp(distr,'CAUCHY')
      error('Sorry: this distribution is not yet supported, it will be shortly!')
elseif strcmp(distr,'HANSEN')
      error('Sorry: this distribution is not yet supported, it will be shortly!')
elseif strcmp(distr,'GC')
      error('Sorry: this distribution is not yet supported, it will be shortly!')
elseif strcmp(distr,'Logistic')
      error('Sorry: this distribution is not yet supported, it will be shortly!')
elseif strcmp(distr,'Laplace')
      error('Sorry: this distribution is not yet supported, it will be shortly!')
elseif strcmp(distr,'Rayleigh')
      error('Sorry: this distribution is not yet supported, it will be shortly!')
end

resids=[];
h=[];
series=[];

if isscalar(y)
     factors = 0;
   else
     factors = sum(parameters(2+p+q:1+p+q+size(y,2)).*y(end,:));
end

% Set initial value to the unconditional
if isequal(strcat(model), 'GARCH')
   h(1:m,1:NumPaths) = (parameters(1)+factors)/(1-sum(parameters(2:1+p)) - sum(parameters(3:1+p+q)));
elseif  isequal(strcat(model), 'GJR')
   h(1:m,1:NumPaths) = (parameters(1)+factors)/(1-sum(parameters(2:1+p)) - sum(parameters(3:1+p+q)) - sum(parameters(4:1+p+q)));
elseif isequal(strcat(model), 'EGARCH')
   h(1:m,1:NumPaths) = exp((parameters(1)+factors)/(1-sum(parameters(2:1+p)) - sum(parameters(3:1+p+q)) - sum(parameters(4:1+p+q))));
elseif isequal(strcat(model), 'NARCH')
   h(1:m,1:NumPaths) = (parameters(1)+factors)/(1-sum(parameters(2:1+p)) - sum(parameters(3:1+p+q))); % This is only an approximation
elseif isequal(strcat(model), 'NGARCH')
   h(1:m,1:NumPaths) = nthroot((parameters(1)+factors)/(1-sum(parameters(2:1+p)) - sum(parameters(3:1+p+q))),parameters(end));
elseif isequal(strcat(model), 'AGARCH')
   h(1:m,1:NumPaths) = (parameters(1)-sum(parameters(2:1+p).*parameters(4:1+p+q))+factors)./(1-sum(parameters(2:1+p)) - sum(parameters(3:1+p+q)));
elseif isequal(strcat(model), 'APGARCH')
   h(1:m,1:NumPaths) = (parameters(1)+factors)/(1-sum(parameters(2:1+p)) - sum(parameters(3:1+p+q))); % This is only an approximation
elseif isequal(strcat(model), 'NAGARCH')
    h(1:m,1:NumPaths) = (parameters(1)-sum(parameters(2:1+p).*parameters(4:1+p+q)) + factors)./(1-sum(parameters(2:1+p)) - sum(parameters(3:1+p+q)));
else error('Invalid GARCH Model');
end

if isempty(H)
    series=randn(1:m,NumPaths).*sqrt(h);
else
    series=sqrt(H(end-m+1:end))*ones(1,NumPaths).*RandomNumbers(1:m,:);
end

if strcmp(model,'GARCH')
    for i = 1:NumPaths
        for t = (m+1):(size(RandomNumbers,1)-m)
            h(t,i) = parameters(1:1+p+q)'* [1 ; series(t-(1:p),i).^2;  h(t-(1:q),i) ];
            series(t,i)=RandomNumbers(t,i)*sqrt(h(t,i));
        end
    end
elseif strcmp(model,'GJR')
    for i = 1:NumPaths
        for t = (m+1):(size(RandomNumbers,1)-m)
            h(t,i) =  parameters(1:1+2*p+q)'*[1; series(t-(1:p),i).^2; h(t-(1:q),i); (series(t-(1:p),i)<0).*(series(t-(1:p),i).^2)];
            series(t,i)=RandomNumbers(t,i)*sqrt(h(t,i));
        end
    end
elseif strcmp(model,'EGARCH')
    for i = 1:NumPaths
        for t = (m+1):(size(RandomNumbers,1)-m)
            h(t,i) = exp(parameters(1:1+2*p+q)'*[1 ; (abs(series(t-(1:p),i))./sqrt(h(t-(1:p),i))-sqrt(2/pi)); log(h(t-(1:q),i)); series(t-(1:p),i)./sqrt(h(t-(1:p),i))]);
            series(t,i)=RandomNumbers(t,i)*sqrt(h(t,i));
        end
    end
elseif strcmp(model,'NARCH')
    for i = 1:NumPaths
        for t = (m+1):(size(RandomNumbers,1)-m)
            h(t,i) = parameters(1:1+p+q)'*[1; (abs(series(t-(1:p),i))).^parameters(2+p+q); h(t-(1:q),i)];
            series(t,i)=RandomNumbers(t,i)*sqrt(h(t,i));
        end
    end
elseif strcmp(model,'NGARCH')
    for i = 1:NumPaths
        for t = (m+1):(size(RandomNumbers,1)-m)
            h(t,i) = (parameters(1:1+p+q)'*[1; (series(t-(1:p),i)).^parameters(2+p+q); h(t-(1:q),i).^parameters(2+p+q)]).^(1/parameters(2+p+q));
            series(t,i)=RandomNumbers(t,i)*sqrt(h(t,i));
        end
    end
elseif strcmp(model,'AGARCH')
    for i = 1:NumPaths
        for t = (m+1):(size(RandomNumbers,1)-m)
            h(t,i) = parameters(1:1+p+q)'*[1; (series(t-(1:p),i) - parameters(2+p+q:1+2*p+q)).^2; h(t-(1:q),i)];
            series(t,i)=RandomNumbers(t,i)*sqrt(h(t,i));
        end
    end
elseif strcmp(model,'APGARCH')
    for i = 1:NumPaths
        for t = (m+1):(size(RandomNumbers,1)-m)
            h(t,i) = (parameters(1:1+p+q)'*[1; (abs(series(t-(1:p),i)) - parameters(2+p+q:1+2*p+q).*series(t-(1:p),i)).^parameters(2+2*p+q); h(t-(1:q),i).^parameters(2+2*p+q)]).^(1/parameters(2+2*p+q));
            series(t,i)=RandomNumbers(t,i)*sqrt(h(t,i));
        end
    end
elseif strcmp(model,'NAGARCH')
    for i = 1:NumPaths
        for t = (m+1):(size(RandomNumbers,1)-m)
            h(t,i) = parameters(1:1+p+q)'*[1; (series(t-(1:p),i)./sqrt(h(t-(1:p),i)) + parameters(2+p+q:1+2*p+q)).^2; h(t-(1:q),i)];
            series(t,i)=RandomNumbers(t,i)*sqrt(h(t,i));
        end
    end
else error('Invalid GARCH Model');
    end

Series=series;
Sigmas=h;

end


function determ = leslie_determinant ( b, di, da )

%*****************************************************************************80
%
%% LESLIE_DETERMINANT returns the determinant of the LESLIE matrix.
%
%  Discussion:
%
%    DETERM = a(4,4) * (
%        a(1,1) * a(2,2) * a(3,3)
%      + a(1,3) * a(2,1) * a(3,2) )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real B, DI, DA, the birth rate, infant mortality rate,
%    and aged mortality rate.  These should be positive values.
%    The mortality rates must be between 0.0 and 1.0.  Reasonable
%    values might be B = 0.025, DI = 0.010, and DA = 0.100
%
%    Output, real DETERM, the determinant.
%
  determ = 9.0 * ( 1.0 - da ) / 10.0 * ...
  ( ...
      5.0 * ( 1.0 - di ) / 6.0 ...
    * 13.0 / 14.0 ...
    * 39.0 / 40.0 ...
  +   b ...
    * ( 1.0 - di ) / 6.0 ...
    * 1.0 / 14.0 ...
   );

  return
end


function V = mttkrp(X,U,n)
%MTTKRP Matricized tensor times Khatri-Rao product for ttensor.
%
%   V = MTTKRP(X,U,n) efficiently calculates the matrix product of the
%   n-mode matricization of X with the Khatri-Rao product of all
%   entries in U, a cell array of matrices, except the nth.  How to
%   most efficiently do this computation depends on the type of tensor
%   involved.
%
%   See also TTENSOR, TTENSOR/TTV
%
%MATLAB Tensor Toolbox.
%Copyright 2015, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2015) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

N = ndims(X);

if (n==1)
    R = size(U{2},2);
else
    R = size(U{1},2);
end

% Compute cell array of weights to multiply into core
W = cell(N,1);
for i = [1:n-1,n+1:N]
  W{i} = (X.u{i}' * U{i});
end
Y = mttkrp(X.core,W,n);

% Find each column of answer by multiplying columns of X.u{n} with weights
V = X.u{n} * Y;


function b = r8sd_mxv ( n, ndiag, offset, a, x )

%*****************************************************************************80
%
%% R8SD_MXV multiplies a R8SD matrix times a vector.
%
%  Discussion:
%
%    The R8SD storage format is for symmetric matrices whose only nonzero entries
%    occur along a few diagonals, but for which these diagonals are not all
%    close enough to the main diagonal for band storage to be efficient.
%
%    In that case, we assign the main diagonal the offset value 0, and
%    each successive superdiagonal gets an offset value 1 higher, until
%    the highest superdiagonal (the A(1,N) entry) is assigned the offset N-1.
%
%    Assuming there are NDIAG nonzero diagonals (ignoring subdiagonals!),
%    we then create an array B that has N rows and NDIAG columns, and simply
%    "collapse" the matrix A to the left:
%
%  Example:
%
%    The "offset" value is printed above each column.
%
%    Original matrix               New Matrix
%
%       0   1   2   3   4   5       0   1   3   5
%
%      11  12   0  14   0  16      11  12  14  16
%      21  22  23   0  25   0      22  23  25  --
%       0  32  33  34   0  36      33  34  36  --
%      41   0  43  44  45   0      44  45  --  --
%       0  52   0  54  55  56      55  56  --  --
%      61   0  63   0  65  66      66  --  --  --
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 February 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    N must be positive.
%
%    Input, integer NDIAG, the number of diagonals that are stored.
%    NDIAG must be at least 1 and no more than N.
%
%    Input, integer OFFSET(NDIAG), the offsets for the diagonal storage.
%
%    Input, real A(N,NDIAG), the R8SD matrix.
%
%    Input, real X(N), the vector to be multiplied by A.
%
%    Output, real B(N), the product A * x.
%
  b(1:n) = 0.0E+00;

  for i = 1 : n
    for jdiag = 1 : ndiag
      j = i + offset(jdiag);
      if ( 1 <= j & j <= n )
        b(i) = b(i) + a(i,jdiag) * x(j);
        if ( offset(jdiag) ~= 0 )
          b(j) = b(j) + a(i,jdiag) * x(i);
        end
      end
    end
  end

  return
end


function wtime_test05 ( )

%*****************************************************************************80
%
%% TEST05 times the matrix multiplication problem problem.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 August 2010
%
%  Author:
%
%    John Burkardt
%
  l_log_min = 1;
  l_log_max = 5;
  l_min = 4^l_log_min;
  l_max = 4^l_log_max;
  rep_num = 5;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST05\n' );
  fprintf ( 1, '  Time the matrix multiplication problem.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Compute C = A * B\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  where\n' );
  fprintf ( 1, '    A is an L by M matrix,\n' );
  fprintf ( 1, '    B is an M by N matrix,\n' );
  fprintf ( 1, '  and so\n' );
  fprintf ( 1, '    C is an L by N matrix.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Minimum value of L = M = N = %d\n', l_min );
  fprintf ( 1, '  Maximum value of L = M = N = %d\n', l_max );
  fprintf ( 1, '  Number of repetitions of the operation: %d\n', rep_num );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Use triple loop for matrix multiplication.\n' );

  for rep = 1 : rep_num

    for l_log = l_log_min : l_log_max - 1

      l = 4^( l_log );
      m = l;
      n = l;

      a = rand ( l, l );
      b = rand ( l, l );

      seconds = wtime ( );

      for i = 1 : l
        for j = 1 : l
          c(i,j) = 0.0;
          for k = 1 : l
            c(i,j) = c(i,j) + a(i,k) * b(k,j);
          end
        end
      end

      delta(l_log,rep) = wtime ( ) - seconds;

    end

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Timing results using nested loops:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    Vector Size  Rep #1        Rep #2        Rep #3        ' );
  fprintf ( 1, 'Rep #4        Rep #5\n' );
  fprintf ( 1, '\n' );
  for l_log = l_log_min : l_log_max - 1
    l = 4^( l_log );
    fprintf ( 1, '  %8d  %12f  %12f  %12f  %12f  %12f\n', ...
      l, delta(l_log,1:rep_num) );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Use C = A * B matrix multiplication.\n' );

  for rep = 1 : rep_num

    for l_log = l_log_min : l_log_max - 1

      l = 4^( l_log );
      m = l;
      n = l;

      a = rand ( l, l );
      b = rand ( l, l );

      seconds = wtime ( );

      c = a * b;

      delta(l_log,rep) = wtime ( ) - seconds;

    end

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Timing results using C = A * B:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    Vector Size  Rep #1        Rep #2        Rep #3        ' );
  fprintf ( 1, 'Rep #4        Rep #5\n' );
  fprintf ( 1, '\n' );
  for l_log = l_log_min : l_log_max - 1
    l = 4^( l_log );
    fprintf ( 1, '  %8d  %12f  %12f  %12f  %12f  %12f\n', ...
      l, delta(l_log,1:rep_num) );
  end

  return
end


function normA = norm(A, n)
%NORM   Norm of a CHEBMATRIX object.
%   NORM(A) computes the Frobenius norm of the CHEBMATRIX object A, defined as
%   the sum of the squares of the 2-norms of each of the blocks.
%
%   NORM(A, 2) or NORM(A, 'fro') is the same as above.
%
%   NORM(A, INF) return the infinity norm of the CHEBMATRIX A, defined as the
%   maximum infinity norm of each of the blocks.
%
%   NORM(A, -INF) returns the negative infinity norm of the CHEBMATRIX A,
%   defined as the minimum of the negative infinity norm of each of the blocks.
%
% See also CHEBMATRIX, CHEBFUN/NORM.

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

% [TODO]: Add support for norms of operators.

% Empty CHEBMATRIX has norm 0.
if ( isempty(A) )
    normA = 0;
    return
end

if ( nargin == 1 )
    n = 'fro'; 	% Frobenius norm is the default.
end

% The norm of a CHEBMATRIX with inf x inf block(s) that are neither
% CHEBFUN2 nor CHEBFUN3 is not supported:
s = cellfun(@(b) min(size(b)), A.blocks);
t = cellfun(@(b) isa(b, 'chebfun2') || isa(b, 'chebfun3'), A.blocks);
if ( ~all(isfinite(s(:))) )
    if ( ~any(t) )
        error('CHEBFUN:CHEBMATRIX:norm:notSupported', ...
            'Norm of a chebmatrix with operator block(s) is not supported.')
    end
end

% Deal with different cases.
switch n

    case {'fro', 2}
        normA = cellfun(@(v) norm(v, 2), A.blocks);
        normA = sqrt(sum(normA(:).^2));

    case {inf, 'inf'}
        normA = cellfun(@(v) norm(v, inf), A.blocks);
        normA = max(normA(:));

    case {-inf, '-inf'}
        normA = cellfun(@(v) norm(v, -inf), A.blocks);
        normA = min(normA(:));

    otherwise
        if ( ~ischar(n) )
            n = num2str(n);
        end
        error('CHEBFUN:CHEBMATRIX:norm:unknown', ...
            'unsupported norm type ''%s''', n)

end

end


function [x, E, s, lambdamax] = tvdip(y, lambda, display, stoptol, maxiter)
% Performs discrete total variation denoising (TVD) using a primal-dual
% interior-point solver. It minimizes the following discrete functional:
%
%  E=(1/2)||y-x||_2^2+lambda*||Dx||_1,
%
% over the variable x, given the input signal y, according to each
% value of the regularization parameter lambda > 0. D is the first
% difference matrix. Uses hot-restarts from each value of lambda to speed
% up convergence for subsequent values: best use of this feature is made by
% ensuring that the chosen lambda values are close to each other.
%
% Usage:
% [x, E, s] = tvdip(y, lambda, display, stoptol, maxiter)
%
% Input arguments:
% - y          Original signal to denoise, size N x 1.
% - lambda     A vector of positive regularization parameters, size L x 1.
%              TVD will be applied to each value in the vector.
% - display    (Optional) Set to 0 to turn off progress display, 1 to turn
%              on. If not specifed, defaults to progress display on.
% - stoptol    (Optional) Precision as determined by duality gap tolerance,
%              if not specified, defaults to 1e-3.
% - maxiter    (Optional) Maximum interior-point iterations, if not
%              specified defaults to 60.
%
% Output arguments:
% - x          Denoised output signal for each value of lambda, size N x L.
% - E          Objective functional at minimum for each lambda, size L x 1.
% - s          Optimization result, 1 = solved, 0 = maximum iterations
%              exceeded before reaching duality gap tolerance, size L x 1.
% - lambdamax  Maximum value of lambda for the given y. If
%              lambda >= lambdamax, the output is the trivial constant
%              solution x = mean(y).
%
% (c) Max Little, 2010. Based around code originally written by
% S.J. Kim, K. Koh, S. Boyd and D. Gorinevsky. If you use this code for
% your research, please cite:
% M.A. Little, Nick S. Jones (2010)
% "Sparse Bayesian Step-Filtering for High-Throughput Analysis of Molecular
% Machine Dynamics", in 2010 IEEE International Conference on Acoustics,
% Speech and Signal Processing, 2010, ICASSP 2010 Proceedings.
%
% This code is released under the terms of GNU General Public License as
% published by the Free Software Foundation; version 2 or later.

error(nargchk(2,5,nargin));
if (nargin < 3)
    display = 1;
end
if (nargin < 4)
    stoptol = 1e-3;
end
if (nargin < 5)
    maxiter = 60;
end

y = y(:);

% Search tuning parameters
ALPHA     = 0.01;   % Backtracking linesearch parameter (0,0.5]
BETA      = 0.5;    % Backtracking linesearch parameter (0,1)
MAXLSITER = 20;     % Max iterations of backtracking linesearch
MU        = 2;      % t update

N = length(y);    % Length of input signal y
M = N-1;          % Size of Dx

% Construct sparse operator matrices
I1 = speye(M,M);
O1 = spalloc(M,1,M);
D = [I1 O1]-[O1 I1];

DDT = D*D';
Dy  = D*y;

% Find max value of lambda
lambdamax = max(abs(DDT\Dy));

if (display)
    fprintf('lambda_max=%5.2e\n', lambdamax);
end

L = length(lambda);
x = zeros(N, L);
s = zeros(L, 1);
E = zeros(L, 1);

% Optimization variables set up once at the start
z    = zeros(M,1);   % Dual variable
mu1  = ones(M,1);    % Dual of dual variable
mu2  = ones(M,1);    % Dual of dual variable

% Work through each value of lambda, with hot-restart on optimization
% variables
for l = 1:L

    t    =  1e-10;
    step =  Inf;
    f1   =  z-lambda(l);
    f2   = -z-lambda(l);

    % Main optimization loop
    s(l) = 1;

    if (display)
        fprintf('Solving for lambda=%5.2e, lambda/lambda_max=%5.2e\nIter# Primal    Dual      Gap\n', ...
            lambda(l), lambda(l)/lambdamax);
    end
    for iters = 0:maxiter

        DTz  = (z'*D)';
        DDTz = D*DTz;
        w    = Dy-(mu1-mu2);

        % Calculate objectives and primal-dual gap
        pobj1 = 0.5*w'*(DDT\w)+lambda(l)*sum(mu1+mu2);
        pobj2 = 0.5*DTz'*DTz+lambda(l)*sum(abs(Dy-DDTz));
        pobj = min(pobj1,pobj2);
        dobj = -0.5*DTz'*DTz+Dy'*z;
        gap  = pobj - dobj;

        if (display)
            fprintf('%5d %7.2e %7.2e %7.2e\n', iters, pobj, dobj, gap);
        end

        % Test duality gap stopping criterion
        if (gap <= stoptol)
            s(l) = 1;
            break;
        end

        if (step >= 0.2)
            t = max(2*M*MU/gap, 1.2*t);
        end

        % Do Newton step
        rz      =  DDTz - w;
        S       =  DDT-sparse(1:M,1:M,mu1./f1+mu2./f2);
        r       = -DDTz + Dy + (1/t)./f1 - (1/t)./f2;
        dz      =  S\r;
        dmu1    = -(mu1+((1/t)+dz.*mu1)./f1);
        dmu2    = -(mu2+((1/t)-dz.*mu2)./f2);

        resDual = rz;
        resCent = [-mu1.*f1-1/t; -mu2.*f2-1/t];
        residual= [resDual; resCent];

        % Perform backtracking linesearch
        negIdx1 = (dmu1 < 0);
        negIdx2 = (dmu2 < 0);
        step = 1;
        if (any(negIdx1))
            step = min( step, 0.99*min(-mu1(negIdx1)./dmu1(negIdx1)) );
        end
        if (any(negIdx2))
            step = min( step, 0.99*min(-mu2(negIdx2)./dmu2(negIdx2)) );
        end

        for liter = 1:MAXLSITER
            newz    =  z  + step*dz;
            newmu1  =  mu1 + step*dmu1;
            newmu2  =  mu2 + step*dmu2;
            newf1   =  newz - lambda(l);
            newf2   = -newz - lambda(l);

            % Update residuals
            newResDual  = DDT*newz - Dy + newmu1 - newmu2;
            newResCent  = [-newmu1.*newf1-1/t; -newmu2.*newf2-1/t];
            newResidual = [newResDual; newResCent];

            if ((max(max(newf1),max(newf2)) < 0) && ...
                    (norm(newResidual) <= (1-ALPHA*step)*norm(residual)))
                break;
            end
            step = BETA*step;
        end

        % Update primal and dual optimization parameters
        z = newz;
        mu1 = newmu1;
        mu2 = newmu2;
        f1 = newf1;
        f2 = newf2;
    end

    x(:,l) = y-D'*z;
    E(l) = 0.5*sum((y-x(:,l)).^2)+lambda(l)*sum(abs(D*x(:,l)));

    % We may have a close solution that does not satisfy the duality gap
    if (iters >= maxiter)
        s(l) = 0;
    end

    if (display)
        if (s(l))
            fprintf('Solved to precision of duality gap %5.2e\n', gap);
        else
            fprintf('Max iterations exceeded - solution may be inaccurate\n');
        end
    end

end


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni

% Inverse DFT of X(k)=[6, -1-j,0,-1+j], k=0,1,2,3

clear;
Xk=[6, -1-j,0,-1+j];
N=length(Xk);
for n=0:N-1
for k=0:N-1
xn(k+1)=Xk(k+1)*exp(j*2*pi*n*k/N);
end
x(n+1)=sum(xn);
end
x=(1/N)*x


classdef Quad < handle
    %QUAD Summary of this class goes here
    %   Detailed explanation goes here

    properties
    V00,V01,V10,V11;
    end

    methods
        function obj =Quad(V00,V01,V10,V11)
            obj.V00 = V00;
            obj.V01 = V01;
            obj.V10 = V10;
            obj.V11 = V11;
        end
        function a = isPointIn(obj,pt)
           in1 = isPointInTriangular(pt,obj.V00,obj.V01,obj.V11);
           in2 = isPointInTriangular(pt,obj.V00,obj.V10,obj.V11);
           if in1==1 || in2==1
               a = 1;
           else
               a = 0;
           end
        end

        function a = isPointsIn(obj,pts)
            in1 = isPointsInTriangular(pts(:,1),pts(:,2),obj.V00,obj.V01,obj.V11);
            in2 = isPointsInTriangular(pts(:,1),pts(:,2),obj.V00,obj.V10,obj.V11);

            a = double(in1|in2);

        end

        function [coefficients,bool] = getBilinearCoordinates(obj,pt)

            a_x = obj.V00.x - obj.V01.x - obj.V10.x + obj.V11.x;
            b_x = -obj.V00.x + obj.V01.x;
            c_x = -obj.V00.x + obj.V10.x;
            d_x = obj.V00.x - pt.x;

            a_y = obj.V00.y - obj.V01.y - obj.V10.y + obj.V11.y;
            b_y = -obj.V00.y + obj.V01.y;
            c_y = -obj.V00.y + obj.V10.y;
            d_y = obj.V00.y - pt.y;

            bigA = -a_y*b_x + b_y*a_x;
            bigB = -a_y*d_x - c_y*b_x + d_y*a_x +b_y*c_x;
            bigC = -c_y*d_x + d_y*c_x;

            tmp1 = -1;
            tmp2 = -1;
            tmp3 = -1;
            tmp4 = -1;

            if bigB*bigB - 4*bigA*bigC >= 0.0
                if abs(bigA) >= 0.000001
                    tmp1 = ( -bigB + sqrt(bigB*bigB - 4*bigA*bigC) ) / ( 2*bigA );
                    tmp2 = ( -bigB - sqrt(bigB*bigB - 4*bigA*bigC) ) / ( 2*bigA );
                else
                    tmp1 = -bigC/bigB;
                end

                if ( tmp1 >= -0.999999 && tmp1 <= 1.000001)

                    tmp3 = -(b_y*tmp1 + d_y) / (a_y*tmp1 + c_y);
                    tmp4 = -(b_x*tmp1 + d_x) / (a_x*tmp1 + c_x);
                    if tmp3 >= -0.999999 && tmp3 <= 1.000001
                        k1 = tmp1;
                        k2 = tmp3;
                    else if tmp4 >= -0.999999 && tmp4 <= 1.000001
                            k1 = tmp1;
                            k2 = tmp4;
                        end
                    end
                end
                if ( tmp2 >= -0.999999 && tmp2 <= 1.000001)

                    if tmp3 >= -0.999999 && tmp3 <= 1.000001
                        k1 = tmp2;
                        k2 = tmp3;
                    else if tmp4 >= -0.999999 && tmp4 <= 1.000001
                            k1 = tmp2;
                            k2 = tmp4;
                        end
                    end
                end
            end

            if k1>=-0.999999 && k1<=1.000001 && k2>=-0.999999 && k2<=1.000001

                coe1 = (1.0-k1)*(1.0-k2);
                coe2 = k1*(1.0-k2);
                coe3 = (1.0-k1)*k2;
                coe4 = k1*k2;

                coefficients(1) = coe1;
                coefficients(2) = coe2;
                coefficients(3) = coe3;
                coefficients(4) = coe4;

                bool = 1;
            else
                bool = 0;
            end

        end

        function minx = getMinX(obj)
            minx = min(obj.V00.x,obj.V01.x);
            minx = min(minx, obj.V10.x);
            minx = min(minx,obj.V11.x);
        end
        function maxx = getMaxX(obj)
            maxx = max(obj.V00.x,obj.V01.x);
            maxx = max(maxx,obj.V10.x);
            maxx = max(maxx,obj.V11.x);
        end
        function miny = getMinY(obj)
            miny = min(obj.V00.y,obj.V01.y);
            miny = min(miny,obj.V10.y);
            miny = min(miny,obj.V11.y);
        end
        function maxy = getMaxY(obj)
           maxy = max(obj.V00.y,obj.V01.y);
           maxy = max(maxy,obj.V10.y);
           maxy = max(maxy,obj.V11.y);
        end

    end
end

function a = isPointInTriangular(pt,V0,V1,V2)
    lambda1 = ((V1.y-V2.y)*(pt.x-V2.x) + (V2.x-V1.x)*(pt.y-V2.y)) / ((V1.y-V2.y)*(V0.x-V2.x) + (V2.x-V1.x)*(V0.y-V2.y));
	lambda2 = ((V2.y-V0.y)*(pt.x-V2.x) + (V0.x-V2.x)*(pt.y-V2.y)) / ((V2.y-V0.y)*(V1.x-V2.x) + (V0.x-V2.x)*(V1.y-V2.y));
    lambda3 = 1-lambda1-lambda2;
    if lambda1 >= 0.0 && lambda1 <= 1.0 && lambda2 >= 0.0 && lambda2 <= 1.0 && lambda3 >= 0.0 && lambda3 <= 1.0
        a = 1; %true
    else
        a = 0; %false
    end
end

function a = isPointsInTriangular(ptx,pty,V0,V1,V2)
    y12 = V1.y - V2.y;
    x21 = V2.x - V1.x;
    x02 = V0.x - V2.x;
    x21 = V2.x - V1.x;
    y02 = V0.y - V2.y;
    y20 = V2.y - V0.y;
    x12 = V1.x - V2.x;
    s1 = y12*x02 + x21*y02;
    s2 = y20*x12 + x02*y12;

    lambda1 = (y12.*(ptx-V2.x) + x21.*(pty-V2.y)) ./ s1;
    lambda2 = (y20.*(ptx-V2.x) + x02.*(pty-V2.y)) ./ s2;
    lambda3 = 1-lambda1-lambda2;

    condition1 = lambda1>=0.0;
    condition2 = lambda1<=1.0;
    condition3 = lambda2>=0.0;
    condition4 = lambda2<=1.0;
    condition5 = lambda3>=0.0;
    condition6 = lambda3<=1.0;

    a = condition1 & condition2 & condition3 & condition4 & condition5 & condition6;

end


%ROTX Rotation about X axis
%
% R = ROTX(THETA) is an SO(3) rotation matrix (3x3) representing a rotation of THETA
% radians about the x-axis.
%
% R = ROTX(THETA, 'deg') as above but THETA is in degrees.
%
% See also ROTY, ROTZ, ANGVEC2R, ROT2.

% Copyright (C) 1993-2015, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

function R = rotx(t, deg)

    if nargin > 1 && strcmp(deg, 'deg')
        t = t *pi/180;
    end

    ct = cos(t);
    st = sin(t);
    R = [
        1   0    0
        0   ct  -st
        0   st   ct
        ];


function I_d = Akashi2016(I)
%Akashi2016 I_d = Akashi2016(I)
%  The core mechanism is herein implemented fully in accordance with their
%  paper. I have not implemented the repeated score trials. It is pending.
%  This is because this method is somewhat slow.
%
%  Convergence criterion is based on Yamamoto and Nakazawa (exp(-15)), you
%  can change it:
%    edit Akashi2016.m
%  Search 'eps' and change it.
%
%  Also, because of random initialization, I believe averaging the
%  estimated diffuse component over many applications of the method will
%  greatly improve the quality.
%
%  See also SIHR.

assert(isa(I, 'float'), 'SIHR:I:notTypeSingleNorDouble', ...
    'Input I is not type single nor double.')
assert(min(I(:)) >= 0 && max(I(:)) <= 1, 'SIHR:I:notWithinRange', ...
    'Input I is not within [0, 1] range.')
[n_row, n_col, n_ch] = size(I);
assert(n_row > 1 && n_col > 1, 'SIHR:I:singletonDimension', ...
    'Input I has a singleton dimension.')
assert(n_ch == 3, 'SIHR:I:notRGB', ...
    'Input I is not a RGB image.')

I = 255 * I;
M = 3; % = number of color channels
N = n_row * n_col; % = number of pixels
R = 7; % = number of color clusters - 1
I = reshape(I, [N, M])';
i_s = ones(3, 1, class(I)) / sqrt(3);
H = 254 * rand(R, N, class(I)) + 1;
W_d = 254 * rand(3, R-1, class(I)) + 1;
W_d = my_normc(W_d); % W_d ./ vecnorm(W_d, 2, 1); % normalize(W_d,1,'norm');
W = [i_s, W_d];
A = ones(M, class(I));
lambda = 3;
eps = exp(-15);
F_t_1 = Inf;
iter = uint16(0);
max_iter = uint16(10e3); % change for later convergence (takes way longer)
% tic
while true
    W_bar = my_normc(W);
    H = H .* ((W_bar') * I) ./ ...
        ((W_bar') * W_bar * H + lambda);
    H_d = H(2:end, :);
    Vl = max(0, I-i_s*H(1, :));
    % W_d = W(:,2:end); % not sure
    W_d_bar = W_bar(:, 2:end);
    W_d = W_d_bar .* (Vl * (H_d') + W_d_bar .* (A * W_d_bar * H_d * (H_d'))) ./ ...
        (W_d_bar * H_d * (H_d') + W_d_bar .* (A * Vl * (H_d')));
    W = [i_s, W_d];
    F_t = 0.5 * norm((I - W * H), 'fro') + lambda * sum(H(:));
    if abs(F_t-F_t_1) < eps * abs(F_t) || iter >= max_iter
        break
    end
    F_t_1 = F_t;
    iter = iter + 1;
end
% toc
W_d = W(:, 2:end);
% h_s = H(1, :);
H_d = H(2:end, :);
% I_s = i_s * h_s;
I_d = W_d * H_d;
% I_s = reshape(full(I_s)', [n_row, n_col, n_ch]) / 255;
I_d = reshape(full(I_d)', [n_row, n_col, n_ch]) / 255;

% figure(1), imshow(I_s)
% figure(2), imshow(I_d)

end


function [Xi W] = SigmaPoints(xm, P, kappa)
%
%
n  = numel(xm);
Xi = zeros(n, 2*n+1);              % sigma points = col of Xi
W  = zeros(n, 1);

Xi(:, 1) = xm;
W(1)     = kappa / (n + kappa);

U = chol((n+kappa)*P);             % U'*U = (n+kappa)*P

for k=1:n
  Xi(:, k+1) = xm + U(k, :)';      % row of U
  W(k+1)     = 1 / (2*(n+kappa));
end

for k=1:n
  Xi(:, n+k+1) = xm - U(k, :)';
  W(n+k+1)      = 1 / (2*(n+kappa));
end


function res = isCounterClockwise(p1, p2, p3, varargin)
%ISCOUNTERCLOCKWISE Compute relative orientation of 3 points
%
%   CCW = isCounterClockwise(P1, P2, P3);
%   Computes the orientation of the 3 points. The returns is:
%   +1 if the path P1->P2->P3 turns Counter-Clockwise (i.e., the point P3
%       is located "on the left" of the line P1-P2)
%   -1 if the path turns Clockwise (i.e., the point P3 lies "on the right"
%       of the line P1-P2)
%   0  if the point P3 is located on the line segment [P1 P2].
%
%   This function can be used in more complicated algorithms: detection of
%   line segment intersections, convex hulls, point in triangle...
%
%   CCW = isCounterClockwise(P1, P2, P3, EPS);
%   Specifies the threshold used for detecting colinearity of the 3 points.
%   Default value is 1e-12 (absolute).
%
%   Example
%   isCounterClockwise([0 0], [10 0], [10 10])
%   ans =
%       1
%   isCounterClockwise([0 0], [0 10], [10 10])
%   ans =
%       -1
%   isCounterClockwise([0 0], [10 0], [5 0])
%   ans =
%       0
%
%   See also
%   points2d, isPointOnLine, isPointInTriangle
%
%   References
%   Algorithm adapated from Sedgewick's book.
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2010-04-09
% Copyright 2011 INRA - Cepia Software Platform.

%   HISTORY
%   2011-05-16 change variable names, add support for point arrays

% get threshold value
eps = 1e-12;
if ~isempty(varargin)
    eps = varargin{1};
end

% ensure all data have same size
np = max([size(p1, 1) size(p2, 1) size(p3,1)]);
if np > 1
    if size(p1,1) == 1
        p1 = repmat(p1, np, 1);
    end
    if size(p2,1) == 1
        p2 = repmat(p2, np, 1);
    end
    if size(p3,1) == 1
        p3 = repmat(p3, np, 1);
    end
end

% init with 0
res = zeros(np, 1);

% extract vector coordinates
x0  = p1(:, 1);
y0  = p1(:, 2);
dx1 = p2(:, 1) - x0;
dy1 = p2(:, 2) - y0;
dx2 = p3(:, 1) - x0;
dy2 = p3(:, 2) - y0;

% check non colinear cases
res(dx1 .* dy2 > dy1 .* dx2) =  1;
res(dx1 .* dy2 < dy1 .* dx2) = -1;

% case of colinear points
ind = abs(dx1 .* dy2 - dy1 .* dx2) < eps;
res(ind( (dx1(ind) .* dx2(ind) < 0) | (dy1(ind) .* dy2(ind) < 0) )) = -1;
res(ind(  hypot(dx1(ind), dy1(ind)) <  hypot(dx2(ind), dy2(ind)) )) =  1;


% Mathematics Q2410741
% https://math.stackexchange.com/questions/2410741
% Matrix differentiation on ∥A−B∘X∥2F+λ∥X∥2F
% References:
%   1.  aa
% Remarks:
%   1.  sa
% TODO:
% 	1.  ds
% Release Notes
% - 1.0.000     30/08/2017
%   *   First release.

%% General Parameters

run('InitScript.m');

figureIdx           = 0; %<! Continue from Question 1
figureCounterSpec   = '%04d';

generateFigures = OFF;

DIFF_MODE_FORWARD   = 1;
DIFF_MODE_BACKWARD  = 2;
DIFF_MODE_CENTRAL   = 3;

%% Simulation Parameters

numRows = 2;

paramLambda = 0.1 * rand([1, 1]);

difMode = DIFF_MODE_FORWARD;
epsVal  = 1e-6;

numSamples      = 1000;
numIterations   = 2500;
stepSize        = 0.001;

%% Generate Data

vX = rand([numRows, 1]);

mA = randn([numRows, numRows]);
mB = randn([numRows, numRows]);

hObjFun = @(vX) (norm(mA - (mB .* (vX * vX.')), 'fro') .^ 2) + (paramLambda * (norm(vX * vX.', 'fro') ^ 2));
% hObjFun = @(vX) sum(sum((mA - mB .* (vX * vX.')) .^ 2)) + (paramLambda * sum(sum((vX * vX.') .^ 2)));

%% Numerical Solution

vGNumerical = CalcFunGrad(vX, hObjFun, difMode, epsVal);

%% Analytic Solution

vG = zeros([numRows, 1]);

for kk = 1:numRows
    for ii = 1:numRows
        for jj = 1:numRows
            if(ii == kk)
                vG(kk) = vG(kk) + (-2 * mB(ii, jj) * vX(jj) * (mA(ii, jj) - mB(ii, jj) * vX(ii) * vX(jj)));
                vG(kk) = vG(kk) + (2 * paramLambda * vX(jj) * (vX(ii) * vX(jj)));
            end
            if(jj == kk)
                vG(kk) = vG(kk) + (-2 * mB(ii, jj) * vX(ii) * (mA(ii, jj) - mB(ii, jj) * vX(ii) * vX(jj)));
                vG(kk) = vG(kk) + (2 * paramLambda * vX(ii) * (vX(ii) * vX(jj)));
            end
        end
    end
end

%% Analysis

disp(['Maximum Error Between Numercial Differntiation to Analytic - ', num2str(max(abs(vG - vGNumerical)))]);

%% Minimization

mZ      = zeros([numSamples, numSamples]);
vXGrid  = linspace(-5, 5, numSamples);

for jj = 1:numSamples
    for ii = 1:numSamples
        mZ(ii, jj) = hObjFun([vXGrid(jj); vXGrid(ii)]);
    end
end

[minVal, argMin] = min(mZ(:));
[minValRowIdx, minValColIdx] = ind2sub([numSamples, numSamples], argMin);

% Numercial Optimization

mX          = zeros([numRows, numIterations]);
vObjFunVal  = zeros([numIterations, 1]);

% Initialization
vX              = rand([numRows, 1]);
vX              = [-2; 4];

mX(:, 1)        = vX;
vObjFunVal(1)   = hObjFun(vX);

for ii = 2:numIterations
    vG              = CalcFunGrad(vX, hObjFun, difMode, epsVal);
    vX              = vX - (stepSize * vG);
    mX(:, ii)       = vX;
    vObjFunVal(ii)  = hObjFun(vX);
end

figureIdx = figureIdx + 1;

hFigure         = figure('Position', [100, 100, 720, 960]);
hAxes           = subplot(3, 1, 1:2);
set(hAxes, 'NextPlot', 'add');
hImageObject    = image('XData', vXGrid, 'YData', vXGrid, 'CData', mZ, 'CDataMapping', 'scaled');
[mCountourLines, hContouObj]  = contour(vXGrid, vXGrid, mZ);
set(hContouObj, 'LineColor', 'white');
hLineSeries(1)  = line(vXGrid(minValColIdx), vXGrid(minValRowIdx));
set(hLineSeries(1), 'LineStyle', 'none', 'Marker', 'diamond', 'MarkerSize', markerSizeLarge, 'MarkerEdgeColor', 'r', 'MarkerFaceColor', 'r');
hLineSeries(2)  = line(mX(2, :), mX(1, :));
set(hLineSeries(2), 'LineStyle', 'none', 'Marker', 'o', 'MarkerSize', 5, 'MarkerEdgeColor', 'm', 'MarkerFaceColor', 'w');
set(hAxes, 'DataAspectRatio', [1, 1, 1]);
set(get(hAxes, 'Title'), 'String', {['Objective Function Value']}, ...
    'FontSize', fontSizeTitle);
set(get(hAxes, 'XLabel'), 'String', {['x_2']}, ...
    'FontSize', fontSizeAxis);
set(get(hAxes, 'YLabel'), 'String', {['x_1']}, ...
    'FontSize', fontSizeAxis);
hLegend = ClickableLegend([hLineSeries(1), hLineSeries(2)], {['Optimal Argument'], ['Gradient Descent Iterations']});

hAxes = subplot(3, 1, 3);
hLineSeries = plot(1:numIterations, vObjFunVal);
set(hLineSeries, 'LineWidth', lineWidthNormal);
set(get(hAxes, 'Title'), 'String', {['Objective Function Value']}, ...
    'FontSize', fontSizeTitle);
set(get(hAxes, 'XLabel'), 'String', {['Iteration Number']}, ...
    'FontSize', fontSizeAxis);
set(get(hAxes, 'YLabel'), 'String', {['Value']}, ...
    'FontSize', fontSizeAxis);

saveas(hFigure,['Figure', num2str(figureIdx, figureCounterSpec), '.png']);

%% Restore Defaults

% set(0, 'DefaultFigureWindowStyle', 'normal');
% set(0, 'DefaultAxesLooseInset', defaultLoosInset);


function [ babel ] = babel( Phi )
%BABEL computes the Babel function of a dictionary
% Columns of Phi must be unit norm
% The function has been defined by Joel A Tropp

% Let us compute the Gram matrix
G = Phi' * Phi;
% Let us take element wise absolute
absG = abs(G);
% Let us sort each row in descending order
GS = sort(absG, 2,'descend');
rowSums = cumsum(GS(:, 2:end), 2);
babel = max(rowSums);


% theta = choose_orientations2(x,y,phi,wtheta,wphi)
%
% we will set the orientation to theta_t = phi_t + s_t * pi
% we want to choose s_t to minimize
% \sum_t cost(s_t|s_{t-1})
% cost(s_t|s_{t-1}) = [wtheta_t*d(\theta_t,\theta_{t-1}) +
%                      wphi(||v_t||^2)*d(\theta_t,angle(v_t))]
%
% we will find the most likely states s_t using the recursion
% cost_t(s_t) = min_{s_{t-1}} { cost_{t-1}(s_{t-1}) + cost(s_t|s_{t-1})
%
% Inputs:
% x: N x 1 vector where x(t) is the x-coordinate of the center of the fly
% at time t
% y: N x 1 vector where y(t) is the y-coordinate of the center of the fly
% at time t
% phi: N x 1 vector where phi(t) is the orientation of the fly at time t
% wtheta: N x 1 vector where wtheta(t) is the weight of the change in
% orientation term at time t
% wphi: N x 1 vector where wphi(t) is the weight of the velocity
% direction term at time t
%
function theta = choose_orientations2(x,y,theta,weight_theta,weight_phi)

inputsz = size(x);
x = x(:);
y = y(:);
theta = theta(:);
weight_phi = weight_phi(:);
weight_theta = weight_theta(:);

% number of frames
N = length(x);

% allocate space for storing the optimal path
stateprev = zeros(N-1,2);

% allocate space for computing costs
tmpcost = zeros(2,1);
costprevnew = zeros(2,1);

% initialize first frame
costprev = zeros(2,1);

% compute velocity
vx = [0;diff(x)];
vy = [0;diff(y)];

% compute angle of velocity
velocityangle = atan2(vy,vx);

% compute iteratively
for t = 2:N,

  % compute for both possible states
  for scurr = 1:2,

    % try both previous states
    thetacurr = theta(t) + (scurr-1)*pi;

    for sprev = 1:2,

      thetaprev = theta(t-1) + (sprev-1)*pi;
      costcurr = weight_theta(t)*angledist(thetaprev,thetacurr) + ...
        weight_phi(t)*angledist(thetacurr,velocityangle(t));
      tmpcost(sprev) = costprev(sprev) + costcurr;

    end

    % choose the minimum
    sprev = argmin(tmpcost);

    % set pointer for path
    stateprev(t-1,scurr) = sprev;

    % set cost
    costprevnew(scurr) = tmpcost(sprev);

  end

  % copy over
  costprev(:) = costprevnew(:);

end

% choose the best last state
scurr = argmin(costprev);

if scurr == 2,
  theta(end) = modrange(theta(end)+pi,-pi,pi);
end

% choose the best states
for t = N-1:-1:1,
  scurr = stateprev(t,scurr);
  if scurr == 2,
    theta(t) = modrange(theta(t)+pi,-pi,pi);
  end
end

theta = reshape(theta,inputsz);


function [x,obj,err,iter] = fusedl1(A,b,lambda,opts)

% Solve the fused Lasso (Fused L1) minimization problem by ADMM
%
% min_x ||x||_1 + lambda*\sum_{i=2}^p |x_i-x_{i-1}|,
%   s.t. Ax=b
%
% ---------------------------------------------
% Input:
%       A       -    d*n matrix
%       b       -    d*1 vector
%       lambda  -    >=0, parameter
%       opts    -    Structure value in Matlab. The fields are
%           opts.tol        -   termination tolerance
%           opts.max_iter   -   maximum number of iterations
%           opts.mu         -   stepsize for dual variable updating in ADMM
%           opts.max_mu     -   maximum stepsize
%           opts.rho        -   rho>=1, ratio used to increase mu
%           opts.DEBUG      -   0 or 1
%
% Output:
%       x       -    n*1 vector
%       obj     -    objective function value
%       err     -    residual
%       iter    -    number of iterations
%
% version 1.0 - 20/06/2016
%
% Written by Canyi Lu (canyilu@gmail.com)
%

tol = 1e-8;
max_iter = 500;
rho = 1.1;
mu = 1e-4;
max_mu = 1e10;
DEBUG = 0;

if ~exist('opts', 'var')
    opts = [];
end
if isfield(opts, 'tol');         tol = opts.tol;              end
if isfield(opts, 'max_iter');    max_iter = opts.max_iter;    end
if isfield(opts, 'rho');         rho = opts.rho;              end
if isfield(opts, 'mu');          mu = opts.mu;                end
if isfield(opts, 'max_mu');      max_mu = opts.max_mu;        end
if isfield(opts, 'DEBUG');       DEBUG = opts.DEBUG;          end

[d,n] = size(A);
x = zeros(n,1);

z = x;
Y1 = zeros(d,1);
Y2 = x;

Atb = A'*b;
I = eye(n);
invAtAI = (A'*A+I)\I;

% parameters for "flsa" (from SLEP package)
tol2 = 1e-10;      % the duality gap for termination
max_step = 50;     % the maximal number of iterations
x0 = zeros(n-1,1); % the starting point

iter = 0;
for iter = 1 : max_iter
    xk = x;
    zk = z;
    % update x.
    % flsa solves min_x 1/2||x-v||_2^2+lambda1*||x||_1+lambda2*\sum_{i=2}^p |x_i-x_{i-1}|
    x = flsa(z-Y2/mu,x0,1/mu,lambda/mu,n,max_step,tol2,1,6);
    % update z
    z = invAtAI*(-A'*Y1/mu+Atb+Y2/mu+x);
    dY1 = A*z-b;
    dY2 = x-z;
    chgx = max(abs(xk-x));
    chgz = max(abs(zk-z));
    chg = max([chgx chgz max(abs(dY1(:))) max(abs(dY2(:)))]);
    if DEBUG
        if iter == 1 || mod(iter, 10) == 0
            obj = comp_fusedl1(x,1,lambda);
            err = sqrt(norm(dY1,'fro')^2+norm(dY2,'fro')^2);
            disp(['iter ' num2str(iter) ', mu=' num2str(mu) ...
                    ', obj=' num2str(obj) ', err=' num2str(err)]);
        end
    end

    if chg < tol
        break;
    end
    Y1 = Y1 + mu*dY1;
    Y2 = Y2 + mu*dY2;
    mu = min(rho*mu,max_mu);
end
obj = comp_fusedl1(x,1,lambda);
err = sqrt(norm(dY1,'fro')^2+norm(dY2,'fro')^2);

function f = comp_fusedl1(x,lambda1,lambda2)
% compute f = lambda1*||x||_1 + lambda2*\sum_{i=2}^p |x_i-x_{i-1}|.
% x - p*1 vector
f = 0;
p = length(x);
for i = 2 : p
   f = f+abs(x(i)-x(i-1));
end
f = lambda1*norm(x,1)+lambda2*f;


function [idp,IDP_p,IDP_p0] = p2idp(p,p0)

% P2IDP Point to inverse-depth point conversion.
%   P2IDP(P,P0) is the inverse depth point anchored at P0 corresponding to
%   the Euclidean point P.
%
%   An inverse-depth point (idp) is a 6-vector:
%
%           idp = [x0 y0 z0 el az rho]',
%
%   where:
%       x0, z0, y0: anchor: the 3D point P0 where where distance is referred to.
%       el, az: azimuth and elevation of the ray through P that starts at P0.
%       rho: inverse of the distance from point P to P0.
%
%   [idp,IDP_p,IDP_p0] returns the Jacobians wrt P and P0.
%
%   See also IDP2P.

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

if nargout == 1
    m   = p-p0;
    rho = 1/sqrt(dot(m,m));
    py  = vec2py(m);

    idp = [p0;py;rho];
else

    m           = p-p0;
    M_p         = 1;
    M_p0        = -1;
    [mn2,MN2_m] = dotJ(m,m);
    rho         = 1/mn2;
    RHO_mn2     = -1/mn2^2;
    [py,PY_m]   = vec2py(m);

    idp = [p0;py;rho];

    RHO_m = RHO_mn2*MN2_m;

    IDP_p = [zeros(3);PY_m*M_p;RHO_m*M_p];
    IDP_p0 = [eye(3);PY_m*M_p0;RHO_m*M_p0];

end

return

%% Jac

syms x y z x0 y0 z0 real
p = [x;y;z];
p0 = [x0;y0;z0];

idp = p2idp(p,p0);

simplify(IDP_p  - jacobian(idp,p))
simplify(IDP_p0 - jacobian(idp,p0))

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

%   # START GPL LICENSE

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

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

%   # END GPL LICENSE


function [plane, inlier_count, inliers] = ransac_depth_plane(x, depth, alpha,...
    iter, p)
%RANSAC_DEPTH_PLANE  Fit a 3D depth plane to a set of input points using RANSAC.
%The distance of a point from the plane is defined as the vertical offset.
%
%   INPUTS:
%
%   -|x|: |P|-by-2 matrix with image-plane coordinates of input points.
%
%   -|depth|: |P|-by-1 vector with depth values for input points.
%
%   -|alpha|: positive constant regulating the inlier threshold used in RANSAC.
%
%   -|iter|: maximum number of RANSAC iterations.
%
%   -|p|: target probability for having obtained a pure inlier set, used to cut
%   down on number of RANSAC iterations.
%
%   OUTPUTS:
%
%   -|plane|: 3-by-1 vector holding the parameters of the fitted plane, so that
%   for each point [x, y, d] it holds d = [x, y, 1] * plane.
%
%   -|inlier_count|: cardinality of the final inlier set.
%
%   -|inliers|: |inlier_count|-by-1 vector with indices of the members of the
%   final inlier set.

% Determine inlier threshold |theta| based on the depth values. Goal: invariance
% to range of depth values.
theta = alpha * median(depth);

% Number of plane parameters.
N = 3;

% Total number of points.
P = length(depth);

% Represent pixel coordinates homogeneously.
x_h = [x, ones(P, 1)];

% Initialize variables to save the best fit across iterations.
inliers = [];
inlier_count = 0;
inlier_ratio = 0;

% Main RANSAC loop.
for i = 1:iter
    colinear = true;
    while colinear
        % Select a sample at random.
        permut = randperm(P);
        sample = permut(1:N);
        x_h_sample = x_h(sample, :);

        % Proceed to plane fitting only if the points of the sample are not
        % colinear, i.e. square matrix with homogeneous coordinates of
        % points in the sample is of full rank.
        if rank(x_h_sample) == N
            colinear = false;
        end
    end

    % Fit a plane to the current sample.
    plane_tmp = fit_depth_plane(x_h_sample, depth(sample));

    % Identify inliers for current fit.

    % Vertical distance.
    inliers_tmp = abs(x_h * plane_tmp - depth) <= theta;
    % Orthogonal distance.
    % plane_tmp_normal = [plane_tmp(1:N-1); 1];
    % inliers_tmp = abs([points(known_pixels_finite, :), depth_known_finite] *...
    %     plane_tmp_normal - plane_tmp(N)) / norm(plane_tmp_normal) <= theta;
    inlier_tmp_count = nnz(inliers_tmp);

    % Update best fit.
    if inlier_tmp_count > inlier_count
        inlier_count = inlier_tmp_count;
        inlier_ratio = inlier_count / P;
        inliers = find(inliers_tmp);
    end

    % An adaptive version of RANSAC is implemented. If a pure inlier sample
    % has been already examined with probability greater that |p|, avoid
    % performing more iterations.
    p_bound = 1 - (1 - inlier_ratio ^ N) ^ i;
    if p <= p_bound
        break;
    end
end

% Perform a final least squares fit to compute the best depth plane with respect
% to all points in the maximum inlier set that has been found with RANSAC.
plane = fit_depth_plane(x_h(inliers, :), depth(inliers));

end


function y = atan(x)
%ATAN         Implements  atan(x)  for intervals
%
%   y = atan(x)
%
%interval standard function implementation
%

% written  12/30/98     S.M. Rump
% modified 08/31/99     S.M. Rump  complex allowed, sparse input,
%                                  major revision, improved accuracy
% modified 09/02/00     S.M. Rump  rounding unchanged after use
% 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/06/07     S.M. Rump  approximate std fcts removed
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
%

  if issparse(x)
    [ix,jx,sx] = find(x);
    [m,n] = size(x);
    y = sparse(ix,jx,atan(full(sx)),m,n);
    return
  end

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

  if x.complex
    y = log( 2./(1-j*x) - 1) / (2*j);
    if rndold
      setround(rndold)
    end
    return
  end

  y = x;

  xinf = x.inf(:);
  xsup = x.sup(:);

  IndexInfPos = ( xinf>=0 );
  len1 = sum(IndexInfPos);
  IndexSupNeg = ( xsup<=0 );

  Y = atan_pos( [ xinf(IndexInfPos) ; -xsup(IndexSupNeg) ] , -1 );
  y.inf(IndexInfPos) = Y(1:len1);
  y.sup(IndexSupNeg) = -Y( len1+1 : end );

  IndexInfNeg = ( xinf<0 );
  len1 = sum(IndexInfNeg);
  IndexSupPos = ( xsup>0 );

  Y = atan_pos( [ -xinf(IndexInfNeg) ; xsup(IndexSupPos) ] , 1 );
  y.inf(IndexInfNeg) = -Y(1:len1);
  y.sup(IndexSupPos) = Y( len1+1 : end );

  setround(rndold)


function [M]=gen_MassMatrix(q,Pcii,Icii,mcii)
%% This function is used to calculate the mass matrix of the KUKA iiwa 7 R 800 manipulator

% Arreguments:
%--------------------
% q: is 1x7 vector, joint nagles vector of the manipulator
% Pcii: is 3X7 matrix while each column represents the local coordinates
% of the center of mass of each link.
% Icii: is (3x3x7) matrix, each 3x3 matrix of which represnets the
% associated link inertial tensor represented in its local inertial frame
% mcii: is (1x7) vector, each element of which specifies a mass of one of
% the links

% Return value:
%--------------------
% M: 7x7 matrix, the mass matrix of the KUKA iiwa 7 R 800.

% Copyright: Mohammad SAFEEA

T=kukaGetKenimaticModelAccelerated(q);
[M]=GetInertiaMatrix5(T,Pcii,Icii,mcii);


function combo_test11 ( )

%*****************************************************************************80
%
%% COMBO_TEST11 tests KNAPSACK_REORDER and KNAPSACK_01.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 January 2011
%
%  Author:
%
%    John Burkardt
%
  n = 5;
  mass_limit = 26.0;
  p = [ 24.0, 13.0, 23.0, 15.0, 16.0 ]';
  w = [ 12.0,  7.0, 11.0,  8.0,  9.0 ]';

  fprintf ( 1, ' \n' );
  fprintf ( 1, 'COMBO_TEST11\n' );
  fprintf ( 1, '  KNAPSACK_REORDER reorders the knapsack data.\n' );
  fprintf ( 1, '  KNAPSACK_01 solves the 0/1 knapsack problem.\n' );

  fprintf ( 1, ' \n' );
  fprintf ( 1, '  Object, Profit, Mass, "Profit Density"\n' );
  fprintf ( 1, ' \n' );
  for i = 1 : n
    fprintf ( 1, '  %6d  %7.3f  %7.3f  %7.3f\n', i, p(i), w(i), p(i) / w(i) );
  end

  [ p, w ] = knapsack_reorder ( n, p, w );

  fprintf ( 1, ' \n' );
  fprintf ( 1, '  After reordering by Profit Density:\n' );
  fprintf ( 1, ' \n' );
  fprintf ( 1, '  Object, Profit, Mass, "Profit Density"\n' );
  fprintf ( 1, ' \n' );
  for i = 1 : n
    fprintf ( 1, '  %6d  %7.3f  %7.3f  %7.3f\n', i, p(i), w(i), p(i) / w(i) );
  end

  fprintf ( 1, ' \n' );
  fprintf ( 1, '  Total mass restriction is %f\n', mass_limit );

  [ x, mass, profit ] = knapsack_01 ( n, mass_limit, p, w );

  fprintf ( 1, ' \n' );
  fprintf ( 1, '  Object, Density, Choice, Profit, Mass\n' );
  fprintf ( 1, ' \n' );
  for i = 1 : n
    fprintf ( 1, '%6d  %7.3f  %7.3f  %7.3f  %7.3f\n', ...
      i, p(i) / w(i), x(i),  x(i) * p(i), x(i) * w(i) );
  end

  fprintf ( 1, ' \n' );
  fprintf ( 1, '  Total:                  %7.3f  %7.3f\n', profit, mass );

  return
end


function jac = p36_jac ( neqn, t, y )

%*****************************************************************************80
%
%% P36_JAC evaluates the jacobian for problem p36.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 February 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NEQN, the number of equations.
%
%    Input, real T, Y(NEQN), the arguments of the jacobian.
%
%    Output, real JAC(NEQN,NEQN), the jacobian matrix.
%
  jac = zeros ( neqn, neqn );

  jac(1,1) =  0.0;
  jac(1,2) =  1.0;

  jac(2,1) = 1.0 - 3.0 * y(1).^2;
  jac(2,2) = 0.0;

  return
end


function Sest = cosamp_cgls(Phi,u,K,tol,maxiterations, err_tol)

% Cosamp algorithm
%   Input
%       K : sparsity of Sest
%       Phi : measurement matrix
%       u: measured vector
%       tol : tolerance for approximation between successive solutions.
%   Output
%       Sest: Solution found by the algorithm
%
% Algorithm as described in "CoSaMP: Iterative signal recovery from
% incomplete and inaccurate samples" by Deanna Needell and Joel Tropp.
%

% This implementation was written by David Mary,
% but modified 20110707 by Bob L. Sturm to make it much clearer,
% and corrected multiple times again and again.
% To begin with, see: http://media.aau.dk/null_space_pursuits/2011/07/ ...
% algorithm-power-hour-compressive-sampling-matching-pursuit-cosamp.html
%
% This script/program is released under the Commons Creative Licence
% with Attribution Non-commercial Share Alike (by-nc-sa)
% http://creativecommons.org/licenses/by-nc-sa/3.0/
% Short Disclaimer: this script is for educational purpose only.
% Longer Disclaimer see  http://igorcarron.googlepages.com/disclaimer

%%Code modified by Praneeth 14March2017 to include case when there is no
%%signal to estimate;replaced pinv() by A\b syntax for speed.
% Initialization
Sest = zeros(size(Phi,2),1);
v = u;
t = 1;
numericalprecision = 1e-12;
T = [];
while ((t <= maxiterations) && (norm(v)/norm(u) > tol) ...
        && (norm(Phi'*v) > err_tol))

  y = abs(Phi'*v);
  [vals,z] = sort(y,'descend');
  Omega = find(y >= vals(2*K) & y > numericalprecision);

  T = union(Omega,T);
  x = v(T);
  %b = pinv(Phi(:,T))*u;
%   b = Phi(:,T) \ u;     %% edit PKN -- 3/3/17
  b = cgls(Phi(:, T), u, 0, err_tol, 3, 0, x);
  [vals,z] = sort(abs(b),'descend');
  Kgoodindices = (abs(b) >= vals(K) & abs(b) > numericalprecision);
  T = T(Kgoodindices);
  Sest = zeros(size(Phi,2),1);
  b = b(Kgoodindices);
  Sest(T) = b;
  v = u - Phi(:,T)*b;
  t = t+1;
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% s10.m %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% function s10(x)
% Shekel10 function
function f = s10(x)
a = [4, 1, 8, 6, 3, 2, 5, 8, 6, 7;
     4, 1, 8, 6, 7, 9, 5, 1, 2, 3.6;
     4, 1, 8, 6, 3, 2, 3, 8, 6, 7;
     4, 1, 8, 6, 7, 9, 3, 1, 2, 3.6];
c = [ 0.1, 0.2, 0.2, 0.4, 0.4, 0.6, 0.3, 0.7, 0.5, 0.5];
if size(x,1) == 1
 x = x';
end
for i=1:10
 b = (x - a(:,i)).^2;
 d(i) = sum(b);
end
f = -sum((c+d).^(-1));


function [PC, powers] = powercentersPD(T, E, wts)
% function [PC, powers] = powercentersPD(T, E, wts)
%
% T: triangulation
% E: set of points
% wts: weights of points in E
%
% The output array PC contains the power centers of the triangles in T.
% That is, row i of PC contains the point that is equidistant from each
% vertex of the triangle specified by row i of T, with respect to the power
% of each vertex. The output array powers contains the power of each power
% center.

[m, n] = size(T);
PC = zeros(m,n-1);
powers = zeros(m,1);

for i=1:m
    tr = E(T(i,:),:);
    wt = wts(T(i,:));
    p = tr(1,:);
    Rp = repmat(p, n-1, 1);
    Pts = tr(2:n,:);
    Ac = 2*(Pts - Rp);

    Rw1 = repmat(wt(1), n-1, 1);
    Wts = wt(2:n);
    Sp1 = repmat(sum(p.^2), n-1, 1);
    SPts = sum(Pts.^2, 2);
    Bc = Rw1 - Wts - Sp1 + SPts;

    pc = Ac \ Bc;

    PC(i,:) = pc;
    powers(i,1) = norm(pc - p')^2 - wt(1);
end


function value = p23_f ( dim_num, point_num, x )

%*****************************************************************************80
%
%% P23_F evaluates the integrand for problem 23.
%
%  Dimension:
%
%    DIM_NUM is arbitrary.
%
%  Region:
%
%    The interior of the unit simplex, for which all X's are nonnegative,
%    and sum ( X(1:N) ) <= 1.
%
%  Integral Parameters:
%
%    C defaults to 1.0.
%    Call P23_R8 to get or set this value.
%
%    E(1:DIM_NUM) defaults to (/ 2, 2, ..., 2 /).
%    Call P23_I4VEC to get or set this value.
%
%  Integrand:
%
%    F(X) = C * X1^E1 * X2^E2 * ... * Xn^En
%
%    C is real, all exponents E are nonnegative integers.
%
%  Exact Integral:
%
%    C * Gamma(E1+1) * Gamma(E2+1) * ... * Gamma(En+1) / Gamma(E1+E2+...+En+1)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 June 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Philip Davis, Philip Rabinowitz,
%    Methods of Numerical Integration,
%    Second Edition,
%    Dover, 2007,
%    ISBN: 0486453391,
%    LC: QA299.3.D28.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the dimension of the argument.
%
%    Input, integer POINT_NUM, the number of points.
%
%    Input, real X(DIM_NUM,POINT_NUM), the evaluation points.
%
%    Output, real VALUE(POINT_NUM), the integrand values.
%
  c = 0.0;
  c = p23_r8 ( 'G', 'C', c );

  e = [];
  e = p23_i4vec ( 'G', 'E', dim_num, e );

  value(1:point_num) = c;

  for point = 1 : point_num

    for dim = 1 : dim_num
      value(point) = value * x(dim,point)^e(dim);
    end

  end

  p23_i4 ( 'I', '#', point_num );

  return
end


function  [Z] =  MCWNNM_ADMM2( Y, NSig, Par )
% This routine solves the following weighted nuclear norm optimization problem with column weights,
%
% min_{X, Z} ||W(Y-X)||_F^2 + ||Z||_w,*  s.t.  X = Z
%
% Inputs:
%        Y      -- 3p^2 x M dimensional noisy matrix, D is the data dimension, and N is the number of image patches.
%        NSig -- 3p^2 x 1 dimensional vector of weights
%        Par   -- structure of parameters
% Output:
%        Z      -- 3p^2 x M dimensional denoised matrix

% tol = 1e-8;
if ~isfield(Par, 'maxIter')
    Par.maxIter = 10;
end
if ~isfield(Par, 'rho')
    Par.rho = 1;
end
if ~isfield(Par, 'mu')
    Par.mu = 1;
end
if ~isfield(Par, 'display')
    Par.display = true;
end
% Initializing optimization variables
% Intialize the weight matrix W
W = 1 ./ (NSig+eps);
% Initializing optimization variables
X = zeros(size(Y));
Z = zeros(size(Y));
A = zeros(size(Y));
%% Start main loop
iter = 0;
PatNum       = size(Y,2);
TempC  = Par.Constant * sqrt(PatNum);
while iter < Par.maxIter
    iter = iter + 1;

    % update X, fix Z and A
    % min_{X} ||W * Y - W * X||_F^2 + 0.5 * rho * ||X - Z + 1/rho * A||_F^2
    X = diag(1 ./ (W.^2 + 0.5 * Par.rho)) * (diag(W.^2) * Y + 0.5 * Par.rho * Z - 0.5 * A);

    % update Z, fix X and A
    % min_{Z} ||Z||_*,w + 0.5 * rho * ||Z - (X + 1/rho * A)||_F^2
    Temp = X + A/Par.rho;
    [U, SigmaTemp, V] =   svd(full(Temp), 'econ');
    [SigmaZ, svp] = ClosedWNNM(diag(SigmaTemp), 2/Par.rho*TempC, eps);
    Z =  U(:, 1:svp) * diag(SigmaZ) * V(:, 1:svp)';
    %     % check the convergence conditions
    %     stopC = max(max(abs(X - Z)));
    %     if Par.display && (iter==1 || mod(iter,10)==0 || stopC<tol)
    %         disp(['iter ' num2str(iter) ',mu=' num2str(Par.mu,'%2.1e') ...
    %             ',rank=' num2str(rank(Z,1e-4*norm(Z,2))) ',stopALM=' num2str(stopC,'%2.3e')]);
    %     end
    %     if stopC < tol
    %         break;
    %     else
    % update the multiplier A, fix Z and X
    A = A + Par.rho * (X - Z);
    Par.rho = min(1e4, Par.mu * Par.rho);
    %     end
end
return;


function [interpV] = finterp3(xInterpV, yInterpV, zInterpV, field3M, xFieldV, yFieldV, zFieldV, OOBV)
%"finterp3"
%   Fast 3D linear interpolation of the data field3M defined at
%   xFieldV,yFieldV,zFieldV, to the points listed in x/y/zInterpV.
%
%   xFieldV and yFieldV are 3 element vectors of the form [start, delta, end],
%   where start is the coordinate of the first column(x) or row(y) and end
%   is the coord of the last column/row, with delta being the spacing.  Delta
%   can be negative if start > end.
%
%   zFieldV must be specified as a list of coordinates for each slice in
%   field3M.
%
%   xInterpV, yInterpV, zInterpV are vectors of equal length indicating the
%   x,y,z, coordinates to interpolate from original data field3M.
%
%   Based on code by J.O.Deasy.
%
%JRA 1/13/05
%
%Usage:
%   function [interpV] = finterp3(xInterpV, yInterpV, zInterpV, field3M, xFieldV, yFieldV, zFieldV, outOfBoundsVal)

siz = size(field3M);

if ~exist('OOBV')
    OOBV = NaN;
end

xDelta = xFieldV(2);
yDelta = yFieldV(2);

%Get r,c,s indices.
% cols = interp1q(reshape(xFieldV, [], 1), reshape(1:length(xFieldV),[], 1), reshape(xInterpV, [], 1));
% rows = interp1q(reshape(yFieldV, [], 1), reshape(1:length(yFieldV),[], 1), reshape(yInterpV, [], 1));
cols = (xInterpV-(xFieldV(1)-xDelta))/xDelta;
rows = (yInterpV-(yFieldV(1)-yDelta))/yDelta;
if length(zFieldV) > 1;
    slcs = interp1(zFieldV,1:length(zFieldV),zInterpV);
else
    slcs = ones(size(cols));    %This effectively negates Z.  All values are in plane.  Bad idea?
end

clear xInterpV yInterpV zInterpV
clear xFieldV yFieldV zFieldV

%Find indices out of bounds.
colNaN = cols >= siz(2) | cols < 1;
rowNaN = rows >= siz(1) | rows < 1;
% slcNaN = slcs >= siz(3) | slcs < 1;

% colNaN = isnan(cols);
% rowNaN = isnan(rows);
slcNaN = isnan(slcs) | slcs < 1 | slcs >= siz(3);

%Set those to a proxy 1.
rows(rowNaN) = 1;
cols(colNaN) = 1;
slcs(slcNaN) = 1;

colFloor = floor(cols);
colMod   = cols - colFloor;
oneMinusColMod = (1-colMod);

rowFloor = floor(rows);
rowMod = rows - rowFloor;
oneMinusRowMod = (1-rowMod);

slcFloor = floor(slcs);
slcMod = slcs - slcFloor;
oneMinusSlcMod = (1-slcMod);

clear rows cols slcs

%Linear indices of lower bound contributing points.
INDEXLIST = rowFloor + (colFloor-1)*siz(1) + (slcFloor-1)*siz(1)*siz(2);

clear rowFloor colFloor slcFloor

%Linear offsets when moving in 3D matrix.
oneRow = 1;
oneCol = siz(1);
oneSlc = siz(1)*siz(2);

%Accumulate contribution from each voxel surrounding x,y,z point.
interpV = double(field3M(INDEXLIST)) .* oneMinusRowMod .* oneMinusColMod .* oneMinusSlcMod;
interpV = interpV + double(field3M(INDEXLIST+(oneRow))) .* rowMod .* oneMinusColMod .* oneMinusSlcMod;
interpV = interpV + double(field3M(INDEXLIST+(oneCol))) .* oneMinusRowMod .* colMod .* oneMinusSlcMod;
interpV = interpV + double(field3M(INDEXLIST+(oneCol+oneRow))) .* rowMod .* colMod .* oneMinusSlcMod;;
interpV = interpV + double(field3M(INDEXLIST+(oneSlc))) .* oneMinusRowMod .* oneMinusColMod .* slcMod;
interpV = interpV + double(field3M(INDEXLIST+(oneSlc+oneRow))) .* rowMod .* oneMinusColMod .* slcMod;
interpV = interpV + double(field3M(INDEXLIST+(oneSlc+oneCol))) .* oneMinusRowMod .* colMod .* slcMod;
interpV = interpV + double(field3M(INDEXLIST+(oneSlc+oneCol+oneRow))) .* rowMod .* colMod .* slcMod;

%Replace proxy 1s with out of bounds vals.
interpV(rowNaN | colNaN | slcNaN) = OOBV;

return;


function r_size  = nwspgr_size ( type, dim, k, sym, compress )

%*****************************************************************************80
%
%% NWSPGR_SIZE determines the size of a sparse grid rule.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 June 2012
%
%  Author:
%
%    Original MATLAB version by Florian Heiss, Viktor Winschel.
%    This MATLAB version by John Burkardt.
%
%  Reference:
%
%    Florian Heiss, Viktor Winschel,
%    Likelihood approximation by numerical integration on sparse grids,
%    Journal of Econometrics,
%    Volume 144, 2008, pages 62-80.
%
%  Parameters:
%
%    Input, string TYPE, selects the 1D integration rule:
%    "kpu": Nested rule for unweighted integral over [0,1];
%    "kpn": Nested rule for integral with Gaussian weight;
%    "gqu": Gaussian quadrature for unweighted integral over [0,1]
%    (Gauss-Legendre);
%    "gqn": Gaussian quadrature for integral with Gaussian weight
%    (Gauss-Hermite);
%    func:  any function name. Function must accept level l and return nodes N
%    and weights W for univariate quadrature rule with polynomial exactness
%    2*L-1 as [n, w] = feval(func,level)
%
%    Input, integer DIM, the dimension of the integration problem.
%
%    Input, integer K, the level.  When using the built in 1D rules, the
%    resulting sparse grid will be exact for polynomials up to total order
%    2*K-1.  When using the built-in quadrature rules, the maximum value of K
%    that is available is 25.
%
%    Input, integer SYM.  This is only useful when the user has
%    chosen to supply the 1D quadrature rule.  In that case, if the rule is
%    symmetric, specifying SYM to be 1 will allow the code to run faster.
%    But this also requires that the user quadrature rule function only
%    return the nonnegative abscissas and their weights.
%
%    Input, integer COMPRESS,
%    0, do not compress the rule ( = do not merge duplicate points.)
%    1, compress the rule.
%
%    Output, integer R_SIZE, the "size" of the rule.
%

%
%  Interpret the inputs.  Use STRCMPI so that case is ignored.
%
  builtinfct = ( strcmpi ( type, 'gqu' ) || ...
                 strcmpi ( type, 'gqn' ) || ...
                 strcmpi ( type, 'kpu' ) || ...
                 strcmpi ( type, 'kpn' ) );

  sym = logical ( sym );
%
%  Retrieve the 1D rules of levels 1 to K.
%  Create cell arrays X1D and W1D containing the node and weight information.
%  The array N1D stores the length or order of each rule.
%
%  The built in functions already return only the positive orthant.
%
%  If the rule is being supplied by the user, and the user indicates that
%  it's a symmetric rule, then similarly keep only the positive orthant.
%
%  The result will be that if SYM is true, whether for built in or user-supplied
%  functions, we have only the positive orthant data stored.
%
  n1d = zeros ( 1, k );
  x1d = cell ( k, 1 );
  w1d = cell ( k, 1 );

  for level = 1 : k

    [ x, w ] = feval ( type, level );

    if ( ( ~ builtinfct ) && sym )
      [ numnew, dummy ] = size ( x );
      [ x, sortvec ] = sortrows ( x );
      w = w ( sortvec );
      x = x((floor(numnew/2)+1):numnew,:);
      w = w((floor(numnew/2)+1):numnew,:);
    end

    n1d(level) = length ( w );
    x1d{level} = x;
    w1d{level} = w;

  end
%
%  Initialization.
%
  minq = max ( 0, k - dim );
  maxq = k - 1;
  nodes = [];
  weights = [];
%
%  Loop for max ( 0, K - DIM ) <= Q <= K - 1.
%
  for q = minq : maxq

    r = length ( weights );
%
%  BQ is the combinatorial coefficient applied to the component
%  product rules which have level Q.
%
    bq = ( -1 )^( maxq - q ) * nchoosek ( dim - 1, dim + q - k );
%
%  Compute the D-dimensional row vectors that sum to DIM+Q.
%
    is = get_seq ( dim, dim + q );
%
%  Preallocate new rows for nodes and weights.
%
    Rq = prod ( n1d(is), 2 );
    sRq = sum ( Rq );
    nodes   = [ nodes;   zeros(sRq,dim) ];
    weights = [ weights; zeros(sRq,1) ];
%
%  Generate each of the product rules indicated by IS, and
%  insert them into NODES and WEIGHTS.
%
    for j = 1 : size(is,1)
      midx = is(j,:);
      [ newn, neww ] = tensor_product ( x1d(midx), w1d(midx) );
      nodes((r+1):(r+Rq(j)),:) = newn;
      weights((r+1):(r+Rq(j))) = bq .* neww;
      r = r + Rq(j);
    end
%
%  Sort the nodes and merge repeated values.
%
    [ nodes, sortvec ] = sortrows ( nodes );
    weights = weights(sortvec);
    keep = 1;
    lastkeep = 1;

    for j = 2 : size(nodes,1)
      if ( compress )

        if ( nodes(j,:) == nodes(j-1,:) )
          weights(lastkeep) = weights(lastkeep) + weights(j);
        else
          lastkeep = j;
          keep = [ keep ; j ];
        end
      else
        lastkeep = j;
        keep = [ keep ; j ];
      end
    end

    nodes = nodes(keep,:);
    weights = weights(keep);

  end
%
%  The rule has been computed.
%  If we used symmetry, we now have to extend the rule.
%
  if ( sym )

    nr = length ( weights );
    m = x1d{1};

    for j = 1 : dim

      keep = zeros(nr,1);
      numnew = 0;

      for r = 1 : nr
        if ( nodes(r,j) ~= m )
          numnew = numnew + 1;
          keep(numnew) = r;
        end
      end

      if ( 0 < numnew )
        nodes = [nodes ; nodes(keep(1:numnew),:)];
        nodes(nr+1:nr+numnew,j) = 2*m - nodes(nr+1:nr+numnew,j);
        weights = [weights ; weights(keep(1:numnew))];
        nr = nr + numnew;
      end

    end

    [ nodes, sortvec ] = sortrows ( nodes );
    weights = weights(sortvec);

  end

  r_size = length ( weights );

  return
end


function fem1d_pack_test01 ( )

%*****************************************************************************80
%
%% FEM1D_PACK_TEST01 tests LOCAL_BASIS_1D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 March 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    None
%
  node_num = 4;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'FEM1D_PACK_TEST01:\n' );
  fprintf ( 1, '  LOCAL_BASIS_1D evaluates the local basis functions\n' );
  fprintf ( 1, '  for a 1D element.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Test that the basis functions, evaluated at the nodes,\n' );
  fprintf ( 1, '  form the identity matrix.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of nodes = %d\n', node_num );

  node_x(1:node_num) = [ 1.0, 2.0, 4.0, 4.5 ];

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Node coordinates:\n' );
  fprintf ( 1, '\n' );
  for j = 1 : node_num
    fprintf ( 1, '  %8d  %7.3f\n', j, node_x(j) );
  end

  for j = 1 : node_num
    x = node_x(j);
    phi_matrix(1:node_num,j) = local_basis_1d ( node_num, node_x, x );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  A(I,J) = PHI(I) at node (J):\n' );
  fprintf ( 1, '\n' );
  for i = 1 : node_num
    for j = 1 : node_num
      fprintf ( 1, '  %7.3f', phi_matrix(i,j) );
    end
    fprintf ( 1, '\n' );
  end

  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The PHI functions should sum to 1 at random X values:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '       X        Sum ( PHI(:)(X) )\n' );
  fprintf ( 1, '\n' );

  for j = 1 : 5
    [ x, seed ] = r8_uniform_ab ( 1.0, 4.5, seed );
    phi = local_basis_1d ( node_num, node_x, x );
    fprintf ( 1, '  %14.6g  %14.6g\n', x, sum ( phi(1:node_num) ) );
  end

  return
end