 function ftab = de_ftab(xrs, mas, varargin)
%function ftab = de_ftab(xrs, mas, [options])
%|
%| For multiple-kVp X-ray imaging, we must evaluate functions f_m(s_1, ..., s_L)
%| for m=1,..,M, where M is the number of kVp settings,
%| and L is the number of material components.
%| f_m(s1, s2) = -log( \int exp(- (m1(E) s1 + m2(E) s2)) dI_m(E) / I_m(E) )
%| (For "dual-energy" imaging, M = L = 2.)
%|
%| This routine builds tables/models of f_m, its inverse, and its derivatives.
%| These are needed for (dual-energy) X-ray CT reconstruction.
%|
%| in
%|	xrs	strum	X-ray spectra; see xray_read_spectra.m
%|	mas	strum	mass attenuation coefficients; see xray_read_mac.m
%|
%| option
%|	'sls'	strum	see de_ftab_sls.m
%|	'ftype'	char	fit type for de_ftab_fit() (default: '')
%|	'fit_args' {}	options for de_ftab_fit() (default: {})
%|			e.g.: {'wt', num2cell(ones(M,1))} weighting for fitting
%|	'ctype'	char	curv type for de_ftab_curv() (default: '')
%|	'show'	0|1	visualize
%|
%| out
%|	ftab	strum
%|		data:
%|			.xrs,.mas,.sls,.mac	based on input
%|			.fit	strum with fm approximation methods
%|				see de_ftab_fit.m
%|			.inv1	strum to invert 1 component BH, e.g., water
%|				see de_ftab_inv1.m
%|			.inv2	strum to invert BH by polynomials,
%|				see de_ftab_inv1.m
%|		methods:
%|			.fm_fun(s1, s2, ...) (probably not needed by user)
%|			.plot_fm()
%|			.plot_mac()
%|			.plot_jac()
%|			.plot_inv()
%|
%| Copyright 2008-6-15, Jeff Fessler, University of Michigan

if nargin == 1 && streq(xrs, 'test'), de_ftab_test, return, end
if nargin < 2, ir_usage, end

ftab.show = false;
ftab.sls = struct([]);
ftab.ftype = ''; % defer to de_ftab_fit() default
ftab.fit_args = {}; % defer to de_ftab_fit() default(s)
ftab.ctype = ''; % defer to de_ftab_curv() default ('newt' is fastest)
ftab = vararg_pair(ftab, varargin);

% X-ray spectra
if isempty(xrs)
%	xrs = {'mono,60,100'}; % monoenergetic for testing
	xrs = {'ps1'};
end
if iscell(xrs)
	xrs = xray_read_spectra(xrs{:});
end
if ftab.show
	clf, xrs.plot
prompt
end
ftab.xrs = xrs;

ftab.MM = size(xrs.sp, 2); % # of spectra

% mas: mass attenuation coefficient (strum), at tabulated energies
if isempty(mas)
	mas = {'soft', 'bone'};	% default materials
end
if iscell(mas)
	mas = xray_read_mac(mas);
end
if ftab.show
	clf, mas.plot('kev', xrs.en)
prompt
end
ftab.mas = mas;

% mac: mass attenuation coefficient, at spectrum energies
ftab.mac = xray_make_mac(xrs, mas);
ftab.LL = ncol(ftab.mac.mac);
if ftab.show
	pr ftab.mac.bar % 'bmassml'
