 function [xs, info] = ir_pwls_os_rlalm(x, Ab, yi, R, varargin)
%function [xs, info] = ir_pwls_os_rlalm(x, Ab, yi, R, [options])
%|
%| penalized weighted least squares estimation / image reconstruction
%| using relaxed linearized augmented lagrangian method with
%| (optionally relaxed) ordered subsets. (relaxed OS-LALM)
%|
%| See ?. 201? IEEE T-MI by Hung Nien & J A Fessler
%| "Relaxed linearized algorithms for faster X-ray CT image reconstruction"
%|	http://dx.doi.org/10.1109/TMI.201?
%|
%|
%| cost(x) = (y-Ax)' W (y-Ax) / 2 + R(x)
%|
%| in
%|	x	[np 1]		initial estimate
%|	Ab	[nd np]		Gblock object (needs abs(Ab) method)
%|				or sparse matrix (implies nsubset=1)
%|	yi	[nb na]		measurements (noisy sinogram data)
%|	R	penalty		object (see Reg1.m), can be []
%|
%| option
%|	niter			# of iterations (default: 1)
%|	wi	[nb na]		weighting sinogram (default: [] for uniform)
%|	pixmax	[1] or [2]	max pixel value, or [min max] (default [0 inf])
%|	denom	[np 1]		precomputed denominator
%|	aai	[nb na]		precomputed row sums of |Ab|
%|	relax0	[1] or [2]	relax0 or (relax0, relax_rate) (default 1)
%|	rho			AL penalty parameter (default: [] for decreasing rho)
%|	alpha			over-relaxation parameter (default: 1.999)
%|	userfun	@		user-defined function handle (see default below)
%|					taking arguments (x, userarg{:})
%|	userarg	{}		user arguments to userfun (default {})
%|	chat
%|
%| out
%|	xs	[np niter]	iterates
%|	info	[niter 1]	time
%|
%| 2015-11-30, Hung Nien, based on ir_pwls_os_lalm
%| 2015-11-30, tweaks by Jeff Fessler

if nargin == 1 && streq(x, 'test'), ir_pwls_os_rlalm_test, return, end
if nargin < 4, ir_usage, end

% defaults
arg.niter = 1;
arg.isave = [];
arg.userfun = @userfun_default;
arg.userarg = {};
arg.pixmax = inf;
arg.chat = false;
arg.wi = [];
arg.aai = [];
arg.rho = []; % default: decreasing rho
arg.alpha = 1.999;
arg.relax0 = 1;
arg.denom = [];
arg.scale_nblock = true; % traditional scaling
arg.update_even_if_denom_0 = true;
arg = vararg_pair(arg, varargin);

arg.isave = iter_saver(arg.isave, arg.niter);

Ab = block_op(Ab, 'ensure'); % make it a block object (if not already)
nblock = block_op(Ab, 'n');
starts = subset_start(nblock);

cpu etic

wi = arg.wi;
if isempty(wi)
	wi = ones(size(yi), class(yi));
