function [z,dz,ymu,ys,fmu,fs,fpi] = acqNegGEI(xi,target,gpstruct,optimState,grad)
%ACQNEGEI Acquisition function for (negative) generalized expected improvement (unsupported).

if nargin < 5 || isempty(grad); grad = 0; end

n = size(xi,1);

if grad == 1 && n > 1
    error('acqNegGEI:gradient', ...
        'Gradient of acquisition function is provided only at one test point XI (row vector).');
end

if grad
    [ymu,ys2,fmu,fs2,hypw,dymu,dys2,dfmu,dfs2] = gppred(xi,gpstruct,'central');
else
    [ymu,ys2,fmu,fs2,hypw] = gppred(xi,gpstruct);
end
fs = sqrt(fs2);
ys = sqrt(ys2);

% Probability of improvement
gammaz = real((target - fmu)./fs);
fpi = 0.5*erfc(-gammaz/sqrt(2));

% Squared expected improvement
z1 = -(fs.*(gammaz.*fpi + exp(-0.5*(gammaz.^2))/sqrt(2*pi))).^2;

% Expected squared improvement
z2 = -fs.^2.*((gammaz.^2+1).*fpi + gammaz.*exp(-0.5*(gammaz.^2))/sqrt(2*pi));

% gamma(1,1,:) = linspace(0,1,11);
gamma(1,1,:) = 0.5;
z = bsxfun(@times,gamma,z1) + bsxfun(@times,1-gamma,z2);

z = sum(bsxfun(@times,hypw(~isnan(hypw)),z(~isnan(hypw),:,:)),1);

[zi,idx] = min(z,[],3);
[~,idx2] = min(zi,[],2);
idx = idx(idx2);
z = z(:,:,idx);

%try
%    z = sum(bsxfun(@times,hypw(~isnan(hypw)),z(~isnan(hypw),:)),1);
%catch
%    z = Inf(1,n);
%    dz = NaN(n,size(xi,2));
%    return;
%end

if grad
    % Gradient of probability of improvement
    dfs = 0.5*dfs2./fs;
    dgammaz = -(dfmu.*fs + (target - fmu).*dfs)./fs2;
    dfpi = -0.5*dgammaz/sqrt(2)*(-2*exp(-gammaz.^2/2)/sqrt(pi));

    % Gradient of expected improvement
    %dz = -(dfs.*gammaz.*fpi + dgammaz.*fs.*fpi + dfpi.*gammaz.*fs) ...
    %    + (fs.*gammaz.*dgammaz - dfs).*exp(-0.5*gammaz.^2)/sqrt(2*pi);
    %dz = sum(bsxfun(@times,hypw,dz(~isnan(hypw),:)),1);
else
    dz = NaN(n,size(xi,2));     % Gradient not estimated
end

end


function errors = test_wiener()

errors = 0;
gsp_reset_seed
N = 20;
M = 3;
sigma = 0.3;

G = gsp_sensor(N);
G = gsp_compute_fourier_basis(G);
g = @(x) sin(x);
psd = @(x) g(x).^2;

Mask = rand(N,1)>0.5;

x = gsp_filter_analysis(G,g,randn(N,M));

Mop =@(x) bsxfun(@times,Mask,x);

y1 = Mop(x);

y2 = Mop(x+sigma*randn(N,M));
clear paramopt
paramopt.maxit = 1000;
paramopt.tol = 1e-12;
sol11 = gsp_wiener_inpainting(G,y1,Mask,psd,0,paramopt);
sol12 = gsp_wiener_inpainting_exact(G,y1,Mask,psd,0);

if norm(sol11-sol12,'fro')/norm(sol11,'fro') > 1e-10
    errors = errors + 1;
    norm(sol11-sol12,'fro')/norm(sol11,'fro')
    warning('Test Wiener opt 1 - ERROR');
else
    disp('Test Wiener opt 1 - OK');
end

sol21 = gsp_wiener_inpainting(G,y2,Mask,psd,sigma.^2,paramopt);
sol22 = gsp_wiener_inpainting_exact(G,y2,Mask,psd,sigma.^2);

if norm(sol21-sol22,'fro')/norm(sol22,'fro')> 1e-10
    errors = errors + 1;
    norm(sol21-sol22,'fro')/norm(sol22,'fro')
    warning('Test Wiener opt 2 - ERROR');
else
    disp('Test Wiener opt 2 - OK');
end

%%

g = gsp_design_expwin(G,0.2);
psd = @(x) g(x).^2;

Mask = rand(N,1)>0.5;
x = gsp_filter_analysis(G,g,randn(N,M));

Mop =@(x) bsxfun(@times,Mask,x);

y1 = Mop(x);

y2 = Mop(x+sigma*randn(N,M));

paramopt.tol = 1e-12;

%%
paramopt.maxit = 1000;
paramopt.gamma = 0.01;
sol11 = gsp_wiener_inpainting(G,y1,Mask,psd,0,paramopt);
sol12 = gsp_wiener_inpainting_exact(G,y1,Mask,psd,0);

if norm(sol11-sol12,'fro')/norm(sol11,'fro') > 1e-3
    errors = errors + 1;
    norm(sol11-sol12,'fro')/norm(sol11,'fro')
    warning('Test Wiener opt 3 - ERROR');
else
    disp('Test Wiener opt 3 - OK');
end

if norm(x-sol12,'fro')/norm(x,'fro') > 1e-10
    errors = errors + 1;
    norm(x-sol12,'fro')/norm(x,'fro')
    warning('Test Wiener opt 4 - ERROR');
else
    disp('Test Wiener opt 4 - OK');
end

%%
% figure(1)
% subplot(121)
% gsp_plot_signal_spectral(G,gsp_gft(G,x(:,1)))
% subplot(122)
% gsp_plot_signal_spectral(G,gsp_gft(G,sol12(:,1)))

%%
paramopt.gamma = 0.1;

paramopt.maxit = 1000;

sol21 = gsp_wiener_inpainting(G,y2,Mask,psd,sigma.^2,paramopt);
sol22 = gsp_wiener_inpainting_exact(G,y2,Mask,psd,sigma.^2);

if norm(sol21-sol22,'fro')/norm(sol22,'fro')> 1e-10
    errors = errors + 1;
    norm(sol21-sol22,'fro')/norm(sol22,'fro')
    warning('Test Wiener opt 5 - ERROR');
else
    disp('Test Wiener opt 5 - OK');
end

end


%  Figure 10.73      Feedback Control of Dynamic Systems, 6e
%                        Franklin, Powell, Emami
%
% Fig. 10.73
% Data for RTP Demo 3-3-99
% Data provided by Dr. Gwen van der Linden
% Data is from System Identification Studies
InputFlux=[3.460064464376177e-1 1.177299050104922e-1 2.838023866104041e-2;
   3.880303397347619e-11 8.024902450324316e-2 1.807231516460469e-2;
   8.004191616976514e-9 2.721604310757543e-3 3.171348842079633e-2];
M_inv=diag([1.000040130716728 5.557442686788876 13.63821806414694]);
Radiation=[5.47621193859299e-2 -8.570695054070524e-3 -8.296135532988507e-4...
      -4.536181077856052e-2;
   -8.570695054070524e-3 8.570946319867835e-3 -1.621311365067015e-7...
      -8.913466080455817e-8;
   -8.296135532988507e-4 -1.621311365067015e-7 8.299854517643017e-4...
      -2.097673289443245e-7];
Conduction=[3.559939609150268e-7 -1.113667477845243e-7 -1.976161155515125e-7...
      -4.701109757899004e-8;
   -1.113667477845243e-7 1.160207476868843e-2 -2.502736022145532e-3...
      -9.099227379795117e-3;
   -1.976161155515125e-7 -2.502736022145532e-3 6.37364815665867e-3...
      -3.870714518397587e-3];
ScaleTemp=diag([0.01 0.01 0.01 0.01]);

clf;
%
sim('fig10_72')
%plot(tout,r,'-');
%hold on;
%plot(tout,y,'--');
%xlabel('Time (sec)');
%ylabel('Temperature (K)');
%hold off;
%pause;
ii=240:876;
plot(tout(ii),r(ii),'-',tout(ii),y(ii,2),'--');
legend('r','y');
grid on;
xlabel('Time (sec)');
ylabel('Temperature (K)');
title('Fig. 10.73(a) Temperature tracking response');
pause;
hold off;
ii=240:876;
plot(tout(ii),u(ii),'-');
xlabel('Time (sec)');
ylabel('Lamp voltage (V)');
grid on;
legend('u');
title('Fig. 10.73(b) Control effort');
%grid
nicegrid


%% BVAR tutorial: Minnesota prior and heteroskedasticity weights
% Author:   Filippo Ferroni and  Fabio Canova
% Date:     09/14/2020, revised 16/12/2020

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 1) baseline  forecasting
% 2) forecasting  with  heteroskedastic  weights
% 3) forecasting  with  optimal  heteroskedastic  weights
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

close all; clc; clear;

addpath ../../cmintools/
addpath ../../bvartools/

%% read the data
if exist('DataCovid.mat','file') == 2
    load DataCovid
else
    url = 'https://fred.stlouisfed.org/';
    c = fred(url);
    series    = {'PAYEMS','UNRATE','PCE','INDPRO','CPIAUCSL','PCEPILFE'};
    startdate = datenum('12/01/1988', 'mm/dd/yyyy');
    enddate   = datenum('07/01/2020', 'mm/dd/yyyy');
    time      = 1988+11/12 : 1/12 : 2020+6/12;

    y=zeros(380,size(series,2)); y0=y; y1=y;
    for kk=1:size(series,2)
        tmp        = fetch(c,series{kk},startdate,enddate);
        if strcmp('UNRATE',series{kk})==1
            y(:,kk)    = tmp.Data(:,2);
            y0(:,kk)    = tmp.Data(:,2);
            y1(:,kk)  =(y0(:,kk));
        else
            y0(:,kk)    = tmp.Data(:,2);
            y1(:,kk)    = log(y0(:,kk));
            % rebase the log variable, transform to a index (2020m1=100)
            y(:,kk)= y1(:,kk) * 100 / y1(time==2019,kk);
        end
    end
    save DataCovid y y1 y0 time
end

% y, y0, y1 are  in DataCovid.

%% Forecast post COVID-19 (July 2020 - last insample data). Minnesota
lags                 = 13;
options.priors.name  = 'Minnesota';
options.fhor         = 24;
options.K            = 1000;
bvar0                = bvar_(y,lags,options);

options.nplots       = [2 3];
% start of the forecast plot - default first date in-sample data
options.time_start   = 2019;
% Titles for subplot
options.varnames = {'PAYMS','UNRATE','PCE','INDPRO','CPIAUCSL','PCEPILFE'};
% multiple credible set - default .68
options.conf_sig_2   = 0.9;
plot_frcst_(bvar0.forecasts.with_shocks,y,time,options)
pause;

% Forecast post COVID-19 (July 2020 - last insample data).
% Minnesota + Heteroskedasticity weights

lags = 13;
tstar  = find(time==2020) + 2; % pick march 2020
% scale the variables by factor <1 in the periods that
% characterize the COVID-19 induced recession
st                   = ones(size(y,1),1);
st(tstar:tstar+2 ,:) = [10 10 10]; % March, April, May
st(1:lags)           = [];

options.heterosked_weights = st;
options.priors.name  = 'Minnesota';
options.fhor         = 24;
bvar1                = bvar_(y,lags,options);

% adding  forecast  without  weights
options.add_frcst = [y; mean(bvar0.forecasts.no_shocks,3)];
plot_frcst_(bvar1.forecasts.with_shocks,y,time,options)
pause;

% Forecast  with  optimal  weights

hyperpara(1)    = 3;		  % tau
hyperpara(2)    = 0.5;		  % decay
hyperpara(3)    = 1;		  % lambda
hyperpara(4)    = 1;		  % mu
hyperpara(5)    = 2;		  % omega
hyperpara(6)    = 2; % s0: scale march 2020
hyperpara(7)    = 2; % s1: scale april 2020
hyperpara(8)    = 2; % s2: scale may   2020
% setting the options
options.index_est          = [1 6:8]; % hyper-parameter over which maximize
options.objective_function = 'bvar_opt_heterosked';
options.tstar              = find(time==2020) + 2; %march 2020
[postmode,logmlike,HH] = bvar_max_hyper(hyperpara,y,lags,options);

disp('weights for 2020:3, 2020:4, 2020:5')
disp(postmode(2:4))
heterosked_esse             = postmode(2:end); % s0, s1, s2
esse                        = ones(size(y,1),1);
esse(options.tstar : options.tstar+2 ,:)   = heterosked_esse;
esse(1:lags)                = [];

options.heterosked_weights = esse;
options.minn_prior_tau = postmode(1);
bvar2                 = bvar_(y,lags,options);
plot_frcst_(bvar2.forecasts.with_shocks,y,time,options)


function y = idualtree3D(w, J, Fsf, sf)

% Inverse 3D Dual-Tree Discrete Wavelet Transform
%
% USAGE:
%   y = idualtree3D(w, J, Fsf, sf)
% INPUT:
%   w - wavelet coefficients
%   J - number of stages
%   Fsf - synthesis filter for the last stage
%   sf - synthesis filters for the preceeding stages
% OUTPUT:
%   y - output arry
% See dualtree3D
%
% WAVELET SOFTWARE AT POLYTECHNIC UNIVERSITY, BROOKLYN, NY
% http://taco.poly.edu/WaveletSoftware/

for k = 1:J
    for m = 1:7
        [w{k}{1}{m} w{k}{2}{m} w{k}{3}{m} w{k}{4}{m}] = ...
            pm4inv(w{k}{1}{m}, w{k}{2}{m}, w{k}{3}{m}, w{k}{4}{m});
    end
end

M = [
    1 1 1
    2 2 1
    2 1 2
    1 2 2
];

% initialize output array
y = zeros(2^J * size(w{J}{1}{1}));