%	pr cond(ftab.mac.bar) % examine condition number
	pr sqrt(cond(ftab.mac.bar' * ftab.mac.bar)) % examine condition number
	de_ftab_plot_mac(ftab);
prompt
end

% material integral sampling:
% s_l is samples of the integral of the density of the lth material type.
if isempty(ftab.sls)
	ftab.sls = de_ftab_sls;
end

% build tables of f_m(s1, s2)
fm_fun = @de_ftab_make_fm;
ftab.fm = fm_fun(ftab);
if ftab.show % figure showing f1, f2.  it looks linear, but it not quite!
	de_ftab_plot_fm(ftab, 'down', 4);
prompt
end

% parametric fit to each fm to form a continuous function!
printm 'fit'
if streq(ftab.ftype, 'exp3')
	ftab.fit = de_ftab_fit_exp3(ftab.xrs, ftab.mac.mac, ftab.fit_args{:});
else
	ftab.fit = de_ftab_fit(ftab.sls.sl, ftab.fm, 'type', ftab.ftype, ...
		'mtype', mas.type, ...
		'kev', xrs.en, 'mac', ftab.mac.mac, ... % needed for exp
		'macbar', ftab.mac.bar, ftab.fit_args{:});
% todo: may be bad (and slow) to use finely sampled energies (kev) for exp fit
%		'type', 'exp');
%ftab.fit = de_ftab_fit(ftab.sl, ftab.fm, 'type', 'poly', 'order', 3, 'dc', 1);
end

ftab.fit = de_ftab_curv(ftab.fit, 'ctype', ftab.ctype);

% Jacobian transformation
ftab.T = pinv(ftab.mac.bar); % [L M] transformation, fs = T f approx s !

% Build 1D inverse of 1st material component (usually water)
% for conventional "water only" beam-hardening correction.
ftab.inv1 = de_ftab_inv1(ftab.fit, ftab.sls.sl{1});

% build polynomial inverse approximation
ftab.inv2 = de_ftab_inv2(ftab.fit, ftab.sls.sl, 'T', ftab.T); % todo: options

%ftab.inv.eval = @(ftab,fhat) de_ftab_invert(ftab,fhat);
%ftab.inv = de_ftab_inv_setup(ftab);

meth = {'fm_fun', fm_fun, '(cell: sl)';
	'show_fm', @de_ftab_plot_fm, '()'; % backward compat
	'plot_fm', @de_ftab_plot_fm, '()';
	'plot_mac', @de_ftab_plot_mac, '()';
	'plot_jac', @de_ftab_plot_jac, '()';
	};

ftab = strum(ftab, meth);

ftab = de_ftab_inv_setup(ftab);

if ftab.show % compare fit to sampled fm, etc.
	ftab.inv1.plot(ftab.fit);
	if ~isempty(ftab.inv2)
		ftab.inv2.plot(ftab.fit, ftab.sls.sl);
	end
%	ftab.fit.show_fm(ftab.sls.sl, ftab.fm)
%	ftab.fit.show_err(ftab.sls.sl, ftab.fm)
	ftab.plot_jac;
	ftab.plot_inv;
%keyboard
end

% for safefty, remove table, forcing user to use ftab.fm_fun or ftab.fit.fmfun !
% ftab = rmfield(ftab, 'fm');

end % de_ftab()

%
% de_ftab_inv_setup()
% add methods related to inverse to ftab
%
function ftab = de_ftab_inv_setup(ftab)

printm 'inv: f -> s'

arg.inv = struct; % place holder
fun = @(ftab, fhat) de_ftab_invert(ftab, fhat);
meth = {'inv_fun', fun, '(fhat)';
	'plot_inv', @de_ftab_plot_inv, '()'};
ftab = strum(arg, meth, 'base', ftab);

end % de_ftab_inv_setup()

%
% de_ftab_plot_inv()
% plot inverse, testing with a different set of samples to see errors
%
function dummy = de_ftab_plot_inv(ftab)
dummy = [];
LL = ftab.LL;
MM = ftab.MM;
if LL ~= 2, warn 'L=2 done only', return, end
if MM < LL, warn('plot_inv with MM=%d < LL=%d skipped', MM, LL), return, end

if 1
	sls = ftab.sls;
	s1 = linspace(0, sls.max(1)+1, length(sls.sl{1}) + 8);
	s2 = linspace(0, sls.max(2)+1, length(sls.sl{2}) + 8);
	ss = ndgrid_jf('mat', s1, s2);
	ftmp = ftab.fit.fmfun(ss);
	stmp = ftab.inv_fun(ftmp);

	err = stmp - ss;
	err = abs(err);
	for ll=1:LL
		printm('worst inverse error l=%d: %g of %g', ll, ...
			max(col(stackpick(err,ll))), max(col(stackpick(ss,ll))))
	end
end

if im && usejava('jvm')
	ss1 = ss(:,:,1);
	ss2 = ss(:,:,2);
	clf, pl = @(n) subplot(220 + n);
	pl(1), mesh(s1, s2, ss1')
	axis tight, xtick, ytick, zwhite, grid
	hold on, plot3(ss1, ss2, stmp(:,:,1), 'y.'), hold off
	xlabel 's1', ylabel 's2', title 's1'

	pl(2), mesh(s1, s2, ss2')
	axis tight, xtick, ytick, zwhite, grid
	hold on, plot3(ss1, ss2, stmp(:,:,2), 'y.'), hold off
	xlabel 's1', ylabel 's2', title 's2'

	pl(3), mesh(s1, s2, err(:,:,1)')
	colormap hsv, caxis([0 max(err(:))]), cbar
	axis tight, xtick, ytick, zwhite, grid
	xlabel 's1', ylabel 's2', title 'error 1'

	pl(4), mesh(s1, s2, err(:,:,2)')
	axis tight, xtick, ytick, zwhite, grid
	xlabel 's1', ylabel 's2', title 'error 2'
prompt
end

end % de_ftab_plot_inv()

%
% de_ftab_plot_mac()
%
function dummy = de_ftab_plot_mac(ftab, varargin)
dummy = [];
xrs = ftab.xrs;
mas = ftab.mas;
mac = ftab.mac;

ltype = {'c:', 'y:', 'g:', 'r:'};
mtype = {'g+', 'r^', 'm*', 'yo'};
arg = {};
for ll=1:ftab.LL
	arg = {arg{:}, xrs.en, mac.mac(:,ll), ltype{ll}};
end
for mm=1:ftab.MM
	arg = {arg{:}, xrs.eff(mm), mac.bar(mm,:), mtype{mm}};
end

plot(arg{:})
axisy(0.1, max(0.7, 1.1*max(mac.bar(:))))
xlabel 'E', ylabel 'mac(E)'
mtype = mas.type;
legend(mtype{:})

prompt
end % de_ftab_plot_mac()

%
% de_ftab_make_fm()
% build tables of f_m(s1, s2, ...)
%
function fm = de_ftab_make_fm(ftab, varargin)
if length(varargin) == 0
	varargin = ftab.sls.sl; % default
end
if length(varargin) == 1 && iscell(varargin{1})
	sll = varargin{1};
else
	sll = ndgrid_jf('cell', varargin{:});
end
fm = de_ftab_fm(sll, ftab.mac.mac, ftab.xrs.Ide);

end % de_ftab_make_fm()

%
% de_ftab_plot_fm()
%
function dummy = de_ftab_plot_fm(ftab, varargin)
dummy = [];
arg.down = 1;
arg = vararg_pair(arg, varargin);

fm = ftab.fm;
sl = ftab.sls.sl;

if ftab.LL == 1
	plot(sl{1}, fm, '-', sl{1}, sl{1} * ftab.mac.bar(:)', ':')
	xlabel 's1'
	ylabel 'f1(s1)'
	name = ftab.xrs.name;
	legend(name{:}, 2)

elseif ftab.LL == 2

	fmax = 1.01 * max(fm(:));
	fmax = ceil(fmax);
	s_max(1) = max(sl{1}(:));
	s_max(2) = max(sl{2}(:));

	i1 = 1:arg.down:length(sl{1});
	i2 = 1:arg.down:length(sl{2});
	s1 = sl{1}(i1);
	s2 = sl{2}(i2);

	pl = @(mm) subplot(1,ftab.MM,mm);

	clf
	for mm=1:ftab.MM
		f1 = fm(i1,i2,mm);
		pl(mm)
		plot(s1, f1', '.-')
		axisx([0 s_max(1)]), xlabel 's_1'
		titlef('f_%d(s)', mm)
		text(-60, 10, '[cm^2/g]')
	end

	if usejava('jvm')
		prompt
	else
		warn 'need jvm for plot_fm mesh'
	return
	end

	clf
	for mm=1:ftab.MM
		f1 = fm(i1,i2,mm);
		pl(mm)
		mesh(s1, s2, f1')
		colormap hsv, caxis([0 fmax]), cbar
		axis([0 s_max(1) 0 s_max(2) 0 fmax])
		xtick, ytick, ztick, zwhite, xlabel s_1, ylabel s_2
		titlef('f_%d(s)', mm)
		text(-60, 10, '[cm^2/g]')
	end
end

end % de_ftab_plot_fm()

%
% de_ftab_test
%
function de_ftab_test
xrs = xray_read_spectra('ps1'); % M=2
%xrs = xray_read_spectra('poly1,60,90,120'); % M=3 test
%xrs = xray_read_spectra('poly1,160', 'filters', {{'copper', 0.1}}); % M < L test
xrs.plot
prompt
mas = xray_read_mac({'water', 'bone'});
if 0
	mas = xray_read_mac({'soft', 'iodine'});
	tmp = mas.mac_raw;
	tmp{2}(:) = tmp{2}(:) / 10; % todo: explore scaling to improve cond #
	mas.mac_raw = tmp;
end
ftab = de_ftab(xrs, mas, 'show', im, ...
	 'ftype','exp', 'fit_args', {'kev', 10:5:160, 'mac', []});
if im
	ftab.plot_fm;
	prompt
	ftab.inv1.plot(ftab.fit);
	if ~isempty(ftab.inv2)
		ftab.inv2.plot(ftab.fit, ftab.sls.sl);
	end
	clf, ftab.plot_mac;
	ftab.plot_jac;
	ftab.plot_inv;
end
end % de_ftab_test


% Crude approximations for the W function (used by bisect)
%
% Syntax:
% crude(xx,nb)
%
% xx is the argument, W(xx)
%
% nb is the branch of the W function needed:
% nb = 0 - upper branch, Wp
% nb <> 0 - lower branch, Wm
%
function c=crude(xx,nb)
%
% D. A. Barry, 23 June 2003 (d.a.barry@ed.ac.uk)
%
% Various constants
%
em=-exp(-1);
em9=-exp(-9);
c13=1/3;
em2=2/em;
s2=sqrt(2);
s21=2*s2-3;
s22=4-3*s2;
s23=s2-2;
if nb == 0
%
%  Calculations for crude Wp
%
   if xx <= 20
      reta=s2*sqrt(1-xx/em);
      an2=4.612634277343749*sqrt(sqrt(reta+1.09556884765625));
      c=reta/(1+reta/(3+(s21*an2+s22)*reta/(s23*(an2+reta))))-1;
   else
      zl=log(xx);
      c=log(xx/log(xx/zl^exp(-1.124491989777808/(.4225028202459761+zl))));
   end
else
%
%  Calculations for crude Wm
%
   if xx <= em9
      zl=log(-xx);
      t=-1-zl;
      ts=sqrt(t);
      c=zl-(2*ts)/(s2+(c13-t/(270+ts*127.0471381349219))*ts);
   else
      zl=log(-xx);
      eta=2-em2*xx;
      c=log(xx/log(-xx/((1-.5043921323068457*(zl+1))*(sqrt(eta)+eta/3)+1)));
   end
end


% computes bootstrap bias corrected accelerated bootstrap confidence
% interval and returns if it includes 0 as a logical vector sig
% mu_est - estimated parameter from complete sample
% bs_est - bootstrap estimates
% jk_est - jacknife estimates
% alpha - desired FPR
%
% Written by Bogdan Petre, 2020
function [sig, CI] = BCa(mu_est, bs_est, jk_est, alpha)
    fprintf('Estimating BCa CIs...\n')
    bias = sum(bs_est < mu_est)./length(bs_est);
    z0 = icdf('norm', bias, 0, 1);

    jk_mean = mean(jk_est);
    num = sum((jk_mean - jk_est).^3);
    den = sum((jk_mean - jk_est).^2);
    a = num ./ (6*den.^(3/2));

    zL = z0 + icdf('norm',alpha/2,0,1);
    alpha1 = normcdf(z0 + zL./(1-a.*zL));
    zU = z0 + icdf('norm',1-alpha/2,0,1);
    alpha2 = normcdf(z0 + zU./(1-a.*zL));

    CI = quantile(bs_est, [alpha1, alpha2]);

    sig = prod(sign(CI),2) > 0;
end


%STUMPC Decision stump classifier
%
%   W = STUMPC(A,CRIT,N)
%   W = A*STUMPC([],CRIT,N)
%   W = A*STUMPC(CRIT,N)
%
% Computation of a decision tree classifier out of a dataset A using
% a binary splitting criterion CRIT:
%   INFCRIT  -  information gain
%   MAXCRIT  -  purity (default)
%   FISHCRIT -  Fisher criterion
% Just N (default N=1) nodes are computed.
%
% see also DATASETS, MAPPINGS, TREEC, TREE_MAP

% Copyright: R.P.W. Duin, r.p.w.duin@37steps.com
% Faculty EWI, Delft University of Technology
% P.O. Box 5031, 2600 GA Delft, The Netherlands

% $Id: stumpc.m,v 1.2 2009/07/10 11:19:20 duin Exp $

function w = stumpc(varargin)

	mapname = 'Decision Stump';
  argin = shiftargin(varargin,{'char','integer'});
  argin = shiftargin(argin,'integer',2);
  argin = setdefaults(argin,[],'maxcrit',1);

  if mapping_task(argin,'definition')
    w = define_mapping(argin,'untrained',mapname);

  elseif mapping_task(argin,'training')			% Train a mapping.

    [a,crit,n] = deal(argin{:});
    islabtype(a,'crisp');
    isvaldset(a,1,2); % at least 1 object per class, 2 classes

    % First get some useful parameters:
    [m,k,c] = getsize(a);
    nlab = getnlab(a);
    tree = maketree(+a,nlab,c,crit,n);

    % Store the results:
    w = prmapping('tree_map','trained',{tree,1},getlablist(a),k,c);
    w = setname(w,mapname);
    w = setcost(w,a);

  end

	return

%MAKETREE General tree building algorithm
%
% 	tree = maketree(A,nlab,c,crit,stop)
%
% Constructs a binary decision tree using the criterion function
% specified in the string crit ('maxcrit', 'fishcrit' or 'infcrit'
% (default)) for a set of objects A. stop is a counter for the number of
% branches that are allowed below the present.
%
% Definition of the resulting tree:
%
% 	tree(n,1) - feature number to be used in node n
% 	tree(n,2) - threshold t to be used
% 	tree(n,3) - node to be processed if value <= t
% 	tree(n,4) - node to be processed if value > t
% 	tree(n,5:4+c) - aposteriori probabilities for all classes in
% 			node n
%
% If tree(n,3) == 0, stop, class in tree(n,1)
%
% This is a low-level routine called by treec.
%
% See also infstop, infcrit, maxcrit, fishcrit and mapt.

% Authors: Guido te Brake, TWI/SSOR, Delft University of Technology
%     R.P.W. Duin, TN/PH, Delft University of Technology
% Copyright: R.P.W. Duin, duin@ph.tn.tudelft.nl
% Faculty of Applied Physics, Delft University of Technology
% P.O. Box 5046, 2600 GA Delft, The Netherlands

function tree = maketree(a,nlab,c,crit,n)
		[m,k] = size(a);

	% Construct the tree:

	% When all objects have the same label, create an end-node:
	if all([nlab == nlab(1)])
		% Avoid giving 0-1 probabilities, but 'regularize' them a bit using
		% a 'uniform' Bayesian prior:
		p = ones(1,c)/(m+c); p(nlab(1)) = (m+1)/(m+c);
		tree = [nlab(1),0,0,0,p];
	else
		% now the tree is recursively constructed further:
		[f,j,t] = feval(crit,+a,nlab); % use desired split criterion

		p = sum(expandd(nlab),1);
		if length(p) < c, p = [p,zeros(1,c-length(p))]; end
		% When the stop criterion is not reached yet, we recursively split
		% further:

		if n >= 1
			% Make the left branch:
			J = find(a(:,j) <= t);
      tl = maketree(+a(J,:),nlab(J),c,crit,n-1);

      % Make the right branch:
			K = find(a(:,j) > t);
			tr = maketree(+a(K,:),nlab(K),c,crit,n-1);

      % Fix the node labelings before the branches can be 'glued'
			% together to a big tree:
			[t1,t2] = size(tl);
      tl = tl + [zeros(t1,2) tl(:,[3 4])>0 zeros(t1,c)];
			[t3,t4] = size(tr);
			tr = tr + (t1+1)*[zeros(t3,2) tr(:,[3 4])>0 zeros(t3,c)];

      % Make the complete tree: the split-node and the branches:
			tree= [[j,t,2,t1+2,(p+1)/(m+c)]; tl; tr];
		else
			% We reached the stop criterion, so make an end-node:
			[mt,cmax] = max(p);
			tree = [cmax,0,0,0,(p+1)/(m+c)];
		end

	end
	return

%MAXCRIT Maximum entropy criterion for best feature split.
%
% 	[f,j,t] = maxcrit(A,nlabels)
%
% Computes the value of the maximum purity f for all features over
% the data set A given its numeric labels. j is the optimum feature,
% t its threshold. This is a low level routine called for constructing
% decision trees.
%
% [1] L. Breiman, J.H. Friedman, R.A. Olshen, and C.J. Stone,
% Classification and regression trees, Wadsworth, California, 1984.

% Copyright: R.P.W. Duin, duin@ph.tn.tudelft.nl
% Faculty of Applied Physics, Delft University of Technology
% P.O. Box 5046, 2600 GA Delft, The Netherlands

function [f,j,t] = maxcrit(a,nlab)
		[m,k] = size(a);
	c = max(nlab);
	% -variable T is an (2c)x k matrix containing:
	%      minimum feature values class 1
	%      maximum feature values class 1
	%      minimum feature values class 2
	%      maximum feature values class 2
	%            etc.
	% -variable R (same size) contains:
	%      fraction of objects which is < min. class 1.
	%      fraction of objects which is > max. class 1.
	%      fraction of objects which is < min. class 2.
	%      fraction of objects which is > max. class 2.
	%            etc.
	% These values are collected and computed in the next loop:
	T = zeros(2*c,k); R = zeros(2*c,k);
	for j = 1:c
		L = (nlab == j);
		if sum(L) == 0
			T([2*j-1:2*j],:) = zeros(2,k);
			R([2*j-1:2*j],:) = zeros(2,k);
		else
			T(2*j-1,:) = min(a(L,:),[],1);
			R(2*j-1,:) = sum(a < ones(m,1)*T(2*j-1,:),1);
			T(2*j,:) = max(a(L,:),[],1);
			R(2*j,:) = sum(a > ones(m,1)*T(2*j,:),1);
		end
	end
	% From R the purity index for all features is computed:
	G = R .* (m-R);
	% and the best feature is found:
	[gmax,tmax] = max(G,[],1);
	[f,j] = max(gmax);
	Tmax = tmax(j);
	if Tmax ~= 2*floor(Tmax/2)
		t = (T(Tmax,j) + max(a(find(a(:,j) < T(Tmax,j)),j)))/2;
	else
		t = (T(Tmax,j) + min(a(find(a(:,j) > T(Tmax,j)),j)))/2;
	end
	return

%INFCRIT The information gain and its the best feature split.
%
% 	[f,j,t] = infcrit(A,nlabels)
%
% Computes over all features the information gain f for its best
% threshold from the dataset A and its numeric labels. For f=1:
% perfect discrimination, f=0: complete mixture. j is the optimum
% feature, t its threshold. This is a lowlevel routine called for
% constructing decision trees.

% Copyright: R.P.W. Duin, duin@ph.tn.tudelft.nl
% Faculty of Applied Physics, Delft University of Technology
% P.O. Box 5046, 2600 GA Delft, The Netherlands

function [g,j,t] = infcrit(a,nlab)
		[m,k] = size(a);
	c = max(nlab);
	mininfo = ones(k,2);
	% determine feature domains of interest
	[sn,ln] = min(a,[],1);
	[sx,lx] = max(a,[],1);
	JN = (nlab(:,ones(1,k)) == ones(m,1)*nlab(ln)') * realmax;
	JX = -(nlab(:,ones(1,k)) == ones(m,1)*nlab(lx)') * realmax;
	S = sort([sn; min(a+JN,[],1); max(a+JX,[],1); sx]);
	% S(2,:) to S(3,:) are interesting feature domains
	P = sort(a);
	Q = (P >= ones(m,1)*S(2,:)) & (P <= ones(m,1)*S(3,:));
	% these are the feature values in those domains
	for f=1:k,		% repeat for all features
		af = a(:,f);
		JQ = find(Q(:,f));
		SET = P(JQ,f)';
		if JQ(1) ~= 1
			SET = [P(JQ(1)-1,f), SET];
		end
		n = length(JQ);
		if JQ(n) ~= m
			SET = [SET, P(JQ(n)+1,f)];
		end
		n = length(SET) -1;
		T = (SET(1:n) + SET(2:n+1))/2; % all possible thresholds
		L = zeros(c,n); R = L;     % left and right node object counts per class
		for j = 1:c
			J = find(nlab==j); mj = length(J);
			if mj == 0
				L(j,:) = realmin*ones(1,n); R(j,:) = L(j,:);
			else
				L(j,:) = sum(repmat(af(J),1,n) <= repmat(T,mj,1)) + realmin;
				R(j,:) = sum(repmat(af(J),1,n) > repmat(T,mj,1)) + realmin;
			end
		end
		infomeas =  - (sum(L .* log10(L./(ones(c,1)*sum(L)))) ...
			       + sum(R .* log10(R./(ones(c,1)*sum(R))))) ...
		    ./ (log10(2)*(sum(L)+sum(R))); % criterion value for all thresholds
		[mininfo(f,1),j] = min(infomeas);     % finds the best
		mininfo(f,2) = T(j);     % and its threshold
	end
	g = 1-mininfo(:,1)';
	[finfo,j] = min(mininfo(:,1));		% best over all features
	t = mininfo(j,2);			% and its threshold
	return

%FISHCRIT Fisher's Criterion and its best feature split
%
% 	[f,j,t] = fishcrit(A,nlabels)
%
% Computes the value of the Fisher's criterion f for all features
% over the dataset A with given numeric labels. Two classes only. j
% is the optimum feature, t its threshold. This is a lowlevel
% routine called for constructing decision trees.

% Copyright R.P.W. Duin, duin@ph.tn.tudelft.nl
% Faculty of Applied Physics, Delft University of Technology
% P.O. Box 5046, 2600 GA Delft, The Netherlands

function [f,j,t] = fishcrit(a,nlab)
		[m,k] = size(a);
	c = max(nlab);
	if c > 2
		error('Not more than 2 classes allowed for Fisher Criterion')
	end
	% Get the mean and variances of both the classes:
	J1 = find(nlab==1);
	J2 = find(nlab==2);
	u = (mean(a(J1,:),1) - mean(a(J2,:),1)).^2;
	s = std(a(J1,:),0,1).^2 + std(a(J2,:),0,1).^2 + realmin;
	% The Fisher ratio becomes:
	f = u ./ s;
	% Find then the best feature:
	[ff,j] = max(f);
	% Given the feature, compute the threshold:
	m1 = mean(a(J1,j),1);
	m2 = mean(a(J2,j),1);
	w1 = m1 - m2; w2 = (m1*m1-m2*m2)/2;
	if abs(w1) < eps % the means are equal, so the Fisher
			 % criterion (should) become 0. Let us set the thresold
			 % halfway the domain
			 t = (max(a(J1,j),[],1) + minc(a(J2,j),[],1)) / 2;
	else
		t = w2/w1;
	end
	return

%INFSTOP Quinlan's Chi-square test for early stopping
%
% 	crt = infstop(A,nlabels,j,t)
%
% Computes the Chi-square test described by Quinlan [1] to be used
% in maketree for forward pruning (early stopping) using dataset A
% and its numeric labels. j is the feature used for splitting and t
% the threshold.
%
% [1] J.R. Quinlan, Simplifying Decision Trees,
% Int. J. Man - Machine Studies, vol. 27, 1987, pp. 221-234.
%
% See maketree, treec, classt, prune

% Guido te Brake, TWI/SSOR, TU Delft.
% Copyright: R.P.W. Duin, duin@ph.tn.tudelft.nl
% Faculty of Applied Physics, Delft University of Technology
% P.O. Box 5046, 2600 GA Delft, The Netherlands

function crt = infstop(a,nlab,j,t)
		[m,k] = size(a);
	c = max(nlab);
	aj = a(:,j);
	ELAB = expandd(nlab);
	L = sum(ELAB(aj <= t,:),1) + 0.001;
	R = sum(ELAB(aj > t,:),1) + 0.001;
	LL = (L+R) * sum(L) / m;
	RR = (L+R) * sum(R) / m;
	crt = sum(((L-LL).^2)./LL + ((R-RR).^2)./RR);
	return

%PRUNEP Pessimistic pruning of a decision tree
%
% 	tree = prunep(tree,a,nlab,num)
%
% Must be called by giving a tree and the training set a. num is the
% starting node, if omitted pruning starts at the root. Pessimistic
% pruning is defined by Quinlan.
%
% See also maketree, treec, mapt

% Guido te Brake, TWI/SSOR, TU Delft.
% Copyright: R.P.W. Duin, duin@ph.tn.tudelft.nl
% Faculty of Applied Physics, Delft University of Technology
% P.O. Box 5046, 2600 GA Delft, The Netherlands

function tree = prunep(tree,a,nlab,num)
		if nargin < 4, num = 1; end;
	[N,k] = size(a);
	c = size(tree,2)-4;
	if tree(num,3) == 0, return, end;
	w = prmapping('treec','trained',{tree,num},[1:c]',k,c);
	ttt=tree_map(prdataset(a,nlab),w);
	J = testc(ttt)*N;
	EA = J + nleaves(tree,num)./2;   % expected number of errors in tree
	P = sum(expandd(nlab,c),1);     % distribution of classes
					%disp([length(P) c])
					[pm,cm] = max(P);     % most frequent class
					E = N - pm;     % errors if substituted by leave
					SD = sqrt((EA * (N - EA))/N);
					if (E + 0.5) < (EA + SD)	     % clean tree while removing nodes
						[mt,kt] = size(tree);
						nodes = zeros(mt,1); nodes(num) = 1; n = 0;
						while sum(nodes) > n;	     % find all nodes to be removed
							n = sum(nodes);
							J = find(tree(:,3)>0 & nodes==1);
							nodes(tree(J,3)) = ones(length(J),1);
							nodes(tree(J,4)) = ones(length(J),1);
						end
						tree(num,:) = [cm 0 0 0 P/N];
						nodes(num) = 0; nc = cumsum(nodes);
						J = find(tree(:,3)>0);% update internal references
						tree(J,[3 4]) = tree(J,[3 4]) - reshape(nc(tree(J,[3 4])),length(J),2);
						tree = tree(~nodes,:);% remove obsolete nodes
					else
						K1 = find(a(:,tree(num,1)) <= tree(num,2));
						K2 = find(a(:,tree(num,1)) >  tree(num,2));

						tree = prunep(tree,a(K1,:),nlab(K1),tree(num,3));
						tree = prunep(tree,a(K2,:),nlab(K2),tree(num,4));
					end
					return

%PRUNET Prune tree by testset
%
% 	tree = prunet(tree,a)
%
% The test set a is used to prune a decision tree.

% Copyright: R.P.W. Duin, duin@ph.tn.tudelft.nl
% Faculty of Applied Physics, Delft University of Technology
% P.O. Box 5046, 2600 GA Delft, The Netherlands

function tree = prunet(tree,a)
		[m,k] = size(a);
	[n,s] = size(tree);
	c = s-4;
	erre = zeros(1,n);
	deln = zeros(1,n);
	w = prmapping('treec','trained',{tree,1},[1:c]',k,c);
	[f,lab,nn] = tree_map(a,w);  % bug, this works only if a is dataset, labels ???
	[fmax,cmax] = max(tree(:,[5:4+c]),[],2);
	nngood = nn([1:n]'+(cmax-1)*n);
	errn = sum(nn,2) - nngood;% errors in each node
	sd = 1;
	while sd > 0
		erre = zeros(n,1);
		deln = zeros(1,n);
		endn = find(tree(:,3) == 0)';	% endnodes
		pendl = max(tree(:,3*ones(1,length(endn)))' == endn(ones(n,1),:)');
		pendr = max(tree(:,4*ones(1,length(endn)))' == endn(ones(n,1),:)');
		pend = find(pendl & pendr);		% parents of two endnodes
		erre(pend) = errn(tree(pend,3)) + errn(tree(pend,4));
		deln = pend(find(erre(pend) >= errn(pend))); % nodes to be leaved
		sd = length(deln);
		if sd > 0
			tree(tree(deln,3),:) = -1*ones(sd,s);
			tree(tree(deln,4),:) = -1*ones(sd,s);
			tree(deln,[1,2,3,4]) = [cmax(deln),zeros(sd,3)];
		end
	end
	return

%NLEAVES Computes the number of leaves in a decision tree
%
% 	number = nleaves(tree,num)
%
% This procedure counts the number of leaves in a (sub)tree of the
% tree by using num. If num is omitted, the root is taken (num = 1).
%
% This is a utility used by maketree.

% Guido te Brake, TWI/SSOR, TU Delft
% Copyright: R.P.W. Duin, duin@ph.tn.tudelft.nl
% Faculty of Applied Physics, Delft University of Technology
% P.O. Box 5046, 2600 GA Delft, The Netherlands

function number = nleaves(tree,num)
		if nargin < 2, num = 1; end
	if tree(num,3) == 0
		number = 1 ;
	else
		number = nleaves(tree,tree(num,3)) + nleaves(tree,tree(num,4));
	end
	return


function [x_best, fit_best, x_pop, fit_pop stats]=complexmethod(fcn_name,bounds,gen_max,x_start,fit_start,fcn_opts,complex_opts)
%[x_best, fit_best, x_pop, fit_pop stats]=complexmethod(fcn_name,bounds,gen_max,x_start,fit_start,fcn_opts,complex_opts)
%
% Implements the Complex Method of Contrained Optimization, as proposed by Box (1965),
% improved by Guin (1968) and Krus (1992), and following the method in Andresson (2001).
%
% Takes inputs of:
% fcn_name: string including a function to maximize.  This function must give outputs of
%   [fitness x] and take inputs of (x,fcn_opts)
% bounds: 2 by n_params matrix of the minimum and maximum bounds for each parameter
% gen_max; number of times to calculate fitness before stopping (this includes the
%    initialization calculations if fit_start is not given
% x_start: a n_population by n_params matrix of initial parameters
% fit_start: a n_params vector of fitnesses of x_start params
% fcn_opts: a variable (or structure) that may be passed to the fcn_name function
% complex_opts: a variable including some options for the optimization (you probably won't need this)
%
% References
%
%Andersson J, "Multiobjective Optimization in Engineering Design - Application to fluid Power %Systems." Doctoral thesis, Division of Fluid and Mechanical Engineering Systems, Department %of Mechanical Engineering, Linkping University, 2001 %http://citeseer.ist.psu.edu/562279.html
%
%Box, M.J., "A new method of constrained optimization and a comparison with other method," %Computer Journal, Vol. 8, No. 1, pp. 42-52, 1965.
%
%Guin J. A., "Modification of the Complex method of constraint optimization," Computer Jornal, %vol. 10, pp. 416-417, 1968. 32(34)
%
%KRUS P., JANSON A., PALMBERG J.-O., “Optimization Based on Simulation for Design of Fluid %Power Systems, in Proceedings of ASME Winter Annual Meeting, Anaheim, USA, 1992.
%
%    Copyright Travis Wiens 2008
%

if nargin<2
	error('Insufficient arguements')
end
if nargin<3
	gen_max=1000;
elseif isempty(gen_max)
    gen_max=1000;
end

if nargin<4
	x_start=[];
end
if nargin<5
	fit_start=[];
end
if nargin<6
	fcn_opts=[];
end
if nargin<7
	complex_opts.alpha=1.3;%constant determines how far to overshoot centroid
	complex_opts.n_r=4;%constant for adjusting to repeated unimprovements
elseif isempty(complex_opts)
    complex_opts.alpha=1.3;%constant determines how far to overshoot centroid
	complex_opts.n_r=4;%constant for adjusting to repeated unimprovements
end

fcn_str=['[fitness, x]=' fcn_name '(x,fcn_opts);'];%string to determine fitness
%output of x allows function to change x if desired

fit_best=-inf;%initial best fitness

n_params=size(bounds,2);

gen=1;%initialize number of generations

alpha=complex_opts.alpha;%copy values
n_r=complex_opts.n_r;

if isempty(x_start)
	f_excess=1.5;%factor to increase population over minimum (must be >=1)
	n_pop=round((n_params+1)*f_excess);
	x_pop=ones(n_pop,1)*bounds(1,:) +(ones(n_pop,1)*(bounds(2,:)-bounds(1,:))).*rand(n_pop,n_params);
else
	x_pop=x_start;
	n_pop=size(x_pop,1);

end

%initialize fitness
if isempty(fit_start)
	fit_pop=zeros(1,n_pop);
	for i=1:n_pop
		x=x_pop(i,:);
		eval(fcn_str); %get fitness for each x
		x_pop(i,:)=x;
		fit_pop(i)=fitness;
		gen=gen+1;
	end
else
	fit_pop=fit_start;
	if numel(fit_pop)~=n_pop
		error('Incorrect size of fit_pop');
	end
end

stats.trace_fitness=zeros(1,gen_max);
while (gen<gen_max)

	[fit_best idx_best]=max(fit_pop);%find best fitness
	x_best=x_pop(idx_best,:);%find best params
	[fit_worst idx_worst]=min(fit_pop);%find worst
	x_worst=x_pop(idx_worst,:);%find best params

	x_centroid=(sum(x_pop)-x_worst)/(n_pop-1);%calc centroid of all but worst x

	x_new=x_centroid+alpha*(x_centroid-x_worst);%"mirror" worst point through centroid

	idx=find(x_new<bounds(1,:));%find params out of bounds
	x_new(idx)=bounds(1,idx);
	idx=find(x_new>bounds(2,:));
	x_new(idx)=bounds(2,idx);

	x=x_new;
	eval(fcn_str); %get fitness for new x
	x_new=x;
	fit_new=fitness;
	stats.trace_fitness(gen)=fitness;
	gen=gen+1;%increment generation

	if fit_new<fit_worst %check if the new point is still worst
		k_r=1;%reset number of iterations
		while (fit_new<fit_worst&gen<gen_max)
			epsilon=(n_r/(n_r+k_r-1))^((n_r+k_r-1)/n_r);
			k_r=k_r+1;%increment number of iterations
			R=rand;
			x_store=x_new;%store previous x_new
			x_new=(x_new+epsilon*x_centroid+(1-epsilon)*x_best)/2+(x_centroid-x_best)*(1-epsilon)*(2*R-1);

			idx=find(x_new<bounds(1,:));%find params out of bounds
			x_new(idx)=bounds(1,idx);
			idx=find(x_new>bounds(2,:));
			x_new(idx)=bounds(2,idx);

			x=x_new;
			eval(fcn_str); %get fitness for new x
			x_new=x;
			fit_new=fitness;
			stats.trace_fitness(gen)=fitness;
			gen=gen+1;%increment generation
		end
	end
	x_pop(idx_worst,:)=x_new;%replace worst params with new params
	fit_pop(idx_worst)=fit_new;
end

[fit_best idx_best]=max(fit_pop);%find best fitness
x_best=x_pop(idx_best,:);%find best params


function [u,Du,eqn,info] = PoissonQ1old(node,elem,pde,bdFlag, option)
%% POISSONQ1 Poisson equation: Q1 bilinear element.
%
%   u = PoissonQ1(node,elem,pde,bdFlag) produces the bilinear finite element
%   approximation of the Poisson equation
%
%       -div(d*grad(u))=f  in \Omega, with
%       Dirichlet boundary condition u=g_D on \Gamma_D,
%       Neumann boundary condition   d*grad(u)*n=g_N on \Gamma_N,
%       Robin boundary condition     g_R*u + d*grad(u)*n=g_N on \Gamma _R
%
%   The quad mesh is given by node and elem and the boundary edge is given by
%   bdFlag. See meshdoc, bddoc for details. The data is given by the
%   structure pde which contains function handles f, g_D, g_N, g_R, or d.
%   For general elliptic equations with convection and reaction
%   coefficients, see ellipticpde.
%
%   The function Poisson assembes the matrix equation AD*u = b and solves
%   it by the direct solver.
%
%   The diffusion coefficient d is a scalar function or a column array with
%   the same length as the elem array.
%
%   u = PoissonQ1(node,elem,pde,bdFlag,option) specifies the options.
%    - option.dquadorder: quadrature order for diffusion coefficients
%    - option.fquadorder: quadrature order for computing right hand side f
%
%   When only one type of boundary condition is imposed, the input argument
%   bdFlag can be skipped. The boundary condition is implicitly given in
%   the pde structure by specifying g_D or g_N only. See examples below.
%
%   [u,Du] = PoissonQ1(node,elem,pde,bdFlag) returns also the
%   non-modified stiffness matrix A, which is semi-definite. The kernel of
%   A consists of constant vectors. The matrix A can be used to evulate the
%   bilinear form A(u,v) = u'*A*v, especially the enery norm of a finite
%   element function u by sqrt(u'*A*u).
%
%   [u,Du,eqn] = PoissonQ1(node,elem,pde,bdFlag) returns also the equation
%   structure eqn, which includes:
%     - eqn.AD:  modified stiffness matrix AD;
%     - eqn.b:   the right hand side.
%   The solution u = AD\b. The output eqn can be used to test other solvers.
%
%
%   Example
%     clear all
%     node=  [0, 0; 1, 0; 1, 1; 0,1];
%     elem = [1,2,3,4];
%     for k = 1:4
%         [node,elem] = uniformrefinequad(node,elem);
%     end
%     % Homogenous Dirichlet boundary condition
%     pde.f = inline('ones(size(p,1),1)','p');
%     pde.g_D = 0;
%     u = PoissonQ1(node,elem,pde);
%     figure(1);
%     showsolution(node,elem,u);
%     % Non-homogenous Dirichlet boundary condition
%     pde.f = inline('-4*ones(size(p,1),1)','p');
%     pde.g_D = inline('p(:,1).^2 + p(:,2).^2','p');
%     u = PoissonQ1(node,elem,pde);
%     figure(2);
%     showsolution(node,elem,u);
%     % Homogenous Neumann boundary condition
%     clear pde
%     pde.f = inline('pi^2*cos(pi*p(:,1)).*cos(pi*p(:,2))','p');
%     u = PoissonQ1(node,elem,pde);
%     figure(3);
%     showsolution(node,elem,u);
%
% See also: Poisson3Q1
%
% Author: Huayi Wei < huayiwei1984@gmail.com>.
%
% Modified by Long Chen. Change the assembling process. Add mg solver and
% update boundary condition.
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

if ~exist('option','var'), option = []; end
[N,Dim] = size(node);
[NT,NV] = size(elem);
Ndof = N;

tic;
%% Assemble stiffness matrix
% generate sparse pattern
ii = zeros(10*NT,1); jj = zeros(10*NT,1);
index = 0;
for i = 1:4
    for j = i:4
        ii(index+1:index+NT) = double(elem(:,i));
        jj(index+1:index+NT) = double(elem(:,j));
        index = index + NT;
    end
end
% quadrature points
if ~isfield(pde,'d'), pde.d = []; end
if ~isfield(option,'dquadorder')
    option.dquadorder = 2;        % default order is exact for quadratic function
end
[pts, w] = quadptsquad(option.dquadorder);
nQuad = size(pts,1);
% compute non-zeros
sA = zeros(10*NT,nQuad);
for p = 1:nQuad
    % Dphi at quadrature points
   [phi, Dphip, J] = quadbasis(node,elem,pts(p,:));
    index = 0;
    for i = 1:4
        for j = i:4
            Aij = 0;
            if isempty(pde.d) || isnumeric(pde.d)
                Aij = Aij + w(p)*dot(Dphip(:,:,i),Dphip(:,:,j),2);
            else
                pxy = zeros(NT, Dim);
                for ip = 1:Dim
                    xi = node(:,ip);
                    pxy(:,ip) = xi(elem)*phi;
                end
                Aij = Aij + w(p)*dot(Dphip(:,:,i),Dphip(:,:,j),2).*pde.d(pxy);
            end
            if ~isempty(pde.d) && isnumeric(pde.d) % d is piecewise constant
                Aij = pde.d.*Aij;
            end
            Aij = Aij.*J;
            sA(index+1:index+NT,p) = Aij;
            index = index + NT;
        end
    end
end
sA = sum(sA,2);
% assemble the matrix
diagIdx = (ii == jj);   upperIdx = ~diagIdx;
A = sparse(ii(diagIdx),jj(diagIdx),sA(diagIdx),Ndof,Ndof);
AU = sparse(ii(upperIdx),jj(upperIdx),sA(upperIdx),Ndof,Ndof);
A = A + AU + AU';
clear Aij ii jj Dphip

%% Assemble the right hand side
b = zeros(Ndof,1);
if ~isfield(option,'fquadorder')
    option.fquadorder = 3;   % default order
end
if ~isfield(pde,'f') || (isreal(pde.f) && (pde.f==0))
    pde.f = [];
end

if ~isempty(pde.f)
    [pts, weight] = quadptsquad(option.fquadorder);
    nQuad = size(pts,1);
    bt = zeros(NT,NV);
    for p = 1:nQuad
		% quadrature points in the x-y coordinate
        [phi, tempvar, J] = quadbasis(node,elem, pts(p,:));
        pxy = zeros(NT, Dim);
        for i = 1:Dim
            xi = node(:,i);
            pxy(:,i) = xi(elem)*phi; % ? questionable
        end
		fp = pde.f(pxy);
        bt = bt + (weight(p)*fp.*J)*phi';
    end
    b = accumarray(elem(:),bt(:),[N 1]);
end
clear pxy bt

%% Set up boundary conditions
if ~exist('bdFlag','var'), bdFlag = []; end
[AD,b,u,freeNode,isPureNeumann] = getbd(b);

%% Record assembling time
assembleTime = toc;
if ~isfield(option,'printlevel'), option.printlevel = 1; end
if option.printlevel >= 2
    fprintf('Time to assemble matrix equation %4.2g s\n',assembleTime);
end

%% Solve the system of linear equations
if isempty(freeNode), return; end
% Set up solver type
if isempty(option) || ~isfield(option,'solver')    % no option.solver
    if Ndof <= 2e3  % Direct solver for small size systems
        option.solver = 'direct';
    else            % MGCG  solver for large size systems
        option.solver = 'mg';
    end
end
solver = option.solver;
% solve
switch solver
    case 'direct'
        tic;
        u(freeNode) = AD(freeNode,freeNode)\b(freeNode);
        residual = norm(b - AD*u);
        info = struct('solverTime',toc,'itStep',0,'err',residual,'flag',2,'stopErr',residual);
    case 'none'
        info = struct('solverTime',[],'itStep',0,'err',[],'flag',3,'stopErr',[]);
    case 'mg'
%         option.x0 = u;
        option.solver = 'CG';
        [u,info] = mg(AD,b,elem,option);
    case 'amg'
        option.solver = 'CG';
        [u(freeNode),info] = amg(AD(freeNode,freeNode),b(freeNode),option);
end
% post-process for pure Neumann problem
if isPureNeumann
    patchArea = accumarray(elem(:),[J;J;J;J]/4, [N 1]);
    uc = sum(u.*patchArea)/sum(J);
    u = u - uc;   % int u = 0
end

%% Output information
eqn = struct('A',AD,'b',b,'freeNode',freeNode);
info.assembleTime = assembleTime;

%% Compute Du
Du = [];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% subfunctions getbd
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function [AD,b,u,freeNode,isPureNeumann] = getbd(b)
    %% Set up of boundary conditions.
    %
    % 1) Modify the matrix for Dirichlet boundary nodes, which are not degree
    % of freedom. Values at these nodes are evaluatation of pde.g_D. The
    % original stiffness matrix A is turn into the matrix AD by enforcing
    % AD(fixedNode,fixedNode)=I, AD(fixedNode,freeNode)=0, AD(freeNode,fixedNode)=0.
    %
    % 2) Modify the right hand side b. The Neumann boundary integral is added
    % to b. For Dirichlet boundary ndoes, b(fixedNode) is the evaluation of
    % pde.g_D.
    %
    % Special attentation should be given for the pure Neumann boundary
    % condition. To enforce the compatible condition, the vector b should have
    % mean value zero. To avoid a singular matrix, the 1st node is chosen as
    % fixedNode.
    %
    % The order of assigning Neumann and Dirichlet boundary condition is
    % important to get the right setting at the intersection nodes of Dirichlet
    % and Neumann boundary edges.
    %
    % Reference: Long Chen. Finite Element Methods and its Programming. Lecture
    % Notes.

    u = zeros(Ndof,1);
    %% Initial check
    if ~isfield(pde,'g_D'), pde.g_D = []; end
    if ~isfield(pde,'g_N'), pde.g_N = []; end
    if ~isfield(pde,'g_R'), pde.g_R = []; end

    %% Part 1: Modify the matrix for Dirichlet and Robin condition
    % Robin boundary condition
    Robin = [];
    isRobin = (bdFlag(:) == 3);
    if any(isRobin)
        allEdge = [elem(:,[1,2]); elem(:,[2,3]); elem(:,[3,4]); elem(:,[4 1])];    % quad
        Robin = allEdge(isRobin,:);
    end
    if ~isempty(Robin) && ~isempty(pde.g_R) && ~(isnumeric(pde.g_R) && (pde.g_R == 0))
        ve = node(Robin(:,1),:) - node(Robin(:,2),:);
        edgeLength = sqrt(sum(ve.^2,2));
        mid = (node(Robin(:,1),:) + node(Robin(:,2),:))/2;
        % int g_R phi_iphi_j ds
        ii = [Robin(:,1),Robin(:,1),Robin(:,2),Robin(:,2)];
        jj = [Robin(:,1),Robin(:,2),Robin(:,1),Robin(:,2)];
        temp = pde.g_R(mid).*edgeLength;
        ss = [1/3*temp, 1/6*temp, 1/6*temp, 1/3*temp];
%         ss = [1/4*temp, 1/4*temp, 1/4*temp, 1/4*temp];
        A = A + sparse(ii,jj,ss,Ndof,Ndof);
    end

    % Find Dirichlet boundary nodes: fixedNode
    fixedNode = []; freeNode = [];
    if ~isempty(bdFlag) % find boundary edges and boundary nodes
        [fixedNode,bdEdge,isBdNode] = findboundary(elem,bdFlag);
        freeNode = find(~isBdNode);
    end
    if isempty(bdFlag) && ~isempty(pde.g_D) && isempty(pde.g_N) && isempty(pde.g_R)
        % no bdFlag, only pde.g_D is given
        [fixedNode,bdEdge,isBdNode] = findboundary(elem);
        freeNode = find(~isBdNode);
    end
    isPureNeumann = false;
    if isempty(fixedNode) && isempty(Robin) % pure Neumann boundary condition
        % pde.g_N could be empty which is homogenous Neumann boundary condition
        isPureNeumann = true;
        fixedNode = 1;
        freeNode = 2:Ndof;    % eliminate the kernel by enforcing u(1) = 0;
    end
    % Modify the matrix
    % Build Dirichlet boundary condition into the matrix AD by enforcing
    % AD(fixedNode,fixedNode)=I, AD(fixedNode,freeNode)=0, AD(freeNode,fixedNode)=0.
    if ~isempty(fixedNode)
        bdidx = zeros(Ndof,1);
        bdidx(fixedNode) = 1;
        Tbd = spdiags(bdidx,0,Ndof,Ndof);
        T = spdiags(1-bdidx,0,Ndof,Ndof);
        AD = T*A*T + Tbd;
    else
        AD = A;
    end

    %% Part 2: Find boundary edges and modify the right hand side b
    % Find boundary edges: Neumann
    Neumann = [];
    if ~isempty(bdFlag)  % bdFlag specifies different bd conditions
        Neumann = bdEdge;
    end
    if isempty(bdFlag) && (~isempty(pde.g_N) || ~isempty(pde.g_R))
        % no bdFlag, only pde.g_N or pde.g_R is given in the input
        [tempvar,Neumann] = findboundary(elem); %#ok<ASGLU>
    end

    % Neumann boundary condition
    if  isnumeric(pde.g_N) && all(pde.g_N == 0)
        pde.g_N = [];
    end
    if ~isempty(Neumann) && ~isempty(pde.g_N)
        el = sqrt(sum((node(Neumann(:,1),:) - node(Neumann(:,2),:)).^2,2));
        if ~isfield(option,'gNquadorder')
            option.gNquadorder = 2;   % default order exact for linear gN
        end
        [lambdagN,weightgN] = quadpts1(option.gNquadorder);
        phigN = lambdagN;                 % linear bases
        nQuadgN = size(lambdagN,1);
        ge = zeros(size(Neumann,1),2);
        for pp = 1:nQuadgN
            % quadrature points in the x-y coordinate
            ppxy = lambdagN(pp,1)*node(Neumann(:,1),:) ...
                 + lambdagN(pp,2)*node(Neumann(:,2),:);
            gNp = pde.g_N(ppxy);
            for igN = 1:2
                ge(:,igN) = ge(:,igN) + weightgN(pp)*phigN(pp,igN)*gNp;
            end
        end
        ge = ge.*repmat(el,1,2);
        b = b + accumarray(Neumann(:), ge(:),[Ndof,1]);
    end
    % The case with non-empty Neumann edges but g_N=0 or g_N=[] corresponds to
    % the zero flux boundary condition on Neumann edges and no modification of
    % A,u,b is needed.

    % Dirichlet boundary condition
    if isnumeric(pde.g_D) && all(pde.g_D == 0)   % zero g_D
        pde.g_D = [];
    end
    if ~isPureNeumann && ~isempty(fixedNode) && ~isempty(pde.g_D)
        if isnumeric(pde.g_D)  % pde.g_D could be a numerical array
            u(fixedNode) = pde.g_D(fixedNode);
        else % pde.g_D is a function handle
            u(fixedNode) = pde.g_D(node(fixedNode,:));
        end
        b = b - A*u;
    end
    if ~isPureNeumann % non-empty Dirichlet boundary condition
        b(fixedNode) = u(fixedNode);
    end
    % The case with non-empty Dirichlet nodes but g_D=0 or g_D=[] corresponds
    % to the zero Dirichlet boundary condition and no modification of u,b is
    % needed.

    % Pure Neumann boundary condition
    if isPureNeumann
        b = b - mean(b);   % compatilbe condition: sum(b) = 0
    end
    end % end of getbd
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

end % end of PoissonQ1


function [H11, H12, H21, H22] = d2Abr_dV2(d2F_dV2, dF_dV1, dF_dV2, F, V, mu)
%D2ABR_DV2   Computes 2nd derivatives of |branch flow|^2 w.r.t. V.
%
%   The derivatives can be take with respect to polar or cartesian coordinates
%   of voltage, depending on the first 3 arguments. Flows could be complex
%   current or complex or real power. Notation below is based on complex power.
%
%   [H11, H12, H21, H22] = D2ABR_DV2(D2F_DV2, DF_DV1, DF_DV2, F, V, MU)
%
%   Returns 4 matrices containing the partial derivatives w.r.t. voltage
%   components (angle, magnitude or real, imaginary) of the product of a
%   vector MU with the 1st partial derivatives of the square of the magnitude
%   of branch flows.
%
%   Takes as inputs a handle to a function that evaluates the 2nd derivatives
%   of the flows (with args V and mu only), sparse first derivative matrices
%   of flow, flow vector, voltage vector V and nl x 1 vector of multipliers
%   MU. Output matrices are sparse.
%
%   Example:
%       f = branch(:, F_BUS);
%       Cf =  sparse(1:nl, f, ones(nl, 1), nl, nb);
%       [Ybus, Yf, Yt] = makeYbus(baseMVA, bus, branch);
%       [dSf_dV1, dSf_dV2, dSt_dV1, dSt_dV2, Sf, St] = ...
%               dSbr_dV(branch, Yf, Yt, V);
%       dF_dV1 = dSf_dV1;
%       dF_dV2 = dSf_dV2;
%       F = Sf;
%       d2F_dV2 = @(V, mu)d2Sbr_dV2(Cf, Yf, V, mu, 0);
%       [H11, H12, H21, H22] = ...
%             d2Abr_dV2(d2F_dV2, dF_dV1, dF_dV2, F, V, mu);
%
%   Here the output matrices correspond to:
%     H11 = d/dV1 (dAF_dV1.' * mu)
%     H12 = d/dV2 (dAF_dV1.' * mu)
%     H21 = d/dV1 (dAF_dV2.' * mu)
%     H22 = d/dV2 (dAF_dV2.' * mu)
%
%   See also DABR_DV, DIBR_DV, DSBR_DV.
%
%   For more details on the derivations behind the derivative code used
%   in MATPOWER information, see:
%
%   [TN2]  R. D. Zimmerman, "AC Power Flows, Generalized OPF Costs and
%          their Derivatives using Complex Matrix Notation", MATPOWER
%          Technical Note 2, February 2010. [Online]. Available:
%          https://matpower.org/docs/TN2-OPF-Derivatives.pdf
%          doi: 10.5281/zenodo.3237866

%   MATPOWER
%   Copyright (c) 2008-2019, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%
%   This file is part of MATPOWER.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://matpower.org for more info.

%% define
nl = length(mu);

diagmu = sparse(1:nl, 1:nl, mu, nl, nl);

[F11, F12, F21, F22] = d2F_dV2(V, conj(F) .* mu);
H11 = 2 * real( F11 + dF_dV1.' * diagmu * conj(dF_dV1) );
H21 = 2 * real( F21 + dF_dV2.' * diagmu * conj(dF_dV1) );
H12 = 2 * real( F12 + dF_dV1.' * diagmu * conj(dF_dV2) );
H22 = 2 * real( F22 + dF_dV2.' * diagmu * conj(dF_dV2) );


% dftfilt2() - discrete complex wavelet filters
%
% Usage:
%   >> wavelet = dftfilt2( freqs, cycles, srate, cyclefact)
%
% Inputs:
%   freqs    - frequency array
%   cycles   - cycles array. If one value is given, all wavelets have
%              the same number of cycles. If two values are given, the
%              two values are used for the number of cycles at the lowest
%              frequency and at the highest frequency, with linear
%              interpolation between these values for intermediate
%              frequencies
%   srate    - sampling rate (in Hz)
%
%   cycleinc - ['linear'|'log'] increase mode if [min max] cycles is
%              provided in 'cycle' parameter. {default: 'linear'}
%   type     - ['sinus'|'morlet'] wavelet type is a sinusoid with
%              cosine (real) and sine (imaginary) parts tapered by
%              a Hanning or Morlet function. 'morlet' is a typical Morlet
%              wavelet (with p=2*pi and sigma=0.7) best matching the
%              'sinus' Hanning taper) {default: 'morlet'}
% Output:
%   wavelet - cell array of wavelet filters
%
% Note: The length of the window is automatically computed from the
%       number of cycles and is always made odd.
%
% Authors: Arnaud Delorme, SCCN/INC/UCSD, La Jolla, 3/28/2003

% Copyright (C) 3/28/2003 Arnaud Delorme 8, SCCN/INC/UCSD, arno@salk.edu
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

function wavelet = dftfilt2( freqs, cycles, srate, cycleinc, type);

    if nargin < 3
        error('3 arguments required');
    end;
    if nargin < 5
        type = 'morlet';
    end;

    % compute number of cycles at each frequency
    % ------------------------------------------
    if length(cycles) == 1
        cycles = cycles*ones(size(freqs));
    elseif length(cycles) == 2
        if nargin == 4 & strcmpi(cycleinc, 'log') % cycleinc
            cycles = linspace(log(cycles(1)), log(cycles(2)), length(freqs));
            cycles = exp(cycles);
        else
            cycles = linspace(cycles(1), cycles(2), length(freqs));
        end;
    end;

    % compute wavelet
    for index = 1:length(freqs)

        % number of cycles depend on window size
        % number of cycles automatically reduced if smaller window
        % note: as the number of cycle changes, the frequency shifts a little
        %       this has to be fixed

        winlen = cycles(index)*srate/freqs(index);
        winlenint = floor(winlen);
        if mod(winlenint,2) == 1, winlenint = winlenint+1; end;
        winval = linspace(winlenint/2, -winlenint/2, winlenint+1);

        if strcmpi(type, 'sinus') % Hanning
            win = exp(2i*pi*freqs(index)*winval/srate);
            wavelet{index} = win .* hanning(length(winval))';

        else % Morlet
            t = freqs(index)*winval/srate;
            p = 2*pi;
            s = cycles(index)/5;
            wavelet{index} = exp(j*t*p)/sqrt(2*pi) .* ...
                (exp(-t.^2/(2*s^2))-sqrt(2)*exp(-t.^2/(s^2)-p^2*s^2/4));
        end;
    end;

    return;

    % testing
    % -------
    wav1 = dftfilt2(5, 5, 256); wav1 = wav1{1};
    abs1 = linspace(-floor(length(wav1)),floor(length(wav1)), length(wav1));
    figure; plot(abs1, real(wav1), 'b');

    wav2 = dftfilt2(5, 3, 256); wav2 = wav2{1};
    abs2 = linspace(-floor(length(wav2)),floor(length(wav2)), length(wav2));
    hold on; plot(abs2, real(wav2), 'r');

    wav3 = dftfilt2(5, 1.4895990, 256); wav3 = wav3{1};
    abs3 = linspace(-floor(length(wav3)),floor(length(wav3)), length(wav3));
    hold on; plot(abs3, real(wav3), 'g');

    wav4 = dftfilt2(5, 8.73, 256); wav4 = wav4{1};
    abs4 = linspace(-floor(length(wav4)),floor(length(wav4)), length(wav4));
    hold on; plot(abs4, real(wav4), 'm');

    % more testing
    % ------------
    freqs = exp(linspace(0,log(10),10));
    win = dftfilt2(freqs, [3 3], 256, 'linear', 'sinus');
    win = dftfilt2(freqs, [3 3], 256, 'linear', 'morlet');

    freqs = [12.0008   13.2675   14.5341   15.8007   17.0674   18.3340   19.6007   20.8673   22.1339   23.4006   24.6672   25.9339   27.2005 28.4671   29.7338   31.0004   32.2670   33.5337   34.8003   36.0670   37.3336   38.6002   39.8669   41.1335   42.4002   43.6668 44.9334   46.2001   47.4667 ...
             48.7334   50.0000];

    win = dftfilt2(freqs, [3 12], 256, 'linear'); size(win)

    winsize = 0;
    for index = 1:length(win)
        winsize = max(winsize,length(win{index}));
    end;
    allwav = zeros(winsize, length(win));
    for index = 1:length(win)
        wav1 = win{index};
        abs1 = linspace(-(length(wav1)-1)/2,(length(wav1)-1)/2, length(wav1));
        allwav(abs1+(winsize+1)/2,index) = wav1(:);
    end;
    figure; imagesc(imag(allwav));

% syemtric hanning function
function w = hanning(n)
if ~rem(n,2)
   w = .5*(1 - cos(2*pi*(1:n/2)'/(n+1)));
   w = [w; w(end:-1:1)];
else
   w = .5*(1 - cos(2*pi*(1:(n+1)/2)'/(n+1)));
   w = [w; w(end-1:-1:1)];
end


function [alphahat epshat etahat] = fastsmo_int(n, y, mis, anymis, allmis, Hdyn, Zdyn, Tdyn, Rdyn, Qdyn, cdyn, Hmat, Zmat, Tmat, Rmat, Qmat, cmat, a1, P1, tol)

%% Kalman filter %%
[d Fns v invF K L L1 QRt] = kalman_int(5, n, y, mis, anymis, allmis, Hdyn, Zdyn, Tdyn, Rdyn, Qdyn, cdyn, Hmat, Zmat, Tmat, Rmat, Qmat, cmat, a1, P1, tol);

%% Initialization %%
D       = (P1 == Inf);
P1(D)   = 0;
P1_inf  = double(D);
if ~Hdyn, H = Hmat; end
if ~Zdyn, Z = Zmat; end
if ~Tdyn, T = Tmat; end
if ~Rdyn, R = Rmat; end
if ~cdyn, c = cmat; end

%% Disturbance smoothing backwards recursion %%
m   = size(a1, 1);
r   = zeros(m, 1);
r1  = zeros(m, 1);
epshat  = zeros(size(y, 1), n);
etahat  = zeros(size(QRt{1}, 1), n);
for t = n : -1 : 1
    if Tdyn, T = Tmat{t}; end
    etahat(:, t) = QRt{t}*r;
    if allmis(t)
        %% Disturbance smoothing when all observations are missing %%
        epshat(:, t) = 0;
        r = T'*r;
        if t <= d, r1 = T'*r1; end
    else
        if Hdyn, H = Hmat{t}; end
        if Zdyn, Z = Zmat{t}; end
        if anymis(t), Z(mis(:, t),:)=[]; H(mis(:, t),:)=[]; H(:,mis(:, t))=[]; end
        if t > d || ~Fns(t)
            %% Normal disturbance smoothing or when F_inf is zero %%
            epshat(~mis(:, t), t) = H*(invF{t}*v{t} - K{t}'*r);
            r = Z'*invF{t}*v{t} + L{t}'*r;
            if t <= d, r1 = T'*r1; end
        else
            %% Exact initial disturbance smoothing when F_inf is nonsingular %%
            epshat(~mis(:, t), t) = -H*K{t}'*r;
            r1 = Z'*invF{t}*v{t} + L{t}'*r1 + L1{t}'*r;
            r = L{t}'*r;
        end
        if anymis(t), if ~Zdyn, Z = Zmat; end, if ~Hdyn, H = Hmat; end, end
    end
end

%% Fast state smoothing %%
alphahat        = zeros(m, n);
alphahat(:, 1)  = a1 + P1*r + P1_inf*r1;
for t = 1 : n-1
    if Tdyn, T = Tmat{t}; end
    if Rdyn, R = Rmat{t}; end
    if cdyn, c = cmat{t}; end
    alphahat(:, t+1) = c + T*alphahat(:, t) + R*etahat(:, t);
end


function low_rank_tensor_completion_embedded()
% Given partial observation of a low rank tensor (possibly including noise),
% attempts to complete it.
%
% function low_rank_tensor_completion_embedded()
%
% NOTE: Tensor Toolbox version 2.6 or higher is required for this factory:
% see https://www.tensortoolbox.org/ or https://gitlab.com/tensors/tensor_toolbox
%
% This example demonstrates how to use the geometry factory for the
% embedded submanifold of fixed-rank tensors in Tucker format:
% fixedranktensorembeddedfactory.
%
% This geometry is described in the article
% "A Riemannian trust-region method for low-rank tensor completion"
% Gennadij Heidel and Volker Schulz, doi:10.1002/nla.2175.
%
% This can be a starting point for many optimization problems of the form:
%
% minimize f(X) such that rank(X) = [r1 ... rd], size(X) = [n1 ... nd].
%
% Important: to keep this example short, the code for the cost function,
% gradient and Hessian do not properly exploit sparsity of the
% observations, which leads to significant slow-downs for large tensors.
% This example file should be considered a starting point for more
% sophisticated implementations.
%
% Input:  None. This example file generates random data with noise.
%
% Output: None.
%
% Please cite the Manopt and Matlab Tensor Toolbox papers as well as the
% research paper:
%     @Article{heidel2018riemannian,
%       Title   = {A {R}iemannian trust-region method for low-rank tensor completion},
%       Author  = {G. Heidel and V. Schulz},
%       Journal = {Numerical Linear Algebra with Applications},
%       Year    = {2018},
%       Volume  = {23},
%       Number  = {6},
%       Pages   = {e1275},
%       Doi     = {10.1002/nla.2175}
%     }
%
% See also: fixedranktensorembeddedfactory

% This file is part of Manopt: www.manopt.org.
% Original author: Gennadij Heidel, January 24, 2019.
% Contributors:
% Change log:

    if ~exist('tenrand', 'file')
        fprintf('Tensor Toolbox version 2.6 or higher is required.\n');
        return;
    end

    % Random data generation with pseudo-random numbers from a
    % uniform distribution on [0, 1].
    tensor_dims = [60 40 20];
    core_dims = [8 6 5];
    total_entries = prod(tensor_dims);
    d = length(tensor_dims);

    % Standard deviation of normally distributed noise.
    % Set sigma to 0 to test the noise-free case.
    sigma = 0.1;

    % Generate a random tensor A of size n1-by-...-by-nd of rank (r1, ..., rd).
    U = cell(1, d);
    R = cell(1, d);
    for i = 1:d
        [U{i}, R{i}] = qr(randn(tensor_dims(i), core_dims(i)), 0);
    end

    Z.U = R;
    Z.G = tenrand(core_dims);
    Core = ttm(Z.G, Z.U);

    Y.U = U;
    Y.G = Core;
    A = ttm(Core, Y.U);

    % Add noise to low-rank tensor
    A = A + sigma*tensor(randn(tensor_dims));

    % Generate a random mask P for observed entries:
    % P(i, j, k) = 1 if the entry (i, j, k) of A is observed,
    %            = 0 otherwise.
    fraction = 0.1; % Fraction of observed entries.
    nr = round(fraction * total_entries);
    ind = randperm(total_entries);
    ind = ind(1 : nr);
    P = false(tensor_dims);
    P(ind) = true;
    % Hence, we observe the nonzero entries in PA:
    P = tensor(P);
    PA = P.*A;
    % Note that an efficient implementation would require evaluating A as a
    % sparse tensor only at the indices of P.

    % Pick the submanifold of tensors of size n1-by-...-by-nd of
    % multilinear rank (r1, ..., rd).
    problem.M = fixedranktensorembeddedfactory(tensor_dims, core_dims);

    % Define the problem cost function.
    % The store structure is used to reduce full tensor evaluations.
    % Again: proper handling of sparse tensors would dramatically reduce
    % the computation time for large tensors. This file only serves as a
    % simple starting point. See help for the Tensor Toolbox regarding
    % sparse tensors. Same comment for gradient and Hessian below.
    problem.cost = @cost;
    function [f, store] = cost(X, store)
        if ~isfield(store, 'PXmPA')
            Xfull = full(X.X);
            store.PXmPA = P.*Xfull - PA;
        end
        f = .5*norm(store.PXmPA)^2;
    end

    % Define the Euclidean gradient of the cost function, that is, the
    % gradient of f(X) seen as a function of X without rank restrictions.
    problem.egrad =  @egrad;
    function [g, store] = egrad(X, store)
        if ~isfield(store, 'PXmPA')
            Xfull = full(X.X);
            store.PXmPA = P.*Xfull - PA;
        end
        g = store.PXmPA;
    end

    % Define the Euclidean Hessian of the cost at X along a vector eta.
    problem.ehess = @ehess;
    function H = ehess(X, eta)
        ambient_H = problem.M.tangent2ambient(X, eta);
        H = P.*ambient_H;
    end

    % Options
    X0 = problem.M.rand();
    options.maxiter = 3000;
    options.maxinner = 100;
    options.maxtime = inf;
    options.storedepth = 3;
    % Target gradient norm
    options.tolgradnorm = 1e-8*problem.M.norm(X0, getGradient(problem, X0));

    % Minimize the cost function using Riemannian trust-regions
    Xtr = trustregions(problem, X0, options);

    % Display some quality metrics for the computed solution
    Xtrfull = full(Xtr.X);
    fprintf('||X-A||_F / ||A||_F = %g\n', norm(Xtrfull - A)/norm(A));
    fprintf('||PX-PA||_F / ||PA||_F = %g\n', norm(P.*Xtrfull - PA)/norm(PA));

end


function truncated_normal_b_pdf_values_test ( )

%*****************************************************************************80
%
%% TRUNCATED_NORMAL_B_PDF_VALUES_TEST tests TRUNCATED_NORMAL_B_PDF_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 September 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TRUNCATED_NORMAL_B_PDF_VALUES_TEST:\n' );
  fprintf ( 1, '  TRUNCATED_NORMAL_B_PDF_VALUES stores values of\n' );
  fprintf ( 1, '  the PDF of the Normal Distribution truncated to (-oo,B].\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '            MU         SIGMA             B             X               FX\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, mu, sigma, b, x, fx ] = ...
      truncated_normal_b_pdf_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %12f  %12f  %12f  %12f  %24.16f\n', ...
      mu, sigma, b, x, fx );

  end

  return
end


function [R, scale]=ARFIT_arqr(v, p, mcor)
%ARQR	QR factorization for least squares estimation of AR model.
%
%  [R, SCALE]=ARQR(v,p,mcor) computes the QR factorization needed in
%  the least squares estimation of parameters of an AR(p) model. If
%  the input flag mcor equals one, a vector of intercept terms is
%  being fitted. If mcor equals zero, the process v is assumed to have
%  mean zero. The output argument R is the upper triangular matrix
%  appearing in the QR factorization of the AR model, and SCALE is a
%  vector of scaling factors used to regularize the QR factorization.
%
%  ARQR is called by ARFIT.
%
%  See also ARFIT.

%  Modified 29-Dec-99
%           24-Oct-10 Tim Mullen (added support for multiple realizatons)
%
%  Author: Tapio Schneider
%          tapio@gps.caltech.edu

  % n:   number of time steps (per realization)
  % m:   number of variables (dimension of state vectors)
  % ntr: number of realizations (trials)
  [n,m,ntr] = size(v);

  ne    = ntr*(n-p);            % number of block equations of size m
  np    = m*p+mcor;             % number of parameter vectors of size m

  % If the intercept vector w is to be fitted, least squares (LS)
  % estimation proceeds by solving the normal equations for the linear
  % regression model
  %
  %                  v(k,:)' = Aaug*u(k,:)' + noise(C)        (1)
  %
  % with Aaug=[w A] and `predictors'
  %
  %              u(k,:) = [1 v(k-1,:) ...  v(k-p,:)].         (2a)
  %
  % If the process mean is taken to be zero, the augmented coefficient
  % matrix is Aaug=A, and the regression model
  %
  %                u(k,:) = [v(k-1,:) ...  v(k-p,:)]          (2b)
  %
  % is fitted.
  % The number np is the dimension of the `predictors' u(k).
  %
  % If multiple realizations are given (ntr > 1), they are appended
  % as additional ntr-1 blocks of rows in the normal equations (1), and
  % the 'predictors' (2) correspondingly acquire additional row blocks.

  % Initialize the data matrix K (of which a QR factorization will be computed)
  K = zeros(ne,np+m);                 % initialize K
  if (mcor == 1)
    % first column of K consists of ones for estimation of intercept vector w
    K(:,1) = ones(ne,1);
  end

  % Assemble `predictors' u in K
  for itr=1:ntr
    for j=1:p
      K((n-p)*(itr-1) + 1 : (n-p)*itr, mcor+m*(j-1)+1 : mcor+m*j) = ...
          squeeze(v(p-j+1:n-j, :, itr));
    end
    % Add `observations' v (left hand side of regression model) to K
    K((n-p)*(itr-1) + 1 : (n-p)*itr, np+1 : np+m) = squeeze(v(p+1:n, :, itr));
  end

  % Compute regularized QR factorization of K: The regularization
  % parameter delta is chosen according to Higham's (1996) Theorem
  % 10.7 on the stability of a Cholesky factorization. Replace the
  % regularization parameter delta below by a parameter that depends
  % on the observational error if the observational error dominates
  % the rounding error (cf. Neumaier, A. and T. Schneider, 2001:
  % "Estimation of parameters and eigenmodes of multivariate
  % autoregressive models", ACM Trans. Math. Softw., 27, 27--57.).
  q     = np + m;             % number of columns of K
  delta = (q^2 + q + 1)*eps;  % Higham's choice for a Cholesky factorization
  scale = sqrt(delta)*sqrt(sum(K.^2));
  R     = triu(qr([K; diag(scale)]));


function [wavespec] = bz_WaveSpec(lfp,varargin)
%[wavespec] = bz_WaveSpec(lfp) calculates the
%wavelet transform of a signal with nfreqs frequencies in the range frange
%[fmin fmax]. Spacing between frequencies can be 'lin' or 'log'.
%Time-frequency resolution is defined by ncyc, the number of cycles in each
%wavelet. Uses Morlet (Gabor) wavelet.
%
%
%INPUT
%    lfp            a buzcode structure with fields lfp.data,
%                                                   lfp.timestamps
%                                                   lfp.samplingRate
%                   -lfp can also be a [t x 1] timeseries signal. in which
%                   case you need to input 'samplingRate'
%    <options>      optional list of property-value pairs (see table below)
%
%    =========================================================================
%     Properties    Values
%    -------------------------------------------------------------------------
%       'frange'    [low frequency, high frequency]     (default: [1 128])
%       'nfreqs'    number of frequencies               (default: 100
%       'roundfreqs' round freqs to unique integer vals (default: false
%                       *Note this may decrease number of freqs
%       'nfreqs'    number of frequencies               (default: 100
%       'ncyc'      number of cycles in your wavelet    (default: 5)
%       'fvector'   predefined vector of frequencies
%       'space'     'log' or 'lin'  spacing of f's      (default: 'log')
%       'samplingRate' (only if input is not a buzcode structure)
%       'intervals'  restrict your spectrogram to timestamps in specific
%                   intervals
%       'chanID'    if lfp structure has multiple channels, which one would
%                   you like to calcaulte the wavelet transform of?
%                   (note: requires field lfp.channels)
%       'showprogress' true/false (default:false)
%       'saveMat '   put the basePath to save an LFP file
%       'MatNameExtraText'   text(X) to add to name as in: 'basename.wavespec(text).lfp.mat'
%       'downsampleout' factor by which to downsample output (default: 1)
%    =========================================================================
%
%OUTPUT
%   wavespec            buzcode-style structure
%       .data           [t x nfreqs] your spectrogram
%       .timestamps     [t x 1] timestamps
%       .freqs          frequencies of each column
%       .samplingRate   (Hz)
%       .channels       Channel indices of channels filtered... taken from lfp input
%       .filterparms    a structure that holds the parameters used for
%                       filtering, for future reference
%

%TO DO:
%   -Matricise For loop?
%   -Don't need to FFT every freq...
%   -update t output for if LFP is a cell array
%
%
%Dependencies
%   WaveFilt
%   MorletWavelet
%   FConv
%
%
%Last Updated: 10/9/15
%DLevenstein
%Modified by Antonio FR, 7/18/18

%% Parse the inputs

%Parameters
parms = inputParser;
addParameter(parms,'frange',[1 128],@isnumeric);
addParameter(parms,'nfreqs',100,@isnumeric);
addParameter(parms,'ncyc',5,@isnumeric);
addParameter(parms,'space','log');
addParameter(parms,'samplingRate',[]);
addParameter(parms,'showprogress',false,@islogical);
addParameter(parms,'roundfreqs',false,@islogical);
addParameter(parms,'saveMatPath',[]);
addParameter(parms,'MatNameExtraText',[]);
addParameter(parms,'fvector',[]);
addParameter(parms,'intervals',[-Inf Inf])
addParameter(parms,'chanID',[])
addParameter(parms,'downsampleout',1)

parse(parms,varargin{:})
frange = parms.Results.frange;
nfreqs = parms.Results.nfreqs;
ncyc = parms.Results.ncyc;
space = parms.Results.space;
samplingRate = parms.Results.samplingRate;
showprogress = parms.Results.showprogress;
roundfreqs = parms.Results.roundfreqs;
saveMatPath = parms.Results.saveMatPath;
MatNameExtraText = parms.Results.MatNameExtraText;
fvector = parms.Results.fvector;
intervals = parms.Results.intervals;
chanID = parms.Results.chanID;
downsampleout = parms.Results.downsampleout;

%Channel restrict
if ~isempty(chanID)
    usechannel = ismember(lfp.channels,chanID);
    lfp.data = lfp.data(:,usechannel);
    lfp.channels = lfp.channels(usechannel);
end

%lfp input
if isstruct(lfp)
    samplingRate = lfp.samplingRate;
elseif isempty(lfp)
    wavespec = lfp;
    return
elseif isnumeric(lfp)
    data_temp = lfp;
    clear lfp
    lfp.data = data_temp;
    lfp.timestamps = [1:length(lfp.data)]'./samplingRate;
end

si = 1./samplingRate;

%Restrict to intervals, with overhang to remove edge effects at transitions
%(then remove later)
overhang = (ncyc)./frange(1);
overint = bsxfun(@(X,Y) X+Y,intervals,overhang.*[-1 1]);
keepIDX = InIntervals(lfp.timestamps,overint);
lfp.data = lfp.data(keepIDX,:);
lfp.timestamps = lfp.timestamps(keepIDX);

%%
if ~isa(lfp.data,'single') || ~isa(lfp.data,'double')
    lfp.data = single(lfp.data);
end

%Frequencies
if ~isempty(fvector)
    freqs = fvector;
else
    fmin = frange(1);
    fmax = frange(2);
    if strcmp(space,'log')
        assert(fmin~=0,'Log-spaced frequencies cannot have min of 0')
        freqs = logspace(log10(fmin),log10(fmax),nfreqs);
    elseif strcmp(space,'lin')
        freqs = linspace(fmin,fmax,nfreqs);
    else
        display('Frequency spacing must be "lin" or "log".')
    end
end

if roundfreqs
    freqs = unique(round(freqs));
end

%Filter with wavelets
nfreqs = size(freqs,2);
nchan = size(lfp.data,2);
ntime = ceil(size(lfp.data,1)./downsampleout);
wavespec.data = nan(ntime,nfreqs,nchan);
wavespec.timestamps = downsample(lfp.timestamps,downsampleout);
for cidx = 1:nchan
    for f_i = 1:nfreqs
        if showprogress
            bz_Counter(f_i,nfreqs,'Wavelet Frequency')
        end
        wavelet = MorletWavelet(freqs(f_i),ncyc,si);
         wavespec.data(:,f_i,cidx) = ...
             downsample(FConv(wavelet',lfp.data(:,cidx)),downsampleout);
    end
end

%% Output in buzcode format
%Remove the overhang from intervals
keepIDX = InIntervals(wavespec.timestamps,intervals);
wavespec.data = wavespec.data(keepIDX,:);
wavespec.timestamps = wavespec.timestamps(keepIDX);

wavespec.freqs = freqs;
wavespec.nfreqs = nfreqs;
wavespec.samplingRate = samplingRate./downsampleout;
if isstruct(lfp) && isfield(lfp,'channels')
    wavespec.channels = lfp.channels;
end
wavespec.filterparms.ncyc = ncyc;
wavespec.filterparms.nfreqs = nfreqs;
wavespec.filterparms.frange = frange;
wavespec.filterparms.space = space;

clear lfp

if saveMatPath
    baseName = bz_BasenameFromBasepath(saveMatPath);
    if ~isempty(MatNameExtraText)
        lfpfilename = fullfile(saveMatPath,[baseName,'.wavespec' MatNameExtraText '.lfp.mat']);
    else
        lfpfilename = fullfile(saveMatPath,[baseName,'.wavespec.lfp.mat']);
    end

    s = whos('wavespec');
    if s.bytes>=1073741824%if greater than 2GB
        disp('wavespec variable greater than 2GB, saving as v7.3 .mat file')
        save(lfpfilename,'wavespec','-v7.3')
    else
        save(lfpfilename,'wavespec')
    end
end

end


function [y, details] = newton(functionf, dfunctionf, yest, yold, ...
                               tolabs, tolrel, maxit, printConsole)
%newton Newton iteration with Armijo step control for the nonlinear function
% and Jacobi. Starting from an estimate yest the function computes with a newton
% iteration the solution matrix y of f(y)=0 up to
% tol = tolrel*||f(y)||+tolabs.
%
% Call:   [matrix scalar scalar] =
%                newton(matrix, matrix, scalar, scalar, scalar, structure)
% Input:  matrix yest, size(yest) = (M,N) - estimate of the solution
% Input:  matrix yold, size(yold) = size(y) - parameter of fideq and dfideq
%         (state at previous time level)
% Input:  scalar tolabs>0 - absolute tolerance
% Input:  scalar tolrel>0 - relative tolerance
% Input:  scalar maxit>=1 - maximal number of iterations
% Input:  printConsole - > 0: print iteration information on console output
% Output: matrix y, size(y) = (M,N) - solution
% Output: scalar res, residuum ||f(y)||
% Output: scalar iter, number of iterations
%
% Author: Raimund Wegener, Stefan Schießl
% Date: 12.10.2011

if (nargin < 8)
    printConsole = 1;
end

%%
% initialization
sigma = 1e-2; alpha = 0.5; lambda = 1;

y = yest; f = functionf(y,yold); psi = 0.5*f(:)'*f(:); res = sqrt(2*psi);
tol= tolrel * res + tolabs;

%%
%iteration
[M,N] = size(y);
iter = 0;
% Flag for the message text
armijoMessage = '';
while (res > tol) && (iter < maxit)
    [i,j,S] = dfunctionf(y);
    dfdy = sparse(i,j,S,M*N,M*N);
    % '\' only gives a warning for singular matrices, generate an error
    % for that.
    lastwarn('');
    % bandden:  backslash uses band solver if band density is > bandden.
    % if bandden = 1.0, never use band solver, if bandden = 0.0, always use
    % band solver.
    spparms('bandden',0);
    del = -dfdy\f(:);
    assert(~strcmp(lastwarn, 'Matrix is singular to working precision.'), ...
        'Step:NotSolvable', 'Matrix is singular to working precision.');
    assert(~strcmp(lastwarn, 'Matrix is close to singular or badly scaled.'), ...
        'Step:NotSolvable', 'Matrix is close to singular or badly scaled.');

    y_ = y + del;
    f_ = functionf(y_,yold);
    psi_ = 0.5*f_(:)'*f_(:);
    if (psi_ > (1-2*sigma)*psi)
        lambda = min(lambda/alpha,1);
        y_ = y + lambda*del; f_ = functionf(y_,yold); psi_ = 0.5*f_(:)'*f_(:);
        while psi_ > (1-2*sigma*lambda)*psi
            lambda = lambda*alpha;
            % step control with Armijo rule
            y_ = y + lambda*del; f_ = functionf(y_,yold); psi_ = 0.5*f_(:)'*f_(:);

        end
        armijoMessage = ['newton/armijo: iter=' num2str(iter) ',\t lambda=' num2str(lambda) '\n'];
        if (printConsole > 1)
            fprintf(armijoMessage)
        end
    end

    y = y_; f = f_; psi = psi_; res = sqrt(2*psi);
    iter = iter + 1;
end
message = ['newton: res=' num2str(res) ', iter=' num2str(iter) '\n' armijoMessage];
if (printConsole > 0)
    fprintf(message)
end

if (isnan(res))
   error('Newton:NoConvergence', 'No solution found.');
end

if (res > tol)
   error('Newton:AccuracyNotReached', ['newton/armijo: Results are inaccurate (Res: ' num2str(res) ').']);
end

details.res = res;
details.iter = iter;
details.message = message;

end


function AxB = Cross(A,B)
%This is a custom implementation of the Cross product operator;
%A and B must be (2x1) symbolic matricies, and AxB is a symbolic variable.
%It assumes that A and B are planar (with components i and j) and that AxB
%is the value of the number out of plane (in the k direction)

AxB = A(1)*B(2) - A(2)*B(1);

end


function x = hatmap(u)
%HATMAP Summary of this function goes here
%   return skew symmetric matrix from a vector
x = [0, -u(3), u(2);
    u(3), 0, -u(1);
    -u(2), u(1), 0];
end


function [D]=patchEdgeLengths(F,V)

% function [D]=patchEdgeLengths(F,V)
% -----------------------------------------------------------------------
% Computes the edge lengths (D) for the patch data specified by the faces
% (F) and vertices (V) arrays. If size(F,2)>2 it is assumed that F indeed
% represents faces. If however size(F,2)==2 it is instead assumed that F is
% an array representing edges. As such it skips the computation of the
% edges array. The edges array used is non-unique by default. See the
% |patchEdges| function for more details if the lengths of a unique set of
% edges is desired.
%
%
% See also: |patchEdges|
%
% Kevin Mattheus Moerman
% gibbon.toolbox@gmail.com
%
% 2014/03/17
% 2019/04/23 Expanded for cell array input
%------------------------------------------------------------------------

%%

if isa(F,'cell')
    D=F; %Initialize as F
    for q=1:1:numel(F) %Loop over cell entries and call function for each cell entry
        D{q}=patchEdgeLengths(F{q},V); %parse cell entry
    end
else
    %Derive edge array
    if size(F,2)>2 %The input is assumed to represent faces hence an edge array is derived
        E=patchEdges(F,0);
    else %It is assumed that the input array represents an edges array
        E=F;
    end

    %Derive edge vertex arrays
    V_E1=V(E(:,1),:);
    V_E2=V(E(:,2),:);

    %Derive difference vectors
    VD=(V_E1-V_E2);

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


function hypervolume = hypervolume3D(F,ub,lb)
% Copyright (c) 2011, Johannes
% All rights reserved.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are
% met:
%
%     * Redistributions of source code must retain the above copyright
%       notice, this list of conditions and the following disclaimer.
%     * Redistributions in binary form must reproduce the above copyright
%       notice, this list of conditions and the following disclaimer in
%       the documentation and/or other materials provided with the distribution
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
% POSSIBILITY OF SUCH DAMAGE.
%
% Method for ND objective function values as described in:
%
% 'M. Fleischer. The measure of Pareto Optima Applications to
%  Multi-objective Metaheuristics. EMO 2003, LNCSS 2632
%  519-533, 2003.'
%
% Author: Johannes W. Kruisselbrink
% Last modified: March 17, 2011

	hypervolume = 0;
    F  = -F' + ones(size(F'));
	[M, l] = size(F); ub = ub + ones(1,M); ub = ub';

	% Remove the duplicates from F and compute Lebesque measure of L
	L = unique(F','rows')';

	while l >= 1
		if (l > 1)
			b = zeros(M,1);
			spawn_vector = repmat(L(:,1), 1, M);
			for i = 1:M
				% Bound b(i) is either the least upper bound of the i-th value of the
				% other points or it is the value of the absolute upper bound ub(i)
				difL = (L(i,2:end) - L(i,1));
				lub = find(difL > 0);
				if (length(lub) > 0)
					b(i) = min(L(i,lub+1));
				else
					b(i) = ub(i);
				end

				b(i) = min((difL > 0) .* L(i,2:end) + (difL <= 0) * ub(i));
				% Update i-th spawn vector
				spawn_vector(i,i) = b(i);
			end

			% Compute lop-Off volume and update lebesgue measure
			lov = prod(b - L(:,1));
			hypervolume = hypervolume + lov;

			% Remove L(:,1) from L
			L = L(:,2:end);

			% Add the spawn_vector to L, but first filter dominated
			% solutions and the solutions that touch the upper bounds
			% from the spawn_vector
			L = nd_filter(L, spawn_vector, ub);

		else
			lov = prod(ub - L(:,1));
			hypervolume = hypervolume + lov;
			L = [];
		end
		% Update l
		[M, l] = size(L);
    end
end

function L = nd_filter(L, spawn_vector, ub)
% Implementation of the filter routine as described in:
%
% 'M. Fleischer. The measure of Pareto Optima Applications
%  to Multi-objective Metaheuristics. EMO 2003, LNCSS 2632
%  519-533, 2003.'
%
% Author: Johannes W. Kruisselbrink
% Last modified: March 17, 2011

	[M, l_L] = size(L);
	[M, l_sp] = size(spawn_vector);
	do_assign = zeros(1, l_sp);

	for i = 1 : l_sp

		% Find if the spawnvector hits the upper bound
		at_ub = false;
		for j = 1:M
			if (spawn_vector(j,i) == ub(j))
				at_ub = true;
				break;
			end
		end

		% For this if statement, the following would be more elegant
		% (replacing the loop above), but less efficient:
		% if (all(spawn_vector(:,i) ~= ub))
		if (at_ub == false)
			do_assign(i) = 1;
			for j = 1 : l_L
				if (weakly_dominates(L(:,j), spawn_vector(:,i)))
					do_assign(i) = 0;
					break;
				end
			end
		end
	end
	L = [spawn_vector(:,find(do_assign == 1)), L];
end

function d = weakly_dominates(fA, fB)
% [d] = weakly_dominates(fA, fB)
%
% Compares two solutions A and B given their objective function
% values fA and fB. Returns whether A weakly dominates B.
%
% Input:
% - fA					- The objective function values of solution A
% - fB					- The objective function values of solution B
%
% Output:
% - d					- d is 1 if fA dominates fB, otherwise d is 0
%
% Author: Johannes W. Kruisselbrink
% Last modified: March 17, 2011

	% Elegant, but not very efficient
	%d = (all(fA <= fB) && any(fA < fB));

	% Not so elegant, but more efficient
	d = true;
	for i = 1:length(fA)
		if (fA(i) > fB(i))
			d = false;
			return
		end
	end
end


function soln = smoothJointTrajectory(problem)

% Joint limits
qLow = problem.qLow;
qUpp = problem.qUpp;
dqMax = problem.dqMax;   %Joint speed limit

% Waypoints
tNode = problem.tNode;   %time
qNode = problem.qNode;   %angle

%Order of interpolating polynomial in each segment;
nGrid = problem.nGrid;

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                      Build problem matricies                            %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

nDecVar = sum(nGrid);  %Number of decision variables
nSegment = length(nGrid);

% Rate constraint and objective matrix along each segment:
for i=1:length(nGrid)
    S(i) = chebyshevSegment(nGrid(i),tNode([i,i+1]),dqMax); %#ok<SAGROW>
end
A = structBlkDiag(S,'A');
H = structBlkDiag(S,'H');
D = structBlkDiag(S,'D');
DD = structBlkDiag(S,'DD');

b = []; t = [];  % Slow memory allocation - ok since few iterations
for i=1:length(nGrid)
    b = [b; S(i).b];  %#ok<AGROW>
    t = [t; S(i).t];  %#ok<AGROW>
end

%%%% Boundary Values:
nCstBc = 2*nSegment + 2*(nSegment+1);  %angle + rate

Aeq = zeros(nCstBc,nDecVar); beq = zeros(nCstBc,1);
finalIdx = cumsum(nGrid);
startIdx = 1 + [0, finalIdx(1:(end-1))];

cstIdx = 0;
for i=1:nSegment   %Angle at start of segment
    cstIdx = cstIdx + 1;
    Aeq(cstIdx,startIdx(i)) = 1;  beq(cstIdx) = qNode(i);
end
for i=1:nSegment   %Angle at end of segment
    cstIdx = cstIdx + 1;
    Aeq(cstIdx,finalIdx(i)) = 1;  beq(cstIdx) = qNode(i+1);
end

cstIdx = cstIdx + 1; Aeq(cstIdx,:) = D(1,:);  %zero initial velocity
cstIdx = cstIdx + 1; Aeq(cstIdx,:) = D(end,:);  %zero final velocity

% Defect constraint on rate at segment boundaries
for i=1:(nSegment-1)
    cstIdx = cstIdx + 1;
    Aeq(cstIdx,:) = D(startIdx(i+1),:) - D(finalIdx(i),:);
end

for i=1:(nSegment-1)
    cstIdx = cstIdx + 1;
    Aeq(cstIdx,:) = DD(startIdx(i+1),:) - DD(finalIdx(i),:);
end

%%%% Options:
options = optimset(...
    'Display','iter',...  % {'iter','final'}
    'Algorithm','interior-point-convex');

%%%% Build Problem:
problem.H = (H+H')/2;   %Correct for numerically introduced asymmetry
problem.f = zeros(nDecVar,1);
problem.Aineq = A;
problem.bineq = b;
problem.Aeq = Aeq;
problem.beq = beq;
problem.lb = qLow*ones(nDecVar,1);
problem.ub = qUpp*ones(nDecVar,1);
problem.x0 = [];
problem.options = options;
problem.solver = 'quadprog';

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                             Solve Problem                               %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

[x, fVal, exitFlag, output] = quadprog(problem);
for i=1:nSegment
    SS.grid.t = S(i).t';
    tSpan = SS.grid.t([1,end]);
    SS.grid.q = x(startIdx(i):finalIdx(i))';
    SS.grid.dq = chebyshevDerivative(SS.grid.q,tSpan);
    SS.interp.t = linspace( tSpan(1), tSpan(2), 5*length(SS.grid.q) );
    [q,dq,ddq] = chebyshevInterpolate(SS.grid.q,SS.interp.t,tSpan);
    SS.interp.q = q;
    SS.interp.dq = dq;
    SS.interp.ddq = ddq;
    soln.segment(i) = SS;
end

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                             Return Solution                             %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

soln.info = output;
soln.info.exitFlag = exitFlag;
soln.info.fVal = fVal;

soln.grid.t = t';
soln.grid.q = x';

end


function points = intersectLineCylinder(line, cylinder, varargin)
%INTERSECTLINECYLINDER Compute intersection points between a line and a cylinder.
%
%   POINTS = intersectLineCylinder(LINE, CYLINDER)
%   Returns intersection points between a line and a cylinder.
%
%   Input parameters:
%   LINE     = [x0 y0 z0  dx dy dz]
%   CYLINDER = [x1 y1 z1 x2 y2 z2 R]
%
%   Output:
%   POINTS   = [x1 y1 z1 ; x2 y2 z2]
%
%   POINTS = intersectLineCylinder(LINE, CYLINDER, 'checkBounds', B)
%   Where B is a boolean (TRUE by default), check if the points are within
%   the bounds defined by the two extreme points. If B is false, the
%   cylinder is considered to be infinite.
%
%   Example
%     % Compute intersection between simple vertical cylinder and line
%     line = [60 60 60 1 2 3];
%     cylinder = [20 50 50 80 50 50 30];
%     points = intersectLineCylinder(line, cylinder);
%     % Display the different shapes
%     figure;
%     drawCylinder(cylinder);
%     hold on; light;
%     axis([0 100 0 100 0 100]);
%     drawLine3d(line);
%     drawPoint3d(points, 'ko');
%
%
%     % Compute intersections when one of the points is outside the
%     % cylinder
%     line = [80 60 60 1 2 3];
%     cylinder = [20 50 50 80 50 50 30];
%     intersectLineCylinder(line, cylinder)
%     ans =
%           67.8690   35.7380   23.6069
%
%
%   See also
%   lines3d, intersectLinePlane, drawCylinder, cylinderSurfaceArea
%
%   References
%   See the link:
%   http://www.gamedev.net/community/forums/topic.asp?topic_id=467789
%

% ------
% Author: David Legland, from a file written by Daniel Trauth (RWTH)
% E-mail: david.legland@inra.fr
% Created: 2007-01-27
% Copyright 2007-2022

%% Parse input arguments

% default arguments
checkBounds = true;
% type of cylinder, one of {'closed', 'open', 'infinite'}
type = 'closed';

% parse inputs
while length(varargin)>1
    var = varargin{1};
    if strcmpi(var, 'checkbounds')
        checkBounds = varargin{2};
    elseif strcmpi(var, 'type')
        type = varargin{2};
    else
        error(['Unkown argument: ' var]);
    end
    varargin(1:2) = [];
end

%% Parse cylinder parameters

% Starting point of the line
l0 = line(1:3);

% Direction vector of the line
dl = line(4:6);

% position of cylinder extremities
c1 = cylinder(1:3);
c2 = cylinder(4:6);

% Direction vector of the cylinder
dc = c2 - c1;

% Radius of the cylinder
r = cylinder(7);

%% Resolution of a quadratic equation to find the increment

% normalisation coefficient corresponding to direction of vector
coef = dc / dot(dc, dc);

% Substitution of parameters
e = dl - dot(dl,dc) * coef;
f = (l0-c1) - dot(l0-c1, dc) * coef;

% Coefficients of 2-nd order equation
A = dot(e, e);
B = 2 * dot(e,f);
C = dot(f,f) - r^2;

% compute discriminant
delta = B^2 - 4*A*C;

% check existence of solution(s)
if delta < 0
    points = zeros(0, 3);
    return;
end

% extract roots
pos1 = (-B + sqrt(delta)) / (2*A);
pos2 = (-B - sqrt(delta)) / (2*A);
posList = [pos1;pos2];

%% Estimation of point positions

% process the smallest position
pos1 = min(posList);

% Point on the line: l0 + x*dl = p
point1 = l0 + pos1 * dl;

% process the greatest position
pos2 = max(posList);

% Point on the line: l0 + x*dl = p
point2 = l0 + pos2 * dl;

% Format result
points = [point1 ; point2];

%% Check if points are located between bounds

% if checkBounds option is not set, we can simply skip the rest
if ~checkBounds || strncmpi(type, 'infinite', 1)
    return;
end

% compute cylinder axis
axis = [c1 dc];

% compute position on axis
ts = linePosition3d(points, axis);

% check bounds for open cylinder
% (keep only intersection points whose projection is between the two
% cylinder extremities)
if strncmpi(type, 'open', 1)
    ind = ts>=0 & ts<=1;
    points = points(ind, :);
    return;
end

% which intersection fall before and after bounds
ind1 = find(ts < 0);
ind2 = find(ts > 1);

% case of both intersection on the same side -> no intersection
if length(ind1) == 2 || length(ind2) == 2
    points = zeros(0, 3);
    return;
end

% Process the remaining case of closed cylinder
% -> compute eventual intersection(s) with end faces
if ~isempty(ind1)
    plane = createPlane(c1, dc);
    points(ind1, :) = intersectLinePlane(line, plane);
end
if ~isempty(ind2)
    plane = createPlane(c2, dc);
    points(ind2, :) = intersectLinePlane(line, plane);
end


% Fig. 5.38  Feedback Control of Dynamic Systems, 6e
%             Franklin, Powell, Emami
%script to generate figure 5.38, Piper Dakota autopilot design

clf
numG = 160*conv ([1 2.5],[1 0.7]);
denG = conv([1 5 40],[1  .03  .06]);
sysG = tf(numG,denG);
sysD=tf([1   3],[1   20]);
sysDG=sysD*sysG;
figure(1)
rlocus(sysG)
axis([-30 2 -12 12])
hold on
pause;
rlocus(sysDG)
hold off
pause;
figure(2)
rlocus(sysG)
axis([-1.5 .1    -.6 .6])
hold on
pause;
rlocus(sysDG)
hold off
pause;
Kp = 0.3;
sysH=tf(1,1);
sysT = feedback (Kp*sysG,sysH);
Kc = 1.5;
sysTD=feedback(Kc*sysDG,sysH);
figure(3)
step(sysT)
hold on
step(sysTD)
nicegrid;
hold off


function phaseChangeEnthalpyMethodExample( varargin )
%phaseChangeEnthalpyMethodExample() demonstrates the 2D phase change using
% the enthalpy method
%   In this example, the heat equation is solved on a 2D initially solid
%   rectangular domain. This file contains three test cases in total.
%   By setting COMPUTE_FLAG to either 1, 2 or 3, different kinds of phase
%   change problems for water-ice are solved. Depending on COMPUTE_FLAG,
%   the visualization is either a plot comparing the numerical solution
%   with the analytical solution, a full 2D visualization of the
%   temperature distribution or a full 2D visualization of the phase
%   distribution within the domain.
%
%   See also StefanProblemAnalyticalSolution.
%
%   Date:   27.09.2015
%   Author: Kai Schueller

close all; % closes all figures

addpath('Functions'); % add Functions folder to the search path

%% Set the flag for the computation:
% COMPUTE_FLAG=1; % Solves the two-phase Stefan problem and plots the
                  % solution together with the analytical solution.
                  % Please note: the analytical solution is for a
                  % semi-infinite domain, while the numerical solution is
                  % for a finite domain. Therefore, the difference between
                  % the solutions increases with increasing time at the
                  % right side of the domain
% COMPUTE_FLAG=2; % Same as COMPUTE_FLAG=1, but 2D visualization without
                  % comparison with analytical solution and Neumann BC
% COMPUTE_FLAG=3; % Same as COMPUTE_FLAG=2, but with convection in the
                  % direction of the heat source (i.e. in negative x) so
                  % that there will be a steady state after some time
COMPUTE_FLAG=1;

%% User Input
% Create the mesh
m=createMesh2D(50,3,2,2);
 visualizeMesh2D(m); % uncomment if mesh should be displayed

% Thermophysical properties of the phase change material (PCM)
rho_L=1000;         % density of the liquid phase [kg/m^3]
rho_S=rho_L;%920;   % density of the solid phase [kg/m^3]
h_melt=333400;      % latent heat of melting [J/kg]
T_m=0;              % melting temperature [degC]
T_L=0.01;           % liquidus temperature [degC]
T_S=-0.01;          % solidus temperature [degC]
k_L=0.6;            % thermal conductivity at liquid phase [W/(mK)]
k_S=2.3;            % thermal conductivity at solid phase [W/(mK)]
c_L=4200;           % Heat capacity liquid Phase [J/(kgK)]
c_S=2000;           % Heat capacity of the solid phase [J/(kgK)]
T_init = -5;

% Create the boundary condition structure
BC = createBC(m);

if COMPUTE_FLAG==1
    % We will later extract a 1D solution, so there is no need for many
    % y-cells
    m=createMesh2D(m.dims(1),2,m.facecenters.x(end),m.facecenters.y(end));
    BC = createBC(m);

    u = [0,0];
    T_Solid=-5;     % initial temperature of the solid phase [degC]
    T_Liquid=5;     % initial temperature of the liquid phase [degC]
    analSol.x=0:0.01:2; % x positions for analytical solution

    % Assign boundary conditions
    BC.left.a(:) = 0; BC.left.b(:)=1; BC.left.c(:)=T_Liquid;   % Dirichlet for the left boundary
    BC.right.a(:) = 0; BC.right.b(:)=1; BC.right.c(:)=T_Solid; % Dirichlet for the right boundary
end
if COMPUTE_FLAG==2
    u = [0,0];
    q=-1000; % heat flux [W/m^2]
    % Assign boundary conditions
    BC.left.a(:) = 1; BC.left.b(:)=0; BC.left.c(:)=q;     % Dirichlet for the left boundary
    BC.right.a(:) = 0; BC.right.b(:)=1; BC.right.c(:)=-5; % Dirichlet for the right boundary
end
if COMPUTE_FLAG==3
    u = [-0.005,0];      % velocity of the phase change material in x and y
    q=-1000; % heat flux [W/m^2]
    % Assign boundary conditions
    BC.left.a(:) = 1; BC.left.b(:)=0; BC.left.c(:)=q;     % Dirichlet for the left boundary
    BC.right.a(:) = 0; BC.right.b(:)=1; BC.right.c(:)=-5; % Dirichlet for the right boundary
end

% Solver settings
dt = 36000; % time step size
final_t = 2000000; % final time
convergence_tolerance=1e-8;
relaxation_coeff=0.7;

%% Set initial values
liquidFraction=createCellVariable(m,0);   % initialize liquid fraction,
                                          % 0 = initially solid
Temp = createCellVariable(m, T_init, BC); % initial temperature

RHS_liquidFraction=constantSourceTerm(liquidFraction);

%% main loop

% we use constant boundary conditions, so there is no need to change them
% within the main loop
[Mbc, RHSbc] = boundaryCondition2D(BC);

% the convection velocity does also not change
u_face = createFaceVariable(m, u);

for t=dt:dt:final_t
    % The transient term is calculated outside of the liquid fraction
    % update loop for each time step. So the liquid fraction of the last
    % time step is used.
    rho_c_mix=createMixCellVar(m,liquidFraction,rho_L.*c_L,rho_S.*c_S);
    [M_trans, RHS_trans] = transientTerm(Temp, 1, rho_c_mix);

    RHS_liquidFraction_old=RHS_liquidFraction;

    iterations=0; % set the iteration counter to zero

    % set the error higher than tolerance to enter the while-loop for the
    % first time
    error=convergence_tolerance+1;

    while error>convergence_tolerance || iterations>10000
        iterations=iterations+1;

        % create mixture values
        k_mix=createMixCellVar(m,liquidFraction,k_L,k_S);
        k_mix_face = harmonicMean(k_mix);
        rho_mix=createMixCellVar(m,liquidFraction,rho_L,rho_S);
        rho_mix_face = harmonicMean(rho_mix);
        c_mix=createMixCellVar(m,liquidFraction,c_L,c_S);
        u_rho_mix_face=u_face;
        u_rho_mix_face.xvalue=rho_mix_face.xvalue.*u_face.xvalue;
        u_rho_mix_face.yvalue=rho_mix_face.yvalue.*u_face.yvalue;

        % calculate the matrix M
        Mdiff = dt*diffusionTerm(k_mix_face);
        Mconv =  dt*convectionTerm(u_rho_mix_face);
        M = M_trans-Mdiff+Mbc+Mconv;

        % calculate the right hand side vector
        RHS_gammaTerm=constantSourceTerm(rho_mix).*h_melt.*(RHS_liquidFraction_old-RHS_liquidFraction);
        RHS = RHS_trans+RHSbc+RHS_gammaTerm;

        Temp_old=Temp; % store the old temperature distribution

        % solve the heat equation to obtain the new temperature
        Temp = solvePDE(m,M, RHS);

        % calculate maximum error between current and previous temperatures
        error=max(max(abs(Temp.value-Temp_old.value)));

        % update the liquid fraction based on the result of heat eq.
        liquidFraction=updateLiquidFraction(liquidFraction,...
                                                Temp,...
                                                c_mix,...
                                                h_melt,...
                                                T_L,...
                                                T_S,...
                                                relaxation_coeff);

        RHS_liquidFraction=constantSourceTerm(liquidFraction);
    end

    if COMPUTE_FLAG==1

        % extract 1D data from solution for the plot
        numSol.T=liquidFraction.value(:,3);

        % now interpolate over the ghost cell and first internal cell
        values_temp(1)=(numSol.T(1)+numSol.T(2))/2;
        values_temp(2)=(numSol.T(end)+numSol.T(end-1))/2;
        numSol.T=[values_temp(1); numSol.T(2:end-1); values_temp(2)];

        % extracs the x-positions
        numSol.x = [m.facecenters.x(1); m.cellcenters.x; m.facecenters.x(end)];

        % calculate analytical solution
        [analSol.T,analSol.InterfacePos] = StefanProblemAnalyticalSolution(...
                                             T_Liquid,...
                                             T_Solid,...
                                             T_m,...
                                             h_melt,...
                                             rho_L,...
                                             k_L,...
                                             k_S,...
                                             c_L,...
                                             c_S,...
                                             analSol.x,...
                                             t);

        % plot the analytical and numerical solution
        if t==dt
            h_fig=figure('NumberTitle','Off');
            set(h_fig,'Name',sprintf('Stefan Problem: %d s (%.2f h)',t,t/3600));
            h1=plot(numSol.x,numSol.T,'-x','color',[0 0 1]);
            hold on;
            %h2=plot(analSol.x,analSol.T,'-','color',[1 0 0]);
            h3=plot([analSol.InterfacePos analSol.InterfacePos],[0 1],'--','color',[0 0 0]);
            legend([h1 h3],'Numerical Solution: Liquid fraction','Analytical Solution: Phase Interface');
            xlabel('x [m]');
            ylabel('Liquid fraction');
            hold off;
        else
            set(h1,'XData',numSol.x,'YData',numSol.T);
            %set(h2,'XData',analSol.x,'YData',analSol.T);
            set(h3,'XData',[analSol.InterfacePos analSol.InterfacePos]);
            set(h_fig,'Name',sprintf('Stefan Problem: %d s (%.2f h)',t,t/3600));
        end
        xlim([0 0.5]);
        saveas(h_fig,['figureSaveGamma/fig_',num2str(t),'.eps'],'epsc');
        drawnow;
    elseif COMPUTE_FLAG==2 || COMPUTE_FLAG==3
        if COMPUTE_FLAG==2
            visualizeCells(Temp);
            shading interp;
        else
            visualizeCells(liquidFraction);
        end

        colormap(jet);
        drawnow;
    end
end

end

%% Additional Functions

% createMixCellVar() calculates the mixture value of a given property by
% using the liquid fraction of the mixture
function mixCellVar=createMixCellVar(m,liquidFraction,var_L,var_S)
    mixCellVar=liquidFraction.value*var_L+(1-liquidFraction.value)*var_S;
    mixCellVar=createCellVariable(m,mixCellVar(2:end-1,2:end-1));
end

% updateGamma() calculates a new liquid fraction that better fits the
% mixture enthalpy
function helpVar = updateLiquidFraction( liquidFraction_old,T,c,h_melt,T_L,T_S,relaxation_coefficient )

    T_corr=liquidFraction_old.value*(T_L-T_S)+T_S;

    liquidFraction_new=min(max(liquidFraction_old.value+...
        relaxation_coefficient*c.value./h_melt.*(T.value-T_corr),0),1);

    % set liquid fraction of corner cells to zero
    liquidFraction_new([1 end],[1 end])=0;
    helpVar.value=liquidFraction_new;
    helpVar.domain=liquidFraction_old.domain;

end