end
if isempty(arg.aai) && isempty(arg.denom)
	arg.aai = reshape(sum(abs(Ab)'), size(yi)); % a_i = sum_j |a_ij|
end

% check input sinogram sizes for OS
if (ndims(yi) ~= 2) || (size(yi,2) == 1 && nblock > 1)
	fail 'bad yi size'
end
if (ndims(wi) ~= 2) || (size(wi,2) == 1 && nblock > 1)
	fail 'bad wi size'
end

relax0 = arg.relax0(1);
if length(arg.relax0) == 1
	relax_rate = 0;
elseif length(arg.relax0) == 2
	relax_rate = arg.relax0(2);
else
	error relax
end

if length(arg.pixmax) == 2
	pixmin = arg.pixmax(1);
	pixmax = arg.pixmax(2);
elseif length(arg.pixmax) == 1
	pixmin = 0;
	pixmax = arg.pixmax;
else
	error pixmax
end

% likelihood denom, if not provided
denom = arg.denom;
if isempty(denom)
	denom = abs(Ab)' * col(arg.aai .* wi); % needs abs(Ab)
        arg = rmfield(arg, 'aai'); % clear to save memory - not needed below
end
if ~arg.update_even_if_denom_0
	% todo: this may not work for LALM because "denom" appears in numerator!
	denom(denom == 0) = inf; % trick: prevents pixels where denom=0 being updated
end

if isempty(R)
	pgrad = 0; % unregularized default
	Rdenom = 0;
end

alpha = arg.alpha;
if alpha<1 || alpha>2
	fail 'alpha should be between 1 and 2'
end

rho = arg.rho;
if isempty(rho)
	rho = @(k) pi/(alpha*k) * sqrt(1 - (pi/(2*(alpha*k)))^2) * (k>1) + (k==1);
else
	rho = @(k) rho; % constant user-specified value
end

[nb na] = size(yi);

x = x(:);
np = length(x);
xs = zeros(np, length(arg.isave), 'single');
if any(arg.isave == 0)
	xs(:, arg.isave == 0) = single(x);
end

%info = zeros(niter,?); % do not initialize since size may change

% initialization
iblock = nblock;
ia = iblock:nblock:na;
li = Ab{iblock} * x;
li = reshape(li, nb, length(ia));
resid = wi(:,ia) .* (li - yi(:,ia));
if arg.scale_nblock
	scale = nblock; % traditional way
else
	scale = na / numel(ia); % alternative - untested
end
zeta = scale * Ab{iblock}' * resid(:);

g = rho(1) * zeta;
h = denom .* x - zeta;

% iterate
for iter = 1:arg.niter
	ticker(mfilename, iter, arg.niter)

	relax = relax0 / (1 + relax_rate * (iter-1));

	% loop over subsets
	for iset = 1:nblock
		k = nblock*(iter-1)+iset;

		num = rho(k) * (denom .* x - h) + (1-rho(k)) * g;
		den = rho(k) * denom;
		if ~isempty(R)
			num = num + R.cgrad(R, x);
			den = den + R.denom(R, x);
		end
		x = x - relax * num ./ den;
		x = max(x, pixmin);
		x = min(x, pixmax);

		iblock = starts(iset);
		ia = iblock:nblock:na;

		li = Ab{iblock} * x;
		li = reshape(li, nb, length(ia));
		resid = wi(:,ia) .* (li - yi(:,ia));

		if arg.scale_nblock
			scale = nblock; % traditional way
		else
			scale = na / numel(ia); % alternative - untested
		end

		zeta = scale * Ab{iblock}' * resid(:); % A' * W * (y - A*x)
		g = (rho(k) * (alpha * zeta + (1-alpha) * g) + g) / (rho(k)+1);
		h = alpha * (denom .* x - zeta) + (1-alpha) * h;
	end

	if any(arg.isave == iter)
		xs(:, arg.isave == iter) = single(x);
	end
	info(iter,:) = arg.userfun(x, arg.userarg{:});
end

% default user function.
% using this evalin('caller', ...) trick, one can compute anything of interest
function out = userfun_default(x, varargin)
chat = evalin('caller', 'arg.chat');
if chat
%	x = evalin('caller', 'x');
	printm('minmax(x) = %g %g', min(x), max(x))
end
out = cpu('etoc');

% ir_pwls_os_rlalm_test()
% test with small quadratic case 1/2 |y - A x|_2^2 + 1/2 |C x|_2^2
% see also ir_ct_fan_beam_sqs_vs_lalm.m
function ir_pwls_os_rlalm_test
rng(0)
nd = 99; np = 38;
A = randn(nd,np);
A = abs(A);
R = Reg1(ones(np,1), 'type_penal', 'mat', 'type_diff', 'sparse', 'beta', 2^0);
C = R.C;
C = full(C);
if 0 % try to make it so unit step size is reasonable?
	D = abs(A)' * (abs(A) * ones(np,1)) + abs(C)' * (abs(C) * ones(np,1));
	A = A * diag(sqrt(1 ./ D));
	C = C * diag(sqrt(1 ./ D));
end
H = A' * A + C' * C;
pr cond(H)

xtrue = ones(np,1); xtrue(10:20) = 2;
yi = A * xtrue; yi = yi + 0.01 * max(yi(:)) * randn(nd,1);
xhat = H \ (A' * yi);
% nrms(xhat, xtrue)

x0 = zeros(np,1);
niter = 100;

printm 'Nesterov' % todo: replace with OGM
gradfun = @(x) A' * (A * x - yi) + C' * (C * x);
step = abs(A') * (abs(A) * ones(np,1)) + abs(C)' * (abs(C) * ones(np,1));
step = 1 / max(step); % todo: use D
xf = ir_fista(x0, 'gradfun', gradfun, 'step', step, 'niter', niter, ...
	'isave', 'all');

printm 'Relaxed LALM'
xl = ir_pwls_os_rlalm(x0, Gmatrix(A), yi, R, 'niter', niter, 'isave', 'all');

printm 'CG'
xc = qpwls_pcg1(x0, A, 1, yi, R.C, 'niter', niter, 'isave', 'all');

if im
	im plc 1 2
	im subplot 1
	plot([xtrue xhat xl(:,end) xc(:,end) xf(:,end)])
	im subplot 2
	xlabel 'iteration', ylabelf 'NRMSD $x^n$ vs $\hat{x}$'
	plot(	0:niter, nrms(xl, xhat), 'r-', ...
		0:niter, nrms(xf, xhat), 'g-', ...
		0:niter, nrms(xc, xhat), 'c-')
	legend('Relaxed LALM', 'FGM', 'CG')
end


function [a]=plus(b,c)
%A=B+C
%   [A]=PLUS(B,C) A is the sum of TT-matrices B and C
%
%
% TT-Toolbox 2.2, 2009-2012
%
%This is TT Toolbox, written by Ivan Oseledets et al.
%Institute of Numerical Mathematics, Moscow, Russia
%webpage: http://spring.inm.ras.ru/osel
%
%For all questions, bugs and suggestions please mail
%ivan.oseledets@gmail.com
%---------------------------
if ( isempty(b) )
   a=c;
   return
elseif (isempty(c) )
   a=b;
   return
end
    a=tt_matrix;
a.n=b.n;
a.m=b.m;
a.tt=b.tt+c.tt;
return
end


%% Microstrip low-pass filter analysis using 3D FDTD code with UPML
%% absorbing borders (ABC)
%
% Here we use FDTD 3D with UPML to calculate scattering coefficients S_{11}
% and S_{21} for planar microstrip low-pass filter following by the original
% paper by D. Sheen, S. Ali, M. Abouzahra, J. Kong "Application of the
% Three-Dimensional Finite-Difference Time-Domain Method to the Analysis of
% planar Microstrip Circuits", IEEE Trans. on Microwave Theory and Techniques
% (http://dx.doi.org/10.1109/22.55775).
% Also |S_{21}| dependence can be taken from the paper "Computational
% electromagnetic method for interconnects and small structures" by C.
% Balanis, A. Policarpou and S. Georgakopoulos
% (http://dx.doi.org/10.1006/spmi.2000.0865)
%% Current code includes some improvements in comparison with the original
%% calculations:
% 1) imposing UPML instead of Mur ABCs;
% 2) using real metal (copper) as a patch conductor material instead of PEC;
% 3) applying matched load at the ends of filter's transmission microstrip
%    lines to prevent physical reflections;
% 4) no "magnetic wall" or "electric wall" conditions at the Ez source plane.
%
function FDTD_3D_Lowpass
close all; clear; clc;
%% Physical constants
   epsilon0 = 8.85418782e-12; mu0 = 1.25663706e-6;
   c = 1.0/sqrt(mu0*epsilon0);

%% Gaussian half-width
   t_half = 15.0e-12;

%% Microstrip transmission lines parameters
   lineW = 2.413e-3;
   lineH = 1.0e-3;
   lineEr = 2.2;
   Z0 = 49.2526;

%% End time
   t_end = 1.5e-9;

%% Total mesh dimensions and grid cells sizes (without PML)
   nx = 80; ny = 100; nz = 16;
   dx = 0.4064e-3; dy = 0.4233e-3; dz = 0.2650e-3;

%% Number of PML layers
   PML = 5;

%% Matrix of material's constants
   number_of_materials = 4;
   % For material of number x = 1,2,3... :
   % Material(x,1) - relative permittivity, Material(x,2) - relative permeability,
   % Material(x,3) - specific conductivity
   % Vacuum
   Material(1,1) = 1.0;   Material(1,2) = 1.0;   Material(1,3) = 0.0;
   % Metal (Copper)
   Material(2,1) = 1.0;   Material(2,2) = 1.0;   Material(2,3) = 5.88e+7;
   % Substrate material (RT/Duroid 5880)
   Material(3,1) = lineEr;   Material(3,2) = 1.0;   Material(3,3) = 0.0;
   % Matched load material is calculated from transmission line parameters
   Material(4,1) = 1.0;   Material(4,2) = 1.0;   Material(4,3) = lineH/(Z0*lineW*dy);

% Add PML layers
   nx = nx + 2*PML; ny = ny + 2*PML; nz = nz + 2*PML;
% Calculate dt
   dt = (1.0/c/sqrt( 1.0/(dx^2) + 1.0/(dy^2) + 1.0/(dz^2)))*0.9999;
   number_of_iterations = ceil(t_end/dt);

%% 3D array for geometry
   Index = ones(nx, ny, nz);

%% Define of low-pass filter geometry
   % Ground plane
   Index((1+PML):(nx-PML), (1+PML):(ny-PML), PML+1) = 2;
   % Rectangular patch (one cell thickness)
   Index((nx/2-25):(nx/2+25), (ny/2-3):(ny/2+3), PML+5) = 2;
   % Transmission line from port 1 to rectangular patch
   Index((nx/2-10):(nx/2-5), (PML+1):ny/2, PML+5) = 2;
   % Transmission line from rectangular patch to port 2
   Index((nx/2+5):(nx/2+10), ny/2:(ny-PML), PML+5) = 2;
   % Dielectric substrate between ground plane and filter patch
   Index((1+PML):(nx-PML), (1+PML):(ny-PML), (PML+2):(PML+4)) = 3;
   % Matched load before port 1
   Index((nx/2-10):(nx/2-5), PML+1, (PML+2):(PML+4)) = 4;
   % Matched load after port 2
   Index((nx/2+5):(nx/2+10), ny-PML, (PML+2):(PML+4)) = 4;

%% 3D FDTD physical (fields) and additional arrays are defined as 'single'
%% to increase performance
   Ex = zeros(nx, ny+1, nz+1, 'single');
   Gx = zeros(nx, ny+1, nz+1, 'single');
   Fx = zeros(nx, ny+1, nz+1, 'single');
   Ey = zeros(nx+1, ny, nz+1, 'single');
   Gy = zeros(nx+1, ny, nz+1, 'single');
   Fy = zeros(nx+1, ny, nz+1, 'single');
   Ez = zeros(nx+1, ny+1, nz, 'single');
   Gz = zeros(nx+1, ny+1, nz, 'single');
   Fz = zeros(nx+1, ny+1, nz, 'single');
   Hx = zeros(nx+1, ny, nz, 'single');
   Bx = zeros(nx+1, ny, nz, 'single');
   Hy = zeros(nx, ny+1, nz, 'single');
   By = zeros(nx, ny+1, nz, 'single');
   Hz = zeros(nx, ny, nz+1, 'single');
   Bz = zeros(nx, ny, nz+1, 'single');

%% FDTD PML coefficients arrays. Here they are already filled with values
%% corresponding to free space
   m = 4; ka_max = 1.0; R_err = 1.0e-16;
   eta = sqrt(mu0/epsilon0*Material(1,1)/Material(1,2));
   k_Ex_c = ones(nx, ny, nz, 'single')*2.0*epsilon0;
   k_Ex_d = ones(nx, ny, nz, 'single')*(-2.0*epsilon0);
   k_Ey_a = ones(nx+1, ny, nz, 'single');
   k_Ey_b = ones(nx+1, ny, nz, 'single')/(2.0*epsilon0);
   k_Gz_a = ones(nx+1, ny, nz, 'single');
   k_Gz_b = ones(nx+1, ny, nz, 'single');
   k_Hy_a = ones(nx, ny, nz, 'single');
   k_Hy_b = ones(nx, ny, nz, 'single')/(2.0*epsilon0);
   k_Hx_c = ones(nx+1, ny, nz, 'single')*2.0*epsilon0/mu0;
   k_Hx_d = ones(nx+1, ny, nz, 'single')*(-2.0*epsilon0/mu0);
   k_Bz_a = ones(nx, ny, nz, 'single');
   k_Bz_b = ones(nx, ny, nz, 'single')*dt;
   k_Gx_a = ones(nx, ny+1, nz, 'single');
   k_Gx_b = ones(nx, ny+1, nz, 'single');
   k_Ey_c = ones(nx, ny, nz, 'single')*2.0*epsilon0;
   k_Ey_d = ones(nx, ny, nz, 'single')*(-2.0*epsilon0);
   k_Ez_a = ones(nx, ny+1, nz, 'single');
   k_Ez_b = ones(nx, ny+1, nz, 'single')/(2.0*epsilon0);
   k_Bx_a = ones(nx, ny, nz, 'single');
   k_Bx_b = ones(nx, ny, nz, 'single')*dt;
   k_Hy_c = ones(nx, ny+1, nz, 'single')*2.0*epsilon0/mu0;
   k_Hy_d = ones(nx, ny+1, nz, 'single')*(-2.0*epsilon0/mu0);
   k_Hz_a = ones(nx, ny, nz, 'single');
   k_Hz_b = ones(nx, ny, nz, 'single')/(2.0*epsilon0);
   k_Ex_a = ones(nx, ny, nz+1, 'single');
   k_Ex_b = ones(nx, ny, nz+1, 'single')/(2.0*epsilon0);
   k_Gy_a = ones(nx, ny, nz+1, 'single');
   k_Gy_b = ones(nx, ny, nz+1, 'single');
   k_Ez_c = ones(nx, ny, nz, 'single')*2.0*epsilon0;
   k_Ez_d = ones(nx, ny, nz, 'single')*(-2.0*epsilon0);
   k_Hx_a = ones(nx, ny, nz, 'single');
   k_Hx_b = ones(nx, ny, nz, 'single')/(2.0*epsilon0);
   k_By_a = ones(nx, ny, nz, 'single');
   k_By_b = ones(nx, ny, nz, 'single')*dt;
   k_Hz_c = ones(nx, ny, nz+1, 'single')*2.0*epsilon0/mu0;
   k_Hz_d = ones(nx, ny, nz+1, 'single')*(-2.0*epsilon0/mu0);

%% General FDTD coefficients
   I = 1:number_of_materials;
   K_a(I) = (2.0*epsilon0*Material(I,1) - Material(I,3)*dt)./...
            (2.0*epsilon0*Material(I,1) + Material(I,3)*dt);
   K_b(I) = 2.0*dt./(2.0*epsilon0*Material(I,1) + Material(I,3)*dt);
   K_c(I) = Material(I,2);
   Ka = single(K_a(Index)); Kb = single(K_b(Index)); Kc = single(K_c(Index));

%% PML coefficients along x-axis
   sigma_max = -(m + 1.0)*log(R_err)/(2.0*eta*PML*dx);
   for I=0:(PML-1)
        sigma_x = sigma_max*((PML - I)/PML)^m;
        ka_x = 1.0 + (ka_max - 1.0)*((PML - I)/PML)^m;
        k_Ey_a(I+1,:,:) = (2.0*epsilon0*ka_x - sigma_x*dt)/...
                          (2.0*epsilon0*ka_x + sigma_x*dt);
        k_Ey_a(nx-I,:,:) = k_Ey_a(I+1,:,:);
        k_Ey_b(I+1,:,:) = 1.0/(2.0*epsilon0*ka_x + sigma_x*dt);
        k_Ey_b(nx-I,:,:) = k_Ey_b(I+1,:,:);
        k_Gz_a(I+1,:,:) = (2.0*epsilon0*ka_x - sigma_x*dt)/...
                          (2.0*epsilon0*ka_x + sigma_x*dt);
        k_Gz_a(nx-I,:,:) = k_Gz_a(I+1,:,:);
        k_Gz_b(I+1,:,:) = 2.0*epsilon0/(2.0*epsilon0*ka_x + sigma_x*dt);
        k_Gz_b(nx-I,:,:) = k_Gz_b(I+1,:,:);
        k_Hx_c(I+1,:,:) = (2.0*epsilon0*ka_x + sigma_x*dt)/mu0;
        k_Hx_c(nx-I,:,:) = k_Hx_c(I+1,:,:);
        k_Hx_d(I+1,:,:) = -(2.0*epsilon0*ka_x - sigma_x*dt)/mu0;
        k_Hx_d(nx-I,:,:) = k_Hx_d(I+1,:,:);

        sigma_x = sigma_max*((PML - I - 0.5)/PML)^m;
        ka_x = 1.0 + (ka_max - 1.0)*((PML - I - 0.5)/PML)^m;
        k_Ex_c(I+1,:,:) = 2.0*epsilon0*ka_x + sigma_x*dt;
        k_Ex_c(nx-I-1,:,:) = k_Ex_c(I+1,:,:);
        k_Ex_d(I+1,:,:) = -(2.0*epsilon0*ka_x - sigma_x*dt);
        k_Ex_d(nx-I-1,:,:) = k_Ex_d(I+1,:,:);
        k_Hy_a(I+1,:,:) = (2.0*epsilon0*ka_x - sigma_x*dt)/...
                          (2.0*epsilon0*ka_x + sigma_x*dt);
        k_Hy_a(nx-I-1,:,:) = k_Hy_a(I+1,:,:);
        k_Hy_b(I+1,:,:) = 1.0/(2.0*epsilon0*ka_x + sigma_x*dt);
        k_Hy_b(nx-I-1,:,:) = k_Hy_b(I+1,:,:);
        k_Bz_a(I+1,:,:) = (2.0*epsilon0*ka_x - sigma_x*dt)/...
                          (2.0*epsilon0*ka_x + sigma_x*dt);
        k_Bz_a(nx-I-1,:,:) = k_Bz_a(I+1,:,:);
        k_Bz_b(I+1,:,:) = 2.0*epsilon0*dt/(2.0*epsilon0*ka_x + sigma_x*dt);
        k_Bz_b(nx-I-1,:,:) = k_Bz_b(I+1,:,:);
   end

%% PML coefficients along y-axis
   sigma_max = -(m + 1.0)*log(R_err)/(2.0*eta*PML*dy);
   for J=0:(PML-1)
        sigma_y = sigma_max*((PML - J)/PML)^m;
        ka_y = 1.0 + (ka_max - 1.0)*((PML - J)/PML)^m;
        k_Gx_a(:,J+1,:) = (2.0*epsilon0*ka_y - sigma_y*dt)/...
                          (2.0*epsilon0*ka_y + sigma_y*dt);
        k_Gx_a(:,ny-J,:) = k_Gx_a(:,J+1,:);
        k_Gx_b(:,J+1,:) = 2.0*epsilon0/(2.0*epsilon0*ka_y + sigma_y*dt);
        k_Gx_b(:,ny-J,:) = k_Gx_b(:,J+1,:);
        k_Ez_a(:,J+1,:) = (2.0*epsilon0*ka_y - sigma_y*dt)/...
                          (2.0*epsilon0*ka_y + sigma_y*dt);
        k_Ez_a(:,ny-J,:) = k_Ez_a(:,J+1,:);
        k_Ez_b(:,J+1,:) = 1.0/(2.0*epsilon0*ka_y + sigma_y*dt);
        k_Ez_b(:,ny-J,:) = k_Ez_b(:,J+1,:);
        k_Hy_c(:,J+1,:) = (2.0*epsilon0*ka_y + sigma_y*dt)/mu0;
        k_Hy_c(:,ny-J,:) = k_Hy_c(:,J+1,:);
        k_Hy_d(:,J+1,:) = -(2.0*epsilon0*ka_y - sigma_y*dt)/mu0;
        k_Hy_d(:,ny-J,:) = k_Hy_d(:,J+1,:);

        sigma_y = sigma_max*((PML - J - 0.5)/PML)^m;
        ka_y = 1.0 + (ka_max - 1.0)*((PML - J - 0.5)/PML)^m;
        k_Ey_c(:,J+1,:) = 2.0*epsilon0*ka_y+sigma_y*dt;
        k_Ey_c(:,ny-J-1,:) = k_Ey_c(:,J+1,:);
        k_Ey_d(:,J+1,:) = -(2.0*epsilon0*ka_y-sigma_y*dt);
        k_Ey_d(:,ny-J-1,:) = k_Ey_d(:,J+1,:);
        k_Bx_a(:,J+1,:) = (2.0*epsilon0*ka_y-sigma_y*dt)/...
                          (2.0*epsilon0*ka_y+sigma_y*dt);
        k_Bx_a(:,ny-J-1,:) = k_Bx_a(:,J+1,:);
        k_Bx_b(:,J+1,:) = 2.0*epsilon0*dt/(2.0*epsilon0*ka_y+sigma_y*dt);
        k_Bx_b(:,ny-J-1,:) = k_Bx_b(:,J+1,:);
        k_Hz_a(:,J+1,:) = (2.0*epsilon0*ka_y-sigma_y*dt)/...
                          (2.0*epsilon0*ka_y+sigma_y*dt);
        k_Hz_a(:,ny-J-1,:) = k_Hz_a(:,J+1,:);
        k_Hz_b(:,J+1,:) = 1.0/(2.0*epsilon0*ka_y+sigma_y*dt);
        k_Hz_b(:,ny-J-1,:) = k_Hz_b(:,J+1,:);
   end

%% PML coefficients along z-axis
   sigma_max = -(m + 1.0)*log(R_err)/(2.0*eta*PML*dz);
   for K=0:(PML-1)
        sigma_z = sigma_max*((PML - K)/PML)^m;
        ka_z = 1.0 + (ka_max - 1.0)*((PML-K)/PML)^m;
        k_Ex_a(:,:,K+1) = (2.0*epsilon0*ka_z - sigma_z*dt)/...
                          (2.0*epsilon0*ka_z + sigma_z*dt);
        k_Ex_a(:,:,nz-K) = k_Ex_a(:,:,K+1);
        k_Ex_b(:,:,K+1) = 1.0/(2.0*epsilon0*ka_z + sigma_z*dt);
        k_Ex_b(:,:,nz-K) = k_Ex_b(:,:,K+1);
        k_Gy_a(:,:,K+1) = (2.0*epsilon0*ka_z - sigma_z*dt)/...
                          (2.0*epsilon0*ka_z + sigma_z*dt);
        k_Gy_a(:,:,nz-K) = k_Gy_a(:,:,K+1);
        k_Gy_b(:,:,K+1) = 2.0*epsilon0/(2.0*epsilon0*ka_z + sigma_z*dt);
        k_Gy_b(:,:,nz-K) = k_Gy_b(:,:,K+1);
        k_Hz_c(:,:,K+1) = (2.0*epsilon0*ka_z + sigma_z*dt)/mu0;
        k_Hz_c(:,:,nz-K) = k_Hz_c(:,:,K+1);
        k_Hz_d(:,:,K+1) = -(2.0*epsilon0*ka_z - sigma_z*dt)/mu0;
        k_Hz_d(:,:,nz-K) = k_Hz_d(:,:,K+1);

        sigma_z = sigma_max*((PML - K - 0.5)/PML)^m;
        ka_z = 1.0 + (ka_max - 1.0)*((PML - K - 0.5)/PML)^m;
        k_Ez_c(:,:,K+1) = 2.0*epsilon0*ka_z + sigma_z*dt;
        k_Ez_c(:,:,nz-K-1) = k_Ez_c(:,:,K+1);
        k_Ez_d(:,:,K+1) = -(2.0*epsilon0*ka_z - sigma_z*dt);
        k_Ez_d(:,:,nz-K-1) = k_Ez_d(:,:,K+1);
        k_Hx_a(:,:,K+1) = (2.0*epsilon0*ka_z - sigma_z*dt)/...
                          (2.0*epsilon0*ka_z + sigma_z*dt);
        k_Hx_a(:,:,nz-K-1) = k_Hx_a(:,:,K+1);
        k_Hx_b(:,:,K+1) = 1.0/(2.0*epsilon0*ka_z + sigma_z*dt);
        k_Hx_b(:,:,nz-K-1) = k_Hx_b(:,:,K+1);
        k_By_a(:,:,K+1) = (2.0*epsilon0*ka_z - sigma_z*dt)/...
                          (2.0*epsilon0*ka_z + sigma_z*dt);
        k_By_a(:,:,nz-K-1) = k_By_a(:,:,K+1);
        k_By_b(:,:,K+1) = 2.0*epsilon0*dt/(2.0*epsilon0*ka_z + sigma_z*dt);
        k_By_b(:,:,nz-K-1) = k_By_b(:,:,K+1);
   end

%% Main 3D FDTD+UPML routine (operates with 'singles' to increase speed)
   hhh = waitbar(0, 'Calculations in progress...');
   tic;
   for T=0:(number_of_iterations-1)
        %% Calculate Fx -> Gx -> Ex
        I = 1:nx; J = 2:ny; K = 2:nz;
        Fx_r = Fx(I,J,K);
        Fx(I,J,K) = Ka(I,J,K).*Fx(I,J,K) + Kb(I,J,K).*...
                    ((Hz(I,J,K) - Hz(I,J-1,K))/dy - (Hy(I,J,K) - Hy(I,J,K-1))/dz);
        Gx_r = Gx(I,J,K);
        Gx(I,J,K) = k_Gx_a(I,J,K).*Gx(I,J,K) + k_Gx_b(I,J,K).*(Fx(I,J,K) - Fx_r);
        Ex(I,J,K) = k_Ex_a(I,J,K).*Ex(I,J,K) + k_Ex_b(I,J,K).*...
                    (k_Ex_c(I,J,K).*Gx(I,J,K) + k_Ex_d(I,J,K).*Gx_r);

        %% Calculate Fy -> Gy -> Ey
        I = 2:nx; J = 1:ny; K = 2:nz;
        Fy_r = Fy(I,J,K);
        Fy(I,J,K) = Ka(I,J,K).*Fy(I,J,K) + Kb(I,J,K).*...
                    ((Hx(I,J,K) - Hx(I,J,K-1))/dz - (Hz(I,J,K) - Hz(I-1,J,K))/dx);
        Gy_r = Gy(I,J,K);
        Gy(I,J,K) = k_Gy_a(I,J,K).*Gy(I,J,K) + k_Gy_b(I,J,K).*(Fy(I,J,K) - Fy_r);
        Ey(I,J,K) = k_Ey_a(I,J,K).*Ey(I,J,K) + k_Ey_b(I,J,K).*...
                    (k_Ey_c(I,J,K).*Gy(I,J,K) + k_Ey_d(I,J,K).*Gy_r);

        %% Calculate Fz -> Gz -> Ez
        I = 2:nx; J = 2:ny; K = 1:nz;
        Fz_r = Fz(I,J,K);
        Fz(I,J,K) = Ka(I,J,K).*Fz(I,J,K) + Kb(I,J,K).*...
                    ((Hy(I,J,K) - Hy(I-1,J,K))/dx - (Hx(I,J,K) - Hx(I,J-1,K))/dy);
        Gz_r = Gz(I,J,K);
        Gz(I,J,K) = k_Gz_a(I,J,K).*Gz(I,J,K) + k_Gz_b(I,J,K).*(Fz(I,J,K) - Fz_r);
        Ez(I,J,K) = k_Ez_a(I,J,K).*Ez(I,J,K) + k_Ez_b(I,J,K).*...
                    (k_Ez_c(I,J,K).*Gz(I,J,K) + k_Ez_d(I,J,K).*Gz_r);

        %% Source of vertical electric field Ez applied to the whole face
        %% plane at the port 1
        if (T*dt<=10.0*t_half)
            Ez((nx/2-10):(nx/2-5), PML+2, (PML+2):(PML+4)) = Source(t_half, 1.0, T*dt);
        end

        %% Save reflected Ez at port 1, passed through Ez at port 2 and Ez
        %% distribution between electrodes
        Incident(T+1) = Source(t_half, 1.0, T*dt);
        Reflected(T+1) = Ez(nx/2-7, PML+2, PML+3) - Incident(T+1);
        Passed(T+1) = Ez(nx/2+7, ny-PML-1, PML+3);
        Ez_saved(1:(ny-2*PML-1), 1:(nx-2*PML-1), T+1) = ...
                Ez((PML+1):(nx-PML-1), (PML+1):(ny-PML-1), PML+3)';

        %% Calculate Bx -> Hx
        I = 2:nx; J = 1:ny; K = 1:nz;
        Bx_r = Bx(I,J,K);
        Bx(I,J,K) = k_Bx_a(I,J,K).*Bx(I,J,K) + k_Bx_b(I,J,K).*...
                    ((Ey(I,J,K+1) - Ey(I,J,K))/dz - (Ez(I,J+1,K) - Ez(I,J,K))/dy);
        Hx(I,J,K) = k_Hx_a(I,J,K).*Hx(I,J,K) + k_Hx_b(I,J,K).*...
                    (k_Hx_c(I,J,K).*Bx(I,J,K) + k_Hx_d(I,J,K).*Bx_r)./Kc(I,J,K);

        %% Calculate By -> Hy
        I = 1:nx; J = 2:ny; K = 1:nz;
        By_r = By(I,J,K);
        By(I,J,K) = k_By_a(I,J,K).*By(I,J,K) + k_By_b(I,J,K).*...
                    ((Ez(I+1,J,K) - Ez(I,J,K))/dx - (Ex(I,J,K+1) - Ex(I,J,K))/dz);
        Hy(I,J,K) = k_Hy_a(I,J,K).*Hy(I,J,K) + k_Hy_b(I,J,K).*...
                    (k_Hy_c(I,J,K).*By(I,J,K) + k_Hy_d(I,J,K).*By_r)./Kc(I,J,K);

        %% Calculate Bz -> Hz
        I = 1:nx; J = 1:ny; K = 2:nz;
        Bz_r = Bz(I,J,K);
        Bz(I,J,K) = k_Bz_a(I,J,K).*Bz(I,J,K) + k_Bz_b(I,J,K).*...
                    ((Ex(I,J+1,K) - Ex(I,J,K))/dy - (Ey(I+1,J,K) - Ey(I,J,K))/dx);
        Hz(I,J,K) = k_Hz_a(I,J,K).*Hz(I,J,K) + k_Hz_b(I,J,K).*...
                    (k_Hz_c(I,J,K).*Bz(I,J,K) + k_Hz_d(I,J,K).*Bz_r)./Kc(I,J,K);

        %% Progress bar updates at every time percent
        if ( mod(T, ceil(number_of_iterations/100)) == 0 )
            waitbar((T+1)/number_of_iterations, hhh);
        end

    end
    toc
    close(hhh);

%% Plots: signals, scattering parameters and Ez field between plates
   figure('units','normalized','outerposition',[0 0 1 1]);
   set(gcf, 'doublebuffer', 'on');
   % Input signal, reflected (at port 1) and passed through (port 2)
     subplot(2,2,1);
     plot(1e9*dt*(0:T), Incident, 'k-', 1e9*dt*(0:T), Reflected, 'r-', 1e9*dt*(0:T), Passed, 'b-', 'LineWidth', 2);
     title('Input, reflected (Port 1) and passed (Port 2) signals', 'FontSize', 18);
     xlabel('Time, [ns]', 'FontSize', 16);
     ylabel('Strength, [V/cm]', 'FontSize', 16);
     legend('Incident (Port 1)' ,'Reflected (Port 1)', 'Passed (Port 2)');
     grid on;
   % |S_11| and |S_21|
     NFFT = 2^nextpow2(T);
     S_11 = fft(Reflected, NFFT)./fft(Incident, NFFT);
     S_21 = fft(Passed, NFFT)./fft(Incident, NFFT);
     subplot(2,2,3);
     f = (0.5/dt)*linspace(0, 1, NFFT/2);
     plot(f*1e-9, 20*log10(abs(S_11(1:NFFT/2))), 'r', ...
          f*1e-9, 20*log10(abs(S_21(1:NFFT/2))), 'b', 'LineWidth', 2);
     title('Scattering parameters - |S_{11}| and |S_{21}|', 'FontSize', 18);
     xlabel('Frequency, [GHz]', 'FontSize', 16);
     ylabel('Magnitude, [dB]', 'FontSize', 16);
     legend('Reflected (Port 1)', 'Passed (Port 2)', 'Location', 'SouthEast');
     xlim([0 20]);
     grid on;
   % Electric field strength Ez between electrodes
     for T=1:20:number_of_iterations
         subplot(2,2,[2 4]);
         contour(1e3*dx*(1:(nx-2*PML-1)), 1e3*dy*(1:(ny-2*PML-1)), ...
         Index((PML+1):(nx-PML-1), (PML+1):(ny-PML-1), PML+5)', 1);
         hold on;
         subplot(2,2,[2 4]);
         pcolor(1e3*dx*(1:(nx-2*PML-1)), 1e3*dy*(1:(ny-2*PML-1)), double(Ez_saved(:, :, T)));
         title(['Time t = ', num2str(round(T*dt*1e12)),' ps'], 'FontSize', 18);
         xlabel('[mm]', 'FontSize', 16);
         ylabel('[mm]', 'FontSize', 16);
         shading interp;
         axis image;
         caxis([-1 1]);
         colorbar;
         drawnow;
     end

%% Gauss function for voltage source
function [res] = Source(t_half, amplitude, t)
% Pulse delay
t0 = 3.0*t_half;
res = amplitude*exp(-0.5*((t-t0)/(t_half/2.35482))^2.0);


function tapas_hgf_binary_mab_plotTraj(r)
% Plots the estimated or generated trajectories for the binary HGF perceptual model for multi-armed
% bandit situations.
%
% Usage example:  est = tapas_fitModel(responses, inputs); tapas_hgf_binary_plotTraj(est);
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2013 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Optional plotting of standard deviations (true or false)
plotsd = true;

% Optional plotting of responses (true or false)
ploty = true;

% Set up display
scrsz = get(0,'screenSize');
outerpos = [0.2*scrsz(3),0.2*scrsz(4),0.8*scrsz(3),0.8*scrsz(4)];
figure(...
    'OuterPosition', outerpos,...
    'Name', 'HGF trajectories');

% Set up colors
colors = [1 0 0; 0.67 0 1; 0 0.67 1; 0.67 1 0];

% Number of bandits
b = r.c_prc.n_bandits;

% Number of trials
n = size(r.u,1);

% Time axis
if size(r.u,2) > 1
    t = r.u(:,end)';
else
    t = ones(1,n);
end

ts = cumsum(t);
ts = [0, ts];

% Number of levels
try
    l = r.c_prc.n_levels;
catch
    l = (length(r.p_prc.p)+1)/5;
end

% Upper levels
for j = 1:l-2

    % Subplots
    subplot(l,1,j);

    if plotsd == true
        upperprior = r.p_prc.mu_0(l-j+1) +sqrt(r.p_prc.sa_0(l-j+1));
        lowerprior = r.p_prc.mu_0(l-j+1) -sqrt(r.p_prc.sa_0(l-j+1));
        upper = [upperprior; r.traj.mu(:,l-j+1,1)+sqrt(r.traj.sa(:,l-j+1,1))];
        lower = [lowerprior; r.traj.mu(:,l-j+1,1)-sqrt(r.traj.sa(:,l-j+1,1))];

        plot(0, upperprior, 'ob', 'LineWidth', 1);
        hold all;
        plot(0, lowerprior, 'ob', 'LineWidth', 1);
        fill([ts, fliplr(ts)], [(upper)', fliplr((lower)')], ...
             'b', 'EdgeAlpha', 0, 'FaceAlpha', 0.15);
    end
    plot(ts, [r.p_prc.mu_0(l-j+1); r.traj.mu(:,l-j+1,1)], 'b', 'LineWidth', 2);
    hold all;
    plot(0, r.p_prc.mu_0(l-j+1), 'ob', 'LineWidth', 2); % prior
    xlim([0 ts(end)]);
    title(['Posterior expectation of x_' num2str(l-j+1)], 'FontWeight', 'bold');
    ylabel(['\mu_', num2str(l-j+1)]);
end

% Second level
subplot(l,1,l-1)

if plotsd == true
    for j=1:b
        upperprior = r.p_prc.mu_0(2) +sqrt(r.p_prc.sa_0(2));
        lowerprior = r.p_prc.mu_0(2) -sqrt(r.p_prc.sa_0(2));
        upper = [upperprior; r.traj.mu(:,2,j)+sqrt(r.traj.sa(:,2,j))];
        lower = [lowerprior; r.traj.mu(:,2,j)-sqrt(r.traj.sa(:,2,j))];

        plot(0, upperprior, 'o', 'Color', colors(j,:), 'LineWidth', 1);
        hold all;
        plot(0, lowerprior, 'o', 'Color', colors(j,:), 'LineWidth', 1);
        fill([ts, fliplr(ts)], [(upper)', fliplr((lower)')], ...
             colors(j,:), 'EdgeAlpha', 0, 'FaceAlpha', 0.15);
    end
end
for j=1:b
    plot(ts, [r.p_prc.mu_0(1); r.traj.mu(:,2,j)], 'Color', colors(j,:), 'LineWidth', 2);
    hold all;
    plot(0, r.p_prc.mu_0(1), 'o', 'Color', colors(j,:), 'LineWidth', 2); % prior
end
xlim([0 ts(end)]);
title('Posterior expectations of x_2', 'FontWeight', 'bold');
ylabel('\mu_2');

% Input level
subplot(l,1,l);

for j=1:b
    plot(ts, [tapas_sgm(r.p_prc.mu_0(2), 1); tapas_sgm(r.traj.mu(:,2,j), 1)], 'Color', colors(j,:), 'LineWidth', 2);
    hold all;
    plot(0, tapas_sgm(r.p_prc.mu_0(2), 1), 'o', 'Color', colors(j,:), 'LineWidth', 2); % prior
end
plot(ts(2:end), r.u(:,1), '.', 'Color', [0 0 0]); % inputs
plot(ts(2:end), r.traj.wt(:,1), 'k') % implied learning rate
if (ploty == true) && ~isempty(find(strcmp(fieldnames(r),'y'))) && ~isempty(r.y)
    y = r.y(:,1);
    if ~isempty(find(strcmp(fieldnames(r),'irr')))
        y(r.irr) = NaN; % weed out irregular responses
        plot(ts(r.irr)+1,  1.08.*ones([1 length(r.irr)]), 'x', 'Color', [1 0.7 0], 'Markersize', 11, 'LineWidth', 2); % irregular responses
    end
    for j=1:b
        plot(find(y==j), 1.08*ones([1 length(find(y==j))]), '.', 'Color', colors(j,:)); % responses
    end
    title(['Response y, input u (black dots), learning rate (fine black line), and posterior expectation of reward s(\mu_2) ', ...
           '(colour coded) for \rho=', num2str(r.p_prc.rho(2:end)), ', \kappa=', ...
           num2str(r.p_prc.ka(2:end)), ', \omega=', num2str(r.p_prc.om(2:end))], ...
      'FontWeight', 'bold');
    ylabel('y, u, s(\mu_2)');
    axis([0 ts(end) -0.15 1.15]);
else
    title(['Input u (black dots), learning rate (fine black line), and posterior expectation of input s(\mu_2) ', ...
           '(red) for \rho=', num2str(r.p_prc.rho(2:end)), ', \kappa=', ...
           num2str(r.p_prc.ka(2:end)), ', \omega=', num2str(r.p_prc.om(2:end))], ...
      'FontWeight', 'bold');
    ylabel('u, s(\mu_2)');
    axis([0 ts(end) -0.1 1.1]);
end
plot(ts(2:end), 0.5, 'k');
xlabel('Trial number');
hold off;


function options = spset(varargin)
% SPSET   Create/alter sparse interpolation OPTIONS structure
%   OPTIONS = SPSET('NAME1',VALUE1,'NAME2',VALUE2,...) creates an
%   options structure OPTIONS in which the named properties have
%   the specified values. Any unspecified properties have default
%   values. It is sufficient to type only the leading characters
%   that uniquely identify the property. Case is ignored for
%   property names.
%
%   OPTIONS = SPSET(OLDOPTS,'NAME1',VALUE1,...) alters an existing
%   options structure OLDOPTS.
%
%   OPTIONS = SPSET(OLDOPTS,NEWOPTS) combines an existing options
%   structure OLDOPTS with a new options structure NEWOPTS. Any new
%   properties overwrite corresponding old properties.
%
%   SPSET with no input arguments displays all property names and
%   their possible values.
%
%   See also SPGET, SPINTERP, SPVALS.
%
%
% SPSET PROPERTIES
%
% GridType - Sparse grid type and basis functions to use
%   [ {Clenshaw-Curtis} | Maximum | NoBoundary | Chebyshev |
%   Gauss-Patterson ]. For an illustration of  the grid types,
%   run CMPGRIDS.
%
% RelTol - Relative error tolerance [ positive scalar {1e-2} ]
%   A relative error tolerance that applies to all hierarchical
%   surpluses of the sparse grid representation. The grid is
%   further refined until all hierarchical surpluses are less than
%   max(RelTol*(max(fevalRange)-min(fevalRange)),AbsTol), with
%   fevalRange containing all results evaluating FUN up to
%   that point.
%
% AbsTol - Absolute error tolerance [ positive scalar {1e-6} ]
%   Used by the error control stated under RelTol.
%
% Vectorized - Vectorized function FUN [ on | {off} ]
%   Indicates if FUN is available for vectorized evaluation.
%   Vectorized coding of FUN can significantly reduce the
%   computation time used by SPVALS. For an example using a
%   vectorized function, please see SPDEMO.
%
% MinDepth - Minimum interpolation depth [ integer {2} ]
%   Minimum number of hierarchical interpolation levels n to
%   compute.
%
% MaxDepth - Maximum interpolation depth [ integer {8} ]
%   Maximum number of hierarchical interpolation levels n to
%   compute. The maximum supported depth (and the default) is 6
%   for the Gauss-Patterson grid.
%
% VariablePositions - Position of the ranges to interpolate in the
%   argument list when FUN is evaluated. [ 1xD vector {[]} ]
%   By setting VariablePositions, SPVALS will evaluate FUN with respect
%   to some of its input parameters, but not necessarily the first
%   D ones. With VARPOS, the actual position of each variable may
%   be set. VARPOS must be a 1xD array.
%
% NumberOfOutputs - Number of outputs [ integer {1} ]
%   If FUN produces multiple outputs (all must be scalar), indicate
%   this here to perform the sparse grid computation for many
%   output variables at once. Also see the example SPDEMOVAROUT.
%
% PrevResults - Previous sparse grid representation [ struct {[]} ]
%   An existing result structure obtained from SPVALS may be
%   provided to further refine an existing sparse grid.
%
% FunctionArgType - Function argument type [ {list} | vector ]
%   Indicates whether the objective function takes the input
%   parameters as a comma-separated list (default) or as a vector.
%
% KeepFunctionValues - Keep the function evaluations [ {off} | on ]
%   If this parameter is set, a structure field 'fvals' is
%   returned, containing a cell array with the function values at
%   the sparse grid points.
%
% KeepGrid - Keep the sparse grid points [ {off} | on ]
%   If this parameter is set, a structure field 'grid' is
%   returned, containing a cell array with the the sparse grid
%   points.
%
% DimensionAdaptive - Set dimension adaptivity option [ {off} | on ]
%   Dimension-adaptive grids try to adaptively find important
%   dimensions and adjust the sparse grid structure
%   accordingly. The implementation of dimension-adaptive sparse
%   grids follows the approach by Gerstner/Griebel.
%
% MinPoints - Minimum number of support nodes [ integer {100} ]
%   This parameter only applies to dimension-adaptive sparse grids,
%   and indicates the minimum number of support nodes
%   (i.e. function evaluations to perform).
%
% MaxPoints - Maximum number of support nodes [ integer {10000} ]
%   This parameter only applies to dimension-adaptive sparse grids.
%   The dimension-adaptive algorithm is aborted once the function
%   evaluation count exceeds this number.
%
% DimadaptDegree - Fine-tuning parameter to alter the degree of
%   dimension-adaptivity [ positive scalar {0.9} ]. A value of 1
%   places complete emphasis on the error estimates, and thus leads
%   to "greedy" dimension-adaptivity. A value of 0 disregards the
%   error estimates, and constructs a conventional sparse grid
%   based on the amount of work involved.
%
% DegreeStrategy - [ {balancing} | depth ] Strategy for the degree
%   of dimensional adaptivity.
%   The 'balancing' strategy balances the number of grid points
%   generated according to the greedy, error estimate-based refine-
%   ment rule compared to the number of points generated by the
%   conventional (regular) sparse grid refinement rule. I.e., a
%   DimadaptDegree value of 0.9 would mean that around 90% of the
%   grid points are generated by the error estimate-based rule, and
%   the remaining points are selected according to the regular
%   rule.
%   The 'depth' strategy makes sure that the maximum level depth
%   reached by the error estimate-based refinement in one dimension
%   does not get too deep compared to the depth reached in the other
%   dimensions. This measurement degree strategy is the one used
%   prior to version 5.1.0 of the toolbox, and is described on page
%   61 of A. Klimke, "Uncertainty modeling using fuzzy arithmetic
%   and sparse grids". This approach is still supported but no
%   the default strategy.
%
% SparseIndices - [ {auto} | on | off ] Manually turn the efficient
%   sparse storage scheme (new feature since version 3.0) of the
%   multi-index arrays on or off. The default switch auto uses the
%   new scheme for the ClenshawCurtis and the Chebyshev grid, and
%   the old (full) storage scheme from version 2.x for the Maximum
%   and the NoBoundary grid (the sparse grid storage scheme is not
%   supported for these two grid types).
%
% DropTol - Drop tolerance [ {auto} | off | 1x2 double vector ]
%   During the sparse grid construction progress, the spvals
%   algorithm may add subgrids with hierarchical surpluses that are
%   all close to zero or of negligible magnitude compared to the
%   surpluses of other sub-grids. In particular, this occurs when
%   additive structure is present in the objective function.
%   To increase the performance of the spinterp algorithm,
%   subgrids where all (absolute) hierarchical surpluses are less
%   than max(relDropTol*(max(fevalRange)-min(fevalRange),absDropTol))
%   can be omitted from the interpolation process by running the
%   sppurge algoritm (see help sppurge).
%   You may specify the absolute and the relative drop tolerance
%   as a vector [ absDropTol, relDropTol ], or turn it off completely
%   (= behavior of version 3.0 and earlier). The switch 'auto' uses
%   the values absDropTol = 0, relDropTol = 100*eps, that is, by
%   default, only a relative drop tolerance is used.
%
% EnableDCT - [ {on} | off ]  Use the DCT when constructing the
%   Chebyshev-Gauss-Lobatto type sparse grid.

% Author : Andreas Klimke, Universitaet Stuttgart
% Version: 1.9
% Date   : November 18, 2007

% Change log:
% V1.0   : Sep 23, 2003
%          Initial release
% V1.1   : Dec 23, 2003
%          Added FunctionArgType handling for more flexibility in
%          defining the objective function.
% V1.2   : Jan 07, 2004
%          Added KeepFunctionValues property to allow the user to
%          access the function values at the sparse grid points.
% V1.3   : Jan 08, 2004
%          Added KeepGrid property to allow the user to access the
%          sparse grid points.
% V1.4   : Apr 22, 2004
%          Added options for dimension-adaptive sparse grids.
% V1.5   : Feb 22, 2005
%          Added handling of sparse sets of indices (useful for
%          high-dimensional interpolation)
% V1.6   : January 13, 2006
%          Added documentation of SparseIndices property
% V1.7   : February 2, 2006
%          Added DropTol property
% V1.8   : March 1, 2006
%          Added DCT feature
% V1.9   : November 18, 2007
%          Added new grid type : Gauss-Patterson
% V2.0   : January 20, 2007
%          Added new parameter for dimensional adaptivity degree
%          strategy.

% ------------------------------------------------------------
% Sparse Grid Interpolation Toolbox
% Copyright (c) 2006 W. Andreas Klimke, Universitaet Stuttgart
% Copyright (c) 2007-2008 W. A. Klimke. All Rights Reserved.
% See LICENSE.txt for license.
% email: klimkeas@ians.uni-stuttgart.de
% web  : http://www.ians.uni-stuttgart.de/spinterp
% ------------------------------------------------------------

% Note: SPSET is similar in syntax and code the the options
% handling with ODEGET and ODESET of the MATLAB ODE suite by Marc
% Reichelt and Lawrence Shampine.

% Print out possible values of properties.
if (nargin == 0) && (nargout == 0)
  fprintf(['          GridType: [ {Clenshaw-Curtis} | Maximum |' ...
					 ' NoBoundary | Chebyshev | Gauss-Patterson ]\n']);
  fprintf('            RelTol: [ positive scalar {1e-2} ]\n');
  fprintf('            AbsTol: [ positive scalar {1e-6} ]\n');
  fprintf('        Vectorized: [ {off} | on ]\n');
	fprintf('          MinDepth: [ positive integer {2} ]\n');
	fprintf('          MaxDepth: [ positive integer {8} ] (6 for Gauss-Patt.)\n');
  fprintf(' VariablePositions: [ double matrix {[]} ]\n');
  fprintf('   NumberOfOutputs: [ positive integer {1} ]\n');
  fprintf('       PrevResults: [ struct {[]} ]\n');
	fprintf('   FunctionArgType: [ {list} | vector ]\n');
	fprintf('KeepFunctionValues: [ {off} | on ]\n');
	fprintf('          KeepGrid: [ {off} | on ]\n');
	fprintf(' DimensionAdaptive: [ {off} | on ]\n');
	fprintf('         MinPoints: [ positive integer {100} ]\n');
	fprintf('         MaxPoints: [ positive integer {10000} ]\n');
	fprintf('    DimadaptDegree: [ positive scalar in [0,1] {0.9} ]\n');
	fprintf('    DegreeStrategy: [ {balancing} | depth ]\n');
	fprintf('     SparseIndices: [ {auto} | on | off ]\n');
	fprintf(['           DropTol: [ {auto} | off |' ...
					 ' [absTol, relTol] (pos. scalars)]\n']);
	fprintf('         EnableDCT: [ {on} | off ]\n');
  fprintf('\n');
  return;
end

Names = {'GridType', 'RelTol', 'AbsTol', 'Vectorized', 'MinDepth', ...
				 'MaxDepth', 'VariablePositions', 'NumberOfOutputs', ...
				 'PrevResults', 'FunctionArgType', 'KeepFunctionValues', ...
				 'KeepGrid', 'DimensionAdaptive', 'MinPoints', ...
				 'MaxPoints', 'DimadaptDegree', 'DegreeStrategy', ...
				 'SparseIndices', 'DropTol', 'EnableDCT'};

m = length(Names);
names = cell(m,1);
for k = 1:m
	names{k} = lower(Names{k});
end

% Combine all leading options structures o1, o2, ... in odeset(o1,o2,...).
options = [];
for k = 1:m
  options.(Names{k}) = [];
end

k = 1;
while k <= nargin
  arg = varargin{k};
  if ischar(arg)                        % arg is an option name
    break;
  end
  if ~isempty(arg)                      % [] is a valid options argument
    if ~isa(arg,'struct')
      msg = sprintf(['Expected argument %d to be a string property' ...
										 ' name or an options structure\ncreated with' ...
										 ' SPSET.'], k);
      error(msg);
    end
    for l = 1:m
      if any(strcmp(fieldnames(arg),Names{l}))
        val = arg.(Names{l});
      else
        val = [];
      end
      if ~isempty(val)
        options.(Names{l}) = val;
      end
    end
		k = k + 1;
  else
		k = k + 1;
		break;
	end
end

% now process the name-value pairs.
if rem(nargin-k+1,2) ~= 0
  error('Arguments must occur in name-value pairs.');
end

expectval = 0;                      % start expecting a name, not a
                                    % value
while k <= nargin
  arg = varargin{k};

  if ~expectval
    if ~ischar(arg)
      msg = sprintf(['Expected argument %d to be a string property' ...
										 ' name.'], k);
      error(msg);
    end
    lowArg = lower(arg);
    matched = strmatch(lowArg,names);
    if isempty(matched)
      msg = sprintf('Unrecognized property name ''%s''.', arg);
      error(msg);
    elseif length(matched) > 1
      msg = sprintf('Ambiguous property name ''%s'' ', arg);
      msg = [msg '(' Names{matched(1)}];
      for l = matched(2:length(matched))'
        msg = [msg ', ' Names{l}];
      end
      msg = sprintf('%s).', msg);
      error(msg);
    end
    expectval = 1;                      % we expect a value next

  else
    options.(Names{matched}) = arg;
    expectval = 0;

  end
  k = k + 1;
end

if expectval
  msg = sprintf('Expected value for property ''%s''.', arg);
  error(msg);
end


function boundary = p11_boundary_nearest ( m, n, point )

%*****************************************************************************80
%
%% P11_BOUNDARY_NEAREST returns a nearest boundary point in problem 11.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 April 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer N, the number of points.
%
%    Input, real POINT(M,N), the coordinates of the points.
%
%    Output, real BOUNDARY(M,N), points on the boundary
%    that are nearest to each point.
%
  q1 = [ ...
    0.00, 0.00; ...
    1.00, 0.00; ...
    0.75, 0.25; ...
    0.25, 0.25 ]';
  q2 = [ ...
    0.00, 0.00; ...
    0.25, 0.25; ...
    0.25, 0.75; ...
    0.00, 1.00 ]';
  q3 = [ ...
    0.50, 0.50; ...
    0.50, 0.25; ...
    0.75, 0.25; ...
    1.00, 0.50 ]';
  q4 = [ ...
    0.50, 0.50; ...
    0.25, 0.50; ...
    0.25, 0.25; ...
    0.50, 0.25 ]';
  q5 = [ ...
    0.50, 0.50; ...
    0.50, 1.00; ...
    0.25, 0.75; ...
    0.25, 0.50 ]';
  t1 = [ ...
    1.00, 0.00; ...
    1.00, 0.50; ...
    0.75, 0.25 ]';
  t2 = [ ...
    0.00, 1.00; ...
    0.25, 0.25; ...
    0.50, 1.00 ]';
  t3 = [ ...
    0.50, 0.50; ...
    1.00, 0.50; ...
    1.00, 1.00 ]';
  t4 = [ ...
    0.50, 0.50; ...
    1.00, 1.00; ...
    0.50, 1.00 ]';

  x1 =  0.0;
  x2 =  0.5;
  x3 = +1.0;
  y1 =  0.0;
  y2 =  0.5;
  y3 = +1.0;

  for j = 1 : n

    if ( point(1,j) <= x1 & point(2,j) <= y1 )

      boundary(1,j) = x1;
      boundary(2,j) = y1;

    elseif ( point(1,j) <= x1 & point(2,j) <= y3 )

      boundary(1,j) = x1;
      boundary(2,j) = point(2,j);

    elseif ( point(1,j) <= x1 & y3 <= point(2,j) )

      boundary(1,j) = x1;
      boundary(2,j) = y3;

    elseif ( point(1,j) <= x3 & point(2,j) <= y1 )

      boundary(1,j) = point(1,j);
      boundary(2,j) = y1;

    elseif ( quad_contains_point_2d ( q1, point(1:2,j) ) )

      boundary(1,j) = point(1,j);
      boundary(2,j) = y1;

    elseif ( quad_contains_point_2d ( q2, point(1:2,j) ) )

      boundary(1,j) = x1;
      boundary(2,j) = point(2,j);

    elseif ( triangle_contains_point_2d ( t1, point(1:2,j) ) )

      boundary(1,j) = x3;
      boundary(2,j) = point(2,j);

    elseif ( quad_contains_point_2d ( q3, point(1:2,j) ) )

      boundary(1,j) = point(1,j);
      boundary(2,j) = y2;

    elseif ( quad_contains_point_2d ( q4, point(1:2,j) ) )

      boundary(1,j) = x2;
      boundary(2,j) = y2;

    elseif ( quad_contains_point_2d ( q5, point(1:2,j) ) )

      boundary(1,j) = x2;
      boundary(2,j) = point(2,j);

    elseif ( triangle_contains_point_2d ( t2, point(1:2,j) ) )

      boundary(1,j) = point(1,j);
      boundary(2,j) = y3;

    elseif ( x1 <= point(1,j) & point(1,j) <= x2 & y3 <= point(2,j) )

      boundary(1,j) = point(1,j);
      boundary(2,j) = y3;

    elseif ( triangle_contains_point_2d ( t3, point(1:2,j) ) )

      boundary(1,j) = point(1,j);
      boundary(2,j) = y2;

    elseif ( triangle_contains_point_2d ( t4, point(1:2,j) ) )

      boundary(1,j) = x2;
      boundary(2,j) = point(2,j);

    elseif ( x2 <= point(1,j) & point(1,j) <= x3 & y3 <= point(2,j) )

      boundary(1,j) = x2;
      boundary(2,j) = y3;

    elseif ( x3 <= point(1,j) & point(2,j) <= y1 )

      boundary(1,j) = x3;
      boundary(2,j) = y1;

    elseif ( x3 <= point(1,j) & y1 <= point(2,j) & point(2,j) <= y2 )

      boundary(1,j) = x3;
      boundary(2,j) = point(2,j);

    elseif ( x3 <= point(1,j) & y2 <= point(2,j) & point(2,j) <= y3 )

      boundary(1,j) = x3;
      boundary(2,j) = y2;

    elseif ( x3 <= point(1,j) & y3 <= point(2,j) & point(2,j) <= point(1,j) )

      boundary(1,j) = x3;
      boundary(2,j) = y2;

    elseif ( x3 <= point(1,j) & y3 <= point(2,j) & point(1,j) <= point(2,j) )

      boundary(1,j) = x2;
      boundary(2,j) = y3;

    else

      fprintf ( 1, '\n' );
      fprintf ( 1, 'P11_BOUNDARY_NEAREST - Fatal error!\n' );
      fprintf ( 1, '  Orphan point = ( %f, %f )\n', point(1:2,j) );
      error ( 'P11_BOUNDARY_NEAREST - Fatal error!' );

    end

  end

  return
end


function [high_fissure_indices, ref_image] = PTKGetMaxFissurePoints(fissure_approximation, lung_mask, fissureness, image_roi, image_size)
    % PTKGetMaxFissurePoints. function for finding candidate points of high
    %     fissureness given an initial fissure approximation.
    %
    %     PTKGetMaxFissurePoints is an intermediate stage in segmenting the
    %     lobes. It is not intended to be a general-purpose algorithm.
    %
    %     For more information, see
    %     [Doel et al., Pulmonary lobe segmentation from CT images using
    %     fissureness, airways, vessels and multilevel B-splines, 2012]
    %
    %     Licence
    %     -------
    %     Part of the TD Pulmonary Toolkit. https://github.com/tomdoel/pulmonarytoolkit
    %     Author: Tom Doel, 2012.  www.tomdoel.com
    %     Distributed under the GNU GPL v3 licence. Please see website for details.
    %

    data_points_indices = find(fissure_approximation);

    bin_size = 2;

    [high_fissure_indices, ref_image] = GetFissurePoints(data_points_indices, image_size, lung_mask, fissureness, image_roi, bin_size);
end

function [maximum_fissureness_indices, ref_image] = GetFissurePoints(indices_for_model, image_size, lung_segmentation, fissureness, image_roi, bin_size)

    % Get the coordinates of every voxel in the lung segmentation
    candidate_indices = find(lung_segmentation.RawImage);

    % An image for debugging
    ref_image = zeros(image_size, 'uint8');
    ref_image(candidate_indices) = 2;

    % Remove points with low fissureness
    max_fissureness = max(fissureness.RawImage(candidate_indices));
    fissureness_threshold = max_fissureness/3;
    intensity_threshold_hu = -900;
    intensity_threshold = image_roi.HounsfieldToGreyscale(intensity_threshold_hu);
    candidate_indices = candidate_indices((fissureness.RawImage(candidate_indices) > fissureness_threshold) & (image_roi.RawImage(candidate_indices) > intensity_threshold));
    ref_image(candidate_indices) = 4;

    if isempty(candidate_indices)
        maximum_fissureness_indices = [];
        return;
    end

    % Find a rotation matrix
    [eigv, m] = GetRotationMatrix(indices_for_model, image_size);

    [x_all, y_all, z_all] = MimImageCoordinateUtilities.FastInd2sub(image_size, candidate_indices);
    X_all = [x_all(:), y_all(:), z_all(:)]';

    % Transform to new basis
    em_all = X_all - m(:, ones(1, size(X_all, 2)));
    em_all = eigv*em_all;

    % We allocate each point to a bin according to the x-y coordinates in the
    % transformed domain
    x1_all = em_all(1, :);
    y1_all = em_all(2, :);

    % Get the coordinates of each of the model points
    [x_model, y_model, z_model] = MimImageCoordinateUtilities.FastInd2sub(image_size, indices_for_model);
    X_model = [x_model(:), y_model(:), z_model(:)]';

    % Transform to new basis
    em_model = X_model - m(:, ones(1, size(X_model, 2)));
    em_model = eigv*em_model;

    % We allocate each point to a bin according to the x-y coordinates in the
    % transformed domain
    x1_model = em_model(1, :);
    y1_model = em_model(2, :);

    % Project the candidate points onto the fissure plane, and remove those that
    % are not within the convex hull formed by the model points on the plane. In
    % effect, we are using the model coordinates (the initial guess) as a bound
    % for the x-y coordinates used to construct the fissure surface.
    xy_coords_model = [x1_model', y1_model'];
    dt = DelaunayTri(xy_coords_model);
    simplex_index = pointLocation(dt, x1_all', y1_all');
    is_valid = ~isnan(simplex_index);
    candidate_indices_ok = candidate_indices(is_valid);

    if isempty(candidate_indices_ok)
        maximum_fissureness_indices = [];
        return;
    end

    % Turn into x,y,z vectors
    [x_all, y_all, z_all] = MimImageCoordinateUtilities.FastInd2sub(image_size, candidate_indices_ok);
    X_all = [x_all(:), y_all(:), z_all(:)]';

    % Transform to new basis
    em_all = X_all - m(:, ones(1, size(X_all, 2)));
    em_all = eigv*em_all;

    % We allocate each point to a bin according to the x-y coordinates in the
    % transformed domain
    x1_all = em_all(1, :);
    y1_all = em_all(2, :);

    [maximum_fissureness_indices, all_maxima] = SortIntoBins(x1_all, y1_all, candidate_indices_ok, fissureness, bin_size, image_size);

    ref_image(all_maxima) = 6;
    ref_image(maximum_fissureness_indices) = 3;

    % Remove non-connected points (outliers)
    [x_all, y_all, z_all] = MimImageCoordinateUtilities.FastInd2sub(image_size, maximum_fissureness_indices);
    points_coords = ArraysToPoints(x_all(:), y_all(:), z_all(:));

    min_dilate_size_mm = 2.5;
    max_dilate_size_mm = 2.5;

    dilate_size_mm = min_dilate_size_mm;

    calculate_again = true;
    target_number_of_points = round(numel(x_all)/2);

    % Perform the removal of connected points with a variable dilation size
    while calculate_again
        points_coords_new = RemoveNonConnectedPoints(points_coords, lung_segmentation, image_size, dilate_size_mm);
        number_of_found_points = numel(points_coords_new);
        if (number_of_found_points >= target_number_of_points) || (dilate_size_mm > max_dilate_size_mm)
            calculate_again = false;
        else
            dilate_size_mm = dilate_size_mm + 0.5;
        end

    end

    [x_all, y_all, z_all] = PointsToArrays(points_coords_new);
    maximum_fissureness_indices = MimImageCoordinateUtilities.FastSub2ind(image_size, x_all(:), y_all(:), z_all(:));
    ref_image(maximum_fissureness_indices) = 1;

end

function [rot_matrix, m] = GetRotationMatrix(indices, image_size)
    [x, y, z] = ind2sub(image_size, indices);
    X = [x, y, z]';

    % Find a suitable basis for these points using PCA
    m = mean(X, 2);
    em = X - m(:, ones(1, size(X, 2)));
    eigv = pts_pca(em);

    rot_matrix = eigv';
end

function [maximum_fissureness_indices, all_maxima] = SortIntoBins(x1, y1, candidate_indices, fissureness, bin_size, image_size)
    binx = floor(x1/bin_size);
    binx = binx - min(binx);
    biny = floor(y1/bin_size);
    biny = biny - min(biny);

    % bin is an array containing the bin to which each element of indices has been allocated
    bin = binx + (max(binx)+1)*biny;

    % Now sort the indices and bins by fissureness
    fissureness_at_indices = fissureness.RawImage(candidate_indices);

    is_maxima = GetMaxima(candidate_indices, bin, fissureness_at_indices, fissureness, image_size);
    all_maxima = candidate_indices(is_maxima);
    bin = bin(is_maxima);
    candidate_indices = candidate_indices(is_maxima);

    fissureness_at_indices = fissureness.RawImage(candidate_indices);

    [~, sorted_indices] = sort(fissureness_at_indices, 'descend');
    indices_sorted_by_fissureness = candidate_indices(sorted_indices);
    bins_sorted_by_fissureness = bin(sorted_indices);

    % Use unique to obtain the first indices corresponding to each bin - this
    % will be the point with the largest fissureness
    maximum_fissureness_indices = [];
    for selection_index = 1 : 1
        [~, bin_indices, ~] = unique(bins_sorted_by_fissureness, 'first');
        maximum_fissureness_indices = [maximum_fissureness_indices; indices_sorted_by_fissureness(bin_indices)];

        bins_sorted_by_fissureness(bin_indices) = [];
        indices_sorted_by_fissureness(bin_indices) = [];
    end

    maximum_fissureness_indices = maximum_fissureness_indices(fissureness.RawImage(maximum_fissureness_indices) > 0);

end

function is_maxima = GetMaxima(candidate_indices, bin, fissureness_at_indices, fissureness, image_size)
    bin_allocation = zeros(image_size, 'int32');
    bin_allocation(candidate_indices) = bin;

    [linear_offsets, ~] = MimImageCoordinateUtilities.GetLinearOffsets(image_size);
    neighbours = repmat(int32(candidate_indices), 1, 6) + repmat(int32(linear_offsets), length(candidate_indices), 1);
    fissureness_neighbours = fissureness.RawImage(neighbours);
    bins_neighbours = bin_allocation(neighbours);
    bins_match = bins_neighbours == repmat(bin', 1, 6);
    fissureness_centre = repmat(fissureness_at_indices, 1, 6);

    % Force neighbouring points in different bins to have a lower fissureness
    fissureness_neighbours(~bins_match) = fissureness_centre(~bins_match) - 1;
    fissureness_less = fissureness_neighbours < fissureness_centre;
    sums = sum(fissureness_less, 2);
    is_maxima = sums == 6;
end

function points_coords = RemoveNonConnectedPoints(points_coords, template_image, image_size, dilate_size_mm)
    voxel_volume = prod(template_image.VoxelSize);
    min_component_size_mm3 = 300;
    min_component_size_voxels = round(min_component_size_mm3/voxel_volume);

    image_mask = false(image_size);
    indices = sub2ind(image_size, points_coords(:,1), points_coords(:,2), points_coords(:,3));
    image_mask(indices) = true;

    tci = template_image.BlankCopy;
    tci.ChangeRawImage(image_mask);
    tci.BinaryMorph(@imdilate, dilate_size_mm);
    connected_image = tci.RawImage;

    connected_components = bwconncomp(connected_image, 26);
    num_components = connected_components.NumObjects;
    for component = 1 : num_components
        pixels = connected_components.PixelIdxList{component};
        if length(pixels) < min_component_size_voxels
            connected_image(pixels) = false;
        end
    end
    image_mask = image_mask & connected_image;
    indices = find(image_mask(:));
    [i2_coords, j2_coords, k2_coords] = ind2sub(image_size, indices);
    points_coords = ArraysToPoints(i2_coords, j2_coords, k2_coords);
end

function points = ArraysToPoints(i, j, k)
    points = [i(:), j(:), k(:), ones(size(i(:)))];
end

function [is, js, ks] = PointsToArrays(points)
    is = points(:, 1);
    js = points(:, 2);
    ks = points(:, 3);
end


function [nodes, edges, faces] = gcontour3d(img)
%GCONTOUR3D Create contour graph of a 3D binary image.
%
%
%
%   ---------
%
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 28/06/2004.
%

nodes = zeros([0 3]);   % 3 coordinates vertices
edges = zeros([0 2]);   % first node and second nodes
faces = zeros([0 4]);   % indices of 4 corners of each square face

D1 = size(img, 1);
D2 = size(img, 2);
D3 = size(img, 3);

% first direction for image
for y = 1:D2
    for z = 1:D3
        % find transitions between the two phases
        ind = find(img(1:D1-1, y, z)~=img(2:D1, y, z));

        % process each transition in direction 1
        for i2 = 1:length(ind)

            % coordinates of each node
            n1 = [ind(i2)+.5 y-.5 z-.5];
            n2 = [ind(i2)+.5 y-.5 z+.5];
            n3 = [ind(i2)+.5 y+.5 z+.5];
            n4 = [ind(i2)+.5 y+.5 z-.5];

            % add the face (and edges) with the 4 given nodes
            [nodes, edges, faces] = addFace(nodes, edges, faces, [n1; n2; n3; n4]);
        end
    end
end

% second direction for image
for x=1:D1
    for z=1:D3
        % find transitions between the two phases
        ind = find(img(x, 1:D2-1, z)~=img(x, 2:D2, z));

        % process each transition in direction 1
        for i2 = 1:length(ind)

            % coordinates of each node
            n1 = [x-.5 ind(i2)+.5 z-.5];
            n2 = [x-.5 ind(i2)+.5 z+.5];
            n3 = [x+.5 ind(i2)+.5 z+.5];
            n4 = [x+.5 ind(i2)+.5 z-.5];

            % add the face (and edges) with the 4 given nodes
            [nodes, edges, faces] = addFace(nodes, edges, faces, [n1; n2; n3; n4]);
        end
    end
end

% third direction for image
for x=1:D1
    for y=1:D2
        % find transitions between the two phases
        ind = find(img(x, y, 1:D3-1)~=img(x, y, 2:D3));

        % process each transition in direction 1
        for i2 = 1:length(ind)

            % coordinates of each node
            n1 = [x-.5 y-.5 ind(i2)+.5];
            n2 = [x-.5 y+.5 ind(i2)+.5];
            n3 = [x+.5 y+.5 ind(i2)+.5];
            n4 = [x+.5 y-.5 ind(i2)+.5];

            % add the face (and edges) with the 4 given nodes
            [nodes, edges, faces] = addFace(nodes, edges, faces, [n1; n2; n3; n4]);
        end
    end
end

return;

function [nodes, edges, faces] = addFace(nodes, edges, faces, faceNodes)
% add given nodes and coresponding face to the graph.

n1 = faceNodes(1,:);
n2 = faceNodes(2,:);
n3 = faceNodes(3,:);
n4 = faceNodes(4,:);

% search indices of each nodes
ind1 = find(ismember(nodes, n1, 'rows'));
ind2 = find(ismember(nodes, n2, 'rows'));
ind3 = find(ismember(nodes, n3, 'rows'));
ind4 = find(ismember(nodes, n4, 'rows'));

% if nodes are not in the list, we add them
if isempty(ind1)
    nodes = [nodes; n1];
    ind1 = size(nodes, 1);
end
if isempty(ind2)
    nodes = [nodes; n2];
    ind2 = size(nodes, 1);
end
if isempty(ind3)
    nodes = [nodes; n3];
    ind3 = size(nodes, 1);
end
if isempty(ind4)
    nodes = [nodes; n4];
    ind4 = size(nodes, 1);
end

% add current face to the list
faces(size(faces, 1)+1, 1:4) = [ind1(1) ind2(1) ind3(1) ind4(1)];

% create edges of the face
% (first index is the smallest one, by convention)
e1 = [min(ind1, ind2) max(ind1, ind2)];
e2 = [min(ind2, ind3) max(ind2, ind3)];
e3 = [min(ind3, ind4) max(ind3, ind4)];
e4 = [min(ind4, ind1) max(ind4, ind1)];

 % if nodes are not in the list, we add them
if isempty(find(ismember(edges, e1, 'rows'), 1))
    edges = [edges; e1];
end
if isempty(find(ismember(edges, e2, 'rows'), 1))
    edges = [edges; e2];
end
if isempty(find(ismember(edges, e3, 'rows'), 1))
    edges = [edges; e3];
end
if isempty(find(ismember(edges, e4, 'rows'), 1))
    edges = [edges; e4];
end


function [y, x] = max(f, flag, dim)
%MAX   Maximum value of a CHEBFUN.
%   MAX(F) and MAX(F, 'global') return the maximum value of the CHEBFUN F.
%
%   [Y, X] = MAX(F) returns also a point X such that F(X) = Y.
%
%   [Y, X] = MAX(F, 'local') returns not just the global maximum value but all
%   of the local maxima.
%
%   If F is complex-valued, absolute values are taken to determine the maxima,
%   but the resulting values correspond to those of the original function.
%
%   If F is array-valued, then the columns of X and Y correspond to the columns
%   of F. NaNs are used to pad Y and X when the 'local' flag is used and the
%   columns are not of the same length.
%
%   H = MAX(F, G), where F and G are CHEBFUNs defined on the same domain,
%   returns a CHEBFUN H such that H(x) = max(F(x), G(x)) for all x in the
%   domain of F and G. Alternatively, either F or G may be a scalar.
%
%   MAX(F, [], DIM) computes the maximum of the CHEBFUN F in the dimension DIM.
%   If DIM = 1 and F is a column CHEBFUN or DIM = 2 and F is a row CHEBFUN, this
%   is equivalent to MAX(F). Otherwise, MAX(F, [], DIM) returns a CHEBFUN which
%   is the maximum across the discrete dimension of F. For example, if F is a
%   quasimatrix with two columns, MAX(F, [], 2) = MAX(F(:,1), F(:,2)).
%
% See also MIN, MINANDMAX, ROOTS.

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

% Trivial empty case.
if ( isempty(f) )
    x = [];
    y = [];
    return
end

if ( nargin == 3 )
    if ( ~any(dim == [1 2]) )
        error('CHEBFUN:CHEBFUN:max:badDim', ...
            'DIM input to CHEBFUN MAX must be 1 or 2.');
    end

    if ( dim ~= 1 + f(1).isTransposed )
        % Take max across discrete dimension of a quasimatrix:
        f = cheb2cell(f);
        y = -realmax;
        for k = 1:numel(f)
            y = maxOfTwoChebfuns(y, f{k});
        end
        y = merge(y);
        return
    end
end

if ( (nargin > 2) && isempty(flag) )
    % MAX(F, [], 1).
    flag = 'global';
end

if ( (nargin == 1) || strcmp(flag, 'global') )
    % MAX(F) or MAX(F, 'global')
    [y, x] = globalMax(f);

elseif ( isa(flag, 'chebfun') || isnumeric(flag) )
    % MAX(F, G)
    y = maxOfTwoChebfuns(f, flag);

elseif ( strcmp(flag, 'local') )
    % MAX(F, 'local')
    [y, x] = localMax(f);

else
    error('CHEBFUN:CHEBFUN:max:flag', 'Unrecognized flag.');

end

end

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%% GLOBAL MAXIMUM %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [y, x] = globalMax(f)

% Call MINANDMAX():
[y, x] = minandmax(f);

% Extract the maximum:
y = y(2,:);
x = x(2,:);

end

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%% LOCAL MAXIMA %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [y, x] = localMax(f)

% Call MINANDMAX():
[y, x] = minandmax(f, 'local');

% Determine which are maxima.

ends = f(1).domain([1, end]).'; % Endpoints of the domain are special.
f = mat2cell(f); % Convert f into a cell of scalar-valued CHEBFUNs.

% Loop over the FUNs:
for k = 1:numel(f)
    % Compute 1st and 2nd derivatives.
    dfk = diff(f{k});
    dfk2 = diff(dfk);

    % For interior extrema, look at 2nd derivative:
    maximaLoc = feval(diff(f{k}, 2), x(:,k)) < 0;

    % For end-points, look at 1st derivative:
    dfk_ends = feval(dfk, ends);
    endptMaxLoc = dfk_ends.*[1, -1]' < 0;

    % If 1st derivative is small at an endpoint, assume it's zero and try to
    % use 2nd derivative to determine if it's a minimum:
    %
    % [TODO]:  What if the 2nd derivative is zero, so that rounding error
    % precludes us from accurately determining the sign?
    smallEndDer = abs(dfk_ends) < 1e3*vscale(dfk)*eps;
    endptMaxLoc(smallEndDer) = feval(dfk2, ends(smallEndDer)) < 0;

    maximaLoc(1) = endptMaxLoc(1);
    maximaLoc(x(:,k) == ends(2)) = endptMaxLoc(2);

    % Set points corresponding to local maxima to NaN:
    y(~maximaLoc,k) = NaN;
    x(~maximaLoc,k) = NaN;

    % Sort the result
    [x(:,k), maximaLoc] = sort(x(:,k));
    y(:,k) = y(maximaLoc,k);
end

% Remove any rows which contain only NaNs.
x(all(isnan(x), 2),:) = [];
y(all(isnan(y), 2),:) = [];

end

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%% MAX(F, G) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function h = maxOfTwoChebfuns(f, g)
% Return the function h(x) = max(f(x), g(x)) for all x.

% If one is complex, use abs(f) and abs(g) to determine which function values to
% keep. (experimental feature)
if ( isreal(f) && isreal(g) && (nargin < 3) )
	S = sign(f - g);
else
	S = sign(abs(f) - abs(g));
end

% Heaviside function (0 where f > g, 1 where f < g);
H = 0.5*(S + 1);
notH = 0.5*(1 - S); % ~H.

% Combine for output:
h = H.*f + notH.*g;

% [TODO]: Enforce continuity?

% TODO: Why simplify?
% Simplify:
h = simplify(h);

end


function polar_info_esti = FastSCL_decoder(llr, L, info_bits, lambda_offset, llr_layer_vec, bit_layer_vec, psi_vec, node_type_matrix, H_crc)
%2018.1.7.14:16 Yu Y. R.
%LLR-based SCL deocoder
%Systematic polar code used
%4 nodes are identified and decoded, i.e., rate0, rate1, rep, spc,
%No BLER loss, while using fast algorithms.
%const
N = length(llr);
m = log2(N);
%memory declared
P = zeros(N - 1, L); %llr is public-used, so N - 1 is enough.
C = zeros(2 * N - 1, 2 * L);
PM = zeros(L, 1);
activepath = zeros(L, 1);
lazy_copy = zeros(m, L);
%initialize the 1st SC decoder
activepath(1) = 1;
lazy_copy(:, 1) = 1;
%decoding starts
%default: in the case of path clone in REP and leaf node,
%origianl always corresponds to bit 0s, while the new path bit 1s.
for i_node = 1 : size(node_type_matrix, 1)
    current_index = node_type_matrix(i_node, 1);
    llr_layer = llr_layer_vec(current_index);
    M = node_type_matrix(i_node, 2);%number of leaf nodes in this constituent node
    reduced_layer = log2(M); %For LLR recursion, this reduced layer denotes where to stop; For Bit recursion, this denotes where to start.
    psi = psi_vec(i_node);%This psi is used for bits recursion
    psi_mod_2 = mod(psi, 2);%To decide whether the bit recuision should continue. 1 : continue; 0 : stop.
    for l_index = 1 : L
        if activepath(l_index) == 0
            continue;
        end
        switch current_index
            case 1
                index_1 = lambda_offset(m);
                for beta = 0 : index_1 - 1
                    P(beta + index_1, l_index) = sign(llr(beta + 1)) *  sign(llr(beta + index_1 + 1)) * min(abs(llr(beta + 1)), abs(llr(beta + index_1 + 1)));
                end
                for i_layer = m - 2 : -1 : reduced_layer
                    index_1 = lambda_offset(i_layer + 1);
                    index_2 = lambda_offset(i_layer + 2);
                    for beta = index_1 : index_2 - 1
                        P(beta, l_index) = sign(P(beta + index_1, l_index)) *  sign(P(beta + index_2, l_index)) * ...
                            min(abs(P(beta + index_1, l_index)), abs(P(beta + index_2, l_index)));
                    end
                end
            case N/2 + 1
                index_1 = lambda_offset(m);
                for beta = 0 : index_1 - 1
                    x_tmp = C(beta + index_1, 2 * l_index - 1);
                    P(beta + index_1, l_index) = (1 - 2 * x_tmp) * llr(beta + 1) + llr(beta + index_1 + 1);
                end
                for i_layer = m - 2 : -1 : reduced_layer
                    index_1 = lambda_offset(i_layer + 1);
                    index_2 = lambda_offset(i_layer + 2);
                    for beta = index_1 : index_2 - 1
                        P(beta, l_index) = sign(P(beta + index_1, l_index)) *  sign(P(beta + index_2, l_index)) * ...
                            min(abs(P(beta + index_1, l_index)), abs(P(beta + index_2, l_index)));
                    end
                end
            otherwise
                index_1 = lambda_offset(llr_layer + 1);
                index_2 = lambda_offset(llr_layer + 2);
                for beta = index_1 : index_2 - 1
                    %lazy copy
                    P(beta, l_index) = (1 - 2 * C(beta, 2 * l_index - 1)) * P(beta + index_1, lazy_copy(llr_layer + 2, l_index)) + P(beta + index_2, lazy_copy(llr_layer + 2, l_index));
                end
                for i_layer = llr_layer - 1 : -1 : reduced_layer
                    index_1 = lambda_offset(i_layer + 1);
                    index_2 = lambda_offset(i_layer + 2);
                    for beta = index_1 : index_2 - 1
                        P(beta, l_index) = sign(P(beta + index_1, l_index)) *  sign(P(beta + index_2, l_index)) * ...
                            min(abs(P(beta + index_1, l_index)), abs(P(beta + index_2, l_index)));
                    end
                end
        end
    end%LLR calculations. You may fold thiis code block.
    index_vec = M : 2 * M - 1;
    switch node_type_matrix(i_node, 3)
        case -1%****RATE 0*****
            x = zeros(M, 1);
            for l_index = 1 : L
                if activepath(l_index) == 0
                    continue;
                end
                C(index_vec, psi_mod_2 + 2 * l_index - 1) = x;
                PM(l_index) = PM(l_index) + sum(log(1 + exp(-P(index_vec, l_index))));
                %Do not forget updating path metric in rate 0 nodes
                %No copy in Rate 0 node
            end
        case 1%*****RATE 1*****
            %input LLR: P(index_vec, :)
            [PM, activepath, selected_subcodeword, lazy_copy] = Rate1(M, P(index_vec, :), activepath, PM, L, lazy_copy);
            %output: estimated sub polar codeword: 'selected_subcodeword'.
            for l_index = 1 : L
                if activepath(l_index) == 0
                    continue
                end
                C(index_vec, 2 * l_index - 1 + psi_mod_2) = selected_subcodeword(:, l_index);
            end
        case 2%*****REP*****
           [PM, activepath, lazy_copy, selected_subcodeword] = Rep(PM, P(index_vec, :), activepath, L, M, lazy_copy);
           for l_index = 1 : L
               if activepath(l_index) == 0
                   continue
               end
               C(index_vec, 2 * l_index - 1 + psi_mod_2) = selected_subcodeword(:, l_index);
           end
        case 3%*******SPC*******
            [PM, activepath, selected_subcodeword, lazy_copy] = SPC(index_vec, P(index_vec, :), activepath, PM, L, lazy_copy);
            for l_index = 1 : L
                if activepath(l_index) == 0
                    continue
                end
                C(index_vec, 2 * l_index - 1 + psi_mod_2) = selected_subcodeword(:, l_index);
            end
    end
    %Internal Bits Update for each active path
    bit_layer = bit_layer_vec(current_index + M - 1);
    for l_index = 1 : L
        if activepath(l_index) == 0
            continue;
        end
        if psi_mod_2 == 1%right child of its mother
            for i_layer = reduced_layer : bit_layer - 1
                index_1 = lambda_offset(i_layer + 1);
                index_2 = lambda_offset(i_layer + 2);
                for beta = index_1 : index_2 - 1
                    C(beta + index_1, 2 * l_index) = mod(C(beta, 2 * lazy_copy(i_layer + 1, l_index) - 1) + C(beta, 2 * l_index), 2);
                    C(beta + index_2, 2 * l_index) = C(beta, 2 * l_index);
                end
            end
            index_1 = lambda_offset(bit_layer + 1);
            index_2 = lambda_offset(bit_layer + 2);
            for beta = index_1 : index_2 - 1
                C(beta + index_1, 2 * l_index - 1) = mod(C(beta, 2 * lazy_copy(bit_layer + 1, l_index) - 1) + C(beta, 2 * l_index), 2);
                C(beta + index_2, 2 * l_index - 1) = C(beta, 2 * l_index);
            end
        end
    end
    %lazy copy
    if i_node < size(node_type_matrix, 1)
        for i_layer = 1 : llr_layer_vec(current_index + M) + 1
            for l_index = 1 : L
                lazy_copy(i_layer, l_index) = l_index;
            end
        end
    end
end
%select the best path
[~, path_ordered] = sort(PM);
for l_index = 1 : L
    path_num = path_ordered(l_index);
    x = C(end - N + 1 : end, 2 * path_num - 1);
    info_with_crc = x(info_bits);
    err = sum(mod(H_crc * info_with_crc, 2));
    if err == 0
        polar_info_esti = info_with_crc;
        break;
    else
        if l_index == L
            x = C(end - N + 1 : end, 2 * path_ordered(1) - 1);
            polar_info_esti = x(info_bits);
        end
    end
end
end


function n = gqu_order ( l )

%*****************************************************************************80
%
%% GQU_ORDER computes the order of a GQU rule from the level.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 June 2012
%
%  Author:
%
%    John Burkardt.
%
%  Parameters:
%
%    Input, integer L, the level of the rule.
%    1 <= L.
%
%    Output, integer N, the order of the rule.
%
  if ( l < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'GQU_ORDER - Fatal error!\n' );
    fprintf ( 1, '  1 <= L required.\n' );
    fprintf ( 1, '  Input L = %d\n', l );
    error (  'GQU_ORDER - Fatal error!' );
  elseif ( l <= 25 )
    n = l;
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, 'GQU_ORDER - Fatal error!\n' );
    fprintf ( 1, '  L <= 25 required.\n' );
    fprintf ( 1, '  Input L = %d\n', l );
    error (  'GQU_ORDER - Fatal error!' );
  end

  return
end


function [y]=tt_rc2(d,n,elem_fun,eps,varargin)
%[Y]=TT_RC2(D,N,ARR,ELEM_FUN,EPS,[OPTIONS])
%The TT-Renormalization-Cross algorithm
%input is: the dimension of the array d, the
%size vector n, the function that computes the prescribed element
%of an array, accuracy parameter eps
%and also additional arguments specified as 'rmax',10,'nswp',10
%'verb',true and so on
% 'x0', x0
%The elem function has syntax elem_fun(ind); all other additional
%information
%has to be passed as "anonymous" arguments

%The algorithm. It needs left & right index sets to be initialized.
%It needs supecores to be precomputed
%It is a good idea to store also the supercores (there will be d-1,
%i think) & update them; also certain indices maybe removed from the
%index set?? we will not do that;
%Two possibilities: current index contains maxvol submatrix;
%If not, increase, increase & increase (maybe even beyond the rank!)
%rank one subtraction can be made very stable

%Default parameters
if ( numel(n) == 1 )
  n=n*ones(1,d);
end
nswp=10;
rmax=1000;
x0=[];
verb=true;
vec=false; %If there is a vectorization in options, i.e. if elem_fun
%supports vectorized computations of many elements at once
for i=1:2:length(varargin)-1
    switch lower(varargin{i})
        case 'nswp'
            nswp=varargin{i+1};
        case 'rmax'
            rmax=lower(varargin{i+1});
        case 'x0'
            x0=varargin{i+1};
        case 'verb'
            verb=varargin{i+1};
        case 'vec'
            vec=varargin{i+1};
        otherwise
            error('Unrecognized option: %s\n',varargin{i});
    end
end

%The initial setup is done.

%We need initial index sets. Say, the vector of all ones (he-he)
%i_left;
%r_left;
%i_right;
%r_right;
i_left=cell(d,1);
i_right=cell(d,1);
r_left=cell(d,1);
r_right=cell(d,1);
ry=ones(d+1,1); %Initial ranks
%Find fiber maximum
ind=2*ones(d,1);
ind=find_fiber_maximum(elem_fun,ind);
for i=1:d
  i_left{i}=ind(i);
  i_right{i}=ind(i);
  r_left{i}=1;
  r_right{i}=1;
end

%Before we get the multiindex sets (helps to compute s-u-p-e-r-c-o-r-e-s)

ind_left=get_multi_left(i_left,r_left,ry);
ind_right=get_multi_right(i_right,r_right,ry);

%Now we have to compute s-u-p-e-r-c-o-r-e-s (not cores!) using ind_left &
%ind_right
super_core=cell(d-1,1);
for i=1:d-1 %There are d-1 s-u-p-e-r-c-o-r-e-s
   %Compute the index set for the i-th one
   index_set=zeros(d,ry(i),n(i),n(i+1),ry(i+2));
   if ( i == 1 )
       ileft=zeros(1,0);
   else
      ileft=ind_left{i-1};
   end
   if ( i == d - 1 )
      iright = zeros(1,0);
   else
      iright=ind_right{i+2};
   end
   for s1=1:ry(i)
      for s2=1:ry(i+2)
        for i1=1:n(i)
           for i2=1:n(i+1)
              index_set(:,s1,i1,i2,s2)=[ileft(s1,:),i1,i2,iright(s2,:)];
           end
        end
      end
   end
   M=ry(i)*n(i)*n(i+1)*ry(i+2);
   index_set=reshape(index_set,[d,M]);
   if ( vec )
      super_core{i}=reshape(elem_fun(index_set),[ry(i),n(i),n(i+1),ry(i+2)]);
   else
       cur_core=zeros(M,1);
      for k=1:M
         cur_core(k)=elem_fun(index_set(:,k));
      end
      super_core{i}=reshape(cur_core,[ry(i),n(i),n(i+1),ry(i+2)]);
   end

end
%Initialization is done.
%Now to the main iteration

%Through all the s-u-p-e-r-c-o-r-e-s
swp=1;
dir='lr';
i=1;
while ( swp <= nswp )
   %The method acts as follows.
   %1) Test if the current supercore is well approximated by the
   %current cross
   %2) If yes, do nothing.
   %3) If no, add the required indices to the cross and perform
   %modifications of the s-u-p-e-r-c-o-r-e-s

   %Nikrena ne soobrajau
   %Bilo: (i1,i2,i3,i4,i5)
   %Pofiksili (i1,i2), uvelichili r2. Rashirilos' mnojestvo (i2,i3,i4,i5)
   %(nikomu ne nujno)
   %V seredine: (i1,i2) (i3,i4,i5)+ - uvelichilos' mojectvo
   %Kogda uvelichili (i3,i4,i5) viros ry(3), uvelichilos 1 superyadro
   cur_core=super_core{i};
   %we have to convert the pair i_left{i} & r_left{i} to the
   %index set for the matrix cur_core

   cur_core=reshape(cur_core,[ry(i)*n(i),n(i+1)*ry(i+2)]);
   %i1=get_full_ind(i_left{i},r_left{i});
   %i2=get_full_ind(i_right{i+1},r_right{i+1});
    %ry(i)*n(i)

    i1=r_left{i}+(i_left{i}-1)*ry(i);
    i2=i_right{i+1}+(r_right{i+1}-1)*n(i+1);
  % if ( numel(i1)==2)
  %     if ( i1(1) == 2 && i1(2) == 2 )
  %   keyboard;
  %     end
  % end
  [i1,i2]=new_cross(cur_core,i1,i2,eps);
       %Increase i_left{i} & i_right{i+1}
       [radd_left,iadd_left]=ind2sub([ry(i);n(i)],i1);
       [iadd_right,radd_right]=ind2sub([n(i+1);ry(i+2)],i2);
       i_left{i}=iadd_left;
       r_left{i}=radd_left;
       %if ( i == 8 )
       %    keyboard
       %end
       i_right{i+1}=iadd_right;
       r_right{i+1}=radd_right;
       ry(i+1)=numel(i1);
       %Zaglushka
       ind_left=get_multi_left(i_left,r_left,ry);

       ind_right=get_multi_right(i_right,r_right,ry);

      %   fprintf('Index set in question: \n');
      %   ind_left{3}
      % if ( i == 3 && swp == 2 )
      %   keyboard;
      % end
       %keyboard;
       if ( i > 1 )
         %Recompute supercore{i-1}
         super_core{i-1}=compute_supercore(i-1,elem_fun,d,n,ry,ind_left,ind_right,vec);
       end
       if ( i < d - 1)
         super_core{i+1}=compute_supercore(i+1,elem_fun,d,n,ry,ind_left,ind_right,vec);
         %Recompute supercore{i+1};
       end

   %Convert iadd1 to i_left and i_right format; compute new
   %ind_left{i+1},ind_right{i+1}
   if ( strcmp(dir,'lr') )
     if ( i == d-1 )
        dir='rl';
     else
        i=i+1;
     end
   else
     if ( i == 1 )
        dir ='lr';
        swp=swp+1;
     else
        i=i-1;
     end
   end
   fprintf('Step %d sweep %d rank=%3.1f \n',i,swp,mean(ry));