for i = 1:4
    f1 = M(i,1);
    f2 = M(i,2);
    f3 = M(i,3);
    yi = w{J+1}{i};
    for k = J:-1:2
        yi = sfb3D(yi, w{k}{i}, sf{f1}, sf{f2}, sf{f3});
    end
    yi = sfb3D(yi, w{1}{i}, Fsf{f1}, Fsf{f2}, Fsf{f3});
    y = y + yi;
end

% normalization
y = y/2;


%DEMO_MULTICLASS  Classification problem demonstration for 3 classes
%                 using Gaussian process prior
%
%  Description
%    The data used in the demonstration program is the same used by
%    Radford M. Neal in his three-way classification example in
%    Software for Flexible Bayesian Modeling
%    (http://www.cs.toronto.edu/~radford/fbm.software.html) The
%    data consists of 1000 4-D vectors which are classified into
%    three classes. The data is generated by drawing the components
%    of vector, x1, x2, x3 and x4, uniformly form (0,1). The class
%    of each vector is selected according to the first two
%    components of the vector, x_1 and x_2. After this a Gaussian
%    noise with standard deviation of 0.1 has been added to every
%    component of the vector. Because there are two irrelevant
%    components in the input vector a prior with ARD should be of
%    help.
%
%    The data is divided into two parts, trainig set of 400 units
%    and test set of 600 units.
%
%    The latent values for N training points and C classes are
%    f=(f1_1,f2_1,...,fN_1,f1_2,f2_2,...,fN_2,...,f1_C,f2_C,...,fN_C)^T,
%    and are given a zero mean Gaussian process prior
%
%      f ~ N(0, K),
%
%    where K is a block diagonal covariance matrix with blocks
%    K_1,...,K_C whose elements are given by K_ij = k(x_i, x_j |
%    th). The function k(x_i, x_j | th) is covariance function and
%    th its parameters.
%
%    In this demo we approximate the posterior distribution with
%    Laplace approximation.
%

% Copyright (c) 2010 Jaakko Riihim�ki, Jarno Vanhatalo, 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.

% Load the data
S = which('demo_multiclass');
L = strrep(S,'demo_multiclass.m','demodata/cdata.txt');
x=load(L);
y=repmat(0,size(x,1),3);
y(x(:,5)==0,1) = 1;
y(x(:,5)==1,2) = 1;
y(x(:,5)==2,3) = 1;
x(:,end)=[];

% Divide the data into training and test parts.
xt = x(401:end,:);
x=x(1:400,:);
yt=y(401:end,:);
y=y(1:400,:);

[n, nin] = size(x);

% Create covariance functions
gpcf1 = gpcf_sexp('lengthScale', ones(1,nin), 'magnSigma2', 1);
% Set the prior for the parameters of covariance functions
pl = prior_t('s2',10,'nu',10);
pm = prior_sqrtt('s2',10,'nu',10);
gpcf1 = gpcf_sexp(gpcf1, 'lengthScale_prior', pl,'magnSigma2_prior', pm);

% Create the GP structure
gp = gp_set('lik', lik_softmax, 'cf', gpcf1, 'jitterSigma2', 1e-2);

% ------- Laplace approximation --------
fprintf(['Softmax model with Laplace integration over the latent\n' ...
         'values and MAP estimate for the parameters\n'])

% Set the approximate inference method
gp = gp_set(gp, 'latent_method', 'Laplace');

% We could also use own covariance function for each output the following
% way
% gp2 = gp_set('lik', lik_softmax, 'cf', {gpcf1 gpcf1 gpcf1}, 'jitterSigma2', 1e-2);
% gp2 = gp_set(gp2, 'latent_method', 'Laplace');
% gp2.comp_cf = {1 2 3};
% [Eft2, Varft2, ~, ~, pyt2] = gp_pred(gp2, x, y, xt, 'yt', ones(size(yt)));

% Set the options for the optimization
opt=optimset('TolFun',1e-3,'TolX',1e-3,'Display','iter');
% Optimize with the BFGS quasi-Newton method
gp=gp_optim(gp,x,y,'opt',opt,'optimf',@fminlbfgs);

% make the prediction for test points
[Eft, Varft, lpyt] = gp_pred(gp, x, y, xt, 'yt', ones(size(yt)));
Eft = reshape(Eft, size(xt,1), size(yt,2));
lpyt = reshape(lpyt, size(xt,1), size(yt,2));

% calculate the percentage of misclassified points
tt = exp(lpyt)==repmat(max(exp(lpyt),[],2),1,size(exp(lpyt),2));
missed = (sum(sum(abs(tt-yt)))/2)/size(yt,1)

% grid for making prediction
xtg1 = meshgrid(linspace(min(x(:,1))-.1, max(x(:,1))+.1, 30));
xtg2 = meshgrid(linspace(min(x(:,2))-.1, max(x(:,2))+.1, 30))';
xtg=[xtg1(:) xtg2(:) repmat(mean(x(:,3:4)), size(xtg1(:),1),1)];

[Eft, Covft, pg] = gp_pred(gp, x, y, xtg, 'yt', ones(size(xtg,1),3));
Eft = reshape(Eft, size(xtg,1), 3);
pg=reshape(pg,size(xtg,1),3);

% plot the train data o=0, x=1
figure, set(gcf, 'color', 'w'), hold on
plot(x(y(:,1)==1,1),x(y(:,1)==1,2),'ro', 'linewidth', 2);
plot(x(y(:,2)==1,1),x(y(:,2)==1,2),'x', 'linewidth', 2);
plot(x(y(:,3)==1,1),x(y(:,3)==1,2),'kd', 'linewidth', 2);
axis([-0.4 1.4 -0.4 1.4])
contour(xtg1, xtg2, reshape(exp(pg(:,1)),30,30),'r', 'linewidth', 2)
contour(xtg1, xtg2, reshape(exp(pg(:,2)),30,30),'b', 'linewidth', 2)
contour(xtg1, xtg2, reshape(exp(pg(:,3)),30,30),'k', 'linewidth', 2)

% MCMC approach

% Set the approximate inference method
% Note that MCMC for latent values requires often more jitter
lat = gp_pred(gp, x, y, x);
gp = gp_set(gp, 'latent_method', 'MCMC', 'jitterSigma2', 1e-4);
gp = gp_set(gp, 'latent_opt', struct('method',@scaled_mh));
gp.latentValues = lat(:);

gp_e(gp_pak(gp), gp, x,y)
gp_g(gp_pak(gp), gp, x,y)
gradcheck(randn(size(gp_pak(gp))), @gp_e, @gp_g, gp, x, y);

% Set the parameters for MCMC...
hmc_opt.steps=10;
hmc_opt.stepadj=0.001;
hmc_opt.nsamples=1;
latent_opt.display=0;
latent_opt.repeat = 20;
latent_opt.sample_latent_scale = 0.05;
hmc2('state', sum(100*clock))

% Sample
[r,g,opt]=gp_mc(gp, x, y, 'hmc_opt', hmc_opt, 'latent_opt', latent_opt, 'nsamples', 1, 'repeat', 15);

% re-set some of the sampling options
hmc_opt.repeat=1;
hmc_opt.steps=4;
hmc_opt.stepadj=0.02;
latent_opt.repeat = 5;
hmc2('state', sum(100*clock));

% Sample
[rgp,g,opt]=gp_mc(gp, x, y, 'nsamples', 400, 'hmc_opt', hmc_opt, 'latent_opt', latent_opt, 'record', r);
% Remove burn-in
rgp=thin(rgp,102);

% Make predictions
[Efs_mc, Varfs_mc, pgs_mc] = gpmc_preds(rgp, x, y, xtg, 'yt', ones(size(xtg,1),3));

Ef_mc = reshape(mean(Efs_mc,2),900,3);
pg_mc = reshape(mean(exp(pgs_mc),2),900,3);

figure, set(gcf, 'color', 'w'), hold on
plot(x(y(:,1)==1,1),x(y(:,1)==1,2),'ro', 'linewidth', 2);
plot(x(y(:,2)==1,1),x(y(:,2)==1,2),'x', 'linewidth', 2);
plot(x(y(:,3)==1,1),x(y(:,3)==1,2),'kd', 'linewidth', 2);
axis([-0.4 1.4 -0.4 1.4])
contour(xtg1, xtg2, reshape(pg_mc(:,1),30,30),'r', 'linewidth', 2)
contour(xtg1, xtg2, reshape(pg_mc(:,2),30,30),'b', 'linewidth', 2)
contour(xtg1, xtg2, reshape(pg_mc(:,3),30,30),'k', 'linewidth', 2)

% With scaled HMCS

gp2 = gp_set(gp, 'latent_opt', struct('method',@scaled_hmc));
gp2.latentValues = lat(:);

% Set the parameters for MCMC...
hmc_opt.steps=10;
hmc_opt.stepadj=0.001;
hmc_opt.nsamples=1;

% latent opt
latent_opt.nsamples=1;
latent_opt.nomit=0;
latent_opt.persistence=0;
latent_opt.repeat=20;
latent_opt.steps=20;
latent_opt.stepadj=0.15;
latent_opt.window=5;
hmc2('state', sum(100*clock))

% Here we make an initialization with
% slow sampling parameters

[rgp2,gp2,opt]=gp_mc(gp2, x, y, 'hmc_opt', hmc_opt, 'latent_opt', latent_opt, 'nsamples', 1, 'repeat', 15);

% re-set some of the sampling options
hmc_opt.repeat=1;
hmc_opt.steps=4;
hmc_opt.stepadj=0.02;
latent_opt.repeat = 20;
hmc2('state', sum(100*clock));

% Sample
[rgp2,g2,opt2]=gp_mc(gp2, x, y, 'nsamples', 200, 'hmc_opt', hmc_opt, 'latent_opt', latent_opt, 'record', rgp2);
% Remove burn-in
rgp2=thin(rgp2,102);

% Make predictions
[Efs_mc, Varfs_mc, pgs_mc] = gpmc_preds(rgp2, x, y, xtg, 'yt', ones(size(xtg,1),3));

Ef_mc2 = reshape(mean(Efs_mc,2),900,3);
pg_mc2 = reshape(mean(exp(pgs_mc),2),900,3);

figure, set(gcf, 'color', 'w'), hold on
plot(x(y(:,1)==1,1),x(y(:,1)==1,2),'ro', 'linewidth', 2);
plot(x(y(:,2)==1,1),x(y(:,2)==1,2),'x', 'linewidth', 2);
plot(x(y(:,3)==1,1),x(y(:,3)==1,2),'kd', 'linewidth', 2);
axis([-0.4 1.4 -0.4 1.4])
contour(xtg1, xtg2, reshape(pg_mc2(:,1),30,30),'r', 'linewidth', 2)
contour(xtg1, xtg2, reshape(pg_mc2(:,2),30,30),'b', 'linewidth', 2)
contour(xtg1, xtg2, reshape(pg_mc2(:,3),30,30),'k', 'linewidth', 2)


function [acc, pred] = CrossValidateKNN(y, X, tCL, k, knn_size);
% [acc, pred] = CrossValidateKNN(y, X, tCL, k, knn_size);
%
% Cross-validation for evaluating the k-nearest neighbor classifier with
% a learned metric.  Performs k-fold cross validation, training on the
% training fold and evaluating on the test fold
%
% y: (n x 1) true labels
%
% X: (n x m) data matrix
%
% tCL: Metric learning algorithm that takes in true labels as first
% argument, and data as a second
%
% k: Number of cross-validated folds
%
% knn_size: size of nearest neighbor window
%
% Returns
% acc: cross-validated accuracy
% pred: predictions on test set for each row in X

[n,m] = size(X);
if (n ~= length(y)),
   disp('ERROR: num rows of X must equal length of y');
   return;
end

%permute the rows of X and y
rp = randperm(n);
y = y(rp);
X = X(rp, :);

pred = zeros(n,1);
for (i=1:k),
   test_start = ceil(n/k * (i-1)) + 1;
   test_end = ceil(n/k * i);

   yt = [];
   Xt = zeros(0, m);
   if (i > 1);
       yt = y(1:test_start-1);
       Xt = X(1:test_start-1,:);
   end
   if (i < k),
       yt = [yt; y(test_end+1:length(y))];
       Xt = [Xt; X(test_end+1:length(y), :)];
   end

   nt = length(yt);
   yt = yt(1:nt);
   Xt = Xt(1:nt, :);

   %train model
   M = feval(tCL, yt, Xt);

   %evaluate model
   XT = X(test_start:test_end, :);
   yT =  y(test_start:test_end);
   pred(test_start:test_end) = KNN(yt, Xt, sqrtm(M), knn_size, XT);
end
acc = sum(pred==y)/n;


function drawIdpLin(MapFig,Lmk,color,MapOpt)

% DRAWIDPLIN  Draw inverse-depth line landmark in MapFig.

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

global Map

% get the lmk from the Map
r   = Lmk.state.r;       % range in Map
idl = Map.x(r);          % mean
IDL = Map.P(r,r);        % covariances matrix
t   = [Lmk.par.endp.t]'; % abscissas of endpoints, t = [t1;t2]

% extract two endpoints - means and covariance
[e1,e2,E1_idl,E2_idl] = idpLinEndpoints(idl,t(1),t(2)); % means and Jacobians
E1 = E1_idl*IDL*E1_idl'; % covariances
E2 = E2_idl*IDL*E2_idl';

% draw the mean:
drawSeg(MapFig.Lmk(Lmk.lmk).mean,[e1;e2],color.mean)

% draw the covariance ellipses
if MapOpt.showEllip
    drawEllipse(MapFig.Lmk(Lmk.lmk).ellipse(1), e1, E1, color.ellip)
    drawEllipse(MapFig.Lmk(Lmk.lmk).ellipse(2), e2, E2, color.ellip)
end

% draw the label
if MapOpt.showLmkId
    e = e2-e1;
    n = null([e e e]);
    n = n(:,2)*sign(n(3,2)); % Inverse depth line's normal vector
    posOffset = 0.2*n;     % label orthogonally out of the line.
    drawLabel(MapFig.Lmk(Lmk.lmk).label,0.5*(e1+e2) + posOffset,num2str(Lmk.id))
end

% ========== 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 S = globMatrixNed3DMass(fun,mesh,fem1,fem2)

%% USAGE: generate stiffness global matrix on a 3D mesh
%
% INPUTS:
% fun --- coefficient function
% mesh --- a struct data contains very rich mesh information.
% fem1 --- global DoF for test function space
% fem2 --- global DoF for trial function space
%
% OUTPUTS:
% [IN JN XN] --- triplets of the sparse matrix from regular elements.
% [II JI XI] --- triplets of the sparse matrix from interface elements.

% Last Modified: 08/07/2020 by Xu Zhang

%% 0. Initializaiton
feEvalBas1 = @EvalNed1Bas3D;
feEvalBas2 = @EvalNed1Bas3D;

dof1 = fem1.ldof; dof2 = fem2.ldof; nloc = dof1*dof2;
nt = length(mesh.t);
A = fem1.area; gx = fem1.gx; gy = fem1.gy; gz = fem1.gz; gw = fem1.gw;
X = zeros(nloc*nt, 1);

coef = feval(fun,gx,gy,gz);
Ibasx = cell(dof1,1); Ibasy = cell(dof1,1); Ibasz = cell(dof1,1);
Jbasx = cell(dof2,1); Jbasy = cell(dof2,1); Jbasz = cell(dof2,1);
for i = 1:dof1
    Ibasx{i} = feEvalBas1(fem1.bas, ':', gx, gy, gz, i, 0, 1).*fem1.t_e_orit(:,i);
    Ibasy{i} = feEvalBas1(fem1.bas, ':', gx, gy, gz, i, 0, 2).*fem1.t_e_orit(:,i);
    Ibasz{i} = feEvalBas1(fem1.bas, ':', gx, gy, gz, i, 0, 3).*fem1.t_e_orit(:,i);
end
for j = 1:dof2
    Jbasx{j} = feEvalBas2(fem2.bas, ':', gx, gy, gz, j, 0, 1).*fem2.t_e_orit(:,j);
    Jbasy{j} = feEvalBas2(fem2.bas, ':', gx, gy, gz, j, 0, 2).*fem2.t_e_orit(:,j);
    Jbasz{j} = feEvalBas2(fem2.bas, ':', gx, gy, gz, j, 0, 3).*fem2.t_e_orit(:,j);
end

I = reshape(repmat(fem1.g2ldof,6,1),nloc*nt,1);
J = repmat(reshape(fem2.g2ldof,dof2*nt,1),6,1);
ind = 0;
for i = 1:dof1
    for j = 1:dof2
        X(ind+1:ind+nt) = A.*(sum(((Ibasx{i}.*(coef.*Jbasx{j})).*gw'),2) + ...
            sum(((Ibasy{i}.*(coef.*Jbasy{j})).*gw'),2) + ...
            sum(((Ibasz{i}.*(coef.*Jbasz{j})).*gw'),2));
        ind = ind + nt;
    end
end
ID = find(X~=0);
S = sparse(I(ID),J(ID),X(ID),size(fem1.gdof,1),size(fem2.gdof,1));


% test_all_fbp

% todo:
% cuboid_im test
% cuboid_proj test

list = {
'cbct_back test'
'ct_geom test'
'image_geom test'
'sino_geom test'
'cylinder_proj test'
'df_example1'
'ellipse_im test'
'ellipse_sino test'
'ellipsoid_proj test'
'ellipsoid_im test'
'fbp_fan_arc_example'
'fbp_fan_arc_point'
'fbp_fan_flat_example'
'fbp_ramp test'
'fbp2_sino_filter test'
'fbp2_example'
'feldkamp_example'
'jaszczak1 test'
'ir_radon_zwart_powell test'
'rebin_helix test' % helix_example
'rect_im test'
'rect_sino test'
%'sphere_proj test'
};

im nan-fail
run_mfile_local(list)


% Proces MSE results into MSE curves: average out and/or limit to a
% subset of temporal indices.
%
% Input:
% - results: a configresults cell produced by kafbox_profiler.
% - inds: array of indices for which to calculate output
%
% Output:
% - MSE_avg_setups: cell containing averaged out MSE curves, structure
% corresponds to "results" cell structure.
%
% This file is part of the Kernel Adaptive Filtering Toolbox for Matlab.
% https://github.com/steven2358/kafbox/

function MSE_avg_setups = kafbox_profiler_msecurves(results,inds)

num_setups = length(results);
MSE_avg_setups = cell(num_setups,1);

for setup_ind = 1:num_setups
    setup_results = results{setup_ind};

    num_configs = length(setup_results);
    MSE_avg_configs = cell(num_configs,1);

    for config_ind = 1:num_configs
        config_results = setup_results{config_ind};

        if isfield(config_results{1},'NMSE')
            N = length(config_results{1}.NMSE); % temporary
        else
            N = length(config_results{1}.MSE);
        end
        MSE = zeros(N,1);

        num_sim = length(config_results);
        for sim_ind = 1:num_sim
            simresults = config_results{sim_ind};

            if isfield(simresults,'NMSE') % temporary
                simresults.MSE = simresults.NMSE;
            end
            if nargin<2
                inds = 1:length(simresults.MSE);
            end

            mm = min(length(simresults.MSE(inds)),N);
            MSE = MSE(1:mm) + simresults.MSE(inds(1:mm))/num_sim;
        end
        MSE_avg_configs{config_ind} = MSE;
    end
    MSE_avg_setups{setup_ind} = MSE_avg_configs;
end


function p06_limit_test ( option )

%*****************************************************************************80
%
%% P06_LIMIT_TEST seeks limit points for problem 6.
%
%  Discussion:
%
%    We want to find points X such that TAN(7) = 0.
%
%    The number of limit points that may be expected depends on the option:
%
%    There are five options, which vary in the value they fix the
%    elevator value X(6):
%
%      Option   Elevator Value    Limit Points
%
%       1        -0.050              1
%       2        -0.008              3
%       3         0.0                2
%       4         0.05               1
%       5         0.1                1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 October 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Raman Mehra, William Kessel, James Carroll,
%    Global stability and contral analysis of aircraft at high angles of attack,
%    Technical Report CR-215-248-1, -2, -3,
%    Office of Naval Research, June 1977.
%
%    Rami Melhem, Werner Rheinboldt,
%    A Comparison of Methods for Determining Turning Points of Nonlinear Equations,
%    Computing,
%    Volume 29, Number 3, September 1982, pages 201-226.
%
%    Albert Schy, Margery Hannah,
%    Prediction of Jump Phenomena in Roll-coupled Maneuvers of Airplanes,
%    Journal of Aircraft,
%    Volume 14, Number 4, 1977,  pages 375-382.
%
%    John Young, Albert Schy, Katherine Johnson,,
%    Prediction of Jump Phenomena in Aircraft Maneuvers, Including
%    Nonlinear Aerodynamic Effects,
%    Journal of Guidance and Control,
%    Volume 1, Number 1, 1978, pages 26-31.
%
%  Parameters:
%
%    Input, integer OPTION, the option index.
%    1, X(6) fixed at -0.050, X(7) free, X(8) fixed at 0.0;
%    2, X(6) fixed at -0.008, X(7) free, X(8) fixed at 0.0;
%    3, X(6) fixed at  0.000, X(7) free, X(8) fixed at 0.0;
%    4, X(6) fixed at  0.050, X(7) free, X(8) fixed at 0.0;
%    5, X(6) fixed at  0.100, X(7) free, X(8) fixed at 0.0.
%
  problem = 6;
  lim = 7;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'P06_LIMIT_TEST\n' );
  fprintf ( 1, '  Compute a series of solutions for problem 6.\n' );
  fprintf ( 1, '  We are trying to find limit points X such that\n' );
  fprintf ( 1, '  TAN(%d) = 0.\n', lim );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The option chosen is %d\n', option );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   #   Tan(7)     X1       X2       X3       X4       X5       X6       X7      X8\n' );
  fprintf ( 1, '                Roll     Pitch    Yaw      Attack   Sideslip Elevator Aileron Rudder\n' );
  fprintf ( 1, '\n' );
%
%  Get problem size.
%
  nvar = p00_nvar ( problem, option );

  lim_num = 0;
%
%  Get starting point.
%
  x2 = p00_start ( problem, option, nvar );
%
%  Get the tangent vector.
%
  tan2 = p00_tan ( problem, option, nvar, x2 );
%
%  For correction of initial point, use variable index 7.
%
  par_index = 7;
%
%  Force F(X) = 0.
%
  step = -1;
  fprintf ( 1, '  %2d %8.1e %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f\n', ...
    step, tan2(lim), x2(1:nvar) );

  [ x2, status ] = p00_newton ( problem, option, nvar, x2, par_index );

  if ( status < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Newton iteration failed on starting point.\n' );
    return
  end

  tan2 = p00_tan ( problem, option, nvar, x2 );

  step = 0;
  fprintf ( 1, '  %2d %8.1e %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f\n', ...
    step, tan2(lim), x2(1:nvar) );
%
%  Get stepsize.
%
  [ h, hmin, hmax ] = p00_stepsize ( problem, option );
%
%  LOOP:
%
  step_max = 30;

  for step = 1 : step_max
%
%  Save old data as X1, TAN1.
%
    x1 = x2;
    tan1 = tan2;

    h_reduction = 0;
%
%  Use X1 + H * TAN1 as a starting estimate for Newton iteration.
%
    while ( 1 )

      if ( hmax < abs ( h ) )
        h = hmax * r8_sign ( h );
      end

      if ( abs ( h ) < hmin )
        h = hmin * r8_sign ( h );
      end

      x2 = x1 + h * tan1;

      par_index = 0;
      [ x2, status ] = p00_newton ( problem, option, nvar, x2, par_index );
%
%  If we didn't get it, can we try again?
%
      if ( status < 0 )

        if ( abs ( h ) <= hmin )
          fprintf ( 1, '\n' );
          fprintf ( 1, '  Cannot decrease stepsize any more.\n' );
          fprintf ( 1, '  Cannot complete the computation.\n' );
          return
        else
          h = h / 4.0;
          h_reduction = h_reduction + 1;
        end
%
%  We computed the point.
%  Should we change the stepsize?
%
      else

        if ( h_reduction == 0 )

          if ( status <= 1 )
            h = h * 4.0;
          elseif ( status <= 3 )
            h = h * 2.0;
          elseif ( 12 <= status )
            h = h / 4.0;
          elseif ( 8 <= status )
            h = h / 2.0;
          end

        end

        break;

      end

    end
%
%  Compute the tangent vector.
%
    tan2 = p00_tan ( problem, option, nvar, x2 );
%
%  Check for a limit point.
%
    if ( tan1(lim) * tan2(lim) <= 0.0 )
      [ x, tan, status ] = p00_limit ( problem, option, nvar, x1, tan1, x2, tan2, lim );
      fprintf ( 1, '   L %8.1e %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f\n', ...
        tan(lim), x(1:nvar) );
      lim_num = lim_num + 1;
    end

    fprintf ( 1, '  %2d %8.1e %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f\n', ...
      step, tan2(lim), x2(1:nvar) );

    if ( step == step_max )
      break
    end

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of limit points found was %d\n', lim_num );

  return
end


% Test trajectory primitives

mdl_puma560

[q,qd,qdd] = jtraj(qz, qr, 20)
q
qd
qdd

[q,qd,qdd] = jtraj(qz, qr, 20, 0.1*mat(ones(1,6)), -0.1*mat(ones(1,6)) )
q
qd
qdd

[q,qd,qdd] = jtraj(qz, qr, [0:0.2:10])
q

t1 = trotx(0.1) * transl(0.2, 0.3, 0.4)
t1
t2 = troty(-0.3) * transl(-0.2, -0.3, 0.6)
t2
ctraj(t1, t2, 5)
ctraj(t1, t2, [0:0.1:1])


function out = mirror_extend(in,bx,by)
% Pad array with mirror reflections of itself.
% bx and by specify the amount of padding to add.
% ***********************************************

[h,w] = size(in);

%First flip up and down
u = flipud(in(2:1+bx,:));
d = flipud(in(h-bx:h-1,:));

in2 = [u' in' d']';

%Next flip left and right
l = fliplr(in2(:, 2:1+by));
r = fliplr(in2(:,w-by:w-1));

%set the 'mirrored' image to out.
out = [l in2 r];

return

%test
% A = [1 2 3 4;5 6 7 8;9 10 11 12]
% B = mirror_extend(A,2,2)


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

%*****************************************************************************80
%
%% TRAN06_VALUES returns some values of the order 6 transportation function.
%
%  Discussion:
%
%    The function is defined by:
%
%      TRAN06(x) = Integral ( 0 <= t <= x ) t^6 * exp(t) / ( exp(t) - 1 )^2 dt
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Allan McLeod,
%    Algorithm 757, MISCFUN: A software package to compute uncommon
%    special functions,
%    ACM Transactions on Mathematical Software,
%    Volume 22, Number 3, September 1996, pages 288-301.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real X, the argument of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 20;

  fx_vec = [ ...
     0.56843405953641209574E-14, ...
     0.59601180165247401484E-08, ...
     0.60978424397580572815E-05, ...
     0.61578909866319494394E-02, ...
     0.18854360275680840514E+00, ...
     0.13319251347921659134E+01, ...
     0.50857202271697616755E+01, ...
     0.13729222365466557122E+02, ...
     0.29579592481641441292E+02, ...
     0.88600835706899853768E+02, ...
     0.10916037113373004909E+03, ...
     0.18224323749575359518E+03, ...
     0.23765383125586756031E+03, ...
     0.29543246745959381136E+03, ...
     0.50681244381280455592E+03, ...
     0.63878231134946125623E+03, ...
     0.72699203556994876111E+03, ...
     0.73230331643146851717E+03, ...
     0.73248692015882096369E+03, ...
     0.73248700462879996604E+03 ];

  x_vec = [ ...
       0.0019531250E+00, ...
       0.0312500000E+00, ...
       0.1250000000E+00, ...
       0.5000000000E+00, ...
       1.0000000000E+00, ...
       1.5000000000E+00, ...
       2.0000000000E+00, ...
       2.5000000000E+00, ...
       3.0000000000E+00, ...
       4.0000000000E+00, ...
       4.2500000000E+00, ...
       5.0000000000E+00, ...
       5.5000000000E+00, ...
       6.0000000000E+00, ...
       8.0000000000E+00, ...
      10.0000000000E+00, ...
      15.0000000000E+00, ...
      20.0000000000E+00, ...
      30.0000000000E+00, ...
      50.0000000000E+00 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

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

  return
end


function x = sample_fs(xf, grid_sz)

% Samples the Fourier series

sz = [size(xf,1) size(xf,2)];

if nargin < 2
    x = prod(sz) * cifft2(xf);
else
    if any(grid_sz < sz)
        error('The grid size must be larger than or equal to the signal size')
    end
    tot_pad = grid_sz - sz;
    pad_sz = ceil(tot_pad/2);
    xf_pad = padarray(xf, pad_sz);
    if any(mod(tot_pad,2) == 1)
        % Handle odd padding
        xf_pad = xf_pad(1:end-mod(tot_pad(1),2), 1:end-mod(tot_pad(2),2), :, :);
    end
    x = prod(grid_sz) * cifft2(xf_pad);
end


function [cum_ret, cumprod_ret, daily_ret, daily_portfolio] ...
    = grw_run(fid, data, N, sigma, tc, opts)
% This file is the run core for the EG strategy.
%
% function [cum_ret, cumprod_ret, daily_ret, daily_portfolio] ...
%           = eg_run(fid, data, tc, opts)
%
% cum_ret: cumulative wealth achived at the end of a period.
% cumprod_ret: cumulative wealth achieved till the end each period.
% daily_ret: daily return achieved by a strategy.
% daily_portfolio: daily portfolio, achieved by the strategy
%
% data: market sequence vectors
% fid: handle for write log file
% gamma: swtiching parameter
% tc: transaction fee rate
% opts: option parameter for behvaioral control
%
% Example: [cum_ret, cumprod_ret, daily_ret, daily_portfolio] ...
%          = grw_run(fid, data, 10, 0.00005, 0, opts);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This file is part of OLPS: http://OLPS.stevenhoi.org/
% Original authors: Bin LI, Steven C.H. Hoi
% Contributors:
% Change log:
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[n, m] = size(data);

% Variables for return, start with uniform weight
cum_ret = 1;
cumprod_ret = ones(n, 1);
daily_ret = ones(n, 1);
day_weight = ones(m, 1)/m;  %#ok<*NASGU>
day_weight_o = zeros(m, 1);
daily_portfolio = zeros(n, m);

% print file head
fprintf(fid, '-------------------------------------\n');
fprintf(fid, 'Parameters [N:%f, sigma:%f, tc:%f]\n', N, sigma, tc);
fprintf(fid, 'day\t Daily Return\t Total return\n');

fprintf(1, '-------------------------------------\n');
if(~opts.quiet_mode)
    fprintf(1, 'Parameters [N:%f, sigma:%f, tc:%f]\n', N, sigma, tc);
    fprintf(1, 'day\t Daily Return\t Total return\n');
end

% choose normal vectors
xi = normrnd(1/m, sigma^2, N, m);
gamma  = zeros(size(xi));

options = optimset('largescale','off', 'display', 'off');

for t = 1:1:n,
    % Calculate t's portfolio at the beginning of t-th trading day
    [day_weight] = grw_kernel(data(1, :), xi);

    % projection
    C = eye(m, m); d = day_weight;
    A = []; b=[];
    Aeq = ones(m, 1)'; beq = 1;
    lb = zeros(m, 1); ub = ones(m, 1);
    day_weight = lsqlin(C, d, A, b, Aeq, beq, lb, ub, day_weight, options);
    if or((day_weight < 0.0001), (day_weight'*ones(m, 1)>1.00001))
        fprintf(1, 't=%d, sum(day_weight)=%d, pause', t, day_weight'*ones(m, 1));
        pause;
    end

    % Normalize the constraint, always useless
    day_weight = day_weight./sum(day_weight);

    % Cal t's return and total return
    daily_ret(t, 1) = (data(t, :)*day_weight)*(1-tc/2*sum(abs(day_weight-day_weight_o)));
    cum_ret = cum_ret * daily_ret(t, 1);
    cumprod_ret(t, 1) = cum_ret;

    % Adjust weight(t, :) for the transaction cost issue
    day_weight_o = day_weight.*data(t, :)'/daily_ret(t, 1);

    % Monte Carlo update
    k = 1;
    while k <= N,   % Checks the size of our new sample
        u = rand(1); % Choose u from \mu[0, 1]
        j = randi([1, N]);  % Choose j randomly from {1, ... , N}

        if (u <= exp(xi(j, :))./sum(exp(xi(j, :)))*data(t, :)'/(max(data(t, :))) )  % accept-reject condition
            gamma(k, :) = xi(j, :);  k = k+1;   % Accepts xi if the above condition holds
        end
    end

    % Portfolio Update step
    h = normrnd(0, sigma^2, N, m);
    xi = gamma + h;

    % Debug information
    % Time consuming part, other way?
    fprintf(fid, '%d\t%f\t%f\n', t, daily_ret(t, 1), cumprod_ret(t, 1));
    if (~opts.quiet_mode)
        if (~mod(t, opts.display_interval)),
            fprintf(1, '%d\t%f\t%f\n', t, daily_ret(t, 1), cumprod_ret(t, 1));
        end
    end
end

% Debug Information
fprintf(fid, 'GRW(N:%d, sigma:%f, tc:%f), Final return: %f\n',...
    N, sigma, tc, cum_ret);
fprintf(fid, '-------------------------------------\n');
fprintf(1, 'GRW(N:%d, sigma:%f, tc:%f), Final return: %f\n',...
    N, sigma, tc, cum_ret);
fprintf(1, '-------------------------------------\n');

end


function [theta,e,hy,hp] = GLM_covComp(y,X,Qy,Qp,my,mp)
% wraps GLM with multiple covariance components estimation into VBA
% function [theta,e] = GLM_covComp(y,X,Qy,Qp,my,mp)
% IN:
%   - y: px1 data vector
%   - X: pxn_phi GLM design matrix
%   - Qy: nqyx1 cell array of data covariance components
%   - Qp: nqpx1 cell array of GLM coef covariance components
%   - my: prior mean on data precision hyperparameters in log-space
%   - mp: prior mean on GLM coef precision hyperparameters in log-space
% OUT:
%   - theta: n_phix1 vector of estimated GLM coef
%   - e: px1 vector of estimated residuals

nqy = length(Qy);
nqp = length(Qp);

[ny,np] = size(X);

dim.p = size(y,1);
dim.n_phi = (np+1)*nqp + (ny+1)*nqy;
dim.n_theta = 0;
dim.n = 0;
dim.n_t = 1;

priors.muPhi = zeros(dim.n_phi,1);
priors.SigmaPhi = zeros(dim.n_phi,dim.n_phi);

ind = cell(4,1);
last = 0;
for i=1:nqy
    ind{1}(i) = last+1;
    ind2i = ind{1}(i)+1:ind{1}(i)+ny;
    ind{2} = [ind{2};ind2i];
    last = ind2i(end);
    priors.muPhi(ind{1}(i)) = my(i);
    priors.SigmaPhi(ind{1}(i),ind{1}(i)) = 4;
    priors.SigmaPhi(ind2i,ind2i) = Qy{i};
end
for j=1:nqp
    ind{3}(j) = last+1;
    ind4j = ind{3}(j)+1:ind{3}(j)+np;
    ind{4} = [ind{4};ind4j];
    last = ind4j(end);
    priors.muPhi(ind{3}(j)) = mp(j);
    priors.SigmaPhi(ind{3}(j),ind{3}(j)) = 4;
    priors.SigmaPhi(ind4j,ind4j) = Qp{j};
end
priors.a_sigma = 1e8;
priors.b_sigma = 1e0;

inG.X = X;
inG.ind = ind;
inG.dim = struct('ny',ny,'np',np,'nqy',nqy,'nqp',nqp);

options = struct(...
    'priors',priors,...
    'inG',inG,...
    'updateHP',0,...
    'MaxIter',128,...
    'GnMaxIter',64);
g_fname = @g_wrapGLM;

[posterior,out] = VBA_NLStateSpaceModel(y,[],[],g_fname,dim,options);

[theta,e,hy,hp] = collapseGLM(posterior.muPhi,inG);

function [gx] = g_wrapGLM(x,P,u,in)
[theta,e] = collapseGLM(P,in);
gx = in.X*theta + e;

function [theta,e,hy,hp] = collapseGLM(P,in)
P1 = P(in.ind{1});
P2 = reshape(P(in.ind{2}),in.dim.ny,in.dim.nqy);
P3 = P(in.ind{3});
P4 = reshape(P(in.ind{4}),in.dim.np,in.dim.nqp);
hp = exp(P3);
theta = P4*hp;
hy = exp(P1);
e = P2*hy;


function y = tan(x)
%TAN          Implements  tan(x)  for intervals
%
%   y = tan(x)
%
%interval standard function implementation
%

% written  12/30/98     S.M. Rump
% modified 09/13/99     S.M. Rump  complex allowed, sparse input, NaN 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 11/20/05     S.M. Rump  fast check for rounding to nearest
% modified 12/04/05     S.M. Rump  extreme values for approximate part
% modified 09/06/07     S.M. Rump  approximate std fcts removed
% modified 08/26/12     S.M. Rump  global variables removed
% modified 10/13/12     S.M. Rump  INTLAB_INTVAL_STDFCTS
%

  if issparse(x)
    [ix,jx,sx] = find(x);
    [m,n] = size(x);
    y = sparse(ix,jx,tan(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 = sin(x)./cos(x);
    setround(rndold)
    return
  end

  INTLAB_STDFCTS_PI = getappdata(0,'INTLAB_STDFCTS_PI');

  y = x;

  % transform x.inf and x.sup mod pi/2
  [ xinfinf , xinfsup , Sinf ] = modpi2(x.inf);
  [ xsupinf , xsupsup , Ssup ] = modpi2(x.sup);

  % indices with result +/- inf
  setround(1)
  delta = x.sup-x.inf;
  indexinf = ( delta >= INTLAB_STDFCTS_PI.PIINF ) | ...
    ( floor(Sinf/4) ~= floor(Ssup/4) );
  Sinf(Sinf>3) = Sinf(Sinf>3) - 4;
  Ssup(Ssup>3) = Ssup(Ssup>3) - 4;

  % transformation of input arguments by modpi2:
  %   [ xinf,xsup,s ] = modpi2(y)  ==>  0 <= s <= 7, 0 <= x <= pi/4 and
  %   x = -pi/2 + y + s*pi/4 + 2k*pi        for s even
  %   x =       - y + (s-1)*pi/4 + 2k*pi    for s odd

  y.inf(:) = -inf;
  y.sup(:) = inf;

  % treat non-infinity intervals
  Sinf(indexinf) = -1;
  Ssup(indexinf) = -1;

  % save warning status
  wng = warning;
  warning off

  % treat infimum
  index = ( Sinf==0 );
  if any(index(:))
    y.inf(index) = 1 ./ ( - tan_pos(xinfinf(index),-1) );
  end
  index = ( Sinf==1 );
  if any(index(:))
    y.inf(index) = - tan_pos(xinfsup(index),1);
  end
  index = ( Sinf==2 );
  if any(index(:))
    y.inf(index) = tan_pos(xinfinf(index),-1);
  end
  index = ( Sinf==3 );
  if any(index(:))
    res = tan_pos(xinfsup(index),1);
    setround(-1)
    y.inf(index) = 1 ./ res;
  end

  % treat supremum
  index = ( Ssup==0 );
  if any(index(:))
    y.sup(index) = 1 ./ ( - tan_pos(xsupsup(index),1) );
  end
  index = ( Ssup==1 );
  if any(index(:))
    y.sup(index) = - tan_pos(xsupinf(index),-1);
  end
  index = ( Ssup==2 );
  if any(index(:))
    y.sup(index) = tan_pos(xsupsup(index),1);
  end
  index = ( Ssup==3 );
  if any(index(:))
    res = tan_pos(xsupinf(index),-1);
    setround(1)
    y.sup(index) = 1 ./ res;
  end

  % restore warning status
  warning(wng);

  index = isnan(x.inf);
  if any(index(:))
    y.inf(index) = NaN;
    y.sup(index) = NaN;
  end

  setround(rndold)


classdef Polyhedron

  properties
    A;
    b;
    Aeq = [];
    beq = [];
    vertices;
    has_vertices = false;
  end

  methods
    function obj = Polyhedron(A, b, Aeq, beq)
      if nargin < 3
        Aeq = [];
      end
      if nargin < 4
        beq = [];
      end
      obj.A = A;
      obj.b = b;
      obj.Aeq = Aeq;
      obj.beq = beq;
    end

    function vertices = getVertices(obj)
      obj = obj.reduce();
      if ~obj.has_vertices
        if exist('cddmex', 'file')
          H = struct('A', [obj.Aeq; obj.A], 'B', [obj.beq; obj.b], 'lin', (1:size(obj.Aeq, 1))');
          V = cddmex('extreme', H);
          obj.vertices = V.V';
        else
          obj.vertices = iris.thirdParty.polytopes.lcon2vert(obj.A, obj.b, obj.Aeq, obj.beq)';
        end
        obj.has_vertices = true;
      end
      vertices = obj.vertices;
    end

    function reduced_poly = reduce(obj)
      % Find a minimal representation of the polyhedron
      if ~exist('cddmex', 'file')
        error('IRIS:MissingDependency', 'This function requires the cddmex tool. The easiest way to get it is using tbxmanager: http://www.tbxmanager.com/');
      end
      H = struct('A', [obj.Aeq; obj.A], 'B', [obj.beq; obj.b], 'lin', (1:size(obj.Aeq, 1))');
      Hred = cddmex('reduce_h', H);
      assert(isempty(Hred.lin), 'as far as I know, Hred.lin should always be empty. That is, the reduced polyhedron should not contain equality constraints. -rdeits');
      reduced_poly = iris.Polyhedron(Hred.A, Hred.B);
    end

    function plotVertices(obj, varargin)
      vertices = obj.getVertices();
      if ~isempty(vertices)
        if size(vertices, 2) > 2
          k = convhull(vertices(1,:), vertices(2,:));
        else
          k = [1:size(vertices, 2), 1];
        end
        plot(vertices(1,k), vertices(2,k), varargin{:});
      end
    end

    function drawLCMGL(obj, lcmgl)
      lcmgl.glBegin(lcmgl.LCMGL_LINES);
      vertices = obj.getVertices();
      if size(vertices, 2) > 2
        k = convhull(vertices(1,:), vertices(2,:));
      else
        k = [1:size(vertices, 2), 1];
      end
      for j = 1:length(k)-1
        lcmgl.glVertex3d(vertices(1,k(j)), vertices(2,k(j)), vertices(3,k(j)));
        lcmgl.glVertex3d(vertices(1,k(j+1)), vertices(2,k(j+1)), vertices(3,k(j+1)));
      end
      lcmgl.glEnd();
    end

    function obj = normalize(obj)
      n = zeros(size(obj.A, 1), 1);
      for j = 1:size(obj.A, 1)
        n(j) = norm(obj.A(j,:));
        obj.A(j,:) = obj.A(j,:) / n(j);
        obj.b(j) = obj.b(j) / n(j);
      end
      obj.A = obj.A(n > 0, :);
      obj.b = obj.b(n > 0);

      n = zeros(size(obj.Aeq, 1), 1);
      for j = 1:size(obj.Aeq, 1)
        n(j) = norm(obj.Aeq(j,:));
        obj.Aeq(j,:) = obj.Aeq(j,:) / n(j);
        obj.beq(j) = obj.beq(j) / n(j);
      end
      obj.Aeq = obj.Aeq(n > 0, :);
      obj.beq = obj.beq(n > 0);
    end

  end

  methods(Static)
    function obj = fromVertices(vertices)
      [A, b] = iris.thirdParty.polytopes.vert2lcon(vertices');
      obj = iris.Polyhedron(A, b);
    end

    function obj = from2DVertices(vertices)
      assert(size(vertices, 1) == 2);
      x = vertices(1,:);
      y = vertices(2,:);
      k = convhull(x,y, 'simplify', true);
      A = [(y(k(2:end)) - y(k(1:end-1)))', (x(k(1:end-1)) - x(k(2:end)))'];
      b = sum(A' .* [x(k(1:end-1)); y(k(1:end-1))], 1)';
      obj = iris.Polyhedron(A, b);
    end

    function obj = from2DVerticesAndPlane(vertices, normal, v)
      assert(size(vertices, 1) == 2);
      % normal' * [x;y;z] = v;
      obj = iris.Polyhedron.from2DVertices(vertices);
      obj.Aeq = reshape(normal, 1, []);
      obj.beq = v;
      obj.A = [obj.A, zeros(size(obj.A, 1), 1)];
    end

    function obj = fromBounds(lb, ub)
      % create a polyhedron representing a bounding box in n dimensions
      dim = length(lb);
      assert(length(lb) == length(ub));
      A = [eye(dim); -eye(dim)];
      b = [reshape(ub,[],1); reshape(-lb,[],1)];
      obj = iris.Polyhedron(A, b);
    end
  end
end


function [ r, seed ] = r8mat_uniform_01 ( m, n, seed )

%*****************************************************************************80
%
%% R8MAT_UNIFORM_01 returns a unit pseudorandom R8MAT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Paul Bratley, Bennett Fox, Linus Schrage,
%    A Guide to Simulation,
%    Springer Verlag, pages 201-202, 1983.
%
%    Bennett Fox,
%    Algorithm 647:
%    Implementation and Relative Efficiency of Quasirandom
%    Sequence Generators,
%    ACM Transactions on Mathematical Software,
%    Volume 12, Number 4, pages 362-376, 1986.
%
%    Peter Lewis, Allen Goodman, James Miller,
%    A Pseudo-Random Number Generator for the System/360,
%    IBM Systems Journal,
%    Volume 8, pages 136-143, 1969.
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns in the array.
%
%    Input, integer SEED, the integer "seed" used to generate
%    the output random number.
%
%    Output, real R(M,N), an array of random values between 0 and 1.
%
%    Output, integer SEED, the updated seed.  This would
%    normally be used as the input seed on the next call.
%
  i4_huge = 2147483647;
  r = zeros ( m, n );

  if ( seed == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8MAT_UNIFORM_01 - Fatal error!\n' );
    fprintf ( 1, '  Input SEED = 0!\n' );
    error ( 'R8MAT_UNIFORM_01 - Fatal error!' );
  end

  for j = 1 : n
    for i = 1 : m

      seed = floor ( seed );

      seed = mod ( seed, i4_huge );

      if ( seed < 0 )
        seed = seed + i4_huge;
      end

      k = floor ( seed / 127773 );

      seed = 16807 * ( seed - k * 127773 ) - k * 2836;

      if ( seed < 0 )
        seed = seed + i4_huge;
      end

      r(i,j) = seed * 4.656612875E-10;

    end
  end

  return
end


function sparse_grid_mixed_unique_index_test ( dim_num, level_max_min, ...
  level_max_max, rule, alpha, beta, tol )

%*****************************************************************************80
%
%  Purpose:
%
%    SPARSE_GRID_MIXED_UNIQUE_INDEX_TEST tests SPARSE_GRID_MIXED_UNIQUE_INDEX.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 March 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer LEVEL_MAX_MIN, LEVEL_MAX_MAX, the minimum and
%    maximum values of LEVEL_MAX.
%
%    Input, integer RULE(DIM_NUM), the rule in each dimension.
%     1, "CC",  Clenshaw Curtis, Closed Fully Nested rule.
%     2, "F2",  Fejer Type 2, Open Fully Nested rule.
%     3, "GP",  Gauss Patterson, Open Fully Nested rule.
%     4, "GL",  Gauss Legendre, Open Weakly Nested rule.
%     5, "GH",  Gauss Hermite, Open Weakly Nested rule.
%     6, "GGH", Generalized Gauss Hermite, Open Weakly Nested rule.
%     7, "LG",  Gauss Laguerre, Open Non Nested rule.
%     8, "GLG", Generalized Gauss Laguerre, Open Non Nested rule.
%     9, "GJ",  Gauss Jacobi, Open Non Nested rule.
%    10, "GW",  Golub Welsch, (presumed) Open Non Nested rule.
%    11, "CC_SE", Clenshaw Curtis Slow Exponential, Closed Fully Nested rule.
%    12, "F2_SE", Fejer Type 2 Slow Exponential, Closed Fully Nested rule.
%    13, "GP_SE", Gauss Patterson Slow Exponential, Closed Fully Nested rule.
%    14, "CC_ME", Clenshaw Curtis Moderate Exponential, Closed Fully Nested rule.
%    15, "F2_ME", Fejer Type 2 Moderate Exponential, Closed Fully Nested rule.
%    16, "GP_ME", Gauss Patterson Moderate Exponential, Closed Fully Nested rule.
%    17, "CCN", Clenshaw Curtis Nested, Linear, Closed Fully Nested rule.
%
%    Input, real ALPHA(DIM_NUM), BETA(DIM_NUM), parameters used for
%    Generalized Gauss Hermite, Generalized Gauss Laguerre, and Gauss Jacobi rules.
%
%    Input, real TOL, the tolerance for point equality.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SPARSE_GRID_MIXED_UNIQUE_INDEX_TEST\n' );
  fprintf ( 1, '  SPARSE_GRID_MIXED_UNIQUE_INDEX returns a mapping between\n' );
  fprintf ( 1, '  the nonunique and unique points in a sparse grid.\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, ' Dimension      Rule     Alpha          Beta\n' );
  fprintf ( 1, '\n' );

  for dim = 1 : dim_num
    fprintf ( 1, '  %8d  %8d', dim, rule(dim) );
    if ( rule(dim) == 6 | rule(dim) == 8 | rule(dim) == 9 )
      fprintf ( 1, '  %12e', alpha(dim) );
    end
    if ( rule(dim) == 9 )
      fprintf ( 1, '  %12e', beta(dim) );
    end
    fprintf ( 1, '\n' );
  end

  for level_max = level_max_min : level_max_max

    point_total_num = sparse_grid_mixed_size_total ( dim_num, level_max, rule );

    point_num = sparse_grid_mixed_size ( dim_num, level_max, rule, alpha, ...
      beta, tol );

    fprintf ( 1, '\n' );
    fprintf ( 1, ' LEVEL_MIN LEVEL_MAX POINT_NUM POINT_NUM\n' );
    fprintf ( 1, '                        Unique     Total\n' );
    fprintf ( 1, '\n' );

    level_min = max ( 0, level_max + 1 - dim_num );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  %8d  %8d  %8d  %8d\n', ...
      level_min, level_max, point_num, point_total_num );

    sparse_unique_index = sparse_grid_mixed_unique_index ( ...
      dim_num, level_max, rule, alpha, beta, tol, point_num, point_total_num );

    fprintf ( 1, '\n' );
    fprintf ( 1, '     POINT    UNIQUE\n' );
    fprintf ( 1, '\n' );
    for point = 1 : point_total_num
      fprintf ( 1, '  %8d  %8d\n', point, sparse_unique_index(point) );
    end

  end

  return
end


function [vert,conn,tria,tnum] = refine2(varargin)
%REFINE2 (Frontal)-Delaunay-refinement for two-dimensional,
%polygonal geometries.
%   [VERT,EDGE,TRIA,TNUM] = REFINE2(NODE,EDGE) returns a co-
%   nstrained Delaunay triangulation of the polygonal region
%   {NODE,EDGE}. NODE is an N-by-2 array of polygonal verti-
%   ces and EDGE is an E-by-2 array of edge indexing. Each
%   row in EDGE represents an edge of the polygon, such that
%   NODE(EDGE(JJ,1),:) and NODE(EDGE(JJ,2),:) are the coord-
%   inates of the endpoints of the JJ-TH edge. If the argum-
%   ent EDGE is omitted it assumed that the vertices in NODE
%   are connected in ascending order.
%
%   [...] = REFINE2(NODE,EDGE,PART) computes a triangulation
%   for a multiply-connected geometry. PART is a cell-array
%   of polygonal "parts", where each element PART{KK} is an
%   array of edge indices defining a given polygonal region.
%   EDGE(PART{KK}, :) is the set of edges in the KK-TH part.
%
%   VERT is a V-by-2 array of XY coordinates in the triangu-
%   lation, EDGE is an array of constrained edges, TRIA is a
%   T-by-3 array of triangles, and TNUM is a T-by-1 array of
%   part indices. Each row of TRIA and EDGE define an eleme-
%   nt. VERT(TRIA(II,1),:), VERT(TRIA(II,2),:) and VERT(TRIA
%   (II,3),:) are the coordinates of the II-TH triangle. The
%   edges in EDGE are defined in a similar manner. NUM is an
%   array of part indexing, such that TNUM(II) is the index
%   of the part in which the II-TH triangle resides.
%
%   [...] = REFINE2(..., OPTS) passes an additional options
%   structure OPTS, containing various user-defined paramet-
%   ers, including:
%
% - OPTS.KIND = {'DELFRONT'}, 'DELAUNAY' -- the type of ref-
%   inement employed. The 'DELFRONT' algorithm is typically
%   slower, but produces higher quality output.
%
% - OPTS.RHO2 = {1.025} -- the maximum allowable radius-edge
%   ratio. Refinement proceeds until all interior triangles
%   satisfy the radius-edge threshold. Smaller radius-edge
%   ratios lead to improved triangle shape, with RHO2=1 req-
%   uiring that all angles exceed 30 degrees. Setting RHO2<1
%   may lead to non-convergence.
%
% - OPTS.REF1 = {'REFINE'}, 'PRESERVE' -- refinement 'flag'
%   for 1-dimensional faces (i.e. edges). The 'PRESERVE' op-
%   tion results in minimal refinement, attempting to retain
%   the initial edges without further subdivision. Edges are
%   split only to satisfy basic geomertical conformance.
%
% - OPTS.REF2 = {'REFINE'}, 'PRESERVE' -- refinement 'flag'
%   for 2-dimensional faces (i.e. trias). The 'PRESERVE' op-
%   tion results in minimal refinement, attempting to retain
%   the initial trias without further subdivision. Trias are
%   split only to satisfy basic geomertical conformance.
%
% - OPTS.SIZ1 = {1.333} -- the normalised rel.-length th-
%   reshold for edge-elements. Each exterior edge is refined
%   until LL/HH<SIZ1, where LL is the edge-length, HH is the
%   edge-centred mesh-size value.
%
% - OPTS.SIZ2 = {1.300} -- the normalised rel.-length th-
%   reshold for tria-elements. Each interior tria is refined
%   until RE/HH<SIZ2, where RE is an effective tria length,
%   based on the circumradius, HH is the tria-centred mesh-
%   size value.
%
% - OPTS.DISP = { +10 } -- refinement verbosity. Set to INF
%   for quiet execution.
%
%   [...] = REFINE2(..., HFUN,ARGS) also passes an optional
%   mesh-size function argument. Setting HFUN = HMAX, where
%   HMAX is a scalar value, imposes a constant size constra-
%   int over the full domain. HFUN can also be defined as a
%   general function handle [HH] = HFUN(PP), where PP is an
%   N-by-2 array of XY coordinates and HH is the associated
%   vector of mesh-size values. User-defined HFUN must be
%   fully vectorised. Additional arguments {A1,A2,...AN} for
%   HFUN can be passed as trailing parameters to REFINE2. In
%   such cases, HFUN must adopt a signature [HH] = HFUN(PP,
%   A1,A2,...,AN). HFUN must return positive values.
%
%   See also SMOOTH2, TRIDIV2, TRICOST, TRIDEMO

%   This routine implements a "multi-refinement" variant of
%   Delaunay-refinement type mesh-generation. Both standard
%   Delaunay-refinement and Frontal-Delaunay type algorithms
%   are available. The Frontal-Delaunay approach is a simpl-
%   ified version of the JIGSAW algorithm, described in:
%
% * D. Engwirda, (2014): "Locally-optimal Delaunay-refineme-
%   nt and optimisation-based mesh generation", Ph.D. Thesis
%   School of Mathematics and Statistics, Univ. of Sydney.
%   http://hdl.handle.net/2123/13148
%
% * D. Engwirda & D. Ivers, (2016): "Off-centre Steiner poi-
%   nts for Delaunay-refinement on curved surfaces", Comput-
%   er-Aided Design, (72), 157--171.
%   http://dx.doi.org/10.1016/j.cad.2015.10.007

%   This work is an extension of the "off-centre" type tech-
%   niques introduced in:
%
% * H. Erten & A. Ungor, (2009): "Quality triangulation with
%   locally optimal Steiner points", SIAM Journal on Scient-
%   ific Comp. 31(3), 2103--2130.
%   http://doi.org/10.1137/080716748
%
% * S. Rebay, (1993): "Efficient Unstructured Mesh Generati-
%   on by Means of Delaunay Triangulation and Bowyer-Watson
%   Algorithm, J. Comp. Physics 106(1), 125--138.
%   http://dx.doi.org/10.1006/jcph.1993.1097

%   Generally speaking, the Delaunay-refinement method impl-
%   emented here is a variantion of the "classical" algorit-
%   hm introduced in:
%
% * J. Ruppert, (1995): "A Delaunay refinement algorithm for
%   quality 2-dimensional mesh generation." Journal of Algo-
%   rithms 18(3), 548--585.
%   http://dx.doi.org/10.1006/jagm.1995.1021
%
%   See also: S. Cheng, T. Dey & J. Shewchuk, (2012): "Dela-
%   unay mesh generation", CRC Press, for comprehensive cov-
%   erage of Delaunay-based meshing techniques.

%   A much more advanced, and fully three-dimensional imple-
%   mentation is available in the JIGSAW library. For addit-
%   ional information, see:
%   https://github.com/dengwirda/jigsaw-matlab

%-----------------------------------------------------------
%   Darren Engwirda : 2017 --
%   Email           : de2363@columbia.edu
%   Last updated    : 13/02/2020
%-----------------------------------------------------------

    node = []; PSLG = []; part = {}; opts = [] ;
    hfun = []; harg = {};

%---------------------------------------------- extract args
    if (nargin>=+1), node = varargin{1}; end
    if (nargin>=+2), PSLG = varargin{2}; end
    if (nargin>=+3), part = varargin{3}; end
    if (nargin>=+4), opts = varargin{4}; end
    if (nargin>=+5), hfun = varargin{5}; end
    if (nargin>=+6), harg = varargin(6:end); end

   [opts] = makeopt(opts) ;

%---------------------------------------------- default EDGE
    nnod = size(node,1) ;

    if (isempty(PSLG))
        PSLG = [(1:nnod-1)',(2:nnod)'; nnod,1] ;
    end

%---------------------------------------------- default PART
    ncon = size(PSLG,1) ;

    if (isempty(part)), part{1} = (1:ncon)'; end

%---------------------------------------------- basic checks
    if (~isnumeric(node) || ~isnumeric(PSLG) || ...
        ~iscell   (part) || ~isstruct (opts) )
        error('refine2:incorrectInputClass' , ...
            'Incorrect input class.') ;
    end

%---------------------------------------------- basic checks
    if (ndims(node) ~= +2 || ndims(PSLG) ~= +2)
        error('refine2:incorrectDimensions' , ...
            'Incorrect input dimensions.');
    end
    if (size(node,2) < +2 || size(PSLG,2) < +2)
        error('refine2:incorrectDimensions' , ...
            'Incorrect input dimensions.');
    end

%---------------------------------------------- basic checks
    if (min([PSLG(:)])<+1 || max([PSLG(:)])>nnod)
        error('refine2:invalidInputs', ...
            'Invalid EDGE input array.') ;
    end

    pmin = cellfun(@min,part);
    pmax = cellfun(@max,part);

    if (min([pmin(:)])<+1 || max([pmax(:)])>ncon)
        error('refine2:invalidInputs', ...
            'Invalid PART input array.') ;
    end

%-------------------------------- prune any non-unique topo.
   [ivec,ivec,jvec] = ...
        unique(sort(PSLG,+2),'rows') ;

    PSLG = PSLG(ivec,:) ;

    for ppos = +1:length(part)

        if ( ~isnumeric(part{ppos}) )
            error (  ...
            'refine2:incorrectInputClass', ...
                'Incorrect input class. ') ;
        end

        part{ppos} = ...
            unique(jvec(part{ppos})) ;

    end

%-------------------------------- check part "manifold-ness"
    for ppos = +1:length(part)

        eloc = PSLG(part{ppos},:) ;
        nadj = ...
            accumarray(eloc(:),1) ;

        if (any(mod(nadj,2) ~= 0) )
        error('refine2:nonmanifoldInputs', ...
            'Non-manifold PART detected.') ;
        end

    end

%---------------------------------------------- output title
    if (~isinf(opts.disp))
        fprintf(1,'\n') ;
        fprintf(1,' Refine triangulation...\n') ;
        fprintf(1,'\n') ;
        fprintf(1,[...
' -------------------------------------------------------\n', ...
'      |ITER.|          |CDT1(X)|          |CDT2(X)|     \n', ...
' -------------------------------------------------------\n', ...
             ] ) ;
    end

%-------------------------------- PASS 0: inflate box bounds
    vert = node; tria = []; tnum = []; iter = 0 ;
    conn = PSLG;

    vmin = min(vert,[],1);      % inflate bbox for stability
    vmax = max(vert,[],1);

    vdel = vmax - 1.*vmin;
    vmin = vmin - .5*vdel;
    vmax = vmax + .5*vdel;

    vbox = [
        vmin(1), vmin(2)
        vmax(1), vmin(2)
        vmax(1), vmax(2)
        vmin(1), vmax(2)
           ] ;
    vert = [vert ; vbox] ;

%-------------------------------- PASS 0: shield sharp feat.
   [vert,conn,tria,tnum,iter] = ...
        cdtbal0(vert,conn,tria,tnum, ...
            node,PSLG,part,opts,hfun,harg,iter);

%-------------------------------- PASS 1: refine 1-simplexes
   [vert,conn,tria,tnum,iter] = ...
        cdtref1(vert,conn,tria,tnum, ...
            node,PSLG,part,opts,hfun,harg,iter);

%-------------------------------- PASS 2: refine 2-simplexes
   [vert,conn,tria,tnum,iter] = ...
        cdtref2(vert,conn,tria,tnum, ...
            node,PSLG,part,opts,hfun,harg,iter);

    if (~isinf(opts.disp)), fprintf(1,'\n'); end

%-------------------------------- trim extra adjacency info.
    tria = tria( :,1:3) ;

%-------------------------------- trim vert. - deflate bbox.
    keep = false(size(vert,1),1);
    keep(tria(:)) = true;
    keep(conn(:)) = true;

    redo = zeros(size(vert,1),1);
    redo(keep) = ...
        (+1:length(find(keep)))';

    conn = ...
        reshape(redo(conn),[],2);
    tria = ...
        reshape(redo(tria),[],3);

    vert = vert(keep,:) ;

end

function [vert,conn,tria,tnum,iter] = ...
            cdtbal0(vert,conn,tria,tnum, ...
                node,PSLG,part,opts,hfun,harg,iter)
%CDTBAL0 constrained Delaunay-refinement for "sharp" 0-dim.
%features at PSLG vertices.
%   [...] = CDTBAL0(...) refines the set of 1-simplex eleme-
%   nts incident to "sharp" features in the PSLG. Specifica-
%   lly, edges that subtend "small" angles are split about a
%   set of new "collar" vertices, equi-distributed about the
%   centre of "sharp" features. Collar size is computed as a
%   min. of the incident edge-len. and local mesh-size cons-
%   traints.

    if (iter <= opts.iter)

    %------------------------------------- build current CDT
       [vert,conn, ...
        tria,tnum] = deltri2(vert,conn, ...
                             node,PSLG, ...
                             part, ...
                             opts.dtri) ;

    %------------------------------------- build current adj
       [edge,tria] = tricon2(tria,conn) ;

       [feat,ftri] = isfeat2(vert, ...
                             edge,tria) ;

        apex = false(size(vert,1), 1) ;
        apex(tria(ftri)) =  true ;

    %------------------------------------- eval. length-fun.
        if (~isempty(hfun))
            if (isnumeric(hfun))
            vlen = hfun * ...
              ones(size(vert,1),1) ;
            else
            vlen = feval( ...
                hfun,vert,harg{:}) ;
            vlen = vlen(:) ;
            end
        else
            vlen = +inf * ...
              ones(size(vert,1),1) ;
        end

    %------------------------------------- form edge vectors
        evec = vert(conn(:,2),:) ...
             - vert(conn(:,1),:) ;
        elen = sqrt(sum(evec.^2,2));
        evec = evec./[elen,elen] ;

    %------------------------------------- min. adj. lengths
        for epos = +1 : size(conn,1)

            ivrt = conn(epos,1) ;
            jvrt = conn(epos,2) ;

            vlen(ivrt) = min( ...
            vlen(ivrt), .67*elen(epos)) ;
            vlen(jvrt) = min( ...
            vlen(jvrt), .67*elen(epos)) ;

        end

    %------------------------------------- mark feature edge
        iref = apex(conn(:,1)) ...      %- refine at vert. 1
            & ~apex(conn(:,2)) ;
        jref = apex(conn(:,2)) ...      %- refine at vert. 2
            & ~apex(conn(:,1)) ;
        dref = apex(conn(:,1)) ...      %- refine at both!
            &  apex(conn(:,2)) ;

        keep =~apex(conn(:,1)) ...      %- refine at neither
            & ~apex(conn(:,2)) ;

    %------------------------------------- protecting collar
        ilen = vlen(conn(iref,1)) ;
        inew = vert(conn(iref,1),:) ...
        + [ilen,ilen].*evec(iref,:) ;

        jlen = vlen(conn(jref,2)) ;
        jnew = vert(conn(jref,2),:) ...
        - [jlen,jlen].*evec(jref,:) ;

        Ilen = vlen(conn(dref,1)) ;
        Inew = vert(conn(dref,1),:) ...
        + [Ilen,Ilen].*evec(dref,:) ;

        Jlen = vlen(conn(dref,2)) ;
        Jnew = vert(conn(dref,2),:) ...
        - [Jlen,Jlen].*evec(dref,:) ;

        vnew = [inew; jnew; Inew; Jnew] ;

    %------------------------------------- add new vert/edge
        iset = (1:size(inew,1))' ...
                + size(vert,1) ;

        jset = (1:size(jnew,1))' ...
                + size(inew,1) + ...
                + size(vert,1) ;

        Iset = (1:size(Inew,1))' ...
                + size(inew,1) + ...
                + size(jnew,1) + ...
                + size(vert,1) ;

        Jset = (1:size(Jnew,1))' ...
                + size(inew,1) + ...
                + size(jnew,1) + ...
                + size(Inew,1) + ...
                + size(vert,1) ;

        vert = [vert ; vnew] ;

        cnew = [conn(iref,1), iset ;
                conn(iref,2), iset ;
                conn(jref,2), jset ;
                conn(jref,1), jset ;
                conn(dref,1), Iset ;
                conn(dref,2), Jset ;
                Iset, Jset] ;
        conn = [conn(keep,:); cnew ] ;

    end

end

function [vert,conn,tria,tnum,iter] = ...
            cdtref1(vert,conn,tria,tnum, ...
                node,PSLG,part,opts,hfun,harg,iter)
%CDTREF1 constrained Delaunay-refinement for 1-simplex elem-
%nts embedded in R^2.
%   [...] = CDTREF1(...) refines the set of 1-simplex eleme-
%   nts embedded in the triangulation until all constraints
%   are satisfied. Specifically, edges are refined until all
%   local mesh-spacing and encroachment conditions are met.
%   Refinement proceeds according to either a Delaunay-refi-
%   nement or Frontal-Delaunay type approach, depending on
%   user-settings. In either case, new steiner vertices are
%   introduced to split "bad" edges - those that violate the
%   set of prescribed constraints. In the "-DR" type process
%   edges are split about their circumballs (midpoints). In
%   the "-FD" approach, new vertices are positioned such th-
%   at mesh-spacing constraints are satisfied in a "locally-
%   optimal" fashion.

    tcpu.full = +0. ;
    tcpu.ball = +0. ;
    tcpu.hfun = +0. ;
    tcpu.encr = +0. ;
    tcpu.offc = +0. ;

    vidx = (1:size(vert,1))';     %- "new" vert list to test

    tnow =  tic ;

    ntol = +1.55;

    while (strcmpi(opts.ref1,'refine'))

        iter = iter + 1 ;

        if (iter>=opts.iter),break; end

    %------------------------------------- calc. circumballs
        ttic = tic ;

        bal1 = cdtbal1(vert,conn) ;

        tcpu.ball = ...
            tcpu.ball + toc(ttic) ;

    %------------------------------------- eval. length-fun.
        ttic = tic ;

        if (~isempty(hfun))
            if (isnumeric(hfun))
            fun0 = hfun * ...
              ones(size(vert,1),1);
            fun1 = hfun ;
            else
            fun0(vidx) = ...
                feval(hfun, ...
            vert(vidx,:), harg{:});
            fun0 = fun0(:) ;
            fun1 = fun0(conn(:,1))...
                 + fun0(conn(:,2));
            fun1 = fun1 / +2. ;
            end
        else
            fun0 = +inf * ...
              ones(size(vert,1),1);
            fun1 = +inf ;
        end

        siz1 = ...
         +4. * bal1(:,3)./(fun1.*fun1) ;

        tcpu.hfun = ...
            tcpu.hfun + toc(ttic) ;

    %------------------------------------- test encroachment
        ttic = tic ;

        bal1(:,3) = ...
            (1.-eps^.75) * bal1(:,3) ;

       [vp,vi] = ...
           findball(bal1,vert(:,1:2));

    %------------------------------------- near=>[vert,edge]
        next = +0;
        ebad = false(size(conn,1),1) ;
        near = zeros(size(conn,1),1) ;
        for ii = +1 : size(vp,1)
            for ip = vp(ii,1):vp(ii,2)
                jj = vi(ip);
                if (ii ~= conn(jj,1) ...
                &&  ii ~= conn(jj,2) )
                next = next + 1;
                near(next,1) = ii;
                near(next,2) = jj;
                end
            end
        end

        near = near(1:next-0,:);

        if (~isempty(near))
    %-- mark edge "encroached" if there is a vert within its
    %-- dia.-ball that is not joined to either of its vert's
    %-- via an existing edge...
            ivrt = conn(near(:,2),1);
            jvrt = conn(near(:,2),2);

            pair = [near(:,1), ivrt];
            ivec = setset2(pair,conn) ;

            pair = [near(:,1), jvrt];
            jvec = setset2(pair,conn) ;

            okay = ~ivec & ~jvec ;

            ebad(near(okay,2))=true ;

        end

        tcpu.encr = ...
            tcpu.encr + toc(ttic);

    %------------------------------------- refinement queues
        ref1 = false(size(conn,1),1);
        ref1(ebad)           = true ;   %- edge encroachment
        ref1(siz1>opts.siz1* ...        %- bad equiv. length
                  opts.siz1) = true ;

        num1 = find(ref1)  ;

    %------------------------------------- dump-out progess!
        if (mod(iter,opts.disp)==0)
            numc = size(conn,1) ;
            numt = size(tria,1) ;
            fprintf(+1, ...
            '%11i %18i %18i\n', ...
            [iter,numc,numt]) ;
        end

    %------------------------------------- nothing to refine
        if (isempty(num1)), break; end

    %------------------------------------- refine "bad" tria
        switch (lower(opts.kind))
        case 'delaunay'
    %------------------------------------- do circ-ball pt's
        new1 = bal1(ref1, 1:2) ;

        vidx = (1:size(new1,1))' ...
                + size(vert,1) ;

        cnew = [conn( ref1,1), vidx
                conn( ref1,2), vidx];
        conn = [conn(~ref1,:); cnew];

    %------------------------------------- update vertex set
        vert = [vert; new1(:,1:2)];

        case 'delfront'
    %-- symmetric off-centre scheme:- refine edges from both
    %-- ends simultaneously, placing new vertices to satisfy
    %-- the worst of mesh-spacing and local voronoi constra-
    %-- ints.

        ttic = tic ;

        evec = vert(conn(ref1,2),:) ...
             - vert(conn(ref1,1),:) ;
        elen = sqrt(sum(evec.^2,2)) ;
        evec = evec ./ [elen, elen] ;

    %------------------------------------- "voro"-type dist.
        vlen = sqrt(bal1(ref1,3));

    %------------------------------------- "size"-type dist.
        ihfn = fun0(conn(ref1,1));
        jhfn = fun0(conn(ref1,2));

    %------------------------------------- bind "safe" dist.
        ilen = min(vlen,ihfn) ;
        jlen = min(vlen,jhfn) ;

    %------------------------------------- locate offcentres
        inew = vert(conn(ref1,1),:) ...
             + [ilen,ilen].*evec ;
        jnew = vert(conn(ref1,2),:) ...
             - [jlen,jlen].*evec ;

    %------------------------------------- iter. "size"-type
        for ioff = +1 : +3
    %------------------------------------- eval. length-fun.
        if (~isempty(hfun))
            if (isnumeric(hfun))
            iprj = hfun * ...
              ones(size(inew,1),1);
            jprj = hfun * ...
              ones(size(jnew,1),1);
            else
            iprj = feval( ...
                hfun,inew,harg{:});
            jprj = feval( ...
                hfun,jnew,harg{:});
            iprj = iprj(:);
            jprj = jprj(:);
            end
        else
            iprj = +inf * ...
              ones(size(inew,1),1);
            jprj = +inf * ...
              ones(size(jnew,1),1);
        end

        iprj = 0.5*ihfn + 0.5*iprj;
        jprj = 0.5*jhfn + 0.5*jprj;

    %------------------------------------- bind "safe" dist.
        ilen = min(vlen,iprj) ;
        jlen = min(vlen,jprj) ;

    %------------------------------------- locate offcentres
        inew = vert(conn(ref1,1),:) ...
             + [ilen,ilen].*evec ;
        jnew = vert(conn(ref1,2),:) ...
             - [jlen,jlen].*evec ;

        end

    %------------------------------------- merge i,j if near
        near = ...
            ilen+jlen>=vlen*ntol ;

        znew = inew(near,:) * .5 ...
             + jnew(near,:) * .5 ;

        inew = inew(~near,1:2) ;
        jnew = jnew(~near,1:2) ;

    %------------------------------------- split constraints
        zset = (1:size(znew,1))' ...
                + size(vert,1) ;

        iset = (1:size(inew,1))' ...
                + size(znew,1) + ...
                + size(vert,1) ;

        jset = (1:size(jnew,1))' ...
                + size(znew,1) + ...
                + size(inew,1) + ...
                + size(vert,1) ;

        set1 = num1( near);
        set2 = num1(~near);

        cnew = [conn( set1,1), zset
                conn( set1,2), zset
                conn( set2,1), iset
                conn( set2,2), jset
                iset, jset ] ;
        conn = [conn(~ref1,:); cnew];

    %------------------------------------- update vertex set
        vert = [vert; znew(:,1:2)];
        vert = [vert; inew(:,1:2)];
        vert = [vert; jnew(:,1:2)];

        vidx = [zset; iset; jset] ;

        tcpu.offc = ...
            tcpu.offc + toc(ttic) ;

        end % switch(lower(opts.kind))

    end

    tcpu.full = ...
        tcpu.full + toc(tnow) ;

    if (~isinf(opts.disp) )
    %------------------------------------- print final stats
        numc = size(conn,1) ;
        numt = size(tria,1) ;
        fprintf(+1, ...
        '%11i %18i %18i\n', ...
        [iter,numc,numt]) ;
    end

    if (opts.dbug)
    %------------------------------------- print debug timer
        fprintf(1,'\n') ;
        fprintf(1,' 1-simplex REF. timer...\n');
        fprintf(1,'\n') ;
        fprintf(1, ...
        ' FULL: %f \n', tcpu.full);
        fprintf(1, ...
        ' BALL: %f \n', tcpu.ball);
        fprintf(1, ...
        ' HFUN: %f \n', tcpu.hfun);
        fprintf(1, ...
        ' ENCR: %f \n', tcpu.encr);
        fprintf(1, ...
        ' OFFC: %f \n', tcpu.offc);
        fprintf(1,'\n') ;
    end

end

function [vert,conn,tria,tnum,iter] = ...
            cdtref2(vert,conn,tria,tnum, ...
                node,PSLG,part,opts,hfun,harg,iter)
%CDTREF2 constrained Delaunay-refinement for 2-simplex elem-
%nts embedded in R^2.
%   [...] = CDTREF2(...) refines the set of 2-simplex eleme-
%   nts embedded in the triangulation until all constraints
%   are satisfied. Specifically, triangles are refined until
%   all local mesh-spacing and element-shape conditions are
%   met. Refinement proceeds according to either a Delaunay-
%   refinement or Frontal-Delaunay type approach, depending
%   on user-settings. In either case, new steiner points are
%   introduced to split "bad" triangles - those that violate
%   the set of prescribed constraints. In the "-DR" type pr-
%   ocess triangles are split about their circumballs. In
%   the "-FD" approach, new vertices are positioned such th-
%   at mesh-spacing and element-shape constraints are satis-
%   fied in a "locally-optimal" fashion.

    tcpu.full = +0. ;
    tcpu.dtri = +0. ;
    tcpu.tcon = +0. ;
    tcpu.ball = +0. ;
    tcpu.hfun = +0. ;
    tcpu.offc = +0. ;
    tcpu.filt = +0. ;

    vidx = (1:size(vert,1))';     %- "new" vert list to test

    tnow =  tic ;

    near = +.775;

    while (strcmpi(opts.ref2,'refine'))

        iter = iter + 1 ;

    %------------------------------------- build current CDT
        ttic = tic ;

        nold = size(vert,1) ;

       [vert,conn, ...
        tria,tnum]= deltri2(vert,conn, ...
                            node,PSLG, ...
                            part, ....
                            opts.dtri) ;

        nnew = size(vert,1) ;

        vidx = ...
       [vidx; (nold:nnew)'] ;

        tcpu.dtri = ...
            tcpu.dtri + toc(ttic) ;

    %------------------------------------- build current adj
        ttic = tic ;

       [edge,tria]= tricon2(tria,conn) ;

        tcpu.tcon = ...
            tcpu.tcon + toc(ttic) ;

        if (iter>=opts.iter),break; end

    %------------------------------------- calc. circumballs
        ttic = tic ;

        bal1 = cdtbal1(vert,conn) ;
        bal2 = cdtbal2(vert, ...
                       edge,tria) ;
        len2 = minlen2(vert,tria) ;

        rho2 = bal2(:,+3) ./ len2 ;

    %------------------------------------- refinement scores
        scr2 = rho2 .* bal2(:,+3) ;

        tcpu.ball = ...
            tcpu.ball + toc(ttic) ;

    %------------------------------------- eval. length-fun.
        ttic = tic ;

        if (~isempty(hfun))
            if (isnumeric(hfun))
            fun0 = hfun * ...
              ones(size(vert,1),1);
            fun2 = hfun ;
            else
            fun0(vidx) = ...
                feval(hfun, ...
            vert(vidx,:), harg{:});
            fun0 = fun0(:) ;
            fun2 = fun0(tria(:,1))...
                 + fun0(tria(:,2))...
                 + fun0(tria(:,3));
            fun2 = fun2 / +3. ;
            end
        else
            fun0 = +inf * ...
              ones(size(vert,1),1);
            fun2 = +inf ;
        end

        siz2 = ...
         +3. * bal2(:,3)./(fun2.*fun2) ;

        tcpu.hfun = ...
            tcpu.hfun + toc(ttic) ;

    %------------------------------------- refinement queues
        ref1 = false(size(conn,1),1);
        ref2 = false(size(tria,1),1);

        stri = isfeat2(vert,edge,tria) ;

        ref2(rho2>opts.rho2* ...        %- bad rad-edge len.
                  opts.rho2) = true ;
        ref2(stri) = false ;
        ref2(siz2>opts.siz2* ...        %- bad equiv. length
                  opts.siz2) = true ;

        num2 = find(ref2);

    %------------------------------------- dump-out progess!
        if (mod(iter,opts.disp)==0)
            numc = size(conn,1) ;
            numt = size(tria,1) ;
            fprintf(+1, ...
            '%11i %18i %18i\n', ...
            [iter,numc,numt]) ;
        end

    %------------------------------------- nothing to refine
        if (isempty(num2)), break; end

       [scr2,idx2] = sort( ...
            scr2(num2),'descend');
        num2 = num2(idx2);

    %------------------------------------- refine "bad" tria
        switch (lower(opts.kind))
        case 'delaunay'
    %------------------------------------- do circ-ball pt's
        new2 = zeros(length(num2),3);
        new2(:,1:2) = bal2(num2,1:2);

        rmin = ...                      %- min. insert radii
            len2(num2)*(1.-eps^.75)^2 ;

        new2(:,  3) = max( ...
            bal2(num2,3)*near^2,rmin) ;

        case 'delfront'
    %-- off-centre scheme -- refine triangles by positioning
    %-- new vertices along a local segment of the voronoi
    %-- diagram, bounded by assoc. circmballs. New points
    %-- are placed to satisfy the worst of local mesh-length
    %-- and element-shape constraints.

        ttic = tic ;

    %------------------------------------- find frontal edge
       [lmin,emin] = ...
            minlen2(vert,tria(num2,:)) ;

        ftri = false(length(num2),1) ;
        epos = zeros(length(num2),1) ;
        tadj = zeros(length(num2),1) ;

        for ii = +1 : length(epos)
            epos(ii) = tria( ...
                num2(ii),emin(ii)+3) ;
        end

    %------------------------------------- find frontal tria
        for enum = +1 : +3

            eidx = tria(num2,enum+3) ;

            ftri = ...
            ftri | edge(eidx,5) > +0 ;

            ione = ...
                num2 ~= edge(eidx,3) ;
            itwo = ~ione ;

            tadj(ione) = ...
                edge(eidx(ione),3);
            tadj(itwo) = ...
                edge(eidx(itwo),4);

            okay = tadj > +0 ;
            tidx = tadj(okay);

            ftri(okay) = ...
            ftri(okay) | ~ref2(tidx) ;

        end

        if (~any(ftri))                 %- can this happen!?
        ftri = true(length(num2),+1) ;
        end

    %------------------------------------- locate offcentres
        emid = vert(edge(epos,+1),:) ...
             + vert(edge(epos,+2),:) ;
        emid = emid * +0.50 ;

        elen = sqrt(lmin(:));

    %------------------------------------- "voro"-type dist.
        vvec = bal2(num2,1:2)-emid ;
        vlen = sqrt(sum(vvec.^2,2));
        vvec = vvec ./ [vlen,vlen] ;

        hmid = fun0(edge(epos,+1),:) ...
             + fun0(edge(epos,+2),:) ;
        hmid = hmid * +0.50 ;

    %------------------------------------- "ball"-type dist.
        rtri = elen * opts.off2 ;
        rfac = elen * +0.50 ;
        dsqr = rtri.^2 - rfac.^2;
        doff = rtri + ...
            sqrt(max(+0.,dsqr)) ;

    %------------------------------------- "size"-type dist.
        dsiz = +sqrt(3.)/2. * hmid ;

    %------------------------------------- bind "safe" dist.
       [dist,ioff] = ...
          min([dsiz,doff,vlen],[],2) ;

    %------------------------------------- locate offcentres
        off2 = ...
        emid + [dist,dist] .* vvec ;

    %------------------------------------- iter. "size"-type
        for isub = +1 : +3
    %------------------------------------- eval. length-fun.
        if (~isempty(hfun))
            if (isnumeric(hfun))
            hprj = hfun * ...
              ones(size(off2,1),1) ;
            else
            hprj = feval( ...
                hfun,off2,harg{:}) ;
            hprj = hprj(:) ;
            end
        else
            hprj = +inf * ...
              ones(size(off2,1),1) ;
        end

    %------------------------------------- "size"-type dist.
        hprj = .33*hmid + .67*hprj ;

        dsiz = +sqrt(3.)/2. * hprj ;

        dsiz(dsiz<elen*.50) = +inf ;    %- edge-ball limiter
        dsiz(dsiz>vlen*.95) = +inf ;    %- circ-ball limiter

    %------------------------------------- bind "safe" dist.
       [dist,ioff] = ...
          min([dsiz,doff,vlen],[],2) ;

    %------------------------------------- locate offcentres
        off2 = ...
        emid + [dist,dist] .* vvec ;

        end

        orad = ...
        sqrt((elen*.5).^2 + dist.^2) ;

    %------------------------------------- do offcentre pt's
        new2 = ...
        zeros(length(find(ftri)),+3) ;
        new2(:,1:2) = off2(ftri,1:2) ;

        rmin = ...                      %- min. insert radii
            lmin(ftri)*(1.-eps^.75)^2 ;

        new2(:,  3) = max( ...
            (orad(ftri)*near).^2,rmin);

        tcpu.offc = ...
            tcpu.offc + toc (ttic) ;

        end % switch(lower(opts.kind))

    %------------------------------------- inter.-ball dist.
        ttic = tic ;

    %------------------------------------- proximity filters
       [vp,vi] = ...
          findball(new2,new2(:,1:2)) ;

        keep = true (size(new2,1),1) ;
        for ii = size(vp,1):-1:+1
            for ip = vp(ii,1) ...
                   : vp(ii,2)
                jj = vi(ip);
                if (keep(jj) && ...
                    keep(ii) && ...
                    jj < ii )

                keep(ii) = false ;
                break;

                end
            end
        end

        new2 = new2(keep,:);

    %------------------------------------- test encroachment
        bal1(:,3) = ...
            (1.-eps^.75) * bal1(:,3);

       [vp,vi] = ...
          findball(bal1,new2(:,1:2));

        keep = true (size(new2,1),1);
        for ii = +1:+1:size(vp,1)
            for ip = vp(ii,1) ...
                   : vp(ii,2)
                jj = vi(ip);
                ref1(jj) =  true ;
                keep(ii) = false ;
            end
        end

    %------------------------------------- leave sharp edges
        ebnd = false(size(edge,1),1);
        ebnd(tria(stri,4:6)) = true ;

        enot = ...
        setset2(conn,edge(ebnd,1:2));

        ref1(enot) = false ;

    %------------------------------------- preserve boundary
        if (strcmp(lower(opts.ref1),...
            'preserve'))
        ref1(:)    = false ;
        end

    %------------------------------------- refinement points
        new2 = new2(keep,:);
        new1 = bal1(ref1,:);

        tcpu.filt = ...
            tcpu.filt + toc(ttic) ;

    %------------------------------------- split constraints
        idx1 = ...
       (1:size(new1))'+size(vert,1) ;

        idx2 = ...
       (1:size(new2))'+size(new1,1) ...
                      +size(vert,1) ;

        cnew = [conn( ref1,1), idx1
                conn( ref1,2), idx1];
        conn = [conn(~ref1,:); cnew];

        vidx = [idx1; idx2];

    %------------------------------------- update vertex set
        nold = size(vert,1);
        vert = [vert; new1(:,1:2)];
        vert = [vert; new2(:,1:2)];
        nnew = size(vert,1);

        if (nnew == nold), break; end   %- we *must* be done

    end

    tcpu.full = ...
        tcpu.full + toc(tnow) ;

    if (~isinf(opts.disp) )
    %------------------------------------- print final stats
        numc = size(conn,1) ;
        numt = size(tria,1) ;
        fprintf(+1, ...
        '%11i %18i %18i\n', ...
        [iter,numc,numt]) ;
    end

    if (opts.dbug)
    %------------------------------------- print debug timer
        fprintf(1,'\n') ;
        fprintf(1,' 2-simplex REF. timer...\n');
        fprintf(1,'\n') ;
        fprintf(1, ...
        ' FULL: %f \n', tcpu.full);
        fprintf(1, ...
        ' DTRI: %f \n', tcpu.dtri);
        fprintf(1, ...
        ' TCON: %f \n', tcpu.tcon);
        fprintf(1, ...
        ' BALL: %f \n', tcpu.ball);
        fprintf(1, ...
        ' HFUN: %f \n', tcpu.hfun);
        fprintf(1, ...
        ' OFFC: %f \n', tcpu.offc);
        fprintf(1, ...
        ' FILT: %f \n', tcpu.filt);
        fprintf(1,'\n') ;
    end

end

function [opts] = makeopt(opts)
%MAKEOPT setup the options structure for REFINE2.

    if (~isfield(opts,'dtri'))
        opts.dtri = 'constrained';
    else
    if (~strcmpi(opts.dtri, 'conforming') && ...
        ~strcmpi(opts.dtri,'constrained') )
        error( ...
    'refine2:invalidOption','Invalid constraint DTRI.');
    end
    end

    if (~isfield(opts,'kind'))
        opts.kind = 'delfront';
    else
    if (~strcmpi(opts.kind, 'delfront') && ...
        ~strcmpi(opts.kind, 'delaunay') )
        error( ...
    'refine2:invalidOption','Invalid refinement KIND.');
    end
    end

    if (~isfield(opts,'ref1'))
        opts.ref1 = 'refine';
    else
    if (~strcmpi(opts.ref1,   'refine') && ...
        ~strcmpi(opts.ref1, 'preserve') )
        error( ...
    'refine2:invalidOption','Invalid refinement REF1.');
    end
    end

    if (~isfield(opts,'ref2'))
        opts.ref2 = 'refine';
    else
    if (~strcmpi(opts.ref2,   'refine') && ...
        ~strcmpi(opts.ref2, 'preserve') )
        error( ...
    'refine2:invalidOption','Invalid refinement REF2.');
    end
    end

    if (~isfield(opts,'iter'))
        opts.iter = +inf;
    else
    if (~isnumeric(opts.iter))
        error('refine2:incorrectInputClass', ...
            'Incorrect input class.');
    end
    if (numel(opts.iter)~= +1)
        error('refine2:incorrectDimensions', ...
            'Incorrect input dimensions.') ;
    end
    if (opts.iter <= +0)
        error('refine2:invalidOptionValues', ...
            'Invalid OPT.ITER selection.') ;
    end
    end

    if (~isfield(opts,'disp'))
        opts.disp = +10 ;
    else
    if (~isnumeric(opts.disp))
        error('refine2:incorrectInputClass', ...
            'Incorrect input class.');
    end
    if (numel(opts.disp)~= +1)
        error('refine2:incorrectDimensions', ...
            'Incorrect input dimensions.') ;
    end
    if (opts.disp <= +0)
        error('refine2:invalidOptionValues', ...
            'Invalid OPT.DISP selection.') ;
    end
    end

    if (~isfield(opts,'rho2'))
        opts.rho2 = 1.025;
    else
    if (~isnumeric(opts.rho2))
        error('refine2:incorrectInputClass', ...
            'Incorrect input class.');
    end
    if (numel(opts.rho2)~= +1)
        error('refine2:incorrectDimensions', ...
            'Incorrect input dimensions.') ;
    end
    if (opts.rho2 < +1.)
        error('refine2:invalidOptionValues', ...
            'Invalid OPT.RHO2 selection.') ;
    end
    end

    if (~isfield(opts,'off2'))
        opts.off2 = 0.933;
    else
    if (~isnumeric(opts.off2))
        error('refine2:incorrectInputClass', ...
            'Incorrect input class.');
    end
    if (numel(opts.off2)~= +1)
        error('refine2:incorrectDimensions', ...
            'Incorrect input dimensions.') ;
    end
    if (opts.off2 < +.7)
        error('refine2:invalidOptionValues', ...
            'Invalid OPT.OFF2 selection.') ;
    end
    end

    if (~isfield(opts,'siz1'))
        opts.siz1 = 1.333;
    else
    if (~isnumeric(opts.siz1))
        error('refine2:incorrectInputClass', ...
            'Incorrect input class.');
    end
    if (numel(opts.siz1)~= +1)
        error('refine2:incorrectDimensions', ...
            'Incorrect input dimensions.') ;
    end
    if (opts.siz1 <= 0.)
        error('refine2:invalidOptionValues', ...
            'Invalid OPT.SIZ1 selection.') ;
    end
    end

    if (~isfield(opts,'siz2'))
        opts.siz2 = 1.300;
    else
    if (~isnumeric(opts.siz2))
        error('refine2:incorrectInputClass', ...
            'Incorrect input class.');
    end
    if (numel(opts.siz2)~= +1)
        error('refine2:incorrectDimensions', ...
            'Incorrect input dimensions.') ;
    end
    if (opts.siz2 <= 0.)
        error('refine2:invalidOptionValues', ...
            'Invalid OPT.SIZ2 selection.') ;
    end
    end

    if (~isfield(opts,'dbug'))
        opts.dbug = false;
    else
    if (~islogical(opts.dbug))
        error('refine2:incorrectInputClass', ...
            'Incorrect input class.');
    end
    if (numel(opts.dbug)~= +1)
        error('refine2:incorrectDimensions', ...
            'Incorrect input dimensions.') ;
    end
    end

end


function hh = errorbar2(x, y, l,u,symbol)
%ERRORBAR Error bar plot.
%   ERRORBAR(X,Y,L,U) plots the graph of vector X vs. vector Y with
%   error bars specified by the vectors L and U.  L and U contain the
%   lower and upper error ranges for each point in Y.  Each error bar
%   is L(i) + U(i) long and is drawn a distance of U(i) above and L(i)
%   below the points in (X,Y).  The vectors X,Y,L and U must all be
%   the same length.  If X,Y,L and U are matrices then each column
%   produces a separate line.
%
%   ERRORBAR(X,Y,E) or ERRORBAR(Y,E) plots Y with error bars [Y-E Y+E].
%   ERRORBAR(...,'LineSpec') uses the color and linestyle specified by
%   the string 'LineSpec'.  See PLOT for possibilities.
%
%   H = ERRORBAR(...) returns a vector of line handles.
%
%   For example,
%      x = 1:10;
%      y = sin(x);
%      e = std(y)*ones(size(x));
%      errorbar(x,y,e)
%   draws symmetric error bars of unit standard deviation.

%   L. Shure 5-17-88, 10-1-91 B.A. Jones 4-5-93
%   Copyright 1984-2000 The MathWorks, Inc.
%   $Revision: 1.1 $  $Date: 2003/05/12 22:36:15 $

if min(size(x))==1,
  npt = length(x);
  x = x(:);
  y = y(:);
    if nargin > 2,
        if ~isstr(l),
            l = l(:);
        end
        if nargin > 3
            if ~isstr(u)
                u = u(:);
            end
        end
    end
else
  [npt,n] = size(x);
end

if nargin == 3
    if ~isstr(l)
        u = l;
        symbol = '-';
    else
        symbol = l;
        l = y;
        u = y;
        y = x;
        [m,n] = size(y);
        x(:) = (1:npt)'*ones(1,n);;
    end
end

if nargin == 4
    if isstr(u),
        symbol = u;
        u = l;
    else
        symbol = '-';
    end
end

if nargin == 2
    l = y;
    u = y;
    y = x;
    [m,n] = size(y);
    x(:) = (1:npt)'*ones(1,n);;
    symbol = '-';
end

u = abs(u);
l = abs(l);

if isstr(x) | isstr(y) | isstr(u) | isstr(l)
    error('Arguments must be numeric.')
end

if ~isequal(size(x),size(y)) | ~isequal(size(x),size(l)) | ~isequal(size(x),size(u)),
  error('The sizes of X, Y, L and U must be the same.');
end

tee = (max(x(:))-min(x(:)))/100;  % make tee .02 x-distance for error bars
xl = x - tee;
xr = x + tee;
ytop = y + u;
ybot = y - l;
n = size(y,2);

% Plot graph and bars
hold_state = ishold;
cax = newplot;
next = lower(get(cax,'NextPlot'));

% build up nan-separated vector for bars
xb = zeros(npt*9,n);
xb(1:9:end,:) = x;
xb(2:9:end,:) = x;
xb(3:9:end,:) = NaN;
xb(4:9:end,:) = xl;
xb(5:9:end,:) = xr;
xb(6:9:end,:) = NaN;
xb(7:9:end,:) = xl;
xb(8:9:end,:) = xr;
xb(9:9:end,:) = NaN;

yb = zeros(npt*9,n);
yb(1:9:end,:) = ytop;
yb(2:9:end,:) = ybot;
yb(3:9:end,:) = NaN;
yb(4:9:end,:) = ytop;
yb(5:9:end,:) = ytop;
yb(6:9:end,:) = NaN;
yb(7:9:end,:) = ybot;
yb(8:9:end,:) = ybot;
yb(9:9:end,:) = NaN;

[ls,col,mark,msg] = colstyle(symbol); if ~isempty(msg), error(msg); end
symbol = [ls mark col]; % Use marker only on data part
esymbol = [ '-' col]; % Make sure bars are solid

h = plot(xb,yb,esymbol); hold on
h = [h;plot(x,y,symbol)];

if ~hold_state, hold off; end

if nargout>0, hh = h; end