end
fprintf('Done! \n');
keyboard;

return
end

function [super_core]=compute_supercore(i,elem_fun,d,n,ry,ind_left,ind_right,vec)
%[super_core]=compute_supercore(i,elem_fun,n,ry,ind_left,ind_right)
   index_set=zeros(d,ry(i),n(i),n(i+1),ry(i+2));
   if ( i == 1 )
       ileft=zeros(1,0);
   else
      ileft=ind_left{i-1};
   end
   if ( i == d - 1 )
      iright = zeros(1,0);
   else
      iright=ind_right{i+2};
   end
   for s1=1:ry(i)
      for s2=1:ry(i+2)
        for i1=1:n(i)
           for i2=1:n(i+1)
              index_set(:,s1,i1,i2,s2)=[ileft(s1,:),i1,i2,iright(s2,:)];
           end
        end
      end
   end
   M=ry(i)*n(i)*n(i+1)*ry(i+2);
   index_set=reshape(index_set,[d,M]);
   if ( vec )
      super_core=reshape(elem_fun(index_set),[ry(i),n(i),n(i+1),ry(i+2)]);
   else
       cur_core=zeros(M,1);
      for k=1:M
         cur_core(k)=elem_fun(index_set(:,k));
      end
      super_core=reshape(cur_core,[ry(i),n(i),n(i+1),ry(i+2)]);
   end

return
end

function [i1,i2]=new_cross(mat,i1,i2,eps)
%[i1,i2]=enlarge_cross(mat,i1,i2,eps)
%Tests whether the current index set gives a reasonable approximation
%to the matrix, and enlarges the basis if necessary.
%The method acts as follows.

%A-C*A11^{-1} R

%sbm=mat(i1,i2);
%[u,s,v]=svd(sbm,'econ');
%nrm=norm(mat);s=diag(s);
%r=numel(find(
%mat_save=mat;
i1=[];
i2=[];
desirata=eps*norm(mat,'fro');
er=norm(mat,'fro');
while ( er > desirata )
   %Find maximal element in mat
   [~,ind]=max(abs(mat(:)));
   ind=tt_ind2sub(size(mat),ind);
   i=ind(1); j=ind(2);
    i1=[i1;i];
    i2=[i2;j];
    u1=mat(:,j); u2=mat(i,:);
    u1=u1/mat(i,j);
    mat=mat-u1*u2;
    er=norm(mat,'fro');
end
% i0=[i1;iadd1]; j0=[i2;iadd2];
% sbm=mat_save(i0,j0);
% ss=svd(sbm);
% fprintf('%10.10e \n',ss)
% sbm=mat_save(i1,i2);
% fprintf('AND PREVIOUS \n');
% ss=svd(sbm);
% fprintf('%10.10e \n',ss)

return
end

function [ind_left]=get_multi_left(i_left,r_left,ry)
%[ind_left]=get_multi_left(i_left,r_left,ry)
%Computes (all) left multiindex sets for a given compact representation
  d=numel(i_left);
  ind_left=cell(d,1);
  ind_left{1}=i_left{1};
  for i=2:d
      %ind_cur=zeros(
      %ind_cur is an array of size ry(i-1) x i
      ind_cur=zeros(ry(i+1),i);
      r_prev=r_left{i};
      ind_prev=ind_left{i-1};
      i_prev=i_left{i};
      for s=1:ry(i+1)
          %ind_prev(p1(s
         ind_cur(s,:)=[ind_prev(r_prev(s),:),i_prev(s)];
      end
      ind_left{i}=ind_cur;
  end
return
end

function [ind_right]=get_multi_right(i_right,r_right,ry)
%[ind_right]=get_multi_right(i_right,r_right,ry)
%Computes (all) right multiindex sets for a given compact representation
  d=numel(i_right);
  ind_right=cell(d,1);
  ind_right{d}=i_right{d};
  for i=d-1:-1:1
      %ind_cur=zeros(
      %ind_cur is an array of size ry(i) x i
      ind_cur=zeros(ry(i),d-i+1);
      r_prev=r_right{i};
      ind_prev=ind_right{i+1};
      i_prev=i_right{i};
      for s=1:ry(i)
         ind_cur(s,:)=[i_prev(s), ind_prev(r_prev(s),:)];
      end
      ind_right{i}=ind_cur;
  end

return
end

function [ind]=find_fiber_maximum(elem_fun,ind)
%[ind]=find_fiber_maximum(elem_fun,ind)
%Simple ALS method to compute (approximate) maximum in a tensor
%In fact, need some non-zero
%fact=2; %If the new one <= fact times larger than the previous, stop

return
end


function cy = zcopy ( n, cx, incx, cy, incy )

%*****************************************************************************80
%
%% ZCOPY copies a complex vector X to a vector Y.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 May 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979.
%
%    Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
%    Basic Linear Algebra Subprograms for Fortran Usage,
%    Algorithm 539,
%    ACM Transactions on Mathematical Software,
%    Volume 5, Number 3, September 1979, pages 308-323.
%
%  Parameters:
%
%    Input, integer N, the number of elements in CX and CY.
%
%    Input, complex CX(*), the first vector.
%
%    Input, integer INCX, the increment between successive entries of CX.
%
%    Input, complex CY(*), the second vector.
%
%    Input, integer INCY, the increment between successive entries of CY.
%
%    Output, complex CY(*), the second vector, with certain elements
%    copied from CX.
%
  cy(1:incy:1+(n-1)*incy) = cx(1:incx:1+(n-1)*incx);

  return
end


%SerialLink.IKINEM Numerical inverse kinematics by minimization
%
% Q = R.ikinem(T) is the joint coordinates corresponding to the robot
% end-effector pose T which is a homogenenous transform.
%
% Q = R.ikinem(T, Q0, OPTIONS) specifies the initial estimate of the joint
% coordinates.
%
% In all cases if T is 4x4xM it is taken as a homogeneous transform sequence
% and R.ikinem() returns the joint coordinates corresponding to each of the
% transforms in the sequence.  Q is MxN where N is the number of robot joints.
% The initial estimate of Q for each time step is taken as the solution
% from the previous time step.
%
% Options::
% 'pweight',P      weighting on position error norm compared to rotation
%                  error (default 1)
% 'stiffness',S    Stiffness used to impose a smoothness contraint on joint
%                  angles, useful when N is large (default 0)
% 'qlimits'        Enforce joint limits
% 'ilimit',L       Iteration limit (default 1000)
% 'nolm'           Disable Levenberg-Marquadt
%
% Notes::
% - PROTOTYPE CODE UNDER DEVELOPMENT, intended to do numerical inverse kinematics
%   with joint limits
% - The inverse kinematic solution is generally not unique, and
%   depends on the initial guess Q0 (defaults to 0).
% - The function to be minimized is highly nonlinear and the solution is
%   often trapped in a local minimum, adjust Q0 if this happens.
% - The default value of Q0 is zero which is a poor choice for most
%   manipulators (eg. puma560, twolink) since it corresponds to a kinematic
%   singularity.
% - Such a solution is completely general, though much less efficient
%   than specific inverse kinematic solutions derived symbolically, like
%   ikine6s or ikine3.% - Uses Levenberg-Marquadt minimizer LMFsolve if it can be found,
%   if 'nolm' is not given, and 'qlimits' false
% - The error function to be minimized is computed on the norm of the error
%   between current and desired tool pose.  This norm is computed from distances
%   and angles and 'pweight' can be used to scale the position error norm to
%   be congruent with rotation error norm.
% - This approach allows a solution to obtained at a singularity, but
%   the joint angles within the null space are arbitrarily assigned.
% - Joint offsets, if defined, are added to the inverse kinematics to
%   generate Q.
% - Joint limits become explicit contraints if 'qlimits' is set.
%
% See also fminsearch, fmincon, SerialLink.fkine, SerialLink.ikine, tr2angvec.

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

function qt = ikinem(robot, tr, varargin)

    opt.pweight = 1;
    opt.stiffness = 0;
    opt.qlimits = false;
    opt.ilimit = 1000;
    opt.lm = true;
    opt.col = 2;

    [opt,args] = tb_optparse(opt, varargin);

    % check if optional argument is a valid q
    q0 = args{1};
    if numel(q0) ~= robot.n
        error('q0 length must match number of joints in robot');
    end

    for i=1:size(tr,3)
        T = tr(:,:,i);

        if opt.qlimits
            % constrained optimization to handle joint limits
            options = optimset('MaxIter', opt.ilimit);
            qlim = robot.qlim;

            [q, ef, exflag, output] = fmincon( @(x) costfun(x, robot, T, opt), q0, ...
                [], [], [], [], ...
                qlim(:,1), qlim(:,2), ...
                [], options);

            if opt.verbose
                fprintf('final error %f, %d iterations, %d evalations\n', ...
                    ef, output.iterations, output.funcCount);
            end

        else
            % no joint limits, unconstrained optimization
            if exist('LMFsolve') == 2 && opt.lm
                [q, ef, count] = LMFsolve( @(x) costfun(x, robot, T, opt), q0, 'MaxIter', opt.ilimit);
                q = q';
                if opt.verbose
                    fprintf('final error %f, %d iterations\n', ...
                        ef, count);
                end
            else
                options = optimset('MaxIter', opt.ilimit);
                [q, ef, exflag, output] = fminsearch( @(x) costfun(x, robot, T, opt), q0, options);

                if opt.verbose
                    fprintf('final error %f, %d iterations, %d evalations\n', ...
                        ef, output.iterations, output.funcCount);
                end
            end
        end

        qt(i,:) = q;
    end

    if opt.verbose
        robot.fkine(qt)
    end
end

% The cost function, this is the value to be minimized
function E = costfun(q, robot, T, opt)

    Tq = robot.fkine(q);
    % find the pose error in SE(3)
    dT = transl(T) - transl(Tq);

    % translation error
    E = norm(dT) * opt.pweight;

    % rotation error
    %  find dot product of
    dd = dot(T(1:3,opt.col), Tq(1:3,opt.col));
    %E = E + (1 - dd)^2*100000 ;
    E = E + acos(dd)^2*1000 ;

    if opt.stiffness > 0
        % enforce a continuity constraint on joints, minimum bend
        E = E + sum( diff(q).^2 ) * opt.stiffness;
    end
end